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