From aa4ee64af2d316e4dcd633916385c5db8d8d8b34 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 4 Jul 2018 11:43:51 +1000 Subject: [PATCH] Copter: handle command_int in base class --- ArduCopter/GCS_Mavlink.cpp | 171 +++++++++++++++++-------------------- ArduCopter/GCS_Mavlink.h | 2 + 2 files changed, 82 insertions(+), 91 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 93d123fdb6..22c9a4f1fe 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -604,6 +604,86 @@ MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavli return GCS_MAVLINK::_handle_command_preflight_calibration(packet); } + +MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet) +{ + switch(packet.command) { + case MAV_CMD_DO_FOLLOW: +#if MODE_FOLLOW_ENABLED == ENABLED + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + copter.g2.follow.set_target_sysid((uint8_t)packet.param1); + return MAV_RESULT_ACCEPTED; + } +#endif + return MAV_RESULT_UNSUPPORTED; + + case MAV_CMD_DO_SET_HOME: { + // assume failure + if (is_equal(packet.param1, 1.0f)) { + // if param1 is 1, use current location + if (!copter.set_home_to_current_location(true)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + } + // ensure param1 is zero + if (!is_zero(packet.param1)) { + return MAV_RESULT_FAILED; + } + // check frame type is supported + if (packet.frame != MAV_FRAME_GLOBAL && + packet.frame != MAV_FRAME_GLOBAL_INT && + packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && + packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + return MAV_RESULT_UNSUPPORTED; + } + // sanity check location + if (!check_latlng(packet.x, packet.y)) { + return MAV_RESULT_FAILED; + } + Location new_home_loc {}; + new_home_loc.lat = packet.x; + new_home_loc.lng = packet.y; + new_home_loc.alt = packet.z * 100; + // handle relative altitude + if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + if (!AP::ahrs().home_is_set()) { + // cannot use relative altitude if home is not set + return MAV_RESULT_FAILED; + } + new_home_loc.alt += copter.ahrs.get_home().alt; + } + if (!copter.set_home(new_home_loc, true)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + } + + case MAV_CMD_DO_SET_ROI: { + // param1 : /* Region of interest mode (not used)*/ + // param2 : /* MISSION index/ target ID (not used)*/ + // param3 : /* ROI index (not used)*/ + // param4 : /* empty */ + // x : lat + // y : lon + // z : alt + // sanity check location + if (!check_latlng(packet.x, packet.y)) { + return MAV_RESULT_FAILED; + } + Location roi_loc; + roi_loc.lat = packet.x; + roi_loc.lng = packet.y; + roi_loc.alt = (int32_t)(packet.z * 100.0f); + copter.flightmode->auto_yaw.set_roi(roi_loc); + return MAV_RESULT_ACCEPTED; + } + default: + return GCS_MAVLINK::handle_command_int_packet(packet); + } +} + void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) { MAV_RESULT result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required @@ -708,97 +788,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_COMMAND_INT: - { - // decode packet - mavlink_command_int_t packet; - mavlink_msg_command_int_decode(msg, &packet); - switch(packet.command) - { - case MAV_CMD_DO_FOLLOW: -#if MODE_FOLLOW_ENABLED == ENABLED - // param1: sysid of target to follow - if ((packet.param1 > 0) && (packet.param1 <= 255)) { - copter.g2.follow.set_target_sysid((uint8_t)packet.param1); - result = MAV_RESULT_ACCEPTED; - } -#endif - break; - - case MAV_CMD_DO_SET_HOME: { - // assume failure - result = MAV_RESULT_FAILED; - if (is_equal(packet.param1, 1.0f)) { - // if param1 is 1, use current location - if (copter.set_home_to_current_location(true)) { - result = MAV_RESULT_ACCEPTED; - } - break; - } - // ensure param1 is zero - if (!is_zero(packet.param1)) { - break; - } - // check frame type is supported - if (packet.frame != MAV_FRAME_GLOBAL && - packet.frame != MAV_FRAME_GLOBAL_INT && - packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && - packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { - break; - } - // sanity check location - if (!check_latlng(packet.x, packet.y)) { - break; - } - Location new_home_loc {}; - new_home_loc.lat = packet.x; - new_home_loc.lng = packet.y; - new_home_loc.alt = packet.z * 100; - // handle relative altitude - if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { - if (!AP::ahrs().home_is_set()) { - // cannot use relative altitude if home is not set - break; - } - new_home_loc.alt += copter.ahrs.get_home().alt; - } - if (copter.set_home(new_home_loc, true)) { - result = MAV_RESULT_ACCEPTED; - } - break; - } - - case MAV_CMD_DO_SET_ROI: { - // param1 : /* Region of interest mode (not used)*/ - // param2 : /* MISSION index/ target ID (not used)*/ - // param3 : /* ROI index (not used)*/ - // param4 : /* empty */ - // x : lat - // y : lon - // z : alt - // sanity check location - if (!check_latlng(packet.x, packet.y)) { - break; - } - Location roi_loc; - roi_loc.lat = packet.x; - roi_loc.lng = packet.y; - roi_loc.alt = (int32_t)(packet.z * 100.0f); - copter.flightmode->auto_yaw.set_roi(roi_loc); - result = MAV_RESULT_ACCEPTED; - break; - } - - default: - result = MAV_RESULT_UNSUPPORTED; - break; - } - - // send ACK or NAK - mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); - break; - } - // Pre-Flight calibration requests case MAVLINK_MSG_ID_COMMAND_LONG: // MAV ID: 76 { diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index f78d9c0e5e..1a203ac9d5 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -35,6 +35,8 @@ protected: void send_position_target_global_int() override; + MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; + private: void handleMessage(mavlink_message_t * msg) override;