diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc index eebd44ecd2c5ec353963d83816b864e60d05d385..3f08e264ba8b9ef6776c1b3d78ac27839498e86d 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 896ac97c61032b0094e2bf92d1551e214f9e7c20..9b45c0ffe2157ebd6cefa6f7b5e328556a130954 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 590196f1bc351c746b51194b1a05c95c0e3c4be4..414ee8aa68a4118e4cacbd5f15d215b1ad05770f 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 e3083c13ad3565de942c964010301423fa2ed738..1688332a713bf4e279f10a1a7752fb1c5ccb3e8b 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 9044fcdf7aac28ab53c0418d7d3c90e92d7149da..d9605d7bcd867b50858e675dbfaa1a29cc5126ef 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 f7934933df514da8e86fcbda8d52efe508d84fec..ea6e4da3df14e339d7042f7e6f960c681cc7f566 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 b6ff6a58e5bd48ccea31e6a9e413c0a7dcd6d319..7493594dd524b66c44caf0c08b2a6b228d2b0fb4 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 0d31064c0770827f6e5db52da69027fcb63ead72..a7d9e488cba265aff875b4d8859b1729152790e9 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;