AP_Mission: added to rewind mission on resume with MAV_CMD_DO_SET_RESUME_REPEAT_DIST

libraries/AP_Mission/AP_Mission.cpp

@@ -112,6 +112,24 @@ void AP_Mission::resume()
         return;
     }
 
+    // rewind the mission wp if the repeat distance has been set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
+    if (_repeat_dist > 0 && _wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) {
+        // if not already in a resume state calculate the position to rewind to
+        Mission_Command tmp_cmd;
+        if (!_flags.resuming_mission && calc_rewind_pos(tmp_cmd)) { 
+            _resume_cmd = tmp_cmd;
+        }
+
+        // resume mission to rewound position
+        if (_resume_cmd.index != AP_MISSION_CMD_INDEX_NONE && start_command(_resume_cmd)) {
+            _nav_cmd = _resume_cmd;
+            _flags.nav_cmd_loaded = true;
+            // set flag to prevent history being re-written
+            _flags.resuming_mission = true;
+            return;
+        }
+    }
+
     // restart active navigation command. We run these on resume()
     // regardless of whether the mission was stopped, as we may be
     // re-entering AUTO mode and the nav_cmd callback needs to be run
@@ -184,6 +202,7 @@ void AP_Mission::reset()
     _prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
     _prev_nav_cmd_id       = AP_MISSION_CMD_ID_NONE;
     init_jump_tracking();
+    reset_wp_history();
 }
 
 /// clear - clears out mission
@@ -226,6 +245,8 @@ void AP_Mission::update()
         return;
     }
 
+    update_exit_position();
+
     // save persistent waypoint_num for watchdog restore
     hal.util->persistent_data.waypoint_num = _nav_cmd.index;
 
@@ -276,6 +297,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
     case MAV_CMD_DO_PARACHUTE:
+    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
         return true;
     default:
         return _cmd_verify_fn(cmd);
@@ -305,6 +327,8 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
         return start_command_camera(cmd);
     case MAV_CMD_DO_PARACHUTE:
         return start_command_parachute(cmd);
+    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
+        return command_do_set_repeat_dist(cmd);
     default:
         return _cmd_start_fn(cmd);
     }
@@ -396,7 +420,7 @@ int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle)
 }
 
 // set_current_cmd - jumps to command specified by index
-bool AP_Mission::set_current_cmd(uint16_t index)
+bool AP_Mission::set_current_cmd(uint16_t index, bool rewind)
 {
     // read command to check for DO_LAND_START
     Mission_Command cmd;
@@ -404,6 +428,11 @@ bool AP_Mission::set_current_cmd(uint16_t index)
         _flags.in_landing_sequence = false;
     }
 
+    // mission command has been set and not as rewind command, don't track history.
+    if (!rewind) {
+        reset_wp_history();
+    }
+
     // sanity check index and that we have a mission
     if (index >= (unsigned)_cmd_total || _cmd_total == 1) {
         return false;
@@ -985,6 +1014,10 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
         cmd.content.winch.release_rate = packet.param4; // release rate in meters/second
         break;
 
+    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
+        cmd.p1 = packet.param1; // Resume repeat distance (m)
+        break;
+
     default:
         // unrecognised command
         return MAV_MISSION_UNSUPPORTED;
@@ -1420,6 +1453,10 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
         packet.param4 = cmd.content.winch.release_rate;     // release rate in meters/second
         break;
 
+    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
+        packet.param1 = cmd.p1; // Resume repeat distance (m)
+        break;
+
     default:
         // unrecognised command
         return false;
@@ -1534,6 +1571,22 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
             if (start_command(_nav_cmd)) {
                 _flags.nav_cmd_loaded = true;
             }
+            // 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 interupted position
+            if(_repeat_dist > 0 && !_flags.resuming_mission && _nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(_nav_cmd.content.location.lat == 0 && _nav_cmd.content.location.lng == 0)) {
+                // update mission history. last index position is always the most recent wp loaded.
+                for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
+                    _wp_index_history[i] = _wp_index_history[i+1];
+                }
+                _wp_index_history[AP_MISSION_MAX_WP_HISTORY-1] = _nav_cmd.index;
+            }
+            // check if the vehicle is resuming and has returned to where it was interupted
+            if (_flags.resuming_mission && _nav_cmd.index == _wp_index_history[AP_MISSION_MAX_WP_HISTORY-1]) {
+                // vehicle has resumed previous position
+                gcs().send_text(MAV_SEVERITY_INFO, "Mission: Returned to interupted WP");
+                _flags.resuming_mission = false;
+            }
+
         }else{
             // set current do command and start it (if not already set)
             if (!_flags.do_cmd_loaded) {
@@ -1645,7 +1698,7 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
                 int16_t jump_times_run = get_jump_times_run(temp_cmd);
                 if (jump_times_run < temp_cmd.content.jump.num_times) {
                     // update the record of the number of times run
-                    if (increment_jump_num_times_if_found) {
+                    if (increment_jump_num_times_if_found && !_flags.resuming_mission) {
                         increment_jump_times_run(temp_cmd, send_gcs_msg);
                     }
                     // continue searching from jump target
@@ -2064,6 +2117,8 @@ const char *AP_Mission::Mission_Command::type() const {
         return "SetROI";
     case MAV_CMD_DO_SET_REVERSE:
         return "SetReverse";
+    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
+        return "SetRepeatDist";
     case MAV_CMD_DO_GUIDED_LIMITS:
         return "GuidedLimits";
     case MAV_CMD_NAV_TAKEOFF:
@@ -2125,6 +2180,114 @@ bool AP_Mission::contains_item(MAV_CMD command) const
     return false;
 }
 
+// reset the mission history to prevent recalling previous mission histories after a mission restart.
+void AP_Mission::reset_wp_history(void)
+{
+    for (uint8_t i = 0; i<AP_MISSION_MAX_WP_HISTORY; i++) {
+        _wp_index_history[i] = AP_MISSION_CMD_INDEX_NONE;
+    }
+    _resume_cmd.index = AP_MISSION_CMD_INDEX_NONE;
+    _flags.resuming_mission = false;
+    _repeat_dist = 0;
+}
+
+// store the latest reported position incase of mission exit and rewind resume
+void AP_Mission::update_exit_position(void)
+{
+    if (!AP::ahrs().get_position(_exit_position)) {
+        _exit_position.lat = 0;
+        _exit_position.lng = 0;
+    }
+}
+
+// calculate the location of the _resume_cmd wp and set as current
+bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
+{
+    // check for a recent history
+    if (_wp_index_history[LAST_WP_PASSED] == AP_MISSION_CMD_INDEX_NONE) {
+        // no saved history so can't rewind
+        return false;
+    }
+
+    // check that we have a valid exit position
+    if (_exit_position.lat == 0 && _exit_position.lng == 0) {
+        return false;
+    }
+
+    Mission_Command temp_cmd;
+    float rewind_distance = _repeat_dist; //(m)
+    uint16_t resume_index;
+    Location prev_loc = _exit_position;
+
+    for (int8_t i = (LAST_WP_PASSED); i>=0; i--) {
+
+        // to get this far there has to be at least one 'passed wp' stored in history.  This is to check incase 
+        // of history array no being completely filled with valid waypoints upon resume.
+        if (_wp_index_history[i] == AP_MISSION_CMD_INDEX_NONE) {
+            // no more stored history
+            resume_index = i+1;
+            break;
+        }
+
+        if (!read_cmd_from_storage(_wp_index_history[i], temp_cmd)) {
+            // if read from storage failed then don't use rewind
+            return false;
+        }
+
+        // calculate distance
+        float disttemp = prev_loc.get_distance(temp_cmd.content.location); //(m)
+        rewind_distance -= disttemp;
+        resume_index = i;
+
+        if (rewind_distance <= 0.0f) {
+            // history rewound enough distance to meet _repeat_dist requirement
+            rewind_cmd = temp_cmd;
+            break;
+        }
+
+        // store wp location as previous
+        prev_loc = temp_cmd.content.location;
+    }
+
+    if (rewind_distance > 0.0f) {
+        // then the history array was rewound all of the way without finding a wp distance > _repeat_dist distance.
+        // the last read temp_cmd will be the furthest cmd back in the history array so resume to that.
+        rewind_cmd = temp_cmd;
+        return true;
+    }
+
+    // if we have got this far the desired rewind distance lies between two waypoints stored in history array.
+    // calculate the location for the mission to resume
+
+    // the last wp read from storage is the wp that is before the resume wp in the mission order
+    Location passed_wp_loc = temp_cmd.content.location;
+
+    // fetch next destination wp
+    if (!read_cmd_from_storage(_wp_index_history[resume_index+1], temp_cmd)) {
+        // if read from storage failed then don't use rewind
+        return false;
+    }
+
+    // determine the length of the mission leg that the resume wp lies in
+    float leg_length = passed_wp_loc.get_distance(temp_cmd.content.location); //(m)
+
+    // calculate the percentage along the leg that resume wp will be positioned
+    float leg_percent = fabsf(rewind_distance)/leg_length;
+
+    // calculate difference vector of mission leg
+    Vector3f dist_vec = passed_wp_loc.get_distance_NED(temp_cmd.content.location);
+
+    // calculate the resume wp position
+    rewind_cmd.content.location.offset(dist_vec.x * leg_percent, dist_vec.y * leg_percent);
+    rewind_cmd.content.location.alt -= dist_vec.z * leg_percent * 100; //(cm)
+
+    // The rewind_cmd.index has the index of the 'last passed wp' from the history array.  This ensures that the mission order
+    // continues as planned without further intervention.  The resume wp is not written to memory so will not perminantely change the mission.
+
+    // if we got this far then mission rewind position was successfully calculated
+    return true;
+}
+
 // singleton instance
 AP_Mission *AP_Mission::_singleton;
 

libraries/AP_Mission/AP_Mission.h

@@ -40,6 +40,9 @@
 #define AP_MISSION_MASK_MISSION_CLEAR       (1<<0)  // If set then Clear the mission on boot
 #define AP_MISSION_MASK_DIST_TO_LAND_CALC   (1<<1)  // Allow distance to best landing calculation to be run on failsafe
 
+#define AP_MISSION_MAX_WP_HISTORY           7       // The maximum number of previous wp commands that will be stored from the active missions history
+#define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2)
+
 /// @class    AP_Mission
 /// @brief    Object managing Mission
 class AP_Mission {
@@ -305,6 +308,7 @@ public:
         _flags.nav_cmd_loaded = false;
         _flags.do_cmd_loaded = false;
         _flags.in_landing_sequence = false;
+        _flags.resuming_mission = false;
         _force_resume = false;
     }
 
@@ -432,7 +436,7 @@ public:
     uint16_t get_current_do_cmd_id() const { return _do_cmd.id; }
 
     // set_current_cmd - jumps to command specified by index
-    bool set_current_cmd(uint16_t index);
+    bool set_current_cmd(uint16_t index, bool rewind = false);
 
     /// load_cmd_from_storage - load command from storage
     ///     true is return if successful
@@ -498,6 +502,9 @@ public:
     // returns true if the mission contains the requested items
     bool contains_item(MAV_CMD command) const;
 
+    // reset the mission history to prevent recalling previous mission histories when restarting missions.
+    void reset_wp_history(void);
+
     // user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -514,8 +521,12 @@ private:
         uint8_t do_cmd_loaded     : 1; // true if a "do"/"conditional" command has been loaded into _do_cmd
         uint8_t do_cmd_all_done   : 1; // true if all "do"/"conditional" commands have been completed (stops unnecessary searching through eeprom for do commands)
         bool in_landing_sequence  : 1; // true if the mission has jumped to a landing
+        bool resuming_mission     : 1; // true if the mission is resuming and set false once the aircraft attains the interupted WP
     } _flags;
 
+    // mission WP resume history
+    uint16_t _wp_index_history[AP_MISSION_MAX_WP_HISTORY]; // storing the nav_cmd index for the last 6 WPs
+
     ///
     /// private methods
     ///
@@ -573,6 +584,12 @@ private:
     // approximate the distance travelled to get to a landing.  DO_JUMP commands are observed in look forward.
     bool distance_to_landing(uint16_t index, float &tot_distance,Location current_loc);
 
+    // calculate the location of a resume cmd wp
+    bool calc_rewind_pos(Mission_Command& rewind_cmd);
+
+    // update progress made in mission to store last position in the event of mission exit
+    void update_exit_position(void);
+
     /// 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);
 
@@ -589,10 +606,12 @@ private:
     // internal variables
     struct Mission_Command  _nav_cmd;   // current "navigation" command.  It's position in the command list is held in _nav_cmd.index
     struct Mission_Command  _do_cmd;    // current "do" command.  It's position in the command list is held in _do_cmd.index
+    struct Mission_Command  _resume_cmd;  // virtual wp command that is used to resume mission if the mission needs to be rewound on resume.
     uint16_t                _prev_nav_cmd_id;       // id of the previous "navigation" command. (WAYPOINT, LOITER_TO_ALT, ect etc)
     uint16_t                _prev_nav_cmd_index;    // index of the previous "navigation" command.  Rarely used which is why we don't store the whole command
     uint16_t                _prev_nav_cmd_wp_index; // index of the previous "navigation" command that contains a waypoint.  Rarely used which is why we don't store the whole command
     bool                    _force_resume;  // when set true it forces mission to resume irrespective of MIS_RESTART param.
+    struct Location         _exit_position;  // the position in the mission that the mission was exited
 
     // jump related variables
     struct jump_tracking_struct {
@@ -603,6 +622,9 @@ private:
     // last time that mission changed
     uint32_t _last_change_time_ms;
 
+    // Distance to repeat on mission resume (m), can be set with MAV_CMD_DO_SET_RESUME_REPEAT_DIST
+    uint16_t _repeat_dist;
+
     // multi-thread support. This is static so it can be used from
     // const functions
     static HAL_Semaphore _rsem;
@@ -612,6 +634,7 @@ private:
     bool start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd);
     bool start_command_camera(const AP_Mission::Mission_Command& cmd);
     bool start_command_parachute(const AP_Mission::Mission_Command& cmd);
+    bool command_do_set_repeat_dist(const AP_Mission::Mission_Command& cmd);
 };
 
 namespace AP {

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -140,3 +140,10 @@ bool AP_Mission::start_command_parachute(const AP_Mission::Mission_Command& cmd)
     return false;
 #endif // HAL_PARACHUTE_ENABLED
 }
+
+bool AP_Mission::command_do_set_repeat_dist(const AP_Mission::Mission_Command& cmd)
+{
+    _repeat_dist = cmd.p1;
+    gcs().send_text(MAV_SEVERITY_INFO, "Resume repeat dist set to %u m",_repeat_dist);
+    return true;
+}