Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: tidy mavlite message handling

Browse Source 
Including calling into reboot-vehicle rather than the hal reboot method.
This commit is contained in:
Peter Barker 2020-10-02 15:41:16 +10:00 committed by Andrew Tridgell
parent e0f8e003ec
commit 9aec1e48af
2 changed files with 40 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp
View File

@ -9,6 +9,7 @@ extern const AP_HAL::HAL& hal;
* Handle the COMMAND_LONG mavlite message * Handle the COMMAND_LONG mavlite message
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg) void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg)
{ {
mavlink_command_long_t mav_command_long {}; 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.param6 = params[5];
mav_command_long.param7 = params[6]; 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 // filter allowed commands
switch (mav_command_long.command) { switch (mav_command_long.command) {
//case MAV_CMD_ACCELCAL_VEHICLE_POS: //case MAV_CMD_ACCELCAL_VEHICLE_POS:
case MAV_CMD_DO_SET_MODE: case MAV_CMD_DO_SET_MODE:
if (AP::vehicle()->set_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) { return handle_command_do_set_mode(mav_command_long);
mav_result = MAV_RESULT_ACCEPTED;
}
break;
//case MAV_CMD_DO_SET_HOME: //case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_FENCE_ENABLE: case MAV_CMD_DO_FENCE_ENABLE:
mav_result = handle_command_do_fence_enable((uint16_t)mav_command_long.param1); return handle_command_do_fence_enable(mav_command_long);
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2)) { return handle_command_preflight_reboot(mav_command_long);
mav_result = handle_command_preflight_reboot();
}
break;
//case MAV_CMD_DO_START_MAG_CAL: //case MAV_CMD_DO_START_MAG_CAL:
//case MAV_CMD_DO_ACCEPT_MAG_CAL: //case MAV_CMD_DO_ACCEPT_MAG_CAL:
//case MAV_CMD_DO_CANCEL_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_LOCATION:
//case MAV_CMD_DO_SET_ROI: //case MAV_CMD_DO_SET_ROI:
case MAV_CMD_PREFLIGHT_CALIBRATION: 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)) { return handle_command_preflight_calibration_baro(mav_command_long);
mav_result = handle_command_preflight_calibration_baro();
}
break;
//case MAV_CMD_BATTERY_RESET: //case MAV_CMD_BATTERY_RESET:
//case MAV_CMD_PREFLIGHT_UAVCAN: //case MAV_CMD_PREFLIGHT_UAVCAN:
//case MAV_CMD_FLASH_BOOTLOADER: //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_COMPONENT_ARM_DISARM:
//case MAV_CMD_FIXED_MAG_CAL_YAW: //case MAV_CMD_FIXED_MAG_CAL_YAW:
default: default:
mav_result = MAV_RESULT_UNSUPPORTED; return MAV_RESULT_UNSUPPORTED;
break;
} }
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) 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); 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()) { if (hal.util->get_soft_armed()) {
return MAV_RESULT_DENIED; return MAV_RESULT_DENIED;
} }
@ -130,14 +137,14 @@ MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_calibration_baro
return MAV_RESULT_ACCEPTED; 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(); AC_Fence *fence = AP::fence();
if (fence == nullptr) { if (fence == nullptr) {
return MAV_RESULT_UNSUPPORTED; return MAV_RESULT_UNSUPPORTED;
} }
switch (param1) { switch ((uint16_t)mav_command_long.param1) {
case 0: case 0:
fence->enable(false); fence->enable(false);
return MAV_RESULT_ACCEPTED; 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 motors. That can be dangerous when a preflight reboot is done with
the pilot close to the aircraft and can also damage the aircraft 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()) { if (hal.util->get_soft_armed()) {
// refuse reboot when armed // refuse reboot when armed
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
@ -262,8 +272,6 @@ void AP_Frsky_MAVliteMsgHandler::process_message(const AP_Frsky_MAVlite_Message
/* /*
* Send a 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) bool AP_Frsky_MAVliteMsgHandler::send_message(AP_Frsky_MAVlite_Message &txmsg)
{ {

11
libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.h
View File

@ -22,11 +22,16 @@ private:
// gcs mavlite methods // gcs mavlite methods
void handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg); void handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg);
void handle_param_set(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); 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); 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; send_mavlite_fn_t _send_fn;
}; };