From 537a259ab65eea12b7d35a8c5cd9026ab7f0d669 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Mon, 24 Feb 2020 22:43:20 +0000 Subject: [PATCH] AP_Mission: added to rewind mission on resume with MAV_CMD_DO_SET_RESUME_REPEAT_DIST --- libraries/AP_Mission/AP_Mission.cpp | 169 ++++++++++++++++++- libraries/AP_Mission/AP_Mission.h | 31 +++- libraries/AP_Mission/AP_Mission_Commands.cpp | 7 + 3 files changed, 200 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index f5097decc6..ec76c573c7 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -68,7 +68,7 @@ void AP_Mission::start() _flags.state = MISSION_RUNNING; reset(); // reset mission to the first command, resets jump tracking - + // advance to the first command if (!advance_current_nav_cmd()) { // on failure set mission complete @@ -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=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; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index bbe15d517a..58a2fb0946 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/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[]; @@ -510,12 +517,16 @@ private: struct Mission_Flags { mission_state state; - uint8_t nav_cmd_loaded : 1; // true if a "navigation" command has been loaded into _nav_cmd - 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) + uint8_t nav_cmd_loaded : 1; // true if a "navigation" command has been loaded into _nav_cmd + 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 { diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index e791f6712d..0f286c0e32 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/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; +}