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

Merge pull request #1363 from DonLakeFlyer/SensorCal 

Disable calibration buttons while calibrating
QGC4.4
Don Gagne 10 years ago
parent
96f4441651 e0476c67e3
commit
5765485a7b
3 changed files with 85 additions and 36 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. 19
      src/AutoPilotPlugins/PX4/SensorsComponent.qml
  2. 85
      src/AutoPilotPlugins/PX4/SensorsComponentController.cc
  3. 17
      src/AutoPilotPlugins/PX4/SensorsComponentController.h

19
src/AutoPilotPlugins/PX4/SensorsComponent.qml
Unescape View File

@ -79,6 +79,14 @@ Rectangle { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -331,6 +345,7 @@ Rectangle {
id: compass2ComponentCombo
FactComboBox {
id: compass1RotationCombo
width: rotationColumnWidth
model: rotations
fact: Fact { name: "CAL_MAG2_ROT" }

85
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
Unescape View File

@ -51,6 +51,7 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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 @@ -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 @@ -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)

17
src/AutoPilotPlugins/PX4/SensorsComponentController.h
Unescape View File

@ -46,6 +46,11 @@ public: @@ -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: @@ -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: @@ -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: @@ -115,6 +126,8 @@ private:
bool _accelCalNoseDownSideInProgress;
bool _accelCalTailDownSideInProgress;
bool _textLoggingStarted;
AutoPilotPlugin* _autopilot;
};

Write Preview
Loading…
Cancel
Save