Browse Source

Simple factory calibration button implemented in QGC.

QGC4.4
batinkov 4 years ago committed by Beat Küng
parent
commit
377293f0c0
  1. 27
      src/AutoPilotPlugins/PX4/SensorsComponentController.cc
  2. 2
      src/AutoPilotPlugins/PX4/SensorsComponentController.h
  3. 15
      src/AutoPilotPlugins/PX4/SensorsSetup.qml
  4. 5
      src/Vehicle/Vehicle.cc
  5. 2
      src/Vehicle/Vehicle.h

27
src/AutoPilotPlugins/PX4/SensorsComponentController.cc

@ -61,6 +61,8 @@ SensorsComponentController::SensorsComponentController(void) @@ -61,6 +61,8 @@ SensorsComponentController::SensorsComponentController(void)
, _unknownFirmwareVersion (false)
, _waitingForCancel (false)
{
connect(_vehicle, &Vehicle::sensorsParametersResetAck, this, &SensorsComponentController::_handleParametersReset);
}
bool SensorsComponentController::usingUDPLink(void)
@ -493,3 +495,28 @@ void SensorsComponentController::cancelCalibration(void) @@ -493,3 +495,28 @@ void SensorsComponentController::cancelCalibration(void)
_cancelButton->setEnabled(false);
_vehicle->stopCalibration();
}
void SensorsComponentController::_handleParametersReset(bool success)
{
if (success) {
qgcApp()->showAppMessage(tr("Reset successful"));
QTimer::singleShot(1000, this, [this]() {
_refreshParams();
});
}
else {
qgcApp()->showAppMessage(tr("Reset failed"));
}
}
void SensorsComponentController::resetFactoryParameters()
{
auto compId = _vehicle->defaultComponentId();
_vehicle->sendMavCommand(compId,
MAV_CMD_PREFLIGHT_STORAGE,
true, // showError
3, // Reset factory parameters
-1); // Don't do anything with mission storage
}

2
src/AutoPilotPlugins/PX4/SensorsComponentController.h

@ -82,6 +82,7 @@ public: @@ -82,6 +82,7 @@ public:
Q_INVOKABLE void calibrateAirspeed(void);
Q_INVOKABLE void cancelCalibration(void);
Q_INVOKABLE bool usingUDPLink(void);
Q_INVOKABLE void resetFactoryParameters();
signals:
void showGyroCalAreaChanged(void);
@ -96,6 +97,7 @@ signals: @@ -96,6 +97,7 @@ signals:
private slots:
void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
void _handleParametersReset(bool success);
private:
void _startLogCalibration(void);

15
src/AutoPilotPlugins/PX4/SensorsSetup.qml

@ -623,6 +623,21 @@ Item { @@ -623,6 +623,21 @@ Item {
}
}
}
QGCButton {
text: qsTr("Factory reset")
width: _buttonWidth
anchors {
right: orientationCalArea.left
rightMargin: ScreenTools.defaultFontPixelWidth/2
bottom: orientationCalArea.bottom
}
onClicked: {
controller.resetFactoryParameters()
}
}
}
}
}

5
src/Vehicle/Vehicle.cc

@ -2984,6 +2984,11 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) @@ -2984,6 +2984,11 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
}
}
if (ack.command == MAV_CMD_PREFLIGHT_STORAGE) {
auto result = (ack.result == MAV_RESULT_ACCEPTED);
emit sensorsParametersResetAck(result);
}
#if !defined(NO_ARDUPILOT_DIALECT)
if (ack.command == MAV_CMD_FLASH_BOOTLOADER && ack.result == MAV_RESULT_ACCEPTED) {
qgcApp()->showAppMessage(tr("Bootloader flash succeeded"));

2
src/Vehicle/Vehicle.h

@ -930,6 +930,8 @@ signals: @@ -930,6 +930,8 @@ signals:
void isROIEnabledChanged ();
void initialConnectComplete ();
void sensorsParametersResetAck (bool success);
private slots:
void _mavlinkMessageReceived (LinkInterface* link, mavlink_message_t message);
void _sendMessageMultipleNext ();

Loading…
Cancel
Save