Commit eb5df6a0 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Add CompassMot support (#3643)

parent 17e1aec9
......@@ -147,6 +147,7 @@ QGCView {
progressBar: progressBar
compassButton: compassButton
accelButton: accelButton
compassMotButton: motorInterferenceButton
nextButton: nextButton
cancelButton: cancelButton
setOrientationsButton: setOrientationsButton
......@@ -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 = [];
......@@ -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)
}
}
......@@ -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
......@@ -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
......
......@@ -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),
......@@ -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);
......@@ -97,6 +100,7 @@ void APMSensorsComponentController::_startVisualCalibration(void)
{
_compassButton->setEnabled(false);
_accelButton->setEnabled(false);
_compassMotButton->setEnabled(false);
_setOrientationsButton->setEnabled(false);
_cancelButton->setEnabled(true);
......@@ -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);
......@@ -178,6 +181,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
_magCalInProgress = false;
_accelCalInProgress = false;
_compassMotCalInProgress = false;
}
void APMSensorsComponentController::calibrateCompass(void)
......@@ -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);
......@@ -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
......
......@@ -34,6 +34,7 @@ public:
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)
......@@ -76,6 +77,7 @@ public:
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);
......@@ -122,6 +124,7 @@ private:
QQuickItem* _progressBar;
QQuickItem* _compassButton;
QQuickItem* _accelButton;
QQuickItem* _compassMotButton;
QQuickItem* _nextButton;
QQuickItem* _cancelButton;
QQuickItem* _setOrientationsButton;
......@@ -131,6 +134,7 @@ private:
bool _magCalInProgress;
bool _accelCalInProgress;
bool _compassMotCalInProgress;
bool _orientationCalDownSideDone;
bool _orientationCalUpsideDownSideDone;
......
......@@ -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;
......@@ -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);
}
......
......@@ -119,7 +119,8 @@ public:
StartCalibrationLevel,
StartCalibrationEsc,
StartCalibrationCopyTrims,
StartCalibrationUavcanEsc
StartCalibrationUavcanEsc,
StartCalibrationCompassMot,
};
enum StartBusConfigType {
......
Markdown is supported
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