diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 64d76fa..2ad32c8 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1515,6 +1515,9 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType) case StartCalibrationEsc: escCal = 1; break; + case StartCalibrationUavcanEsc: + escCal = 2; + break; } mavlink_message_t msg; @@ -1555,6 +1558,54 @@ void UAS::stopCalibration(void) sendMessage(msg); } +void UAS::startBusConfig(UASInterface::StartBusConfigType calType) +{ + int actuatorCal = 0; + + switch (calType) { + case StartBusConfigActuators: + actuatorCal = 1; + break; + } + + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + 0, // target component + MAV_CMD_PREFLIGHT_UAVCAN, // command id + 0, // 0=first transmission of command + actuatorCal, // actuators + 0, + 0, + 0, + 0, + 0, + 0); + sendMessage(msg); +} + +void UAS::stopBusConfig(void) +{ + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + 0, // target component + MAV_CMD_PREFLIGHT_UAVCAN, // command id + 0, // 0=first transmission of command + 0, + 0, + 0, + 0, + 0, + 0, + 0); + sendMessage(msg); +} + void UAS::startDataRecording() { mavlink_message_t msg; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index a7ff66e..adbba6e 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -916,6 +916,9 @@ public slots: void startCalibration(StartCalibrationType calType); void stopCalibration(void); + void startBusConfig(StartBusConfigType calType); + void stopBusConfig(void); + void startDataRecording(); void stopDataRecording(); void deleteSettings(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 9c3bbf1..4851e94 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -249,7 +249,12 @@ public: StartCalibrationAccel, StartCalibrationLevel, StartCalibrationEsc, - StartCalibrationCopyTrims + StartCalibrationCopyTrims, + StartCalibrationUavcanEsc + }; + + enum StartBusConfigType { + StartBusConfigActuators }; /// Starts the specified calibration @@ -258,6 +263,12 @@ public: /// Ends any current calibration virtual void stopCalibration(void) = 0; + /// Starts the specified bus configuration + virtual void startBusConfig(StartBusConfigType calType) = 0; + + /// Ends any current bus configuration + virtual void stopBusConfig(void) = 0; + public slots: /** @brief Set a new name for the system */