From 332fcb32261baaaf77da64cb43ba330fda869b01 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 4 Jul 2018 12:45:30 +1000 Subject: [PATCH] Sub: handle command_int in base class --- ArduSub/GCS_Mavlink.cpp | 150 +++++++++++++++++++--------------------- ArduSub/GCS_Mavlink.h | 1 + 2 files changed, 72 insertions(+), 79 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index c8cced8f89..cefacffc24 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -661,6 +661,77 @@ MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_ return GCS_MAVLINK::_handle_command_preflight_calibration(packet); } +MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet) +{ + switch (packet.command) { + + case MAV_CMD_DO_SET_HOME: { + // assume failure + if (is_equal(packet.param1, 1.0f)) { + // if param1 is 1, use current location + if (sub.set_home_to_current_location(true)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + } + // 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_FAILED; + } + // 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 += sub.ahrs.get_home().alt; + } + if (sub.set_home(new_home_loc, true)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + } + + 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); + sub.set_auto_yaw_roi(roi_loc); + return MAV_RESULT_ACCEPTED; + } + default: + return GCS_MAVLINK::handle_command_int_packet(packet); + } +} + + void GCS_MAVLINK_Sub::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 @@ -736,85 +807,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) } - 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_SET_HOME: { - // assume failure - result = MAV_RESULT_FAILED; - if (is_equal(packet.param1, 1.0f)) { - // if param1 is 1, use current location - if (sub.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 += sub.ahrs.get_home().alt; - } - if (sub.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); - sub.set_auto_yaw_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 // decode packet diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index df32a90ae2..5ed63fd2e7 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -26,6 +26,7 @@ protected: MAV_RESULT _handle_command_preflight_calibration_baro() override; MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; // override sending of scaled_pressure3 to send on-board temperature: void send_scaled_pressure3() override;