Skip to content
GitLab
Unverified Commit 979e9922 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files

Merge pull request #9010 from DonLakeFlyer/APMGyroCal 

ArduPilot: Sensors - Add separate gyro cal support for ArduCopter based vehicles
parents c47886df d46cdc93
Hide whitespace changes
Inline Side-by-side
src/AutoPilotPlugins/APM/APMSensorsComponent.qml
View file @ 979e9922
......@@ -569,6 +569,26 @@ SetupPage {
} // QGCViewDialog
} // Component - calibratePressureDialogComponent
Component {
id: calibrateGyroDialogComponent
QGCViewDialog {
id: calibrateGyroDialog
function accept() {
controller.calibrateGyro()
calibrateGyroDialog.hideDialog()
}
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: qsTr("For Gyroscope calibration you will need to place your vehicle on a surface and leave it still.\n\nClick Ok to start calibration.")
}
}
}
QGCFlickable {
id: buttonFlickable
anchors.left: parent.left
......@@ -621,12 +641,19 @@ SetupPage {
}
}
QGCButton {
width: _buttonWidth
text: qsTr("Gyro")
visible: activeVehicle && (activeVehicle.multiRotor | activeVehicle.rover)
onClicked: mainWindow.showComponentDialog(calibrateGyroDialogComponent, qsTr("Calibrate Gyro"), mainWindow.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok)
}
QGCButton {
width: _buttonWidth
text: _calibratePressureText
onClicked: mainWindow.showComponentDialog(calibratePressureDialogComponent, _calibratePressureText, mainWindow.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok)
readonly property string _calibratePressureText: activeVehicle.fixedWing ? qsTr("Cal Baro/Airspeed") : qsTr("Calibrate Pressure")
readonly property string _calibratePressureText: activeVehicle.fixedWing ? qsTr("Baro/Airspeed") : qsTr("Pressure")
}
QGCButton {
......
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
View file @ 979e9922
......@@ -294,7 +294,7 @@ void APMSensorsComponentController::calibrateAccel(void)
_calTypeInProgress = CalTypeAccel;
_vehicle->setConnectionLostEnabled(false);
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationAccel);
_vehicle->startCalibration(Vehicle::CalibrationAccel);
}
void APMSensorsComponentController::calibrateMotorInterference(void)
......@@ -305,7 +305,7 @@ void APMSensorsComponentController::calibrateMotorInterference(void)
_appendStatusLog(tr("Raise the throttle slowly to between 50% ~ 75% (the props will spin!) for 5 ~ 10 seconds."));
_appendStatusLog(tr("Quickly bring the throttle back down to zero"));
_appendStatusLog(tr("Press the Next button to complete the calibration"));
_uas->startCalibration(UASInterface::StartCalibrationCompassMot);
_vehicle->startCalibration(Vehicle::CalibrationAPMCompassMot);
}
void APMSensorsComponentController::levelHorizon(void)
......@@ -314,7 +314,7 @@ void APMSensorsComponentController::levelHorizon(void)
_vehicle->setConnectionLostEnabled(false);
_startLogCalibration();
_appendStatusLog(tr("Hold the vehicle in its level flight position."));
_uas->startCalibration(UASInterface::StartCalibrationLevel);
_vehicle->startCalibration(Vehicle::CalibrationLevel);
}
void APMSensorsComponentController::calibratePressure(void)
......@@ -323,7 +323,16 @@ void APMSensorsComponentController::calibratePressure(void)
_vehicle->setConnectionLostEnabled(false);
_startLogCalibration();
_appendStatusLog(tr("Requesting pressure calibration..."));
_uas->startCalibration(UASInterface::StartCalibrationPressure);
_vehicle->startCalibration(Vehicle::CalibrationAPMPressureAirspeed);
}
void APMSensorsComponentController::calibrateGyro(void)
{
_calTypeInProgress = CalTypeGyro;
_vehicle->setConnectionLostEnabled(false);
_startLogCalibration();
_appendStatusLog(tr("Requesting gyro calibration..."));
_vehicle->startCalibration(Vehicle::CalibrationGyro);
}
void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
......@@ -444,203 +453,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
_stopCalibration(StopCalibrationFailed);
return;
}
#if 0
if (text.contains(QLatin1Literal("progress <"))) {
QString percent = text.split("<").last().split(">").first();
bool ok;
int p = percent.toInt(&ok);
if (ok && _progressBar) {
_progressBar->setProperty("value", (float)(p / 100.0));
}
return;
}
QString anyKey(QStringLiteral("and press any"));
if (text.contains(anyKey)) {
text = text.left(text.indexOf(anyKey)) + QStringLiteral("and click Next to continue.");
_nextButton->setEnabled(true);
}
_appendStatusLog(text);
qCDebug(APMSensorsComponentControllerLog) << text << severity;
if (text.contains(QLatin1String("Calibration successful"))) {
_stopCalibration(StopCalibrationSuccess);
return;
}
if (text.contains(QLatin1String("FAILED"))) {
_stopCalibration(StopCalibrationFailed);
return;
}
// All calibration messages start with [cal]
QString calPrefix(QStringLiteral("[cal] "));
if (!text.startsWith(calPrefix)) {
return;
}
text = text.right(text.length() - calPrefix.length());
QString calStartPrefix(QStringLiteral("calibration started: "));
if (text.startsWith(calStartPrefix)) {
text = text.right(text.length() - calStartPrefix.length());
_startVisualCalibration();
if (text == QLatin1Literal("accel") || text == QLatin1Literal("mag") || text == QLatin1Literal("gyro")) {
// Reset all progress indication
_orientationCalDownSideDone = false;
_orientationCalUpsideDownSideDone = false;
_orientationCalLeftSideDone = false;
_orientationCalRightSideDone = false;
_orientationCalTailDownSideDone = false;
_orientationCalNoseDownSideDone = false;
_orientationCalDownSideInProgress = false;
_orientationCalUpsideDownSideInProgress = false;
_orientationCalLeftSideInProgress = false;
_orientationCalRightSideInProgress = false;
_orientationCalNoseDownSideInProgress = false;
_orientationCalTailDownSideInProgress = false;
// Reset all visibility
_orientationCalDownSideVisible = false;
_orientationCalUpsideDownSideVisible = false;
_orientationCalLeftSideVisible = false;
_orientationCalRightSideVisible = false;
_orientationCalTailDownSideVisible = false;
_orientationCalNoseDownSideVisible = false;
_orientationCalAreaHelpText->setProperty("text", "Place your vehicle into one of the Incomplete orientations shown below and hold it still");
if (text == "accel") {
_calTypeInProgress = CalTypeAccel;
_orientationCalDownSideVisible = true;
_orientationCalUpsideDownSideVisible = true;
_orientationCalLeftSideVisible = true;
_orientationCalRightSideVisible = true;
_orientationCalTailDownSideVisible = true;
_orientationCalNoseDownSideVisible = true;
} else if (text == "mag") {
_calTypeInProgress = CalTypeOffboardCompass;
_orientationCalDownSideVisible = true;
_orientationCalUpsideDownSideVisible = true;
_orientationCalLeftSideVisible = true;
_orientationCalRightSideVisible = true;
_orientationCalTailDownSideVisible = true;
_orientationCalNoseDownSideVisible = true;
} else {
Q_ASSERT(false);
}
emit orientationCalSidesDoneChanged();
emit orientationCalSidesVisibleChanged();
emit orientationCalSidesInProgressChanged();
_updateAndEmitShowOrientationCalArea(true);
}
return;
}
if (text.endsWith(QLatin1Literal("orientation detected"))) {
QString side = text.section(" ", 0, 0);
qDebug() << "Side started" << side;
if (side == QLatin1Literal("down")) {
_orientationCalDownSideInProgress = true;
if (_calTypeInProgress == CalTypeOffboardCompass) {
_orientationCalDownSideRotate = true;
}
} else if (side == QLatin1Literal("up")) {
_orientationCalUpsideDownSideInProgress = true;
if (_calTypeInProgress == CalTypeOffboardCompass) {
_orientationCalUpsideDownSideRotate = true;
}
} else if (side == QLatin1Literal("left")) {
_orientationCalLeftSideInProgress = true;
if (_calTypeInProgress == CalTypeOffboardCompass) {
_orientationCalLeftSideRotate = true;
}
} else if (side == QLatin1Literal("right")) {
_orientationCalRightSideInProgress = true;
if (_calTypeInProgress == CalTypeOffboardCompass) {
_orientationCalRightSideRotate = true;
}
} else if (side == QLatin1Literal("front")) {
_orientationCalNoseDownSideInProgress = true;
if (_calTypeInProgress == CalTypeOffboardCompass) {
_orientationCalNoseDownSideRotate = true;
}
} else if (side == QLatin1Literal("back")) {
_orientationCalTailDownSideInProgress = true;
if (_calTypeInProgress == CalTypeOffboardCompass) {
_orientationCalTailDownSideRotate = true;
}
}
if (_calTypeInProgress == CalTypeOffboardCompass) {
_orientationCalAreaHelpText->setProperty("text", tr("Rotate the vehicle continuously as shown in the diagram until marked as Completed"));
} else {
_orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation"));
}
emit orientationCalSidesInProgressChanged();
emit orientationCalSidesRotateChanged();
return;
}
if (text.endsWith(QLatin1Literal("side done, rotate to a different side"))) {
QString side = text.section(" ", 0, 0);
qDebug() << "Side finished" << side;
if (side == QLatin1Literal("down")) {
_orientationCalDownSideInProgress = false;
_orientationCalDownSideDone = true;
_orientationCalDownSideRotate = false;
} else if (side == QLatin1Literal("up")) {
_orientationCalUpsideDownSideInProgress = false;
_orientationCalUpsideDownSideDone = true;
_orientationCalUpsideDownSideRotate = false;
} else if (side == QLatin1Literal("left")) {
_orientationCalLeftSideInProgress = false;
_orientationCalLeftSideDone = true;
_orientationCalLeftSideRotate = false;
} else if (side == QLatin1Literal("right")) {
_orientationCalRightSideInProgress = false;
_orientationCalRightSideDone = true;
_orientationCalRightSideRotate = false;
} else if (side == QLatin1Literal("front")) {
_orientationCalNoseDownSideInProgress = false;
_orientationCalNoseDownSideDone = true;
_orientationCalNoseDownSideRotate = false;
} else if (side == QLatin1Literal("back")) {
_orientationCalTailDownSideInProgress = false;
_orientationCalTailDownSideDone = true;
_orientationCalTailDownSideRotate = false;
}
_orientationCalAreaHelpText->setProperty("text", tr("Place you vehicle into one of the orientations shown below and hold it still"));
emit orientationCalSidesInProgressChanged();
emit orientationCalSidesDoneChanged();
emit orientationCalSidesRotateChanged();
return;
}
if (text.startsWith(QLatin1Literal("calibration done:"))) {
_stopCalibration(StopCalibrationSuccess);
return;
}
if (text.startsWith(QLatin1Literal("calibration cancelled"))) {
_stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
return;
}
if (text.startsWith(QLatin1Literal("calibration failed"))) {
_stopCalibration(StopCalibrationFailed);
return;
}
#endif
}
void APMSensorsComponentController::_refreshParams(void)
......@@ -685,7 +497,7 @@ void APMSensorsComponentController::cancelCalibration(void)
emit waitingForCancelChanged();
// The firmware doesn't always allow us to cancel calibration. The best we can do is wait
// for it to timeout.
_uas->stopCalibration();
_vehicle->stopCalibration();
}
}
......@@ -728,36 +540,18 @@ bool APMSensorsComponentController::usingUDPLink(void)
void APMSensorsComponentController::_handleCommandAck(mavlink_message_t& message)
{
if (_calTypeInProgress == CalTypeLevelHorizon) {
mavlink_command_ack_t commandAck;
mavlink_msg_command_ack_decode(&message, &commandAck);
if (commandAck.command == MAV_CMD_PREFLIGHT_CALIBRATION) {
switch (commandAck.result) {
case MAV_RESULT_ACCEPTED:
_appendStatusLog(tr("Level horizon complete"));
_stopCalibration(StopCalibrationSuccessShowLog);
break;
default:
_appendStatusLog(tr("Level horizon failed"));
_stopCalibration(StopCalibrationFailed);
break;
}
}
}
if (_calTypeInProgress == CalTypePressure) {
if (_calTypeInProgress == CalTypeLevelHorizon || _calTypeInProgress == CalTypeGyro || _calTypeInProgress == CalTypePressure) {
mavlink_command_ack_t commandAck;
mavlink_msg_command_ack_decode(&message, &commandAck);
if (commandAck.command == MAV_CMD_PREFLIGHT_CALIBRATION) {
switch (commandAck.result) {
case MAV_RESULT_ACCEPTED:
_appendStatusLog(tr("Pressure calibration success"));
_appendStatusLog(tr("Successfully completed"));
_stopCalibration(StopCalibrationSuccessShowLog);
break;
default:
_appendStatusLog(tr("Pressure calibration fail"));
_appendStatusLog(tr("Failed"));
_stopCalibration(StopCalibrationFailed);
break;
}
......
src/AutoPilotPlugins/APM/APMSensorsComponentController.h
View file @ 979e9922
......@@ -7,9 +7,7 @@
*
****************************************************************************/
#ifndef APMSensorsComponentController_H
#define APMSensorsComponentController_H
#pragma once
#include <QObject>
......@@ -31,70 +29,72 @@ public:
APMSensorsComponentController(void);
~APMSensorsComponentController();
Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog)
Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar)
Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog)
Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar)
Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText)
Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText)
Q_PROPERTY(bool compassSetupNeeded READ compassSetupNeeded NOTIFY setupNeededChanged)
Q_PROPERTY(bool accelSetupNeeded READ accelSetupNeeded NOTIFY setupNeededChanged)
Q_PROPERTY(bool compassSetupNeeded READ compassSetupNeeded NOTIFY setupNeededChanged)
Q_PROPERTY(bool accelSetupNeeded READ accelSetupNeeded NOTIFY setupNeededChanged)
Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged)
Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged)
Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideDone MEMBER _orientationCalUpsideDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalLeftSideDone MEMBER _orientationCalLeftSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalRightSideDone MEMBER _orientationCalRightSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalNoseDownSideDone MEMBER _orientationCalNoseDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalTailDownSideDone MEMBER _orientationCalTailDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideDone MEMBER _orientationCalUpsideDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalLeftSideDone MEMBER _orientationCalLeftSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalRightSideDone MEMBER _orientationCalRightSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalNoseDownSideDone MEMBER _orientationCalNoseDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalTailDownSideDone MEMBER _orientationCalTailDownSideDone NOTIFY orientationCalSidesDoneChanged)
Q_PROPERTY(bool orientationCalDownSideVisible MEMBER _orientationCalDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideVisible MEMBER _orientationCalUpsideDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalLeftSideVisible MEMBER _orientationCalLeftSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalRightSideVisible MEMBER _orientationCalRightSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalNoseDownSideVisible MEMBER _orientationCalNoseDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalTailDownSideVisible MEMBER _orientationCalTailDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalDownSideVisible MEMBER _orientationCalDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideVisible MEMBER _orientationCalUpsideDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalLeftSideVisible MEMBER _orientationCalLeftSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalRightSideVisible MEMBER _orientationCalRightSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalNoseDownSideVisible MEMBER _orientationCalNoseDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalTailDownSideVisible MEMBER _orientationCalTailDownSideVisible NOTIFY orientationCalSidesVisibleChanged)
Q_PROPERTY(bool orientationCalDownSideInProgress MEMBER _orientationCalDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideInProgress MEMBER _orientationCalUpsideDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalLeftSideInProgress MEMBER _orientationCalLeftSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalRightSideInProgress MEMBER _orientationCalRightSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalNoseDownSideInProgress MEMBER _orientationCalNoseDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalTailDownSideInProgress MEMBER _orientationCalTailDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalDownSideInProgress MEMBER _orientationCalDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideInProgress MEMBER _orientationCalUpsideDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalLeftSideInProgress MEMBER _orientationCalLeftSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalRightSideInProgress MEMBER _orientationCalRightSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalNoseDownSideInProgress MEMBER _orientationCalNoseDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalTailDownSideInProgress MEMBER _orientationCalTailDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
Q_PROPERTY(bool orientationCalDownSideRotate MEMBER _orientationCalDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideRotate MEMBER _orientationCalUpsideDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalLeftSideRotate MEMBER _orientationCalLeftSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalRightSideRotate MEMBER _orientationCalRightSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalNoseDownSideRotate MEMBER _orientationCalNoseDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalTailDownSideRotate MEMBER _orientationCalTailDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalDownSideRotate MEMBER _orientationCalDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalUpsideDownSideRotate MEMBER _orientationCalUpsideDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalLeftSideRotate MEMBER _orientationCalLeftSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalRightSideRotate MEMBER _orientationCalRightSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalNoseDownSideRotate MEMBER _orientationCalNoseDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool orientationCalTailDownSideRotate MEMBER _orientationCalTailDownSideRotate NOTIFY orientationCalSidesRotateChanged)
Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged)
Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged)
Q_PROPERTY(bool compass1CalSucceeded READ compass1CalSucceeded NOTIFY compass1CalSucceededChanged)
Q_PROPERTY(bool compass2CalSucceeded READ compass2CalSucceeded NOTIFY compass2CalSucceededChanged)
Q_PROPERTY(bool compass3CalSucceeded READ compass3CalSucceeded NOTIFY compass3CalSucceededChanged)
Q_PROPERTY(bool compass1CalSucceeded READ compass1CalSucceeded NOTIFY compass1CalSucceededChanged)
Q_PROPERTY(bool compass2CalSucceeded READ compass2CalSucceeded NOTIFY compass2CalSucceededChanged)
Q_PROPERTY(bool compass3CalSucceeded READ compass3CalSucceeded NOTIFY compass3CalSucceededChanged)
Q_PROPERTY(double compass1CalFitness READ compass1CalFitness NOTIFY compass1CalFitnessChanged)
Q_PROPERTY(double compass2CalFitness READ compass2CalFitness NOTIFY compass2CalFitnessChanged)
Q_PROPERTY(double compass3CalFitness READ compass3CalFitness NOTIFY compass3CalFitnessChanged)
Q_PROPERTY(double compass1CalFitness READ compass1CalFitness NOTIFY compass1CalFitnessChanged)
Q_PROPERTY(double compass2CalFitness READ compass2CalFitness NOTIFY compass2CalFitnessChanged)
Q_PROPERTY(double compass3CalFitness READ compass3CalFitness NOTIFY compass3CalFitnessChanged)
Q_INVOKABLE void calibrateCompass(void);
Q_INVOKABLE void calibrateAccel(void);
Q_INVOKABLE void calibrateMotorInterference(void);
Q_INVOKABLE void levelHorizon(void);
Q_INVOKABLE void calibratePressure(void);
Q_INVOKABLE void cancelCalibration(void);
Q_INVOKABLE void nextClicked(void);
Q_INVOKABLE bool usingUDPLink(void);
Q_INVOKABLE void calibrateCompass (void);
Q_INVOKABLE void calibrateAccel (void);
Q_INVOKABLE void calibrateGyro (void);
Q_INVOKABLE void calibrateMotorInterference (void);
Q_INVOKABLE void levelHorizon (void);
Q_INVOKABLE void calibratePressure (void);
Q_INVOKABLE void cancelCalibration (void);
Q_INVOKABLE void nextClicked (void);
Q_INVOKABLE bool usingUDPLink (void);
bool compassSetupNeeded(void) const;
bool accelSetupNeeded(void) const;
bool compassSetupNeeded (void) const;
bool accelSetupNeeded (void) const;
typedef enum {
CalTypeAccel,
CalTypeGyro,
CalTypeOnboardCompass,
CalTypeOffboardCompass,
CalTypeLevelHorizon,
......@@ -113,40 +113,40 @@ public:
double compass3CalFitness(void) const { return _rgCompassCalFitness[2]; }
signals:
void showGyroCalAreaChanged(void);
void showOrientationCalAreaChanged(void);
void orientationCalSidesDoneChanged(void);
void orientationCalSidesVisibleChanged(void);
void orientationCalSidesInProgressChanged(void);
void orientationCalSidesRotateChanged(void);
void resetStatusTextArea(void);
void waitingForCancelChanged(void);
void setupNeededChanged(void);
void calibrationComplete(CalType_t calType);
void compass1CalSucceededChanged(bool compass1CalSucceeded);
void compass2CalSucceededChanged(bool compass2CalSucceeded);
void compass3CalSucceededChanged(bool compass3CalSucceeded);
void compass1CalFitnessChanged(double compass1CalFitness);
void compass2CalFitnessChanged(double compass2CalFitness);
void compass3CalFitnessChanged(double compass3CalFitness);
void setAllCalButtonsEnabled(bool enabled);
void showGyroCalAreaChanged (void);
void showOrientationCalAreaChanged (void);
void orientationCalSidesDoneChanged (void);
void orientationCalSidesVisibleChanged (void);
void orientationCalSidesInProgressChanged (void);
void orientationCalSidesRotateChanged (void);
void resetStatusTextArea (void);
void waitingForCancelChanged (void);
void setupNeededChanged (void);
void calibrationComplete (CalType_t calType);
void compass1CalSucceededChanged (bool compass1CalSucceeded);
void compass2CalSucceededChanged (bool compass2CalSucceeded);
void compass3CalSucceededChanged (bool compass3CalSucceeded);
void compass1CalFitnessChanged (double compass1CalFitness);
void compass2CalFitnessChanged (double compass2CalFitness);
void compass3CalFitnessChanged (double compass3CalFitness);
void setAllCalButtonsEnabled (bool enabled);
private slots:
void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
void _handleUASTextMessage (int uasId, int compId, int severity, QString text);
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
void _mavCommandResult (int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
private:
void _startLogCalibration(void);
void _startVisualCalibration(void);
void _appendStatusLog(const QString& text);
void _refreshParams(void);
void _hideAllCalAreas(void);
void _resetInternalState(void);
void _handleCommandAck(mavlink_message_t& message);
void _handleMagCalProgress(mavlink_message_t& message);
void _handleMagCalReport(mavlink_message_t& message);
void _restorePreviousCompassCalFitness(void);
void _startLogCalibration (void);
void _startVisualCalibration (void);
void _appendStatusLog (const QString& text);
void _refreshParams (void);
void _hideAllCalAreas (void);
void _resetInternalState (void);
void _handleCommandAck (mavlink_message_t& message);
void _handleMagCalProgress (mavlink_message_t& message);
void _handleMagCalReport (mavlink_message_t& message);
void _restorePreviousCompassCalFitness (void);
enum StopCalibrationCode {
StopCalibrationSuccess,
......@@ -212,5 +212,3 @@ private:
static const int _supportedFirmwareCalVersion = 2;
};
#endif
src/AutoPilotPlugins/Common/RadioComponentController.cc
View file @ 979e9922
......@@ -701,7 +701,7 @@ void RadioComponentController::_writeCalibration(void)
if (!_uas) return;
if (_px4Vehicle()) {
_uas->stopCalibration();
_vehicle->stopCalibration();
}
if (!_px4Vehicle() && (_vehicle->vehicleType() == MAV_TYPE_HELICOPTER || _vehicle->multiRotor()) && _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionThrottle]].reversed) {
......@@ -798,7 +798,7 @@ void RadioComponentController::_startCalibration(void)
// Let the mav known we are starting calibration. This should turn off motors and so forth.
if (_px4Vehicle()) {
_uas->startCalibration(UASInterface::StartCalibrationRadio);
_vehicle->startCalibration(Vehicle::CalibrationRadio);
}
_nextButton->setProperty("text", tr("Next"));
......@@ -815,7 +815,7 @@ void RadioComponentController::_stopCalibration(void)
if (_uas) {
if (_px4Vehicle()) {
_uas->stopCalibration();
_vehicle->stopCalibration();
}
_setInternalCalibrationValuesFromParameters();
} else {
......@@ -1024,7 +1024,7 @@ void RadioComponentController::_signalAllAttitudeValueChanges(void)
void RadioComponentController::copyTrims(void)
{
_uas->startCalibration(UASInterface::StartCalibrationCopyTrims);
_vehicle->startCalibration(Vehicle::CalibrationCopyTrims);
}
bool RadioComponentController::_px4Vehicle(void) const
......
src/AutoPilotPlugins/PX4/PowerComponentController.cc
View file @ 979e9922
......@@ -7,10 +7,6 @@
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#include "PowerComponentController.h"
#include "QGCMAVLink.h"
#include "UAS.h"
......@@ -27,7 +23,7 @@ void PowerComponentController::calibrateEsc(void)
{
_warningMessages.clear();
connect(_vehicle, &Vehicle::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
_uas->startCalibration(UASInterface::StartCalibrationEsc);
_vehicle->startCalibration(Vehicle::CalibrationEsc);
}
void PowerComponentController::busConfigureActuators(void)
......
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
View file @ 979e9922
......@@ -192,31 +192,31 @@ void SensorsComponentController::_stopCalibration(SensorsComponentController::St
void SensorsComponentController::calibrateGyro(void)
{
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationGyro);
_vehicle->startCalibration(Vehicle::CalibrationGyro);
}
void SensorsComponentController::calibrateCompass(void)
{
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationMag);
_vehicle->startCalibration(Vehicle::CalibrationMag);
}
void SensorsComponentController::calibrateAccel(void)
{
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationAccel);
_vehicle->startCalibration(Vehicle::CalibrationAccel);
}
void SensorsComponentController::calibrateLevel(void)
{
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationLevel);
_vehicle->startCalibration(Vehicle::CalibrationLevel);
}
void SensorsComponentController::calibrateAirspeed(void)
{
_startLogCalibration();
_uas->startCalibration(UASInterface::StartCalibrationAirspeed);
_vehicle->startCalibration(Vehicle::CalibrationPX4Airspeed);
}
void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
......@@ -485,5 +485,5 @@ void SensorsComponentController::cancelCalibration(void)
_waitingForCancel = true;
emit waitingForCancelChanged();
_cancelButton->setEnabled(false);
_uas->stopCalibration();
_vehicle->stopCalibration();
}
src/Vehicle/Vehicle.cc
View file @ 979e9922
......@@ -1325,8 +1325,8 @@ void Vehicle::_handleHighLatency(mavlink_message_t& message)
const double altitude;
} coordinate {
highLatency.latitude / (double)1E7,
highLatency.longitude / (double)1E7,
static_cast<double>(highLatency.altitude_amsl)
highLatency.longitude / (double)1E7,
static_cast<double>(highLatency.altitude_amsl)
};
_coordinate.setLatitude(coordinate.latitude);
......@@ -3584,6 +3584,87 @@ void Vehicle::rebootVehicle()
sendMessageOnLinkThreadSafe(priorityLink(), msg);
}
void Vehicle::startCalibration(Vehicle::CalibrationType calType)
{
float param1 = 0;
float param2 = 0;
float param3 = 0;
float param4 = 0;
float param5 = 0;
float param6 = 0;
float param7 = 0;
switch (calType) {
case CalibrationGyro:
param1 = 1;
break;
case CalibrationMag:
param2 = 1;
break;
case CalibrationRadio:
param4 = 1;
break;
case CalibrationCopyTrims:
param4 = 2;
break;
case CalibrationAccel:
param5 = 1;
break;
case CalibrationLevel:
param5 = 2;
break;
case CalibrationEsc:
param7 = 1;
break;
case CalibrationPX4Airspeed:
param6 = 1;
break;
case CalibrationPX4Pressure:
param3 = 1;
break;
case CalibrationAPMCompassMot:
param6 = 1;
break;
case CalibrationAPMPressureAirspeed:
param3 = 1;
break;
case CalibrationAPMPreFlight:
param3 = 1; // GroundPressure/Airspeed
if (multiRotor() || rover()) {
// Gyro cal for ArduCopter only
param1 = 1;
}
}
// We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn
// causes the retry logic to break down.
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(_mavlink->getSystemId(),
_mavlink->getComponentId(),
priorityLink()->mavlinkChannel(),
&msg,
id(),
defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
0, // 0=first transmission of command
param1, param2, param3, param4, param5, param6, param7);
sendMessageOnLinkThreadSafe(priorityLink(), msg);
}
void Vehicle::stopCalibration(void)
{
sendMavCommand(defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
true, // showError
0, // gyro cal
0, // mag cal
0, // ground pressure
0, // radio cal
0, // accel cal
0, // airspeed cal
0); // unused
}
void Vehicle::setSoloFirmware(bool soloFirmware)
{
if (soloFirmware != _soloFirmware) {
......
src/Vehicle/Vehicle.h
View file @ 979e9922
......@@ -645,7 +645,7 @@ public:
Q_PROPERTY(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged)
Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged)
Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged)
Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged)
Q_PROPERTY(bool iARDURsROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged)
Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged)
Q_PROPERTY(bool readyToFlyAvailable READ readyToFlyAvailable NOTIFY readyToFlyAvailableChanged) ///< true: readyToFly signalling is available on this vehicle
Q_PROPERTY(bool readyToFly READ readyToFly NOTIFY readyToFlyChanged)
......@@ -985,6 +985,24 @@ public:
/// @return the maximum version
unsigned maxProtoVersion () const { return _maxProtoVersion; }
enum CalibrationType {
CalibrationRadio,
CalibrationGyro,
CalibrationMag,
CalibrationAccel,
CalibrationLevel,
CalibrationEsc,
CalibrationCopyTrims,
CalibrationAPMCompassMot,
CalibrationAPMPressureAirspeed,
CalibrationAPMPreFlight,
CalibrationPX4Airspeed,
CalibrationPX4Pressure,
};
void startCalibration (CalibrationType calType);
void stopCalibration (void);
Fact* roll () { return &_rollFact; }
Fact* pitch () { return &_pitchFact; }
Fact* heading () { return &_headingFact; }
......
src/uas/UAS.cc
View file @ 979e9922
......@@ -348,95 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message)
#pragma warning(pop, 0)
#endif
void UAS::startCalibration(UASInterface::StartCalibrationType calType)
{
if (!_vehicle) {
return;
}
int gyroCal = 0;
int magCal = 0;
int airspeedCal = 0;
int radioCal = 0;
int accelCal = 0;
int pressureCal = 0;
int escCal = 0;
switch (calType) {
case StartCalibrationGyro:
gyroCal = 1;
break;
case StartCalibrationMag:
magCal = 1;
break;
case StartCalibrationAirspeed:
airspeedCal = 1;
break;
case StartCalibrationRadio:
radioCal = 1;
break;
case StartCalibrationCopyTrims:
radioCal = 2;
break;
case StartCalibrationAccel:
accelCal = 1;
break;
case StartCalibrationLevel:
accelCal = 2;
break;
case StartCalibrationPressure:
pressureCal = 1;
break;
case StartCalibrationEsc:
escCal = 1;
break;
case StartCalibrationUavcanEsc:
escCal = 2;
break;
case StartCalibrationCompassMot:
airspeedCal = 1; // ArduPilot, bit of a hack
break;
}
// We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn
// causes the retry logic to break down.
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
uasId,
_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
0, // 0=first transmission of command
gyroCal, // gyro cal
magCal, // mag cal
pressureCal, // ground pressure
radioCal, // radio cal
accelCal, // accel cal
airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot
escCal); // esc cal
_vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
}
void UAS::stopCalibration(void)
{
if (!_vehicle) {
return;
}
_vehicle->sendMavCommand(_vehicle->defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
true, // showError
0, // gyro cal
0, // mag cal
0, // ground pressure
0, // radio cal
0, // accel cal
0, // airspeed cal
0); // unused
}
void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
{
if (!_vehicle) {
......
src/uas/UAS.h
View file @ 979e9922
......@@ -180,9 +180,6 @@ public slots:
/** @brief Receive a message from one of the communication links. */
virtual void receiveMessage(mavlink_message_t message);
void startCalibration(StartCalibrationType calType);
void stopCalibration(void);
void startBusConfig(StartBusConfigType calType);
void stopBusConfig(void);
......
src/uas/UASInterface.h
View file @ 979e9922
......@@ -39,31 +39,11 @@ public:
/** @brief The time interval the robot is switched on **/
virtual quint64 getUptime() const = 0;
enum StartCalibrationType {
StartCalibrationRadio,
StartCalibrationGyro,
StartCalibrationMag,
StartCalibrationAirspeed,
StartCalibrationAccel,
StartCalibrationLevel,
StartCalibrationPressure,
StartCalibrationEsc,
StartCalibrationCopyTrims,
StartCalibrationUavcanEsc,
StartCalibrationCompassMot,
};
enum StartBusConfigType {
StartBusConfigActuators,
EndBusConfigActuators,
};
/// Starts the specified calibration
virtual void startCalibration(StartCalibrationType calType) = 0;
/// Ends any current calibration
virtual void stopCalibration(void) = 0;
/// Starts the specified bus configuration
virtual void startBusConfig(StartBusConfigType calType) = 0;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment