From 9d4088b080b1f21fe98ed1b9c2d47e50bc563245 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 20 Aug 2020 10:27:06 -0700 Subject: [PATCH] Move calibration methods to Vehicle --- .../Common/RadioComponentController.cc | 8 +- .../PX4/PowerComponentController.cc | 6 +- .../PX4/SensorsComponentController.cc | 12 +-- src/Vehicle/Vehicle.cc | 85 ++++++++++++++++++++- src/Vehicle/Vehicle.h | 20 ++++- src/uas/UAS.cc | 89 ---------------------- src/uas/UAS.h | 3 - src/uas/UASInterface.h | 20 ----- 8 files changed, 113 insertions(+), 130 deletions(-) diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc index eebd44e..3f08e26 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.cc +++ b/src/AutoPilotPlugins/Common/RadioComponentController.cc @@ -701,7 +701,7 @@ void RadioComponentController::_writeCalibration(void) if (!_uas) return; if (_px4Vehicle()) { - _uas->stopCalibration(); + _vehicle->stopCalibration(); } if (!_px4Vehicle() && (_vehicle->vehicleType() == MAV_TYPE_HELICOPTER || _vehicle->multiRotor()) && _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionThrottle]].reversed) { @@ -798,7 +798,7 @@ void RadioComponentController::_startCalibration(void) // Let the mav known we are starting calibration. This should turn off motors and so forth. if (_px4Vehicle()) { - _uas->startCalibration(UASInterface::StartCalibrationRadio); + _vehicle->startCalibration(Vehicle::CalibrationRadio); } _nextButton->setProperty("text", tr("Next")); @@ -815,7 +815,7 @@ void RadioComponentController::_stopCalibration(void) if (_uas) { if (_px4Vehicle()) { - _uas->stopCalibration(); + _vehicle->stopCalibration(); } _setInternalCalibrationValuesFromParameters(); } else { @@ -1024,7 +1024,7 @@ void RadioComponentController::_signalAllAttitudeValueChanges(void) void RadioComponentController::copyTrims(void) { - _uas->startCalibration(UASInterface::StartCalibrationCopyTrims); + _vehicle->startCalibration(Vehicle::CalibrationCopyTrims); } bool RadioComponentController::_px4Vehicle(void) const diff --git a/src/AutoPilotPlugins/PX4/PowerComponentController.cc b/src/AutoPilotPlugins/PX4/PowerComponentController.cc index 896ac97..9b45c0f 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponentController.cc +++ b/src/AutoPilotPlugins/PX4/PowerComponentController.cc @@ -7,10 +7,6 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #include "PowerComponentController.h" #include "QGCMAVLink.h" #include "UAS.h" @@ -27,7 +23,7 @@ void PowerComponentController::calibrateEsc(void) { _warningMessages.clear(); connect(_vehicle, &Vehicle::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage); - _uas->startCalibration(UASInterface::StartCalibrationEsc); + _vehicle->startCalibration(Vehicle::CalibrationEsc); } void PowerComponentController::busConfigureActuators(void) diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 590196f..414ee8a 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -192,31 +192,31 @@ void SensorsComponentController::_stopCalibration(SensorsComponentController::St void SensorsComponentController::calibrateGyro(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationGyro); + _vehicle->startCalibration(Vehicle::CalibrationGyro); } void SensorsComponentController::calibrateCompass(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationMag); + _vehicle->startCalibration(Vehicle::CalibrationMag); } void SensorsComponentController::calibrateAccel(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationAccel); + _vehicle->startCalibration(Vehicle::CalibrationAccel); } void SensorsComponentController::calibrateLevel(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationLevel); + _vehicle->startCalibration(Vehicle::CalibrationLevel); } void SensorsComponentController::calibrateAirspeed(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationAirspeed); + _vehicle->startCalibration(Vehicle::CalibrationPX4Airspeed); } void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text) @@ -485,5 +485,5 @@ void SensorsComponentController::cancelCalibration(void) _waitingForCancel = true; emit waitingForCancelChanged(); _cancelButton->setEnabled(false); - _uas->stopCalibration(); + _vehicle->stopCalibration(); } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index e3083c1..1688332 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1324,8 +1324,8 @@ void Vehicle::_handleHighLatency(mavlink_message_t& message) const double altitude; } coordinate { highLatency.latitude / (double)1E7, - highLatency.longitude / (double)1E7, - static_cast(highLatency.altitude_amsl) + highLatency.longitude / (double)1E7, + static_cast(highLatency.altitude_amsl) }; _coordinate.setLatitude(coordinate.latitude); @@ -3583,6 +3583,87 @@ void Vehicle::rebootVehicle() sendMessageOnLinkThreadSafe(priorityLink(), msg); } +void Vehicle::startCalibration(Vehicle::CalibrationType calType) +{ + float param1 = 0; + float param2 = 0; + float param3 = 0; + float param4 = 0; + float param5 = 0; + float param6 = 0; + float param7 = 0; + + switch (calType) { + case CalibrationGyro: + param1 = 1; + break; + case CalibrationMag: + param2 = 1; + break; + case CalibrationRadio: + param4 = 1; + break; + case CalibrationCopyTrims: + param4 = 2; + break; + case CalibrationAccel: + param5 = 1; + break; + case CalibrationLevel: + param5 = 2; + break; + case CalibrationEsc: + param7 = 1; + break; + case CalibrationPX4Airspeed: + param6 = 1; + break; + case CalibrationPX4Pressure: + param3 = 1; + break; + case CalibrationAPMCompassMot: + param6 = 1; + break; + case CalibrationAPMPressureAirspeed: + param3 = 1; + break; + case CalibrationAPMPreFlight: + param3 = 1; // GroundPressure/Airspeed + if (multiRotor() || rover()) { + // Gyro cal for ArduCopter only + param1 = 1; + } + } + + // We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn + // causes the retry logic to break down. + mavlink_message_t msg; + mavlink_msg_command_long_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + id(), + defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + param1, param2, param3, param4, param5, param6, param7); + sendMessageOnLinkThreadSafe(priorityLink(), msg); +} + +void Vehicle::stopCalibration(void) +{ + sendMavCommand(defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + true, // showError + 0, // gyro cal + 0, // mag cal + 0, // ground pressure + 0, // radio cal + 0, // accel cal + 0, // airspeed cal + 0); // unused +} + void Vehicle::setSoloFirmware(bool soloFirmware) { if (soloFirmware != _soloFirmware) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 9044fcd..d9605d7 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -641,7 +641,7 @@ public: Q_PROPERTY(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged) Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged) Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged) - Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) + Q_PROPERTY(bool iARDURsROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged) Q_PROPERTY(bool readyToFlyAvailable READ readyToFlyAvailable NOTIFY readyToFlyAvailableChanged) ///< true: readyToFly signalling is available on this vehicle Q_PROPERTY(bool readyToFly READ readyToFly NOTIFY readyToFlyChanged) @@ -980,6 +980,24 @@ public: /// @return the maximum version unsigned maxProtoVersion () const { return _maxProtoVersion; } + enum CalibrationType { + CalibrationRadio, + CalibrationGyro, + CalibrationMag, + CalibrationAccel, + CalibrationLevel, + CalibrationEsc, + CalibrationCopyTrims, + CalibrationAPMCompassMot, + CalibrationAPMPressureAirspeed, + CalibrationAPMPreFlight, + CalibrationPX4Airspeed, + CalibrationPX4Pressure, + }; + + void startCalibration (CalibrationType calType); + void stopCalibration (void); + Fact* roll () { return &_rollFact; } Fact* pitch () { return &_pitchFact; } Fact* heading () { return &_headingFact; } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index f793493..ea6e4da 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -348,95 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message) #pragma warning(pop, 0) #endif -void UAS::startCalibration(UASInterface::StartCalibrationType calType) -{ - if (!_vehicle) { - return; - } - - int gyroCal = 0; - int magCal = 0; - int airspeedCal = 0; - int radioCal = 0; - int accelCal = 0; - int pressureCal = 0; - int escCal = 0; - - switch (calType) { - case StartCalibrationGyro: - gyroCal = 1; - break; - case StartCalibrationMag: - magCal = 1; - break; - case StartCalibrationAirspeed: - airspeedCal = 1; - break; - case StartCalibrationRadio: - radioCal = 1; - break; - case StartCalibrationCopyTrims: - radioCal = 2; - break; - case StartCalibrationAccel: - accelCal = 1; - break; - case StartCalibrationLevel: - accelCal = 2; - break; - case StartCalibrationPressure: - pressureCal = 1; - break; - case StartCalibrationEsc: - escCal = 1; - break; - case StartCalibrationUavcanEsc: - escCal = 2; - break; - case StartCalibrationCompassMot: - airspeedCal = 1; // ArduPilot, bit of a hack - break; - } - - // We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn - // causes the retry logic to break down. - mavlink_message_t msg; - mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - 0, // 0=first transmission of command - gyroCal, // gyro cal - magCal, // mag cal - pressureCal, // ground pressure - radioCal, // radio cal - accelCal, // accel cal - airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot - escCal); // esc cal - _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); -} - -void UAS::stopCalibration(void) -{ - if (!_vehicle) { - return; - } - - _vehicle->sendMavCommand(_vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - true, // showError - 0, // gyro cal - 0, // mag cal - 0, // ground pressure - 0, // radio cal - 0, // accel cal - 0, // airspeed cal - 0); // unused -} - void UAS::startBusConfig(UASInterface::StartBusConfigType calType) { if (!_vehicle) { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index b6ff6a5..7493594 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -180,9 +180,6 @@ public slots: /** @brief Receive a message from one of the communication links. */ virtual void receiveMessage(mavlink_message_t message); - void startCalibration(StartCalibrationType calType); - void stopCalibration(void); - void startBusConfig(StartBusConfigType calType); void stopBusConfig(void); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 0d31064..a7d9e48 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -39,31 +39,11 @@ public: /** @brief The time interval the robot is switched on **/ virtual quint64 getUptime() const = 0; - enum StartCalibrationType { - StartCalibrationRadio, - StartCalibrationGyro, - StartCalibrationMag, - StartCalibrationAirspeed, - StartCalibrationAccel, - StartCalibrationLevel, - StartCalibrationPressure, - StartCalibrationEsc, - StartCalibrationCopyTrims, - StartCalibrationUavcanEsc, - StartCalibrationCompassMot, - }; - enum StartBusConfigType { StartBusConfigActuators, EndBusConfigActuators, }; - /// Starts the specified calibration - virtual void startCalibration(StartCalibrationType calType) = 0; - - /// Ends any current calibration - virtual void stopCalibration(void) = 0; - /// Starts the specified bus configuration virtual void startBusConfig(StartBusConfigType calType) = 0;