Browse Source

Send radio cal start to Vehicle in all cases

QGC4.4
Don Gagne 3 years ago committed by Don Gagne
parent
commit
a083c668a2
  1. 2
      src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
  2. 8
      src/AutoPilotPlugins/Common/RadioComponentController.cc
  3. 2
      src/AutoPilotPlugins/PX4/SensorsComponentController.cc
  4. 4
      src/Vehicle/Vehicle.cc
  5. 2
      src/Vehicle/Vehicle.h

2
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@ -442,7 +442,7 @@ void APMSensorsComponentController::cancelCalibration(void)
emit waitingForCancelChanged(); emit waitingForCancelChanged();
// The firmware doesn't always allow us to cancel calibration. The best we can do is wait // The firmware doesn't always allow us to cancel calibration. The best we can do is wait
// for it to timeout. // for it to timeout.
_vehicle->stopCalibration(); _vehicle->stopCalibration(true /* showError */);
} }
} }

8
src/AutoPilotPlugins/Common/RadioComponentController.cc

@ -797,9 +797,7 @@ void RadioComponentController::_startCalibration(void)
_resetInternalCalibrationValues(); _resetInternalCalibrationValues();
// Let the mav known we are starting calibration. This should turn off motors and so forth. // Let the mav known we are starting calibration. This should turn off motors and so forth.
if (_px4Vehicle()) {
_vehicle->startCalibration(Vehicle::CalibrationRadio); _vehicle->startCalibration(Vehicle::CalibrationRadio);
}
_nextButton->setProperty("text", tr("Next")); _nextButton->setProperty("text", tr("Next"));
_cancelButton->setEnabled(true); _cancelButton->setEnabled(true);
@ -814,9 +812,9 @@ void RadioComponentController::_stopCalibration(void)
_currentStep = -1; _currentStep = -1;
if (_uas) { if (_uas) {
if (_px4Vehicle()) { // Only PX4 is known to support this command in all versions. For other firmware which may or may not
_vehicle->stopCalibration(); // support this we don't show errors on failure.
} _vehicle->stopCalibration(_px4Vehicle() ? true : false /* showError */);
_setInternalCalibrationValuesFromParameters(); _setInternalCalibrationValuesFromParameters();
} else { } else {
_resetInternalCalibrationValues(); _resetInternalCalibrationValues();

2
src/AutoPilotPlugins/PX4/SensorsComponentController.cc

@ -493,7 +493,7 @@ void SensorsComponentController::cancelCalibration(void)
_waitingForCancel = true; _waitingForCancel = true;
emit waitingForCancelChanged(); emit waitingForCancelChanged();
_cancelButton->setEnabled(false); _cancelButton->setEnabled(false);
_vehicle->stopCalibration(); _vehicle->stopCalibration(true /* showError */);
} }
void SensorsComponentController::_handleParametersReset(bool success) void SensorsComponentController::_handleParametersReset(bool success)

4
src/Vehicle/Vehicle.cc

@ -3305,11 +3305,11 @@ void Vehicle::startCalibration(Vehicle::CalibrationType calType)
sendMessageOnLinkThreadSafe(sharedLink.get(), msg); sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
} }
void Vehicle::stopCalibration(void) void Vehicle::stopCalibration(bool showError)
{ {
sendMavCommand(defaultComponentId(), // target component sendMavCommand(defaultComponentId(), // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id MAV_CMD_PREFLIGHT_CALIBRATION, // command id
true, // showError showError,
0, // gyro cal 0, // gyro cal
0, // mag cal 0, // mag cal
0, // ground pressure 0, // ground pressure

2
src/Vehicle/Vehicle.h

@ -613,7 +613,7 @@ public:
}; };
void startCalibration (CalibrationType calType); void startCalibration (CalibrationType calType);
void stopCalibration (void); void stopCalibration (bool showError);
void startUAVCANBusConfig(void); void startUAVCANBusConfig(void);
void stopUAVCANBusConfig(void); void stopUAVCANBusConfig(void);

Loading…
Cancel
Save