diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 9e5e7745a49b6a..195cc37cf0a4e0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -236,7 +236,8 @@ class GCS_MAVLINK msg.sysid, msg.compid, result, - mission_type); + mission_type, + opaque_id_for_mission_type(mission_type)); } // packetReceived is called on any successful decode of a mavlink message @@ -328,6 +329,7 @@ class GCS_MAVLINK // mission is up to: virtual MISSION_STATE mission_state(const class AP_Mission &mission) const; // send a mission_current message for the supplied waypoint + uint32_t opaque_id_for_mission_type(MAV_MISSION_TYPE type) const; void send_mission_current(const class AP_Mission &mission, uint16_t seq); // common send functions diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 188f8918ac0d09..ad3f7982f08f0a 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -601,7 +601,8 @@ void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t &msg) msg.sysid, msg.compid, MAV_MISSION_UNSUPPORTED, - packet.mission_type); + packet.mission_type, + 0); return; } @@ -660,6 +661,21 @@ MISSION_STATE GCS_MAVLINK::mission_state(const AP_Mission &mission) const return MISSION_STATE_UNKNOWN; } +// returns the opaque ID used for identifying a mission (or fence, or +// rally) on the vehicle. Returns 0 on failure. +uint32_t GCS_MAVLINK::opaque_id_for_mission_type(MAV_MISSION_TYPE type) const +{ + MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type); + if (prot == nullptr) { + return 0; + } + + uint32_t opaque_id = 0; + UNUSED_RESULT(prot->opaque_id(opaque_id)); + + return opaque_id; +} + void GCS_MAVLINK::send_mission_current(const class AP_Mission &mission, uint16_t seq) { auto num_commands = mission.num_commands(); @@ -679,7 +695,11 @@ void GCS_MAVLINK::send_mission_current(const class AP_Mission &mission, uint16_t seq, num_commands, // total mission_state(mission), // mission_state - mission_mode); // mission_mode + mission_mode, // mission_mode + opaque_id_for_mission_type(MAV_MISSION_TYPE_MISSION), + 0, // fence_id, 0 meaning unsupported + 0 // rally_id, 0 meaning unsupported + ); } #if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED @@ -735,7 +755,8 @@ void GCS_MAVLINK::handle_mission_count(const mavlink_message_t &msg) msg.sysid, msg.compid, MAV_MISSION_UNSUPPORTED, - packet.mission_type); + packet.mission_type, + 0); return; } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp index bf42339d124a64..389b70baa7c136 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol.cpp @@ -29,13 +29,21 @@ void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link, mission_request_warning_sent = false; } -void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link, +void MissionItemProtocol::handle_mission_clear_all(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); + if (receiving) { + send_mission_ack(_link, msg, MAV_MISSION_ERROR); + return; + } + if (!clear_all_items()) { + send_mission_ack(_link, msg, MAV_MISSION_ERROR); + return; + } + link = &_link; + receiving = true; + timelast_receive_ms = AP_HAL::millis(); + transfer_is_complete(_link, msg); } bool MissionItemProtocol::mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const @@ -94,6 +102,9 @@ void MissionItemProtocol::handle_mission_count( if (packet.count == 0) { // no requests to send... + link = &_link; + receiving = true; + timelast_receive_ms = AP_HAL::millis(); transfer_is_complete(_link, msg); return; } @@ -121,11 +132,15 @@ void MissionItemProtocol::handle_mission_request_list( // reply with number of commands in the mission. The GCS will // then request each command separately CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_COUNT); + uint32_t _opaque_id = 0; + UNUSED_RESULT(opaque_id(_opaque_id)); mavlink_msg_mission_count_send(_link.get_chan(), msg.sysid, msg.compid, item_count(), - mission_type()); + mission_type(), + _opaque_id + ); } void MissionItemProtocol::handle_mission_request_int(GCS_MAVLINK &_link, @@ -314,8 +329,23 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons void MissionItemProtocol::transfer_is_complete(const GCS_MAVLINK &_link, const mavlink_message_t &msg) { const MAV_MISSION_RESULT result = complete(_link); - send_mission_ack(_link, msg, result); free_upload_resources(); + uint32_t _opaque_id; + if (!receiving) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + if (supports_opaque_id() && !opaque_id(_opaque_id)) { + // the opaque ID can't currently be calculated; we definitely + // want to have it in the mission ack to avoid race + // conditions. Defer sending the mission ack until it is + // available: + deferred_mission_ack.link = &_link; + deferred_mission_ack.sysid = msg.sysid; + deferred_mission_ack.compid = msg.compid; + deferred_mission_ack.opaque_id = 0; + return; + } + send_mission_ack(_link, msg, result); receiving = false; link = nullptr; } @@ -332,13 +362,23 @@ void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg, void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const +{ + send_mission_ack(_link, msg.sysid, msg.compid, result); +} +void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link, + uint8_t sysid, + uint8_t compid, + MAV_MISSION_RESULT result) const { CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ACK); + uint32_t _opaque_id = 0; + UNUSED_RESULT(opaque_id(_opaque_id)); mavlink_msg_mission_ack_send(_link.get_chan(), - msg.sysid, - msg.compid, + sysid, + compid, result, - mission_type()); + mission_type(), + _opaque_id); } /** @@ -377,18 +417,22 @@ void MissionItemProtocol::update() INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link); return; } + + const mavlink_channel_t chan = link->get_chan(); // stop waypoint receiving if timeout const uint32_t tnow = AP_HAL::millis(); if (tnow - timelast_receive_ms > upload_timeout_ms) { receiving = false; timeout(); - const mavlink_channel_t chan = link->get_chan(); if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK)) { + uint32_t _opaque_id = 0; + UNUSED_RESULT(opaque_id(_opaque_id)); mavlink_msg_mission_ack_send(chan, dest_sysid, dest_compid, MAV_MISSION_OPERATION_CANCELLED, - mission_type()); + mission_type(), + _opaque_id); } link = nullptr; free_upload_resources(); @@ -400,6 +444,20 @@ void MissionItemProtocol::update() timelast_request_ms = tnow; link->send_message(next_item_ap_message_id()); } + + // send any deferred transfer acceptance (to allow for + // asynchronous opaque-id calculation) + if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK) && + deferred_mission_ack.link != nullptr && + deferred_mission_ack.opaque_id != 0) { + send_mission_ack(*deferred_mission_ack.link, + deferred_mission_ack.sysid, + deferred_mission_ack.compid, + MAV_MISSION_ACCEPTED); + deferred_mission_ack.link = nullptr; + receiving = false; + link = nullptr; + } } #endif // HAL_GCS_ENABLED diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.h b/libraries/GCS_MAVLink/MissionItemProtocol.h index a7df605a428ec9..1be9374c6a8663 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol.h @@ -49,11 +49,11 @@ class MissionItemProtocol 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, + void handle_mission_clear_all(GCS_MAVLINK &link, const mavlink_message_t &msg); void queued_request_send(); - void update(); + virtual void update(); bool active_link_is(const GCS_MAVLINK *_link) const { return _link == link; }; @@ -64,6 +64,7 @@ class MissionItemProtocol // a method for GCS_MAVLINK to send warnings about received // MISSION_ITEM messages (we should be getting MISSION_ITEM_INT) void send_mission_item_warning(); + virtual bool opaque_id(uint32_t &checksum) const { return false; } protected: @@ -77,6 +78,13 @@ class MissionItemProtocol uint16_t request_last; // last request index + struct { + const GCS_MAVLINK *link; + uint8_t sysid; + uint8_t compid; + uint32_t opaque_id; // subclass fills this in when it is ready + } deferred_mission_ack; + private: virtual void truncate(const mavlink_mission_count_t &packet) = 0; @@ -117,6 +125,10 @@ class MissionItemProtocol 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; + void send_mission_ack(const GCS_MAVLINK &_link, + uint8_t sysid, + uint8_t compid, + MAV_MISSION_RESULT result) const; virtual uint16_t item_count() const = 0; virtual uint16_t max_items() const = 0; @@ -140,4 +152,6 @@ class MissionItemProtocol virtual void timeout() {}; bool mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const; + + virtual bool supports_opaque_id() const { return false; } }; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp index 3605dc152b85f1..b9f36c05ec49db 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp @@ -61,6 +61,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_l #if HAL_LOGGING_ENABLED AP::logger().Write_EntireMission(); #endif + invalidate_checksum(); return MAV_MISSION_ACCEPTED; } @@ -74,11 +75,14 @@ MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_l // try to educate the GCS on the actual size of the mission: const mavlink_channel_t chan = _link.get_chan(); if (HAVE_PAYLOAD_SPACE(chan, MISSION_COUNT)) { + uint32_t _opaque_id = 0; + IGNORE_RETURN(opaque_id(_opaque_id)); mavlink_msg_mission_count_send(chan, msg.sysid, msg.compid, mission.num_commands(), - MAV_MISSION_TYPE_MISSION); + MAV_MISSION_TYPE_MISSION, + _opaque_id); } return MAV_MISSION_ERROR; } @@ -147,4 +151,128 @@ void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &pack mission.truncate(packet.count); } +// returns a unique ID for this mission +bool MissionItemProtocol_Waypoints::opaque_id(uint32_t &checksum) const +{ + WITH_SEMAPHORE(mission.get_semaphore()); + switch (checksum_state.state) { + case ChecksumState::READY: + if (mission.last_change_time_ms() != checksum_state.mission_change_time_ms) { + return false; + } + checksum = checksum_state.checksum; + // can't use zero as the field is an extension field in mavlink2: + if (checksum == 0) { + checksum = UINT32_MAX; + } + return true; + case ChecksumState::CALCULATING: + case ChecksumState::ERROR: + return false; + } + return false; +} + +void MissionItemProtocol_Waypoints::invalidate_checksum() +{ + WITH_SEMAPHORE(mission.get_semaphore()); + + checksum_state.state = ChecksumState::CALCULATING; + checksum_state.checksum = 0; + checksum_state.current_waypoint = 1; + checksum_state.count = mission.num_commands(); + checksum_state.mission_change_time_ms = mission.last_change_time_ms(); +} + +void MissionItemProtocol_Waypoints::update_checksum() +{ + // update the checksum if required: + + WITH_SEMAPHORE(mission.get_semaphore()); + + const uint32_t mission_last_change_time_ms = mission.last_change_time_ms(); + + if (mission_last_change_time_ms == checksum_state.last_calculate_time_ms) { + return; + } + + // decide whether we need to start calculating the checksum from + // the start; we may be partially through the calculation and need + // to start again + bool do_initialisation = false; + switch (checksum_state.state) { + case ChecksumState::READY: + do_initialisation = true; + break; + case ChecksumState::CALCULATING: + if (checksum_state.mission_change_time_ms != mission_last_change_time_ms) { + // mission changed part-way through our calculations + do_initialisation = true; + } + break; + case ChecksumState::ERROR: + do_initialisation = true; + break; + } + + if (do_initialisation) { + invalidate_checksum(); + } + + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - mission.last_change_time_ms() < 500) { + // don't start to calculate unless the mission's been + // unchanged for a while. + return; + } + + // AP: Took 2.178000ms to checksum 373 points (5.839142ms/1000 points + for (uint16_t count = 0; + count<16 && checksum_state.current_waypoint