From 93ca24398742975c345e341b60ea3889b6366433 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2019 12:32:05 +1000 Subject: [PATCH] GCS_MAVLink: rearrange mission item handling code This splits the missionitemprotocol handling entirely into separate header files and separate compilation units. --- libraries/GCS_MAVLink/GCS.h | 255 +---------- libraries/GCS_MAVLink/GCS_Common.cpp | 420 ------------------ libraries/GCS_MAVLink/GCS_Rally.cpp | 86 +--- libraries/GCS_MAVLink/MissionItemProtocol.cpp | 305 +++++++++++++ libraries/GCS_MAVLink/MissionItemProtocol.h | 111 +++++ .../GCS_MAVLink/MissionItemProtocol_Rally.cpp | 121 +++++ .../GCS_MAVLink/MissionItemProtocol_Rally.h | 38 ++ .../MissionItemProtocol_Waypoints.cpp | 139 ++++++ .../MissionItemProtocol_Waypoints.h | 68 +++ libraries/GCS_MAVLink/ap_message.h | 75 ++++ 10 files changed, 864 insertions(+), 754 deletions(-) create mode 100644 libraries/GCS_MAVLink/MissionItemProtocol.cpp create mode 100644 libraries/GCS_MAVLink/MissionItemProtocol.h create mode 100644 libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp create mode 100644 libraries/GCS_MAVLink/MissionItemProtocol_Rally.h create mode 100644 libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp create mode 100644 libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h create mode 100644 libraries/GCS_MAVLink/ap_message.h diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index f671fcad71..4cb5171253 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -15,7 +15,10 @@ #include #include #include -#include + +#include "MissionItemProtocol_Waypoints.h" +#include "MissionItemProtocol_Rally.h" +#include "ap_message.h" #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0 @@ -25,79 +28,6 @@ #define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false #define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false -// GCS Message ID's -/// NOTE: to ensure we never block on sending MAVLink messages -/// please keep each MSG_ to a single MAVLink message. If need be -/// create new MSG_ IDs for additional messages on the same -/// stream -enum ap_message : uint8_t { - MSG_HEARTBEAT, - MSG_ATTITUDE, - MSG_LOCATION, - MSG_SYS_STATUS, - MSG_POWER_STATUS, - MSG_MEMINFO, - MSG_NAV_CONTROLLER_OUTPUT, - MSG_CURRENT_WAYPOINT, - MSG_VFR_HUD, - MSG_SERVO_OUTPUT_RAW, - MSG_RC_CHANNELS, - MSG_RC_CHANNELS_RAW, - MSG_RAW_IMU, - MSG_SCALED_IMU, - MSG_SCALED_IMU2, - MSG_SCALED_IMU3, - MSG_SCALED_PRESSURE, - MSG_SCALED_PRESSURE2, - MSG_SCALED_PRESSURE3, - MSG_SENSOR_OFFSETS, - MSG_GPS_RAW, - MSG_GPS_RTK, - MSG_GPS2_RAW, - MSG_GPS2_RTK, - MSG_SYSTEM_TIME, - MSG_SERVO_OUT, - MSG_NEXT_MISSION_REQUEST_WAYPOINTS, - MSG_NEXT_MISSION_REQUEST_RALLY, - MSG_NEXT_PARAM, - MSG_FENCE_STATUS, - MSG_AHRS, - MSG_SIMSTATE, - MSG_AHRS2, - MSG_AHRS3, - MSG_HWSTATUS, - MSG_WIND, - MSG_RANGEFINDER, - MSG_DISTANCE_SENSOR, - MSG_TERRAIN, - MSG_BATTERY2, - MSG_CAMERA_FEEDBACK, - MSG_MOUNT_STATUS, - MSG_OPTICAL_FLOW, - MSG_GIMBAL_REPORT, - MSG_MAG_CAL_PROGRESS, - MSG_MAG_CAL_REPORT, - MSG_EKF_STATUS_REPORT, - MSG_LOCAL_POSITION, - MSG_PID_TUNING, - MSG_VIBRATION, - MSG_RPM, - MSG_WHEEL_DISTANCE, - MSG_MISSION_ITEM_REACHED, - MSG_POSITION_TARGET_GLOBAL_INT, - MSG_POSITION_TARGET_LOCAL_NED, - MSG_ADSB_VEHICLE, - MSG_BATTERY_STATUS, - MSG_AOA_SSA, - MSG_LANDING, - MSG_ESC_TELEMETRY, - MSG_ORIGIN, - MSG_HOME, - MSG_NAMED_FLOAT, - MSG_EXTENDED_SYS_STATE, - MSG_LAST // MSG_LAST must be the last entry in this enum -}; - // convenience macros for defining which ap_message ids are in which streams: #define MAV_STREAM_ENTRY(stream_name) \ { \ @@ -107,183 +37,6 @@ enum ap_message : uint8_t { } #define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 } -// MissionItemProtocol objects are used for transfering missions from -// a GCS to ArduPilot and vice-versa. -// -// There exists one MissionItemProtocol instance for each of the types -// of item that might be transfered - e.g. MissionItemProtocol_Rally -// for rally point uploads. These objects are static in GCS_MAVLINK -// and used by all of the backends. -// -// While prompting the GCS for items required to complete the mission, -// a link is stored to the link the MissionItemProtocol should send -// requests out on, and the "receiving" boolean is true. In this -// state downloading of items (and the item count!) is blocked. -// Starting of uploads (for the same protocol) is also blocked - -// essentially the GCS uploading a set of items (e.g. a mission) has a -// mutex over the mission. -class MissionItemProtocol -{ -public: - - // note that all of these methods are named after the packet they - // are handling; the "mission" part just comes as part of that. - void handle_mission_request_list(const class GCS_MAVLINK &link, - const mavlink_mission_request_list_t &packet, - const mavlink_message_t &msg); - void handle_mission_request_int(const GCS_MAVLINK &link, - const mavlink_mission_request_int_t &packet, - const mavlink_message_t &msg); - void handle_mission_request(const GCS_MAVLINK &link, - const mavlink_mission_request_t &packet, - const mavlink_message_t &msg); - - void handle_mission_count(class GCS_MAVLINK &link, - const mavlink_mission_count_t &packet, - const mavlink_message_t &msg); - void handle_mission_write_partial_list(GCS_MAVLINK &link, - const mavlink_message_t &msg, - const mavlink_mission_write_partial_list_t &packet); - - // called on receipt of a MISSION_ITEM or MISSION_ITEM_INT packet; - // the former is converted to the latter. - void handle_mission_item(const mavlink_message_t &msg, - const mavlink_mission_item_int_t &cmd); - - void handle_mission_clear_all(const GCS_MAVLINK &link, - const mavlink_message_t &msg); - - void queued_request_send(); - void update(); - - bool active_link_is(const GCS_MAVLINK *_link) const { return _link == link; }; - - virtual MAV_MISSION_TYPE mission_type() const = 0; - - bool receiving; // currently sending requests and expecting items - -protected: - - GCS_MAVLINK *link; // link currently receiving waypoints on - - // return the ap_message which can be queued to be sent to send a - // item request to the GCS: - virtual ap_message next_item_ap_message_id() const = 0; - - virtual bool clear_all_items() = 0; - - uint16_t request_last; // last request index - -private: - - virtual void truncate(const mavlink_mission_count_t &packet) = 0; - - uint16_t request_i; // request index - - // waypoints - uint8_t dest_sysid; // where to send requests - uint8_t dest_compid; // " - uint32_t timelast_receive_ms; - uint32_t timelast_request_ms; - const uint16_t upload_timeout_ms = 8000; - - // support for GCS getting waypoints etc from us: - virtual MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) = 0; - - void init_send_requests(GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const int16_t _request_first, - const int16_t _request_last); - - void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const; - void send_mission_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const; - - virtual uint16_t item_count() const = 0; - virtual uint16_t max_items() const = 0; - - virtual MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &mission_item_int) = 0; - virtual MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &mission_item_int) = 0; - - virtual void complete(const GCS_MAVLINK &_link) {}; - virtual void timeout() {}; - - void convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(const mavlink_mission_request_t &request, - mavlink_mission_request_int_t &request_int); -}; - -class MissionItemProtocol_Waypoints : public MissionItemProtocol { -public: - MissionItemProtocol_Waypoints(AP_Mission &_mission) : - mission(_mission) {} - void truncate(const mavlink_mission_count_t &packet) override; - MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_MISSION; } - - void complete(const GCS_MAVLINK &_link) override; - void timeout() override; - -protected: - - bool clear_all_items() override WARN_IF_UNUSED; - - ap_message next_item_ap_message_id() const override { - return MSG_NEXT_MISSION_REQUEST_WAYPOINTS; - } - -private: - AP_Mission &mission; - - uint16_t item_count() const override { return mission.num_commands(); } - uint16_t max_items() const override { return mission.num_commands_max(); } - - MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED; - MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED; - - MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; -}; - -class MissionItemProtocol_Rally : public MissionItemProtocol { -public: - MissionItemProtocol_Rally(AP_Rally &_rally) : - rally(_rally) {} - void truncate(const mavlink_mission_count_t &packet) override; - MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_RALLY; } - - void complete(const GCS_MAVLINK &_link) override; - void timeout() override; - -protected: - - ap_message next_item_ap_message_id() const override { - return MSG_NEXT_MISSION_REQUEST_RALLY; - } - bool clear_all_items() override WARN_IF_UNUSED; - -private: - AP_Rally &rally; - - uint16_t item_count() const override { - return rally.get_rally_total(); - } - uint16_t max_items() const override { return rally.get_rally_max(); } - - MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; - MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; - - MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; - - static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret) WARN_IF_UNUSED; - -}; - /// /// @class GCS_MAVLINK /// @brief MAVLink transport control class diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 8d46e2856c..c6cb412e72 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -180,78 +180,6 @@ GCS_MAVLINK::setup_uart(uint8_t instance) } } - -/** - * @brief Send the next pending waypoint, called from deferred message - * handling code - */ -void MissionItemProtocol::queued_request_send() -{ - if (!receiving) { - return; - } - if (request_i > request_last) { - return; - } - if (link == nullptr) { - AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); - return; - } - mavlink_msg_mission_request_send( - link->get_chan(), - dest_sysid, - dest_compid, - request_i, - mission_type()); - timelast_request_ms = AP_HAL::millis(); -} - -void MissionItemProtocol::update() -{ - if (!receiving) { - // we don't need to do anything unless we're sending requests - return; - } - if (link == nullptr) { - AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); - return; - } - // stop waypoint receiving if timeout - const uint32_t tnow = AP_HAL::millis(); - if (tnow - timelast_receive_ms > upload_timeout_ms) { - receiving = false; - timeout(); - link = nullptr; - return; - } - // resend request if we haven't gotten one: - const uint32_t wp_recv_timeout_ms = 1000U + (link->get_stream_slowdown_ms()*20); - if (tnow - timelast_request_ms > wp_recv_timeout_ms) { - timelast_request_ms = tnow; - link->send_message(next_item_ap_message_id()); - } -} - -void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg, - MAV_MISSION_RESULT result) const -{ - if (link == nullptr) { - AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); - return; - } - send_mission_ack(*link, msg, result); -} -void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - MAV_MISSION_RESULT result) const -{ - mavlink_msg_mission_ack_send(_link.get_chan(), - msg.sysid, - msg.compid, - result, - mission_type()); -} - void GCS_MAVLINK::send_meminfo(void) { unsigned __brkval = 0; @@ -543,62 +471,6 @@ void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t &msg) prot->handle_mission_request_list(*this, packet, msg); } -void MissionItemProtocol::handle_mission_request_list( - const GCS_MAVLINK &_link, - const mavlink_mission_request_list_t &packet, - const mavlink_message_t &msg) -{ - if (receiving) { - // someone is uploading a mission; reject fetching of points - // until done or timeout - send_mission_ack(_link, msg, MAV_MISSION_DENIED); - return; - } - - // reply with number of commands in the mission. The GCS will - // then request each command separately - mavlink_msg_mission_count_send(_link.get_chan(), - msg.sysid, - msg.compid, - item_count(), - mission_type()); -} - -void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link, - const mavlink_mission_request_int_t &packet, - const mavlink_message_t &msg) -{ - if (receiving) { - // someone is uploading a mission; reject fetching of points - // until done or timeout - send_mission_ack(_link, msg, MAV_MISSION_DENIED); - return; - } - - mavlink_mission_item_int_t ret_packet{}; - - ret_packet.target_system = msg.sysid; - ret_packet.target_component = msg.compid; - ret_packet.seq = packet.seq; - ret_packet.mission_type = packet.mission_type; - - const MAV_MISSION_RESULT result_code = get_item(_link, msg, packet, ret_packet); - - if (result_code != MAV_MISSION_ACCEPTED) { - // send failure message - send_mission_ack(_link, msg, result_code); - return; - } - - // we already have a filled structure, use it in place of _send: - _mav_finalize_message_chan_send(_link.get_chan(), - MAVLINK_MSG_ID_MISSION_ITEM_INT, - (const char *)&ret_packet, - MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, - MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, - MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); -} - /* handle a MISSION_REQUEST mavlink packet */ @@ -615,48 +487,6 @@ void GCS_MAVLINK::handle_mission_request_int(const mavlink_message_t &msg) prot->handle_mission_request_int(*this, packet, msg); } -MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) -{ - if (packet.seq != 0 && // always allow HOME to be read - packet.seq >= mission.num_commands()) { - // try to educate the GCS on the actual size of the mission: - mavlink_msg_mission_count_send(_link.get_chan(), - msg.sysid, - msg.compid, - mission.num_commands(), - MAV_MISSION_TYPE_MISSION); - return MAV_MISSION_ERROR; - } - - AP_Mission::Mission_Command cmd; - - // retrieve mission from eeprom - if (!mission.read_cmd_from_storage(packet.seq, cmd)) { - return MAV_MISSION_ERROR; - } - - if (!AP_Mission::mission_cmd_to_mavlink_int(cmd, ret_packet)) { - return MAV_MISSION_ERROR; - } - - // set packet's current field to 1 if this is the command being executed - if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) { - ret_packet.current = 1; - } else { - ret_packet.current = 0; - } - - // set auto continue to 1 - ret_packet.autocontinue = 1; // 1 (true), 0 (false) - - ret_packet.command = cmd.id; - - return MAV_MISSION_ACCEPTED; -} - void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg) { // decode @@ -670,46 +500,6 @@ void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg) prot->handle_mission_request(*this, packet, msg); } -void MissionItemProtocol::convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(const mavlink_mission_request_t &request, mavlink_mission_request_int_t &request_int) -{ - request_int.target_system = request.target_system; - request_int.target_component = request.target_component; - request_int.seq = request.seq; - request_int.mission_type = request.mission_type; -} - -void MissionItemProtocol::handle_mission_request(const GCS_MAVLINK &_link, - const mavlink_mission_request_t &packet, - const mavlink_message_t &msg -) -{ - mavlink_mission_request_int_t request_int; - convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(packet, request_int); - - mavlink_mission_item_int_t item_int{}; - item_int.target_system = msg.sysid; - item_int.target_component = msg.compid; - - MAV_MISSION_RESULT ret = get_item(_link, msg, request_int, item_int); - if (ret != MAV_MISSION_ACCEPTED) { - return; - } - - mavlink_mission_item_t ret_packet{}; - ret = AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(item_int, ret_packet); - if (ret != MAV_MISSION_ACCEPTED) { - return; - } - - // we already have a filled structure, use it in place of _send: - _mav_finalize_message_chan_send(_link.get_chan(), - MAVLINK_MSG_ID_MISSION_ITEM, - (const char *)&ret_packet, - MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, - MAVLINK_MSG_ID_MISSION_ITEM_LEN, - MAVLINK_MSG_ID_MISSION_ITEM_CRC); -} - /* handle a MISSION_SET_CURRENT mavlink packet */ @@ -747,66 +537,6 @@ void GCS_MAVLINK::handle_mission_count(const mavlink_message_t &msg) prot->handle_mission_count(*this, packet, msg); } -void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const int16_t _request_first, - const int16_t _request_last) -{ - // set variables to help handle the expected receiving of commands from the GCS - timelast_receive_ms = AP_HAL::millis(); // set time we last received commands to now - receiving = true; // record that we expect to receive commands - request_i = _request_first; // reset the next expected command number to zero - request_last = _request_last; // record how many commands we expect to receive - - dest_sysid = msg.sysid; // record system id of GCS who wants to upload the mission - dest_compid = msg.compid; // record component id of GCS who wants to upload the mission - - link = &_link; - - timelast_request_ms = AP_HAL::millis(); - link->send_message(next_item_ap_message_id()); -} - -void MissionItemProtocol::handle_mission_count( - GCS_MAVLINK &_link, - const mavlink_mission_count_t &packet, - const mavlink_message_t &msg) -{ - if (receiving) { - // someone is already uploading a mission. If we are - // receiving from someone then we will allow them to restart - - // otherwise we deny. - if (msg.sysid != dest_sysid || msg.compid != dest_compid) { - // reject another upload until - send_mission_ack(_link, msg, MAV_MISSION_DENIED); - return; - } - } - - if (packet.count > max_items()) { - send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE); - return; - } - - truncate(packet); - - if (packet.count == 0) { - // no requests to send... - send_mission_ack(_link, msg, MAV_MISSION_ACCEPTED); - complete(_link); - return; - } - - // start waypoint receiving - init_send_requests(_link, msg, 0, packet.count-1); -} - -void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &packet) -{ - // new mission arriving, truncate mission to be the same length - mission.truncate(packet.count); -} - /* handle a MISSION_CLEAR_ALL mavlink packet */ @@ -826,26 +556,6 @@ void GCS_MAVLINK::handle_mission_clear_all(const mavlink_message_t &msg) prot->handle_mission_clear_all(*this, msg); } -bool MissionItemProtocol_Waypoints::clear_all_items() -{ - return mission.clear(); -} - -bool MissionItemProtocol_Rally::clear_all_items() -{ - rally.truncate(0); - return true; -} - -void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link, - const mavlink_message_t &msg) -{ - bool success = true; - success = success && !receiving; - success = success && clear_all_items(); - send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR); -} - bool GCS_MAVLINK::requesting_mission_items() const { for (uint8_t i=0; ihandle_mission_write_partial_list(*this, msg, packet); } -void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_write_partial_list_t &packet) -{ - - // start waypoint receiving - if ((unsigned)packet.start_index > item_count() || - (unsigned)packet.end_index > item_count() || - packet.end_index < packet.start_index) { - gcs().send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22 - send_mission_ack(_link, msg, MAV_MISSION_ERROR); - return; - } - - init_send_requests(_link, msg, packet.start_index, packet.end_index); -} - - /* pass mavlink messages to the AP_Mount singleton */ @@ -1031,118 +723,6 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg) prot->handle_mission_item(msg, packet); } -MAV_MISSION_RESULT MissionItemProtocol_Waypoints::replace_item(const mavlink_mission_item_int_t &mission_item_int) -{ - AP_Mission::Mission_Command cmd; - - const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd); - if (res != MAV_MISSION_ACCEPTED) { - return res; - } - - // sanity check for DO_JUMP command - if (cmd.id == MAV_CMD_DO_JUMP) { - if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) { - return MAV_MISSION_ERROR; - } - } - if (!mission.replace_cmd(cmd.index, cmd)) { - return MAV_MISSION_ERROR; - } - return MAV_MISSION_ACCEPTED; -} - -MAV_MISSION_RESULT MissionItemProtocol_Waypoints::append_item(const mavlink_mission_item_int_t &mission_item_int) -{ - // sanity check for DO_JUMP command - AP_Mission::Mission_Command cmd; - - const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd); - if (res != MAV_MISSION_ACCEPTED) { - return res; - } - - if (cmd.id == MAV_CMD_DO_JUMP) { - if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) { - return MAV_MISSION_ERROR; - } - } - - if (!mission.add_cmd(cmd)) { - return MAV_MISSION_ERROR; - } - return MAV_MISSION_ACCEPTED; -} - -void MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link) -{ - _link.send_text(MAV_SEVERITY_INFO, "Flight plan received"); - AP::logger().Write_EntireMission(); -} -void MissionItemProtocol_Waypoints::timeout() -{ - link->send_text(MAV_SEVERITY_WARNING, "Mission upload timeout"); -} - -void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, const mavlink_mission_item_int_t &cmd) -{ - if (link == nullptr) { - AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); - return; - } - - // check if this is the requested waypoint - if (cmd.seq != request_i) { - send_mission_ack(msg, MAV_MISSION_INVALID_SEQUENCE); - return; - } - // make sure the item is coming from the system that initiated the upload - if (msg.sysid != dest_sysid) { - send_mission_ack(msg, MAV_MISSION_DENIED); - return; - } - if (msg.compid != dest_compid) { - send_mission_ack(msg, MAV_MISSION_DENIED); - return; - } - - const uint16_t _item_count = item_count(); - - MAV_MISSION_RESULT result; - if (cmd.seq < _item_count) { - // command index is within the existing list, replace the command - result = replace_item(cmd); - } else if (cmd.seq == _item_count) { - // command is at the end of command list, add the command - result = append_item(cmd); - } else { - // beyond the end of the command list, return an error - result = MAV_MISSION_ERROR; - } - if (result != MAV_MISSION_ACCEPTED) { - send_mission_ack(msg, result); - return; - } - - // update waypoint receiving state machine - timelast_receive_ms = AP_HAL::millis(); - request_i++; - - if (request_i > request_last) { - send_mission_ack(msg, MAV_MISSION_ACCEPTED); - complete(*link); - receiving = false; - link = nullptr; - return; - } - // if we have enough space, then send the next WP request immediately - if (HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST)) { - queued_request_send(); - } else { - link->send_message(next_item_ap_message_id()); - } -} - ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const { // MSG_NEXT_MISSION_REQUEST doesn't correspond to a mavlink message directly. diff --git a/libraries/GCS_MAVLink/GCS_Rally.cpp b/libraries/GCS_MAVLink/GCS_Rally.cpp index 8534793f2d..1ec08f80c8 100644 --- a/libraries/GCS_MAVLink/GCS_Rally.cpp +++ b/libraries/GCS_MAVLink/GCS_Rally.cpp @@ -1,5 +1,7 @@ /* - GCS MAVLink functions related to rally points + GCS MAVLink functions related to upload and download of rally + points with the ArduPilot-specific protocol comprised of + MAVLINK_MSG_ID_RALLY_POINT and MAVLINK_MSG_ID_RALLY_FETCH_POINT. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -101,85 +103,3 @@ void GCS_MAVLINK::handle_common_rally_message(const mavlink_message_t &msg) break; } } - -MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) -{ - RallyLocation rallypoint; - - if (!rally.get_rally_point_with_index(packet.seq, rallypoint)) { - return MAV_MISSION_INVALID_SEQUENCE; - } - - ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; - ret_packet.command = MAV_CMD_NAV_RALLY_POINT; - ret_packet.x = rallypoint.lat; - ret_packet.y = rallypoint.lng; - ret_packet.z = rallypoint.alt; - - return MAV_MISSION_ACCEPTED; -} - -void MissionItemProtocol_Rally::truncate(const mavlink_mission_count_t &packet) -{ - rally.truncate(packet.count); -} -void MissionItemProtocol_Rally::timeout() -{ - link->send_text(MAV_SEVERITY_WARNING, "Rally upload timeout"); -} - -MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret) -{ - if (cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && - cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) { - return MAV_MISSION_UNSUPPORTED_FRAME; - } - if (!check_lat(cmd.x)) { - return MAV_MISSION_INVALID_PARAM5_X; - } - if (!check_lng(cmd.y)) { - return MAV_MISSION_INVALID_PARAM6_Y; - } - if (cmd.z < INT16_MIN || cmd.z > INT16_MAX) { - return MAV_MISSION_INVALID_PARAM7; - } - ret.lat = cmd.x; - ret.lng = cmd.y; - ret.alt = cmd.z; - return MAV_MISSION_ACCEPTED; -} - -MAV_MISSION_RESULT MissionItemProtocol_Rally::replace_item(const mavlink_mission_item_int_t &cmd) -{ - RallyLocation rallyloc; - const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc); - if (ret != MAV_MISSION_ACCEPTED) { - return ret; - } - if (!rally.set_rally_point_with_index(cmd.seq, rallyloc)) { - return MAV_MISSION_ERROR; - } - return MAV_MISSION_ACCEPTED; -} - -MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd) -{ - RallyLocation rallyloc; - const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc); - if (ret != MAV_MISSION_ACCEPTED) { - return ret; - } - if (!rally.append(rallyloc)) { - return MAV_MISSION_ERROR; - } - return MAV_MISSION_ACCEPTED; -} - -void MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link) -{ - _link.send_text(MAV_SEVERITY_INFO, "Rally points received"); - AP::logger().Write_Rally(); -} diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp new file mode 100644 index 0000000000..19037429f5 --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol.cpp @@ -0,0 +1,305 @@ +#include "MissionItemProtocol.h" + +#include "GCS.h" + +void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const int16_t _request_first, + const int16_t _request_last) +{ + // set variables to help handle the expected receiving of commands from the GCS + timelast_receive_ms = AP_HAL::millis(); // set time we last received commands to now + receiving = true; // record that we expect to receive commands + request_i = _request_first; // reset the next expected command number to zero + request_last = _request_last; // record how many commands we expect to receive + + dest_sysid = msg.sysid; // record system id of GCS who wants to upload the mission + dest_compid = msg.compid; // record component id of GCS who wants to upload the mission + + link = &_link; + + timelast_request_ms = AP_HAL::millis(); + link->send_message(next_item_ap_message_id()); +} + +void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link, + const mavlink_message_t &msg) +{ + bool success = true; + success = success && !receiving; + success = success && clear_all_items(); + send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR); +} + +void MissionItemProtocol::handle_mission_count( + GCS_MAVLINK &_link, + const mavlink_mission_count_t &packet, + const mavlink_message_t &msg) +{ + if (receiving) { + // someone is already uploading a mission. If we are + // receiving from someone then we will allow them to restart - + // otherwise we deny. + if (msg.sysid != dest_sysid || msg.compid != dest_compid) { + // reject another upload until + send_mission_ack(_link, msg, MAV_MISSION_DENIED); + return; + } + } + + if (packet.count > max_items()) { + send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE); + return; + } + + truncate(packet); + + if (packet.count == 0) { + // no requests to send... + send_mission_ack(_link, msg, MAV_MISSION_ACCEPTED); + complete(_link); + return; + } + + // start waypoint receiving + init_send_requests(_link, msg, 0, packet.count-1); +} + +void MissionItemProtocol::handle_mission_request_list( + const GCS_MAVLINK &_link, + const mavlink_mission_request_list_t &packet, + const mavlink_message_t &msg) +{ + if (receiving) { + // someone is uploading a mission; reject fetching of points + // until done or timeout + send_mission_ack(_link, msg, MAV_MISSION_DENIED); + return; + } + + // reply with number of commands in the mission. The GCS will + // then request each command separately + mavlink_msg_mission_count_send(_link.get_chan(), + msg.sysid, + msg.compid, + item_count(), + mission_type()); +} + +void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link, + const mavlink_mission_request_int_t &packet, + const mavlink_message_t &msg) +{ + if (receiving) { + // someone is uploading a mission; reject fetching of points + // until done or timeout + send_mission_ack(_link, msg, MAV_MISSION_DENIED); + return; + } + + mavlink_mission_item_int_t ret_packet{}; + + ret_packet.target_system = msg.sysid; + ret_packet.target_component = msg.compid; + ret_packet.seq = packet.seq; + ret_packet.mission_type = packet.mission_type; + + const MAV_MISSION_RESULT result_code = get_item(_link, msg, packet, ret_packet); + + if (result_code != MAV_MISSION_ACCEPTED) { + // send failure message + send_mission_ack(_link, msg, result_code); + return; + } + + // we already have a filled structure, use it in place of _send: + _mav_finalize_message_chan_send(_link.get_chan(), + MAVLINK_MSG_ID_MISSION_ITEM_INT, + (const char *)&ret_packet, + MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, + MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, + MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +} + +void MissionItemProtocol::handle_mission_request(const GCS_MAVLINK &_link, + const mavlink_mission_request_t &packet, + const mavlink_message_t &msg +) +{ + // convert into a MISSION_REQUEST_INT and reuse its handling code + mavlink_mission_request_int_t request_int; + request_int.target_system = packet.target_system; + request_int.target_component = packet.target_component; + request_int.seq = packet.seq; + request_int.mission_type = packet.mission_type; + + mavlink_mission_item_int_t item_int{}; + item_int.target_system = msg.sysid; + item_int.target_component = msg.compid; + + MAV_MISSION_RESULT ret = get_item(_link, msg, request_int, item_int); + if (ret != MAV_MISSION_ACCEPTED) { + return; + } + + mavlink_mission_item_t ret_packet{}; + ret = AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(item_int, ret_packet); + if (ret != MAV_MISSION_ACCEPTED) { + return; + } + + // we already have a filled structure, use it in place of _send: + _mav_finalize_message_chan_send(_link.get_chan(), + MAVLINK_MSG_ID_MISSION_ITEM, + (const char *)&ret_packet, + MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, + MAVLINK_MSG_ID_MISSION_ITEM_LEN, + MAVLINK_MSG_ID_MISSION_ITEM_CRC); +} + +void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_write_partial_list_t &packet) +{ + + // start waypoint receiving + if ((unsigned)packet.start_index > item_count() || + (unsigned)packet.end_index > item_count() || + packet.end_index < packet.start_index) { + gcs().send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22 + send_mission_ack(_link, msg, MAV_MISSION_ERROR); + return; + } + + init_send_requests(_link, msg, packet.start_index, packet.end_index); +} + +void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, const mavlink_mission_item_int_t &cmd) +{ + if (link == nullptr) { + AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); + return; + } + + // check if this is the requested waypoint + if (cmd.seq != request_i) { + send_mission_ack(msg, MAV_MISSION_INVALID_SEQUENCE); + return; + } + // make sure the item is coming from the system that initiated the upload + if (msg.sysid != dest_sysid) { + send_mission_ack(msg, MAV_MISSION_DENIED); + return; + } + if (msg.compid != dest_compid) { + send_mission_ack(msg, MAV_MISSION_DENIED); + return; + } + + const uint16_t _item_count = item_count(); + + MAV_MISSION_RESULT result; + if (cmd.seq < _item_count) { + // command index is within the existing list, replace the command + result = replace_item(cmd); + } else if (cmd.seq == _item_count) { + // command is at the end of command list, add the command + result = append_item(cmd); + } else { + // beyond the end of the command list, return an error + result = MAV_MISSION_ERROR; + } + if (result != MAV_MISSION_ACCEPTED) { + send_mission_ack(msg, result); + return; + } + + // update waypoint receiving state machine + timelast_receive_ms = AP_HAL::millis(); + request_i++; + + if (request_i > request_last) { + send_mission_ack(msg, MAV_MISSION_ACCEPTED); + complete(*link); + receiving = false; + link = nullptr; + return; + } + // if we have enough space, then send the next WP request immediately + if (HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST)) { + queued_request_send(); + } else { + link->send_message(next_item_ap_message_id()); + } +} + +void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg, + MAV_MISSION_RESULT result) const +{ + if (link == nullptr) { + AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); + return; + } + send_mission_ack(*link, msg, result); +} +void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + MAV_MISSION_RESULT result) const +{ + mavlink_msg_mission_ack_send(_link.get_chan(), + msg.sysid, + msg.compid, + result, + mission_type()); +} + +/** + * @brief Send the next pending waypoint, called from deferred message + * handling code + */ +void MissionItemProtocol::queued_request_send() +{ + if (!receiving) { + return; + } + if (request_i > request_last) { + return; + } + if (link == nullptr) { + AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); + return; + } + mavlink_msg_mission_request_send( + link->get_chan(), + dest_sysid, + dest_compid, + request_i, + mission_type()); + timelast_request_ms = AP_HAL::millis(); +} + +void MissionItemProtocol::update() +{ + if (!receiving) { + // we don't need to do anything unless we're sending requests + return; + } + if (link == nullptr) { + AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link); + return; + } + // stop waypoint receiving if timeout + const uint32_t tnow = AP_HAL::millis(); + if (tnow - timelast_receive_ms > upload_timeout_ms) { + receiving = false; + timeout(); + link = nullptr; + return; + } + // resend request if we haven't gotten one: + const uint32_t wp_recv_timeout_ms = 1000U + (link->get_stream_slowdown_ms()*20); + if (tnow - timelast_request_ms > wp_recv_timeout_ms) { + timelast_request_ms = tnow; + link->send_message(next_item_ap_message_id()); + } +} diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.h b/libraries/GCS_MAVLink/MissionItemProtocol.h new file mode 100644 index 0000000000..13a953b672 --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol.h @@ -0,0 +1,111 @@ +#pragma once + +#include "GCS_MAVLink.h" + +#include "ap_message.h" + +#include + +// MissionItemProtocol objects are used for transfering missions from +// a GCS to ArduPilot and vice-versa. +// +// There exists one MissionItemProtocol instance for each of the types +// of item that might be transfered - e.g. MissionItemProtocol_Rally +// for rally point uploads. These objects are static in GCS_MAVLINK +// and used by all of the backends. +// +// While prompting the GCS for items required to complete the mission, +// a link is stored to the link the MissionItemProtocol should send +// requests out on, and the "receiving" boolean is true. In this +// state downloading of items (and the item count!) is blocked. +// Starting of uploads (for the same protocol) is also blocked - +// essentially the GCS uploading a set of items (e.g. a mission) has a +// mutex over the mission. +class MissionItemProtocol +{ +public: + + // note that all of these methods are named after the packet they + // are handling; the "mission" part just comes as part of that. + void handle_mission_request_list(const class GCS_MAVLINK &link, + const mavlink_mission_request_list_t &packet, + const mavlink_message_t &msg); + void handle_mission_request_int(const GCS_MAVLINK &link, + const mavlink_mission_request_int_t &packet, + const mavlink_message_t &msg); + void handle_mission_request(const GCS_MAVLINK &link, + const mavlink_mission_request_t &packet, + const mavlink_message_t &msg); + + void handle_mission_count(class GCS_MAVLINK &link, + const mavlink_mission_count_t &packet, + const mavlink_message_t &msg); + void handle_mission_write_partial_list(GCS_MAVLINK &link, + const mavlink_message_t &msg, + const mavlink_mission_write_partial_list_t &packet); + + // called on receipt of a MISSION_ITEM or MISSION_ITEM_INT packet; + // the former is converted to the latter. + void handle_mission_item(const mavlink_message_t &msg, + const mavlink_mission_item_int_t &cmd); + + void handle_mission_clear_all(const GCS_MAVLINK &link, + const mavlink_message_t &msg); + + void queued_request_send(); + void update(); + + bool active_link_is(const GCS_MAVLINK *_link) const { return _link == link; }; + + virtual MAV_MISSION_TYPE mission_type() const = 0; + + bool receiving; // currently sending requests and expecting items + +protected: + + GCS_MAVLINK *link; // link currently receiving waypoints on + + // return the ap_message which can be queued to be sent to send a + // item request to the GCS: + virtual ap_message next_item_ap_message_id() const = 0; + + virtual bool clear_all_items() = 0; + + uint16_t request_last; // last request index + +private: + + virtual void truncate(const mavlink_mission_count_t &packet) = 0; + + uint16_t request_i; // request index + + // waypoints + uint8_t dest_sysid; // where to send requests + uint8_t dest_compid; // " + uint32_t timelast_receive_ms; + uint32_t timelast_request_ms; + const uint16_t upload_timeout_ms = 8000; + + // support for GCS getting waypoints etc from us: + virtual MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_request_int_t &packet, + mavlink_mission_item_int_t &ret_packet) = 0; + + void init_send_requests(GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const int16_t _request_first, + const int16_t _request_last); + + void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const; + void send_mission_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const; + + virtual uint16_t item_count() const = 0; + virtual uint16_t max_items() const = 0; + + virtual MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &mission_item_int) = 0; + virtual MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &mission_item_int) = 0; + + virtual void complete(const GCS_MAVLINK &_link) {}; + virtual void timeout() {}; +}; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp new file mode 100644 index 0000000000..5fad95eca4 --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp @@ -0,0 +1,121 @@ +/* + Implementation details for transfering rally point information using + the MISSION_ITEM protocol to and from a GCS. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include "MissionItemProtocol_Rally.h" + +#include +#include +#include + + +MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd) +{ + RallyLocation rallyloc; + const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc); + if (ret != MAV_MISSION_ACCEPTED) { + return ret; + } + if (!rally.append(rallyloc)) { + return MAV_MISSION_ERROR; + } + return MAV_MISSION_ACCEPTED; +} + +void MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link) +{ + _link.send_text(MAV_SEVERITY_INFO, "Rally points received"); + AP::logger().Write_Rally(); +} + +bool MissionItemProtocol_Rally::clear_all_items() +{ + rally.truncate(0); + return true; +} + +MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret) +{ + if (cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && + cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) { + return MAV_MISSION_UNSUPPORTED_FRAME; + } + if (!check_lat(cmd.x)) { + return MAV_MISSION_INVALID_PARAM5_X; + } + if (!check_lng(cmd.y)) { + return MAV_MISSION_INVALID_PARAM6_Y; + } + if (cmd.z < INT16_MIN || cmd.z > INT16_MAX) { + return MAV_MISSION_INVALID_PARAM7; + } + ret.lat = cmd.x; + ret.lng = cmd.y; + ret.alt = cmd.z; + return MAV_MISSION_ACCEPTED; +} + +MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_request_int_t &packet, + mavlink_mission_item_int_t &ret_packet) +{ + RallyLocation rallypoint; + + if (!rally.get_rally_point_with_index(packet.seq, rallypoint)) { + return MAV_MISSION_INVALID_SEQUENCE; + } + + ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + ret_packet.command = MAV_CMD_NAV_RALLY_POINT; + ret_packet.x = rallypoint.lat; + ret_packet.y = rallypoint.lng; + ret_packet.z = rallypoint.alt; + + return MAV_MISSION_ACCEPTED; +} + +uint16_t MissionItemProtocol_Rally::item_count() const { + return rally.get_rally_total(); +} + +uint16_t MissionItemProtocol_Rally::max_items() const { + return rally.get_rally_max(); +} + +MAV_MISSION_RESULT MissionItemProtocol_Rally::replace_item(const mavlink_mission_item_int_t &cmd) +{ + RallyLocation rallyloc; + const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc); + if (ret != MAV_MISSION_ACCEPTED) { + return ret; + } + if (!rally.set_rally_point_with_index(cmd.seq, rallyloc)) { + return MAV_MISSION_ERROR; + } + return MAV_MISSION_ACCEPTED; +} + +void MissionItemProtocol_Rally::timeout() +{ + link->send_text(MAV_SEVERITY_WARNING, "Rally upload timeout"); +} + +void MissionItemProtocol_Rally::truncate(const mavlink_mission_count_t &packet) +{ + rally.truncate(packet.count); +} diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h new file mode 100644 index 0000000000..f1d5de514b --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h @@ -0,0 +1,38 @@ +#pragma once + +#include "MissionItemProtocol.h" + +class MissionItemProtocol_Rally : public MissionItemProtocol { +public: + MissionItemProtocol_Rally(class AP_Rally &_rally) : + rally(_rally) {} + void truncate(const mavlink_mission_count_t &packet) override; + MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_RALLY; } + + void complete(const GCS_MAVLINK &_link) override; + void timeout() override; + +protected: + + ap_message next_item_ap_message_id() const override { + return MSG_NEXT_MISSION_REQUEST_RALLY; + } + bool clear_all_items() override WARN_IF_UNUSED; + +private: + AP_Rally &rally; + + uint16_t item_count() const override; + uint16_t max_items() const override; + + MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; + MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; + + MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_request_int_t &packet, + mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; + + static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, class RallyLocation &ret) WARN_IF_UNUSED; + +}; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp new file mode 100644 index 0000000000..72999d202b --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp @@ -0,0 +1,139 @@ +/* + Implementation details for transfering waypoint information using + the MISSION_ITEM protocol to and from a GCS. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include "MissionItemProtocol_Waypoints.h" + +#include +#include + +#include "GCS.h" + +MAV_MISSION_RESULT MissionItemProtocol_Waypoints::append_item(const mavlink_mission_item_int_t &mission_item_int) +{ + // sanity check for DO_JUMP command + AP_Mission::Mission_Command cmd; + + const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd); + if (res != MAV_MISSION_ACCEPTED) { + return res; + } + + if (cmd.id == MAV_CMD_DO_JUMP) { + if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) { + return MAV_MISSION_ERROR; + } + } + + if (!mission.add_cmd(cmd)) { + return MAV_MISSION_ERROR; + } + return MAV_MISSION_ACCEPTED; +} + +bool MissionItemProtocol_Waypoints::clear_all_items() +{ + return mission.clear(); +} + +void MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link) +{ + _link.send_text(MAV_SEVERITY_INFO, "Flight plan received"); + AP::logger().Write_EntireMission(); +} + +MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_request_int_t &packet, + mavlink_mission_item_int_t &ret_packet) +{ + if (packet.seq != 0 && // always allow HOME to be read + packet.seq >= mission.num_commands()) { + // try to educate the GCS on the actual size of the mission: + mavlink_msg_mission_count_send(_link.get_chan(), + msg.sysid, + msg.compid, + mission.num_commands(), + MAV_MISSION_TYPE_MISSION); + return MAV_MISSION_ERROR; + } + + AP_Mission::Mission_Command cmd; + + // retrieve mission from eeprom + if (!mission.read_cmd_from_storage(packet.seq, cmd)) { + return MAV_MISSION_ERROR; + } + + if (!AP_Mission::mission_cmd_to_mavlink_int(cmd, ret_packet)) { + return MAV_MISSION_ERROR; + } + + // set packet's current field to 1 if this is the command being executed + if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) { + ret_packet.current = 1; + } else { + ret_packet.current = 0; + } + + // set auto continue to 1 + ret_packet.autocontinue = 1; // 1 (true), 0 (false) + + ret_packet.command = cmd.id; + + return MAV_MISSION_ACCEPTED; +} + +uint16_t MissionItemProtocol_Waypoints::item_count() const { + return mission.num_commands(); +} + +uint16_t MissionItemProtocol_Waypoints::max_items() const { + return mission.num_commands_max(); +} + +MAV_MISSION_RESULT MissionItemProtocol_Waypoints::replace_item(const mavlink_mission_item_int_t &mission_item_int) +{ + AP_Mission::Mission_Command cmd; + + const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd); + if (res != MAV_MISSION_ACCEPTED) { + return res; + } + + // sanity check for DO_JUMP command + if (cmd.id == MAV_CMD_DO_JUMP) { + if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) { + return MAV_MISSION_ERROR; + } + } + if (!mission.replace_cmd(cmd.index, cmd)) { + return MAV_MISSION_ERROR; + } + return MAV_MISSION_ACCEPTED; +} + +void MissionItemProtocol_Waypoints::timeout() +{ + link->send_text(MAV_SEVERITY_WARNING, "Mission upload timeout"); +} + +void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &packet) +{ + // new mission arriving, truncate mission to be the same length + mission.truncate(packet.count); +} diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h new file mode 100644 index 0000000000..f30c9cfcdf --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h @@ -0,0 +1,68 @@ +#pragma once + +#include "MissionItemProtocol.h" + +class MissionItemProtocol_Waypoints : public MissionItemProtocol { +public: + MissionItemProtocol_Waypoints(class AP_Mission &_mission) : + mission(_mission) {} + + // mission_type returns the MAV_MISSION mavlink enumeration value + // which this module is responsible for handling + MAV_MISSION_TYPE mission_type() const override { + return MAV_MISSION_TYPE_MISSION; + } + + // complete() is called by the base class after all waypoints have + // been received. _link is the link which the last item was + // transfered on. + void complete(const GCS_MAVLINK &_link) override; + // timeout() is called by the base class in the case that the GCS + // does not transfer all waypoints to the vehicle. + void timeout() override; + // truncate() is called to set the absolute number of items. It + // must be less than or equal to the current number of items (you + // can't truncate-to a longer list) + void truncate(const mavlink_mission_count_t &packet) override; + +protected: + + // clear_all_items() is called to clear all items on the vehicle + bool clear_all_items() override WARN_IF_UNUSED; + + // next_item_ap_message_id returns an item from the ap_message + // enumeration which (when acted upon by the GCS class) will send + // a mavlink message to the GCS requesting it upload the next + // required waypoint. + ap_message next_item_ap_message_id() const override { + return MSG_NEXT_MISSION_REQUEST_WAYPOINTS; + } + +private: + AP_Mission &mission; + + // append_item() is called by the base class to add the supplied + // item to the end of the list of stored items. + MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED; + + // get_item() fills in ret_packet based on packet; _link is the + // link the request was received on, and msg is the undecoded + // request. Note that msg may not actually decode to a + // request_int_t! + MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_request_int_t &packet, + mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; + + // item_count() returns the number of stored items + uint16_t item_count() const override; + + // item_count() returns the maximum number of items which could be + // stored on-board + uint16_t max_items() const override; + + // replace_item() replaces an item in the stored list + MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED; + +}; + diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h new file mode 100644 index 0000000000..c62424af4f --- /dev/null +++ b/libraries/GCS_MAVLink/ap_message.h @@ -0,0 +1,75 @@ +// GCS Message ID's +/// NOTE: to ensure we never block on sending MAVLink messages +/// please keep each MSG_ to a single MAVLink message. If need be +/// create new MSG_ IDs for additional messages on the same +/// stream + +#pragma once + +enum ap_message : uint8_t { + MSG_HEARTBEAT, + MSG_ATTITUDE, + MSG_LOCATION, + MSG_SYS_STATUS, + MSG_POWER_STATUS, + MSG_MEMINFO, + MSG_NAV_CONTROLLER_OUTPUT, + MSG_CURRENT_WAYPOINT, + MSG_VFR_HUD, + MSG_SERVO_OUTPUT_RAW, + MSG_RC_CHANNELS, + MSG_RC_CHANNELS_RAW, + MSG_RAW_IMU, + MSG_SCALED_IMU, + MSG_SCALED_IMU2, + MSG_SCALED_IMU3, + MSG_SCALED_PRESSURE, + MSG_SCALED_PRESSURE2, + MSG_SCALED_PRESSURE3, + MSG_SENSOR_OFFSETS, + MSG_GPS_RAW, + MSG_GPS_RTK, + MSG_GPS2_RAW, + MSG_GPS2_RTK, + MSG_SYSTEM_TIME, + MSG_SERVO_OUT, + MSG_NEXT_MISSION_REQUEST_WAYPOINTS, + MSG_NEXT_MISSION_REQUEST_RALLY, + MSG_NEXT_PARAM, + MSG_FENCE_STATUS, + MSG_AHRS, + MSG_SIMSTATE, + MSG_AHRS2, + MSG_AHRS3, + MSG_HWSTATUS, + MSG_WIND, + MSG_RANGEFINDER, + MSG_DISTANCE_SENSOR, + MSG_TERRAIN, + MSG_BATTERY2, + MSG_CAMERA_FEEDBACK, + MSG_MOUNT_STATUS, + MSG_OPTICAL_FLOW, + MSG_GIMBAL_REPORT, + MSG_MAG_CAL_PROGRESS, + MSG_MAG_CAL_REPORT, + MSG_EKF_STATUS_REPORT, + MSG_LOCAL_POSITION, + MSG_PID_TUNING, + MSG_VIBRATION, + MSG_RPM, + MSG_WHEEL_DISTANCE, + MSG_MISSION_ITEM_REACHED, + MSG_POSITION_TARGET_GLOBAL_INT, + MSG_POSITION_TARGET_LOCAL_NED, + MSG_ADSB_VEHICLE, + MSG_BATTERY_STATUS, + MSG_AOA_SSA, + MSG_LANDING, + MSG_ESC_TELEMETRY, + MSG_ORIGIN, + MSG_HOME, + MSG_NAMED_FLOAT, + MSG_EXTENDED_SYS_STATE, + MSG_LAST // MSG_LAST must be the last entry in this enum +};