zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #9010 from DonLakeFlyer/APMGyroCal 

ArduPilot: Sensors - Add separate gyro cal support for ArduCopter based vehicles
QGC4.4
Don Gagne 5 years ago committed by GitHub
parent
c47886df19 d46cdc9355
commit
979e9922f7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 239 additions and 437 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      src/AutoPilotPlugins/APM/APMSensorsComponent.qml
  2. 240
      src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
  3. 164
      src/AutoPilotPlugins/APM/APMSensorsComponentController.h
  4. 8
      src/AutoPilotPlugins/Common/RadioComponentController.cc
  5. 6
      src/AutoPilotPlugins/PX4/PowerComponentController.cc
  6. 12
      src/AutoPilotPlugins/PX4/SensorsComponentController.cc
  7. 85
      src/Vehicle/Vehicle.cc
  8. 20
      src/Vehicle/Vehicle.h
  9. 89
      src/uas/UAS.cc
  10. 3
      src/uas/UAS.h
  11. 20
      src/uas/UASInterface.h

29
src/AutoPilotPlugins/APM/APMSensorsComponent.qml
Unescape View File

@ -569,6 +569,26 @@ SetupPage { @@ -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
@ -623,10 +643,17 @@ SetupPage { @@ -623,10 +643,17 @@ 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 {

240
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
Unescape View File

@ -294,7 +294,7 @@ void APMSensorsComponentController::calibrateAccel(void) @@ -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) @@ -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) @@ -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) @@ -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, @@ -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) @@ -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) @@ -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;
}

164
src/AutoPilotPlugins/APM/APMSensorsComponentController.h
Unescape View File

@ -7,9 +7,7 @@ @@ -7,9 +7,7 @@
*
****************************************************************************/
#ifndef APMSensorsComponentController_H
#define APMSensorsComponentController_H
#pragma once
#include <QObject>
@ -31,70 +29,72 @@ public: @@ -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: @@ -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: @@ -212,5 +212,3 @@ private:
static const int _supportedFirmwareCalVersion = 2;
};
#endif

8
src/AutoPilotPlugins/Common/RadioComponentController.cc
Unescape View File

@ -701,7 +701,7 @@ void RadioComponentController::_writeCalibration(void) @@ -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) @@ -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) @@ -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) @@ -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

6
src/AutoPilotPlugins/PX4/PowerComponentController.cc
Unescape View File

@ -7,10 +7,6 @@ @@ -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) @@ -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)

12
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
Unescape View File

@ -192,31 +192,31 @@ void SensorsComponentController::_stopCalibration(SensorsComponentController::St @@ -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) @@ -485,5 +485,5 @@ void SensorsComponentController::cancelCalibration(void)
_waitingForCancel = true;
emit waitingForCancelChanged();
_cancelButton->setEnabled(false);
_uas->stopCalibration();
_vehicle->stopCalibration();
}

85
src/Vehicle/Vehicle.cc
Unescape View File

@ -1325,8 +1325,8 @@ void Vehicle::_handleHighLatency(mavlink_message_t& message) @@ -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() @@ -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) {

20
src/Vehicle/Vehicle.h
Unescape View File

@ -645,7 +645,7 @@ public: @@ -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: @@ -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; }

89
src/uas/UAS.cc
Unescape View File

@ -348,95 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message) @@ -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) {

3
src/uas/UAS.h
Unescape View File

@ -180,9 +180,6 @@ public slots: @@ -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);

20
src/uas/UASInterface.h
Unescape View File

@ -39,31 +39,11 @@ public: @@ -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;

Write Preview
Loading…
Cancel
Save