Skip to content

Commit

Permalink
Add CompassMot support (mavlink#3643)
Browse files Browse the repository at this point in the history
  • Loading branch information
@DonLakeFlyer
DonLakeFlyer authored Jun 25, 2016
1 parent 17e1aec commit eb5df6a
Show file tree
Hide file tree
Showing 5 changed files with 137 additions and 35 deletions.
77 changes: 76 additions & 1 deletion src/AutoPilotPlugins/APM/APMSensorsComponent.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ QGCView {
progressBar: progressBar
compassButton: compassButton
accelButton: accelButton
compassMotButton: motorInterferenceButton
nextButton: nextButton
cancelButton: cancelButton
setOrientationsButton: setOrientationsButton
Expand All @@ -171,6 +172,7 @@ QGCView {
"INS_GYROFFS_X", "INS_GYROFFS_Y", "INS_GYROFFS_Z",
"INS_GYR2OFFS_X", "INS_GYR2OFFS_Y", "INS_GYR2OFFS_Z",
"INS_GYR3OFFS_X", "INS_GYR3OFFS_Y", "INS_GYR3OFFS_Z" ]
showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok)
} else if (_orientationDialogCalType == _calTypeCompass) {
_postCalibrationDialogText = qsTr("Compass calibration complete. ")
_postCalibrationDialogParams = [];
Expand Down Expand Up @@ -198,8 +200,8 @@ QGCView {
_postCalibrationDialogParams.push("COMPASS_OFS3_Y")
_postCalibrationDialogParams.push("COMPASS_OFS3_Z")
}
showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok)
}
showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok)
}
}

Expand Down Expand Up @@ -376,6 +378,72 @@ QGCView {
} // QGCViewDialog
} // Component - orientationsDialogComponent

Component {
id: compassMotDialogComponent

QGCViewDialog {
id: compassMotDialog

function accept() {
controller.calibrateMotorInterference()
compassMotDialog.hideDialog()
}

QGCFlickable {
anchors.fill: parent
contentHeight: columnLayout.height
clip: true

Column {
id: columnLayout
anchors.margins: ScreenTools.defaultFontPixelWidth
anchors.left: parent.left
anchors.right: parent.right
anchors.top: parent.top
spacing: ScreenTools.defaultFontPixelHeight

QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: "This is recommended for vehicles that have only an internal compass and on vehicles where there is significant interference on the compass from the motors, power wires, etc. " +
"CompassMot only works well if you have a battery current monitor because the magnetic interference is linear with current drawn. " +
"It is technically possible to set-up CompassMot using throttle but this is not recommended."
}

QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: "Disconnect your props, flip them over and rotate them one position around the frame. " +
"In this configuration they should push the copter down into the ground when the throttle is raised."
}

QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: "Secure the copter (perhaps with tape) so that it does not move."
}

QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: "Turn on your transmitter and keep throttle at zero."
}

QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: "Click Ok to start CompassMot calibration."
}
} // Column
} // QGCFlickable
} // QGCViewDialog
}

QGCViewPanel {
id: panel
anchors.fill: parent
Expand Down Expand Up @@ -410,6 +478,13 @@ QGCView {
}
}

QGCButton {
id: motorInterferenceButton
width: parent.buttonWidth
text: qsTr("CompassMot")
onClicked: showDialog(compassMotDialogComponent, qsTr("CompassMot - Compass Motor Interference Calibration"), qgcView.showDialogFullWidth, StandardButton.Cancel | StandardButton.Ok)
}

QGCButton {
id: nextButton
width: parent.buttonWidth
Expand Down
29 changes: 24 additions & 5 deletions src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,14 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
_progressBar(NULL),
_compassButton(NULL),
_accelButton(NULL),
_compassMotButton(NULL),
_nextButton(NULL),
_cancelButton(NULL),
_setOrientationsButton(NULL),
_showOrientationCalArea(false),
_magCalInProgress(false),
_accelCalInProgress(false),
_compassMotCalInProgress(false),
_orientationCalDownSideDone(false),
_orientationCalUpsideDownSideDone(false),
_orientationCalLeftSideDone(false),
Expand Down Expand Up @@ -86,8 +88,9 @@ void APMSensorsComponentController::_startLogCalibration(void)

_compassButton->setEnabled(false);
_accelButton->setEnabled(false);
_compassMotButton->setEnabled(false);
_setOrientationsButton->setEnabled(false);
if (_accelCalInProgress) {
if (_accelCalInProgress || _compassMotCalInProgress) {
_nextButton->setEnabled(true);
}
_cancelButton->setEnabled(false);
Expand All @@ -97,6 +100,7 @@ void APMSensorsComponentController::_startVisualCalibration(void)
{
_compassButton->setEnabled(false);
_accelButton->setEnabled(false);
_compassMotButton->setEnabled(false);
_setOrientationsButton->setEnabled(false);
_cancelButton->setEnabled(true);

Expand Down Expand Up @@ -133,14 +137,13 @@ void APMSensorsComponentController::_resetInternalState(void)

void APMSensorsComponentController::_stopCalibration(APMSensorsComponentController::StopCalibrationCode code)
{
if (_accelCalInProgress) {
_vehicle->setConnectionLostEnabled(true);
}
_vehicle->setConnectionLostEnabled(true);

disconnect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);

_compassButton->setEnabled(true);
_accelButton->setEnabled(true);
_compassMotButton->setEnabled(true);
_setOrientationsButton->setEnabled(true);
_nextButton->setEnabled(false);
_cancelButton->setEnabled(false);
Expand Down Expand Up @@ -178,6 +181,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll

_magCalInProgress = false;
_accelCalInProgress = false;
_compassMotCalInProgress = false;
}

void APMSensorsComponentController::calibrateCompass(void)
Expand All @@ -188,12 +192,23 @@ void APMSensorsComponentController::calibrateCompass(void)

void APMSensorsComponentController::calibrateAccel(void)
{
_accelCalInProgress = true;
_vehicle->setConnectionLostEnabled(false);
_startLogCalibration();
_accelCalInProgress = true;
_uas->startCalibration(UASInterface::StartCalibrationAccel);
}

void APMSensorsComponentController::calibrateMotorInterference(void)
{
_compassMotCalInProgress = true;
_vehicle->setConnectionLostEnabled(false);
_startLogCalibration();
_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);
}

void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
Q_UNUSED(compId);
Expand Down Expand Up @@ -457,6 +472,10 @@ void APMSensorsComponentController::nextClicked(void)
mavlink_msg_command_ack_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, &ack);

_vehicle->sendMessageOnPriorityLink(msg);

if (_compassMotCalInProgress) {
_stopCalibration(StopCalibrationSuccess);
}
}

bool APMSensorsComponentController::compassSetupNeeded(void) const
Expand Down
4 changes: 4 additions & 0 deletions src/AutoPilotPlugins/APM/APMSensorsComponentController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ class APMSensorsComponentController : public FactPanelController

Q_PROPERTY(QQuickItem* compassButton MEMBER _compassButton)
Q_PROPERTY(QQuickItem* accelButton MEMBER _accelButton)
Q_PROPERTY(QQuickItem* compassMotButton MEMBER _compassMotButton)
Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
Q_PROPERTY(QQuickItem* setOrientationsButton MEMBER _setOrientationsButton)
Expand Down Expand Up @@ -76,6 +77,7 @@ class APMSensorsComponentController : public FactPanelController

Q_INVOKABLE void calibrateCompass(void);
Q_INVOKABLE void calibrateAccel(void);
Q_INVOKABLE void calibrateMotorInterference(void);
Q_INVOKABLE void cancelCalibration(void);
Q_INVOKABLE void nextClicked(void);
Q_INVOKABLE bool usingUDPLink(void);
Expand Down Expand Up @@ -122,6 +124,7 @@ private slots:
QQuickItem* _progressBar;
QQuickItem* _compassButton;
QQuickItem* _accelButton;
QQuickItem* _compassMotButton;
QQuickItem* _nextButton;
QQuickItem* _cancelButton;
QQuickItem* _setOrientationsButton;
Expand All @@ -131,6 +134,7 @@ private slots:

bool _magCalInProgress;
bool _accelCalInProgress;
bool _compassMotCalInProgress;

bool _orientationCalDownSideDone;
bool _orientationCalUpsideDownSideDone;
Expand Down
59 changes: 31 additions & 28 deletions src/uas/UAS.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -766,33 +766,36 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
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 StartCalibrationEsc:
escCal = 1;
break;
case StartCalibrationUavcanEsc:
escCal = 2;
break;
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 StartCalibrationEsc:
escCal = 1;
break;
case StartCalibrationUavcanEsc:
escCal = 2;
break;
case StartCalibrationCompassMot:
airspeedCal = 1; // ArduPilot, bit of a hack
break;
}

mavlink_message_t msg;
Expand All @@ -808,7 +811,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
0, // ground pressure
radioCal, // radio cal
accelCal, // accel cal
airspeedCal, // airspeed cal
airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot
escCal); // esc cal
_vehicle->sendMessageOnPriorityLink(msg);
}
Expand Down
3 changes: 2 additions & 1 deletion src/uas/UASInterface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,8 @@ class UASInterface : public QObject
StartCalibrationLevel,
StartCalibrationEsc,
StartCalibrationCopyTrims,
StartCalibrationUavcanEsc
StartCalibrationUavcanEsc,
StartCalibrationCompassMot,
};

enum StartBusConfigType {
Expand Down

0 comments on commit eb5df6a

Please sign in to comment.