diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc index 5b523e983c42fa7f71876745e27ffedb25c005f9..90f78113aa9a5da65199ab0afb757534f0afd2d4 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc +++ b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc @@ -153,11 +153,11 @@ void FlightModesComponentController::setSendLiveRCSwitchRanges(bool start) _rgRCReversed[i] = floatReversed == -1.0f; } - _uas->startRadioControlCalibration(); + _uas->startCalibration(UASInterface::StartCalibrationRadio); connect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged); } else { disconnect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged); - _uas->endRadioControlCalibration(); + _uas->stopCalibration(); _initRcValues(); emit switchLiveRangeChanged(); } diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h index 0db0c3e14b1c825a0c7fa12ccd0fb4806061084a..f215e4bc62a0726ef9490756c0ab59ea24f808db 100644 --- a/src/qgcunittest/MockUAS.h +++ b/src/qgcunittest/MockUAS.h @@ -105,6 +105,8 @@ public: virtual QString getSystemTypeName() { Q_ASSERT(false); return _bogusString; }; virtual int getAutopilotType() { return MAV_AUTOPILOT_PX4; }; virtual QGCUASFileManager* getFileManager() {Q_ASSERT(false); return NULL; } + virtual void startCalibration(StartCalibrationType calType) { Q_UNUSED(calType); return; }; + virtual void stopCalibration() { Q_ASSERT(false); return; }; /** @brief Send a message over this link (to this or to all UAS on this link) */ virtual void sendMessage(LinkInterface* link, mavlink_message_t message){ Q_UNUSED(link); Q_UNUSED(message); Q_ASSERT(false); } @@ -152,11 +154,6 @@ public slots: virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) { Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); Q_ASSERT(false); }; virtual void setLocalPositionOffset(float x, float y, float z, float yaw) { Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); Q_ASSERT(false); }; - virtual void startRadioControlCalibration(int param) { Q_UNUSED(param); return; }; - virtual void endRadioControlCalibration() { return; }; - virtual void startMagnetometerCalibration() { Q_ASSERT(false); }; - virtual void startGyroscopeCalibration() { Q_ASSERT(false); }; - virtual void startPressureCalibration() { Q_ASSERT(false); }; virtual void setBatterySpecs(const QString& specs) { Q_UNUSED(specs); Q_ASSERT(false); }; virtual QString getBatterySpecs() { Q_ASSERT(false); return _bogusString; }; virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1b3c0e31cca8468e12e377078ee6b1a378dfae85..ae97ec0186d82459ea3f3715a77422e0b01a98c8 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1439,19 +1439,67 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) Q_UNUSED(yaw); } -void UAS::startRadioControlCalibration(int param) +void UAS::startCalibration(UASInterface::StartCalibrationType calType) { + int gyroCal = 0; + int magCal = 0; + int airspeedCal = 0; + int radioCal = 0; + int accelCal = 0; + + switch (calType) { + case StartCalibrationGyro: + gyroCal = 1; + break; + case StartCalibrationMag: + magCal = 1; + break; + case StartCalibrationAirspeed: + airspeedCal = 1; + break; + case StartCalibrationRadio: + radioCal = 1; + break; + case StartCalibrationAccel: + accelCal = 1; + break; + } + mavlink_message_t msg; - // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, param, 0, 0, 0); + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + 0, // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + gyroCal, // gyro cal + magCal, // mag cal + 0, // ground pressure + radioCal, // radio cal + accelCal, // accel cal + airspeedCal, // airspeed cal + 0); // unused sendMessage(msg); } -void UAS::endRadioControlCalibration() +void UAS::stopCalibration(void) { mavlink_message_t msg; - // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + 0, // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + 0, // gyro cal + 0, // mag cal + 0, // ground pressure + 0, // radio cal + 0, // accel cal + 0, // airspeed cal + 0); // unused sendMessage(msg); } @@ -1469,30 +1517,6 @@ void UAS::stopDataRecording() sendMessage(msg); } -void UAS::startMagnetometerCalibration() -{ - mavlink_message_t msg; - // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 1, 0, 0, 0, 0, 0); - sendMessage(msg); -} - -void UAS::startGyroscopeCalibration() -{ - mavlink_message_t msg; - // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1, 0, 0, 0, 0, 0, 0); - sendMessage(msg); -} - -void UAS::startPressureCalibration() -{ - mavlink_message_t msg; - // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 1, 0, 0, 0, 0); - sendMessage(msg); -} - /** * Check if time is smaller than 40 years, assuming no system without Unix * timestamp runs longer than 40 years continuously without reboot. In worst case diff --git a/src/uas/UAS.h b/src/uas/UAS.h index fadb9b2867537ebef5e7c81672f873d437d80e7b..3a689d033a48a754a31ad1e458491fc7ca833acc 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -863,11 +863,8 @@ public slots: /** @brief Add an offset in body frame to the setpoint */ void setLocalPositionOffset(float x, float y, float z, float yaw); - void startRadioControlCalibration(int param=1); - void endRadioControlCalibration(); - void startMagnetometerCalibration(); - void startGyroscopeCalibration(); - void startPressureCalibration(); + void startCalibration(StartCalibrationType calType); + void stopCalibration(void); void startDataRecording(); void stopDataRecording(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 4d0c34bce0fe5c5e5bcef8f740b9943cda19146c..6a69e5423dd88bbdd06ad376c6c794e72d3ad58c 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -242,6 +242,20 @@ public: static const unsigned int WAYPOINT_RADIUS_DEFAULT_FIXED_WING = 25; static const unsigned int WAYPOINT_RADIUS_DEFAULT_ROTARY_WING = 5; + + enum StartCalibrationType { + StartCalibrationRadio, + StartCalibrationGyro, + StartCalibrationMag, + StartCalibrationAirspeed, + StartCalibrationAccel + }; + + /// Starts the specified calibration + virtual void startCalibration(StartCalibrationType calType) = 0; + + /// Ends any current calibration + virtual void stopCalibration(void) = 0; public slots: @@ -344,12 +358,6 @@ public slots: virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0; virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0; - virtual void startRadioControlCalibration(int param=1) = 0; - virtual void endRadioControlCalibration() = 0; - virtual void startMagnetometerCalibration() = 0; - virtual void startGyroscopeCalibration() = 0; - virtual void startPressureCalibration() = 0; - /** @brief Return if this a rotary wing */ virtual bool isRotaryWing() = 0; /** @brief Return if this is a fixed wing */ diff --git a/src/ui/px4_configuration/PX4RCCalibration.cc b/src/ui/px4_configuration/PX4RCCalibration.cc index 6d9302b0f619c3b77b194fefe3cccf6c367ad78b..9bf76c54bad71451bf9cf73bb0afad9e723e28c0 100644 --- a/src/ui/px4_configuration/PX4RCCalibration.cc +++ b/src/ui/px4_configuration/PX4RCCalibration.cc @@ -892,7 +892,7 @@ void PX4RCCalibration::_writeCalibration(void) { if (!_mav) return; - _mav->endRadioControlCalibration(); + _mav->stopCalibration(); _validateCalibration(); @@ -986,7 +986,7 @@ void PX4RCCalibration::_startCalibration(void) _resetInternalCalibrationValues(); // Let the mav known we are starting calibration. This should turn off motors and so forth. - _mav->startRadioControlCalibration(); + _mav->startCalibration(UASInterface::StartCalibrationRadio); _ui->rcCalNext->setText(tr("Next")); _ui->rcCalCancel->setEnabled(true); @@ -1001,7 +1001,7 @@ void PX4RCCalibration::_stopCalibration(void) _currentStep = -1; if (_mav) { - _mav->endRadioControlCalibration(); + _mav->stopCalibration(); _setInternalCalibrationValuesFromParameters(); } else { _resetInternalCalibrationValues();