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

Add CompassMot support (#3643)

QGC4.4
Don Gagne 9 years ago committed by GitHub
parent
17e1aec998
commit
eb5df6a061
5 changed files with 137 additions and 35 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. 77
      src/AutoPilotPlugins/APM/APMSensorsComponent.qml
  2. 29
      src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
  3. 4
      src/AutoPilotPlugins/APM/APMSensorsComponentController.h
  4. 59
      src/uas/UAS.cc
  5. 3
      src/uas/UASInterface.h

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

@ -147,6 +147,7 @@ QGCView { @@ -147,6 +147,7 @@ QGCView {
progressBar: progressBar
compassButton: compassButton
accelButton: accelButton
compassMotButton: motorInterferenceButton
nextButton: nextButton
cancelButton: cancelButton
setOrientationsButton: setOrientationsButton
@ -171,6 +172,7 @@ QGCView { @@ -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 { @@ -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 { @@ -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
@ -411,6 +479,13 @@ QGCView { @@ -411,6 +479,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
text: qsTr("Next")

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

@ -24,12 +24,14 @@ APMSensorsComponentController::APMSensorsComponentController(void) : @@ -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) @@ -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) @@ -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) @@ -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 @@ -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) @@ -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) @@ -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

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

@ -34,6 +34,7 @@ public: @@ -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: @@ -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: @@ -122,6 +124,7 @@ private:
QQuickItem* _progressBar;
QQuickItem* _compassButton;
QQuickItem* _accelButton;
QQuickItem* _compassMotButton;
QQuickItem* _nextButton;
QQuickItem* _cancelButton;
QQuickItem* _setOrientationsButton;
@ -131,6 +134,7 @@ private: @@ -131,6 +134,7 @@ private:
bool _magCalInProgress;
bool _accelCalInProgress;
bool _compassMotCalInProgress;
bool _orientationCalDownSideDone;
bool _orientationCalUpsideDownSideDone;

59
src/uas/UAS.cc
Unescape View File

@ -766,33 +766,36 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType) @@ -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) @@ -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);
}

3
src/uas/UASInterface.h
Unescape View File

@ -119,7 +119,8 @@ public: @@ -119,7 +119,8 @@ public:
StartCalibrationLevel,
StartCalibrationEsc,
StartCalibrationCopyTrims,
StartCalibrationUavcanEsc
StartCalibrationUavcanEsc,
StartCalibrationCompassMot,
};
enum StartBusConfigType {

Write Preview
Loading…
Cancel
Save