Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: added to rewind mission on resume with MAV_CMD_DO_SET_RESUME_REPEAT_DIST

Browse Source
This commit is contained in:
Gone4Dirt 2020-02-24 22:43:20 +00:00 committed by Andrew Tridgell
parent 0f7de04d79
commit 537a259ab6
3 changed files with 200 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

169
libraries/AP_Mission/AP_Mission.cpp
View File

@ -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<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;

31
libraries/AP_Mission/AP_Mission.h
View File

@ -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 {

7
libraries/AP_Mission/AP_Mission_Commands.cpp
View File

@ -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;
}