AP_Mission: add Jump_TAG support

libraries/AP_Mission/AP_Mission.cpp

@@ -280,6 +280,12 @@ void AP_Mission::update()
 
     update_exit_position();
 
+    // mission_change events
+    if (_last_change_time_prev_ms != _last_change_time_ms) {
+        _last_change_time_prev_ms = _last_change_time_ms;
+        on_mission_timestamp_change();
+    }
+
     // save persistent waypoint_num for watchdog restore
     hal.util->persistent_data.waypoint_num = _nav_cmd.index;
 
@@ -317,6 +323,12 @@ void AP_Mission::update()
     }
 }
 
+// handle events for when the mission has been updated (but maybe not changed)
+void AP_Mission::on_mission_timestamp_change()
+{
+    _jump_tag.age = 0;
+}
+
 bool AP_Mission::verify_command(const Mission_Command& cmd)
 {
     switch (cmd.id) {
@@ -337,6 +349,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
     case MAV_CMD_DO_AUX_FUNCTION:
     case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
     case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+    case MAV_CMD_JUMP_TAG:
         return true;
     default:
         return _cmd_verify_fn(cmd);
@@ -382,6 +395,10 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
         return command_do_set_repeat_dist(cmd);
     case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
         return start_command_do_gimbal_manager_pitchyaw(cmd);
+    case MAV_CMD_JUMP_TAG:
+        _jump_tag.tag = cmd.content.jump.target;
+        _jump_tag.age = 1;
+        FALLTHROUGH; // fall through in case the vehicle handles tag events
     default:
         return _cmd_start_fn(cmd);
     }
@@ -1044,10 +1061,15 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
         break;
 
     case MAV_CMD_DO_JUMP:                               // MAV ID: 177
-        cmd.content.jump.target = packet.param1;        // jump-to command number
+    case MAV_CMD_DO_JUMP_TAG:                           // MAV ID: 601
+        cmd.content.jump.target = packet.param1;        // jump-to command/tag number
         cmd.content.jump.num_times = packet.param2;     // repeat count
         break;
 
+    case MAV_CMD_JUMP_TAG:                              // MAV ID: 600
+        cmd.content.jump.target = packet.param1;        // jump-to tag number
+        break;
+
     case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
         cmd.content.speed.speed_type = packet.param1;   // 0 = airspeed, 1 = ground speed
         cmd.content.speed.target_ms = packet.param2;    // target speed in m/s
@@ -1528,10 +1550,15 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
         break;
 
     case MAV_CMD_DO_JUMP:                               // MAV ID: 177
-        packet.param1 = cmd.content.jump.target;        // jump-to command number
+    case MAV_CMD_DO_JUMP_TAG:                           // MAV ID: 601
+        packet.param1 = cmd.content.jump.target;        // jump-to command/tag number
         packet.param2 = cmd.content.jump.num_times;     // repeat count
         break;
 
+    case MAV_CMD_JUMP_TAG:                              // MAV ID: 600
+        packet.param1 = cmd.content.jump.target;        // jump-to tag number
+        break;
+
     case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
         packet.param1 = cmd.content.speed.speed_type;   // 0 = airspeed, 1 = ground speed
         packet.param2 = cmd.content.speed.target_ms;    // speed in m/s
@@ -1844,6 +1871,10 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
             _nav_cmd = cmd;
             if (start_command(_nav_cmd)) {
                 _flags.nav_cmd_loaded = true;
+                if (_jump_tag.age > 0 && _jump_tag.age < UINT16_MAX) {
+                    // we're tracking a tag so increase it's age on every new NAV item
+                    _jump_tag.age++;
+                }
             }
             // save a loaded wp index in history array for when _repeat_dist is set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
             // and prevent history being re-written until vehicle returns to interrupted position
@@ -1939,6 +1970,13 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
             return false;
         }
 
+        // check for do-jump-tag command and convert target tag to do-jump target index and do-jump to it
+        if (temp_cmd.id == MAV_CMD_DO_JUMP_TAG) {
+            // convert tmp_cmd target from a target tag to a target index
+            temp_cmd.content.jump.target = get_index_of_jump_tag(temp_cmd.content.jump.target);
+            temp_cmd.id = MAV_CMD_DO_JUMP;
+        }
+
         // check for do-jump command
         if (temp_cmd.id == MAV_CMD_DO_JUMP) {
 
@@ -2025,6 +2063,34 @@ bool AP_Mission::get_next_do_cmd(uint16_t start_index, Mission_Command& cmd)
 /// jump handling methods
 ///
 
+// Set the mission index to the first JUMP_TAG with this tag.
+// Returns true on success, else false if no appropriate JUMP_TAG match can be found or if setting the index failed
+bool AP_Mission::jump_to_tag(const uint16_t tag)
+{
+    const uint16_t index = get_index_of_jump_tag(tag);
+    if (index == 0) {
+        return false;
+    }
+    return set_current_cmd(index);
+}
+
+// find the first JUMP_TAG with this tag and return its index.
+// Returns 0 if no appropriate JUMP_TAG match can be found.
+uint16_t AP_Mission::get_index_of_jump_tag(const uint16_t tag) const
+{
+    const auto count = num_commands();
+    for (uint16_t i = 1; i < count; i++) {
+        Mission_Command tmp;
+        if (!read_cmd_from_storage(i, tmp)) {
+            continue;
+        }
+        if (tmp.id == MAV_CMD_JUMP_TAG && tmp.content.jump.target == tag) {
+            return i;
+        }
+    }
+    return 0;
+}
+
 // init_jump_tracking - initialise jump_tracking variables
 void AP_Mission::init_jump_tracking()
 {
@@ -2467,6 +2533,10 @@ const char *AP_Mission::Mission_Command::type() const
         return "Scripting";
     case MAV_CMD_DO_JUMP:
         return "Jump";
+    case MAV_CMD_DO_JUMP_TAG:
+        return "JumpToTag";
+    case MAV_CMD_JUMP_TAG:
+        return "Tag";
     case MAV_CMD_DO_GO_AROUND:
         return "Go Around";
 #if AP_SCRIPTING_ENABLED

libraries/AP_Mission/AP_Mission.h

@@ -659,6 +659,14 @@ public:
     bool get_item(uint16_t index, mavlink_mission_item_int_t& result) const ;
     bool set_item(uint16_t index, mavlink_mission_item_int_t& source) ;
 
+    // Set the mission index to the first JUMP_TAG with this tag.
+    // Returns true on success, else false if no appropriate JUMP_TAG match can be found or if setting the index failed
+    bool jump_to_tag(const uint16_t tag);
+
+    // find the first JUMP_TAG with this tag and return its index.
+    // Returns 0 if no appropriate JUMP_TAG match can be found.
+    uint16_t get_index_of_jump_tag(const uint16_t tag) const;
+
 private:
     static AP_Mission *_singleton;
 
@@ -666,6 +674,11 @@ private:
 
     static bool stored_in_location(uint16_t id);
 
+    struct {
+        uint16_t age;   // a value of 0 means we have never seen a tag. Once a tag is seen, age will increment every time the mission index changes.
+        uint16_t tag;   // most recent tag that was successfully jumped to. Only valid if age > 0
+    } _jump_tag;
+
     struct Mission_Flags {
         mission_state state;
         bool nav_cmd_loaded;         // true if a "navigation" command has been loaded into _nav_cmd
@@ -744,6 +757,8 @@ private:
     // update progress made in mission to store last position in the event of mission exit
     void update_exit_position(void);
 
+    void on_mission_timestamp_change();
+
     /// sanity checks that the masked fields are not NaN's or infinite
     static MAV_MISSION_RESULT sanity_check_params(const mavlink_mission_item_int_t& packet);
 
@@ -779,6 +794,7 @@ private:
 
     // last time that mission changed
     uint32_t _last_change_time_ms;
+    uint32_t _last_change_time_prev_ms;
 
     // memoisation of contains-relative:
     bool _contains_terrain_alt_items;  // true if the mission has terrain-relative items