Skip to content

Q
qgroundcontrol
Commit 5765485a authored by Don Gagne's avatar Don Gagne
Browse files
Options

Merge pull request #1363 from DonLakeFlyer/SensorCal 

Disable calibration buttons while calibrating
parents 96f44416 e0476c67
Hide whitespace changes
Inline Side-by-side
Showing with 85 additions and 36 deletions
src/AutoPilotPlugins/PX4/SensorsComponent.qml
View file @ 5765485a
...@@ -79,6 +79,14 @@ Rectangle { ...@@ -79,6 +79,14 @@ Rectangle {
onLoaded: { onLoaded: {
controller.statusLog = statusTextArea controller.statusLog = statusTextArea
controller.progressBar = progressBar controller.progressBar = progressBar
controller.compassButton = compassButton
controller.gyroButton = gyroButton
controller.accelButton = accelButton
controller.airspeedButton = airspeedButton
controller.compass0RotationCombo = compass0RotationCombo
controller.compass1RotationCombo = compass1RotationCombo
controller.compass2RotationCombo = compass2RotationCombo
controller.boardRotationCombo = boardRotationCombo
} }
} }
...@@ -97,12 +105,12 @@ Rectangle { ...@@ -97,12 +105,12 @@ Rectangle {
spacing: 20 spacing: 20
QGCLabel { text: "Calibrate:"; anchors.baseline: firstButton.baseline } QGCLabel { text: "Calibrate:"; anchors.baseline: compassButton.baseline }
IndicatorButton { IndicatorButton {
property Fact fact: Fact { name: "CAL_MAG0_ID" } property Fact fact: Fact { name: "CAL_MAG0_ID" }
id: firstButton id: compassButton
width: parent.buttonWidth width: parent.buttonWidth
text: "Compass" text: "Compass"
indicatorGreen: fact.value != 0 indicatorGreen: fact.value != 0
...@@ -112,6 +120,7 @@ Rectangle { ...@@ -112,6 +120,7 @@ Rectangle {
IndicatorButton { IndicatorButton {
property Fact fact: Fact { name: "CAL_GYRO0_ID" } property Fact fact: Fact { name: "CAL_GYRO0_ID" }
id: gyroButton
width: parent.buttonWidth width: parent.buttonWidth
text: "Gyroscope" text: "Gyroscope"
indicatorGreen: fact.value != 0 indicatorGreen: fact.value != 0
...@@ -121,6 +130,7 @@ Rectangle { ...@@ -121,6 +130,7 @@ Rectangle {
IndicatorButton { IndicatorButton {
property Fact fact: Fact { name: "CAL_ACC0_ID" } property Fact fact: Fact { name: "CAL_ACC0_ID" }
id: accelButton
width: parent.buttonWidth width: parent.buttonWidth
text: "Accelerometer" text: "Accelerometer"
indicatorGreen: fact.value != 0 indicatorGreen: fact.value != 0
...@@ -130,6 +140,7 @@ Rectangle { ...@@ -130,6 +140,7 @@ Rectangle {
IndicatorButton { IndicatorButton {
property Fact fact: Fact { name: "SENS_DPRES_OFF" } property Fact fact: Fact { name: "SENS_DPRES_OFF" }
id: airspeedButton
width: parent.buttonWidth width: parent.buttonWidth
text: "Airspeed" text: "Airspeed"
visible: controller.fixedWing visible: controller.fixedWing
...@@ -280,6 +291,7 @@ Rectangle { ...@@ -280,6 +291,7 @@ Rectangle {
QGCLabel { text: "Autpilot Orientation" } QGCLabel { text: "Autpilot Orientation" }
FactComboBox { FactComboBox {
id: boardRotationCombo
width: rotationColumnWidth; width: rotationColumnWidth;
model: rotations model: rotations
fact: Fact { name: "SENS_BOARD_ROT" } fact: Fact { name: "SENS_BOARD_ROT" }
...@@ -295,6 +307,7 @@ Rectangle { ...@@ -295,6 +307,7 @@ Rectangle {
id: compass0ComponentCombo id: compass0ComponentCombo
FactComboBox { FactComboBox {
id: compass0RotationCombo
width: rotationColumnWidth width: rotationColumnWidth
model: rotations model: rotations
fact: Fact { name: "CAL_MAG0_ROT" } fact: Fact { name: "CAL_MAG0_ROT" }
...@@ -313,6 +326,7 @@ Rectangle { ...@@ -313,6 +326,7 @@ Rectangle {
id: compass1ComponentCombo id: compass1ComponentCombo
FactComboBox { FactComboBox {
id: compass1RotationCombo
width: rotationColumnWidth width: rotationColumnWidth
model: rotations model: rotations
fact: Fact { name: "CAL_MAG1_ROT" } fact: Fact { name: "CAL_MAG1_ROT" }
...@@ -331,6 +345,7 @@ Rectangle { ...@@ -331,6 +345,7 @@ Rectangle {
id: compass2ComponentCombo id: compass2ComponentCombo
FactComboBox { FactComboBox {
id: compass1RotationCombo
width: rotationColumnWidth width: rotationColumnWidth
model: rotations model: rotations
fact: Fact { name: "CAL_MAG2_ROT" } fact: Fact { name: "CAL_MAG2_ROT" }
......
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
View file @ 5765485a
...@@ -51,6 +51,7 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo ...@@ -51,6 +51,7 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo
_accelCalRightSideInProgress(false), _accelCalRightSideInProgress(false),
_accelCalNoseDownSideInProgress(false), _accelCalNoseDownSideInProgress(false),
_accelCalTailDownSideInProgress(false), _accelCalTailDownSideInProgress(false),
_textLoggingStarted(false),
_autopilot(autopilot) _autopilot(autopilot)
{ {
Q_ASSERT(autopilot); Q_ASSERT(autopilot);
...@@ -69,9 +70,37 @@ void SensorsComponentController::_appendStatusLog(const QString& text) ...@@ -69,9 +70,37 @@ void SensorsComponentController::_appendStatusLog(const QString& text)
Q_ARG(QVariant, varText)); Q_ARG(QVariant, varText));
} }
void SensorsComponentController::calibrateGyro(void) void SensorsComponentController::_startCalibration(void)
{ {
_beginTextLogging(); _beginTextLogging();
_hideAllCalAreas();
_compassButton->setEnabled(false);
_gyroButton->setEnabled(false);
_accelButton->setEnabled(false);
_airspeedButton->setEnabled(false);
}
void SensorsComponentController::_stopCalibration(bool failed)
{
_compassButton->setEnabled(true);
_gyroButton->setEnabled(true);
_accelButton->setEnabled(true);
_airspeedButton->setEnabled(true);
_progressBar->setProperty("value", failed ? 0 : 1);
_updateAndEmitGyroCalInProgress(false);
_refreshParams();
if (failed) {
QGCMessageBox::warning("Calibration", "Calibration failed. Calibration log will be displayed.");
_hideAllCalAreas();
}
}
void SensorsComponentController::calibrateGyro(void)
{
_startCalibration();
UASInterface* uas = _autopilot->uas(); UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas); Q_ASSERT(uas);
...@@ -80,8 +109,7 @@ void SensorsComponentController::calibrateGyro(void) ...@@ -80,8 +109,7 @@ void SensorsComponentController::calibrateGyro(void)
void SensorsComponentController::calibrateCompass(void) void SensorsComponentController::calibrateCompass(void)
{ {
_hideAllCalAreas(); _startCalibration();
_beginTextLogging();
UASInterface* uas = _autopilot->uas(); UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas); Q_ASSERT(uas);
...@@ -90,7 +118,7 @@ void SensorsComponentController::calibrateCompass(void) ...@@ -90,7 +118,7 @@ void SensorsComponentController::calibrateCompass(void)
void SensorsComponentController::calibrateAccel(void) void SensorsComponentController::calibrateAccel(void)
{ {
_beginTextLogging(); _startCalibration();
UASInterface* uas = _autopilot->uas(); UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas); Q_ASSERT(uas);
...@@ -99,8 +127,7 @@ void SensorsComponentController::calibrateAccel(void) ...@@ -99,8 +127,7 @@ void SensorsComponentController::calibrateAccel(void)
void SensorsComponentController::calibrateAirspeed(void) void SensorsComponentController::calibrateAirspeed(void)
{ {
_hideAllCalAreas(); _startCalibration();
_beginTextLogging();
UASInterface* uas = _autopilot->uas(); UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas); Q_ASSERT(uas);
...@@ -109,9 +136,12 @@ void SensorsComponentController::calibrateAirspeed(void) ...@@ -109,9 +136,12 @@ void SensorsComponentController::calibrateAirspeed(void)
void SensorsComponentController::_beginTextLogging(void) void SensorsComponentController::_beginTextLogging(void)
{ {
UASInterface* uas = _autopilot->uas(); if (!_textLoggingStarted) {
Q_ASSERT(uas); _textLoggingStarted = true;
connect(uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage); UASInterface* uas = _autopilot->uas();
Q_ASSERT(uas);
connect(uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
}
} }
void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text) void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
...@@ -129,7 +159,7 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in ...@@ -129,7 +159,7 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
} }
QStringList ignorePrefixList; QStringList ignorePrefixList;
ignorePrefixList << "[cmd]" << "[mavlink pm]" << "[ekf check]" << "[pm]" << "[inav]"; ignorePrefixList << "[cmd]" << "[mavlink pm]" << "[ekf check]" << "[pm]" << "[inav]" << "IN AIR MODE" << "LANDED MODE";
foreach (QString ignorePrefix, ignorePrefixList) { foreach (QString ignorePrefix, ignorePrefixList) {
if (text.startsWith(ignorePrefix)) { if (text.startsWith(ignorePrefix)) {
return; return;
...@@ -150,11 +180,9 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in ...@@ -150,11 +180,9 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
_appendStatusLog(text); _appendStatusLog(text);
if (text == "gyro calibration: started") { if (text == "gyro calibration: started") {
_hideAllCalAreas();
_updateAndEmitShowGyroCalArea(true); _updateAndEmitShowGyroCalArea(true);
_updateAndEmitGyroCalInProgress(true); _updateAndEmitGyroCalInProgress(true);
} else if (text == "accel calibration: started") { } else if (text == "accel calibration: started") {
_hideAllCalAreas();
_accelCalDownSideDone = false; _accelCalDownSideDone = false;
_accelCalUpsideDownSideDone = false; _accelCalUpsideDownSideDone = false;
_accelCalLeftSideDone = false; _accelCalLeftSideDone = false;
...@@ -227,34 +255,27 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in ...@@ -227,34 +255,27 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
_accelCalTailDownSideInProgress = false; _accelCalTailDownSideInProgress = false;
emit accelCalSidesDoneChanged(); emit accelCalSidesDoneChanged();
emit accelCalSidesInProgressChanged(); emit accelCalSidesInProgressChanged();
_refreshParams(); _stopCalibration(false /* success */);
} else if (text == "gyro calibration: done") { } else if (text == "gyro calibration: done") {
_progressBar->setProperty("value", 1); _stopCalibration(false /* success */);
_updateAndEmitGyroCalInProgress(false);
_refreshParams();
} else if (text == "mag calibration: done" || text == "dpress calibration: done") { } else if (text == "mag calibration: done" || text == "dpress calibration: done") {
_progressBar->setProperty("value", 1); _stopCalibration(false /* success */);
_refreshParams();
} else if (text.endsWith(" calibration: failed")) { } else if (text.endsWith(" calibration: failed")) {
QGCMessageBox::warning("Calibration", "Calibration failed. Calibration log will be displayed."); _stopCalibration(true /* failed */);
_hideAllCalAreas();
_progressBar->setProperty("value", 0);
_updateAndEmitGyroCalInProgress(false);
_refreshParams();
} }
} }
void SensorsComponentController::_refreshParams(void) void SensorsComponentController::_refreshParams(void)
{ {
#if 0 _autopilot->refreshParameter("CAL_MAG0_ID");
// FIXME: Not sure if firmware issue yet _autopilot->refreshParameter("CAL_GYRO0_ID");
_autopilot->refreshParametersPrefix("CAL_"); _autopilot->refreshParameter("CAL_ACC0_ID");
_autopilot->refreshParametersPrefix("SENS_"); _autopilot->refreshParameter("SENS_DPRES_OFF");
#else
// Sending too many parameter requests like above doesn't seem to work. So for now, _autopilot->refreshParameter("CAL_MAG0_ROT");
// ask for everything back _autopilot->refreshParameter("CAL_MAG1_ROT");
_autopilot->refreshAllParameters(); _autopilot->refreshParameter("CAL_MAG2_ROT");
#endif _autopilot->refreshParameter("SENS_BOARD_ROT");
} }
bool SensorsComponentController::fixedWing(void) bool SensorsComponentController::fixedWing(void)
......
src/AutoPilotPlugins/PX4/SensorsComponentController.h
View file @ 5765485a
...@@ -46,6 +46,11 @@ public: ...@@ -46,6 +46,11 @@ public:
Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog) Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog)
Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar) Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar)
Q_PROPERTY(QQuickItem* compassButton MEMBER _compassButton)
Q_PROPERTY(QQuickItem* gyroButton MEMBER _gyroButton)
Q_PROPERTY(QQuickItem* accelButton MEMBER _accelButton)
Q_PROPERTY(QQuickItem* airspeedButton MEMBER _airspeedButton)
Q_PROPERTY(bool showGyroCalArea MEMBER _showGyroCalArea NOTIFY showGyroCalAreaChanged) Q_PROPERTY(bool showGyroCalArea MEMBER _showGyroCalArea NOTIFY showGyroCalAreaChanged)
Q_PROPERTY(bool showAccelCalArea MEMBER _showAccelCalArea NOTIFY showAccelCalAreaChanged) Q_PROPERTY(bool showAccelCalArea MEMBER _showAccelCalArea NOTIFY showAccelCalAreaChanged)
...@@ -83,6 +88,8 @@ private slots: ...@@ -83,6 +88,8 @@ private slots:
void _handleUASTextMessage(int uasId, int compId, int severity, QString text); void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
private: private:
void _startCalibration(void);
void _stopCalibration(bool failed);
void _beginTextLogging(void); void _beginTextLogging(void);
void _appendStatusLog(const QString& text); void _appendStatusLog(const QString& text);
void _refreshParams(void); void _refreshParams(void);
...@@ -93,8 +100,12 @@ private: ...@@ -93,8 +100,12 @@ private:
void _updateAndEmitShowGyroCalArea(bool show); void _updateAndEmitShowGyroCalArea(bool show);
void _updateAndEmitShowAccelCalArea(bool show); void _updateAndEmitShowAccelCalArea(bool show);
QQuickItem* _statusLog; QQuickItem* _statusLog;
QQuickItem* _progressBar; QQuickItem* _progressBar;
QQuickItem* _compassButton;
QQuickItem* _gyroButton;
QQuickItem* _accelButton;
QQuickItem* _airspeedButton;
bool _showGyroCalArea; bool _showGyroCalArea;
bool _showAccelCalArea; bool _showAccelCalArea;
...@@ -115,6 +126,8 @@ private: ...@@ -115,6 +126,8 @@ private:
bool _accelCalNoseDownSideInProgress; bool _accelCalNoseDownSideInProgress;
bool _accelCalTailDownSideInProgress; bool _accelCalTailDownSideInProgress;
bool _textLoggingStarted;
AutoPilotPlugin* _autopilot; AutoPilotPlugin* _autopilot;
}; };
......
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