From 9aec1e48afe20312693081addf28f42396f81155 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 2 Oct 2020 15:41:16 +1000 Subject: [PATCH] AP_Frsky_Telem: tidy mavlite message handling Including calling into reboot-vehicle rather than the hal reboot method. --- .../AP_Frsky_MAVliteMsgHandler.cpp | 56 +++++++++++-------- .../AP_Frsky_MAVliteMsgHandler.h | 11 +++- 2 files changed, 40 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp index c977d8fb24..ac954923c0 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp @@ -9,6 +9,7 @@ extern const AP_HAL::HAL& hal; * Handle the COMMAND_LONG mavlite message * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ + void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg) { mavlink_command_long_t mav_command_long {}; @@ -39,24 +40,22 @@ void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Mess mav_command_long.param6 = params[5]; mav_command_long.param7 = params[6]; - MAV_RESULT mav_result = MAV_RESULT_FAILED; + const MAV_RESULT mav_result = handle_command(mav_command_long); + send_command_ack(mav_result, mav_command_long.command); +} + +MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command(const mavlink_command_long_t &mav_command_long) +{ // filter allowed commands switch (mav_command_long.command) { //case MAV_CMD_ACCELCAL_VEHICLE_POS: case MAV_CMD_DO_SET_MODE: - if (AP::vehicle()->set_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) { - mav_result = MAV_RESULT_ACCEPTED; - } - break; + return handle_command_do_set_mode(mav_command_long); //case MAV_CMD_DO_SET_HOME: case MAV_CMD_DO_FENCE_ENABLE: - mav_result = handle_command_do_fence_enable((uint16_t)mav_command_long.param1); - break; + return handle_command_do_fence_enable(mav_command_long); case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2)) { - mav_result = handle_command_preflight_reboot(); - } - break; + return handle_command_preflight_reboot(mav_command_long); //case MAV_CMD_DO_START_MAG_CAL: //case MAV_CMD_DO_ACCEPT_MAG_CAL: //case MAV_CMD_DO_CANCEL_MAG_CAL: @@ -72,10 +71,7 @@ void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Mess //case MAV_CMD_DO_SET_ROI_LOCATION: //case MAV_CMD_DO_SET_ROI: case MAV_CMD_PREFLIGHT_CALIBRATION: - if (is_equal(mav_command_long.param1,0.0f) && is_equal(mav_command_long.param2,0.0f) && is_equal(mav_command_long.param3,1.0f)) { - mav_result = handle_command_preflight_calibration_baro(); - } - break; + return handle_command_preflight_calibration_baro(mav_command_long); //case MAV_CMD_BATTERY_RESET: //case MAV_CMD_PREFLIGHT_UAVCAN: //case MAV_CMD_FLASH_BOOTLOADER: @@ -93,10 +89,17 @@ void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Mess //case MAV_CMD_COMPONENT_ARM_DISARM: //case MAV_CMD_FIXED_MAG_CAL_YAW: default: - mav_result = MAV_RESULT_UNSUPPORTED; - break; + return MAV_RESULT_UNSUPPORTED; } - send_command_ack(mav_result, mav_command_long.command); + return MAV_RESULT_UNSUPPORTED; +} + +MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_set_mode(const mavlink_command_long_t &mav_command_long) +{ + if (AP::vehicle()->set_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; } void AP_Frsky_MAVliteMsgHandler::send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid) @@ -112,8 +115,12 @@ void AP_Frsky_MAVliteMsgHandler::send_command_ack(const MAV_RESULT mav_result, c send_message(txmsg); } -MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_calibration_baro() +MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_calibration_baro(const mavlink_command_long_t &mav_command_long) { + if (!(is_equal(mav_command_long.param1,0.0f) && is_equal(mav_command_long.param2,0.0f) && is_equal(mav_command_long.param3,1.0f))) { + return MAV_RESULT_FAILED; + } + if (hal.util->get_soft_armed()) { return MAV_RESULT_DENIED; } @@ -130,14 +137,14 @@ MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_calibration_baro return MAV_RESULT_ACCEPTED; } -MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(uint16_t param1) +MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(const mavlink_command_long_t &mav_command_long) { AC_Fence *fence = AP::fence(); if (fence == nullptr) { return MAV_RESULT_UNSUPPORTED; } - switch (param1) { + switch ((uint16_t)mav_command_long.param1) { case 0: fence->enable(false); return MAV_RESULT_ACCEPTED; @@ -229,8 +236,11 @@ void AP_Frsky_MAVliteMsgHandler::handle_param_set(const AP_Frsky_MAVlite_Message motors. That can be dangerous when a preflight reboot is done with the pilot close to the aircraft and can also damage the aircraft */ -MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_reboot(void) +MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_reboot(const mavlink_command_long_t &mav_command_long) { + if (!(is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2))) { + return MAV_RESULT_FAILED; + } if (hal.util->get_soft_armed()) { // refuse reboot when armed return MAV_RESULT_FAILED; @@ -262,8 +272,6 @@ void AP_Frsky_MAVliteMsgHandler::process_message(const AP_Frsky_MAVlite_Message /* * Send a mavlite message - * Message is chunked in sport packets pushed in the tx queue - * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ bool AP_Frsky_MAVliteMsgHandler::send_message(AP_Frsky_MAVlite_Message &txmsg) { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.h b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.h index 16d08fc593..2987c9276c 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.h @@ -22,11 +22,16 @@ private: // gcs mavlite methods void handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg); void handle_param_set(const AP_Frsky_MAVlite_Message &rxmsg); + void handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg); + + MAV_RESULT handle_command(const mavlink_command_long_t &mav_command_long); + MAV_RESULT handle_command_preflight_calibration_baro(const mavlink_command_long_t &mav_command_long); + MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &mav_command_long); + MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &mav_command_long); + MAV_RESULT handle_command_preflight_reboot(const mavlink_command_long_t &mav_command_long); + void send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid); - MAV_RESULT handle_command_preflight_calibration_baro(); - MAV_RESULT handle_command_do_fence_enable(uint16_t param1); - MAV_RESULT handle_command_preflight_reboot(void); send_mavlite_fn_t _send_fn; };