Implement new mission opaque ID protocol

AntennaTracker/GCS_Mavlink.cpp

@@ -597,7 +597,9 @@ void GCS_MAVLINK_Tracker::handle_message_mission_item(const mavlink_message_t &m
             msg.sysid,
             msg.compid,
             result,
-            MAV_MISSION_TYPE_MISSION);
+            MAV_MISSION_TYPE_MISSION,
+            0
+            );
 }
 #endif
 

Tools/autotest/ArduPlane_Tests/MissionOpaqueID/empty.txt

@@ -0,0 +1 @@
+QGC WPL 110

Tools/autotest/ArduPlane_Tests/MissionOpaqueID/flaps.txt

@@ -0,0 +1,13 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.390015	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361279	149.164230	30.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361229	149.163025	80.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364563	149.163773	80.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364384	149.164795	80.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361027	149.164093	80.000000	1
+6	0	0	177	2.000000	3.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+7	0	3	189	0.000000	0.000000	0.000000	0.000000	-35.362915	149.162613	60.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.363136	149.162750	60.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365467	149.164215	55.000000	1
+10	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365009	149.165482	39.889999	1
+11	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.363041	149.165222	0.000000	1

Tools/autotest/ArduPlane_Tests/MissionOpaqueID/jumpcount.txt

@@ -0,0 +1,9 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165238	584.090027	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.359833	149.164703	41.029999	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359585	149.161392	100.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366463	149.162231	100.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366131	149.164581	100.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359272	149.163757	100.000000	1
+6	0	0	177	2.000000	2.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+7	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359272	149.163757	100.000000	1

Tools/autotest/ArduPlane_Tests/MissionOpaqueID/justhome.txt

@@ -0,0 +1,2 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.362938	149.165085	650.000000	1

Tools/autotest/arduplane.py

@@ -4120,6 +4120,48 @@ def attempt_fence_breached_disable(start_mode, end_mode, expected_mode, action):
         attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=6)
         attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=7)
 
+    def get_mission_checksum(self):
+        self.drain_mav()
+        m = self.poll_message('MISSION_CURRENT')
+        print("Got %s" % str(m))
+        return m.mission_id
+
+    def MissionOpaqueID(self):
+        '''test support for vehicle identifying mission via opaque ID'''
+        jumpcount_checksum = 2006072681
+        for missionstuff in [("text.txt", 4264322242),
+                             ("empty.txt", 4294967295),
+                             ("justhome.txt", 4294967295),
+                             ("jumpcount.txt", jumpcount_checksum),
+                             ]:
+            (filename, expected_checksum) = missionstuff
+            self.progress("Checking checksum for (%s)" % (filename,))
+            self.load_mission(filename)
+            self.assert_cached_message_field_values('MISSION_ACK', {
+                "opaque_id": expected_checksum,
+            })
+            # ensure it appears in the MISSION_CURRENT message:
+            self.assert_received_message_field_values('MISSION_CURRENT', {
+                "mission_id": expected_checksum,
+            }, poll=True)
+
+        # fly the jump count mission, make sure the checksum doesn't
+        # change over time
+        self.load_mission("jumpcount.txt", strict=False)
+        self.set_current_waypoint(0, check_afterwards=False)
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        for i in range(2):
+            self.wait_current_waypoint(2, timeout=120)
+            self.wait_current_waypoint(3, timeout=120)
+            mission_checksum = self.get_mission_checksum()
+            if mission_checksum != jumpcount_checksum:
+                raise NotAchievedException(
+                    "Mission checksum changed during mission; got=%u want=%u i=%u" %
+                    (jumpcount_checksum, mission_checksum, i))
+        self.fly_home_land_and_disarm()
+
     def _MAV_CMD_DO_AUX_FUNCTION(self, run_cmd):
         '''Test triggering Auxiliary Functions via mavlink'''
         self.context_collect('STATUSTEXT')
@@ -6396,6 +6438,7 @@ def tests1b(self):
             self.AutotuneFiltering,
             self.MegaSquirt,
             self.Hirth,
+            self.MissionOpaqueID,
             self.MSP_DJI,
             self.SpeedToFly,
             self.GlideSlopeThresh,

Tools/autotest/vehicle_test_suite.py

@@ -4396,7 +4396,7 @@
     def assert_message_field_values(self, m, fieldvalues, verbose=True, epsilon=None):
         if self.message_has_field_values(m, fieldvalues, verbose=verbose, epsilon=epsilon):
             return
         raise NotAchievedException("Did not get expected field values")
 
     def assert_cached_message_field_values(self, message, fieldvalues, verbose=True, very_verbose=False, epsilon=None):
         '''checks the most-recently received instance of message to ensure it
@@ -5507,8 +5507,20 @@
         self.progress("check %s upload/download: upload %u items" %
                       (itype, len(items),))
         self.upload_using_mission_protocol(mission_type, items)
+        upload_opaque_id = self.get_cached_message('MISSION_ACK').opaque_id
+        if upload_opaque_id == 0:
+            raise NotAchievedException("Expected non-zero upload opaque_id")
+        self.progress("Upload opaque_id=%s" % upload_opaque_id)
         self.progress("check %s upload/download: download items" % itype)
         downloaded_items = self.download_using_mission_protocol(mission_type)
+        download_opaque_id = self.get_cached_message('MISSION_COUNT').opaque_id
+        if download_opaque_id == 0:
+            raise NotAchievedException("Expected non-zero download opaque_id")
+        if upload_opaque_id != download_opaque_id:
+            raise NotAchievedException(
+                "Expected upload id and download id to be same; upload=%s download=%s" %
+                (upload_opaque_id, download_opaque_id))
+
         if len(items) != len(downloaded_items):
             raise NotAchievedException("Did not download same number of items as uploaded want=%u got=%u" %
                                        (len(items), len(downloaded_items)))

libraries/GCS_MAVLink/GCS.h

@@ -237,7 +237,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
@@ -332,6 +333,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

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -616,7 +616,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;
     }
 
@@ -675,6 +676,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();
@@ -694,7 +710,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
@@ -750,7 +770,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;
     }
 

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 && cancel_upload(_link, msg);
-    success = success && clear_all_items();
-    send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
+    if (!cancel_upload(_link, msg)) {
+        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
@@ -105,6 +113,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;
     }
@@ -132,11 +143,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,
@@ -325,8 +340,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;
 }
@@ -343,13 +373,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);
 }
 
 /**
@@ -388,18 +428,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();
@@ -411,6 +455,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

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:
 
     // returns true if we are either not receiving, or we successfully
@@ -121,6 +129,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;
@@ -144,4 +156,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; }
 };