5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 16:08:28 -04:00

Copter: auto detects mission changes

This commit is contained in:
Randy Mackay 2021-03-04 17:19:05 +09:00
parent 238d102a56
commit 96267553cf
2 changed files with 84 additions and 3 deletions

View File

@ -415,6 +415,8 @@ private:
bool verify_command(const AP_Mission::Mission_Command& cmd);
void exit_mission();
bool check_for_mission_change(); // detect external changes to mission
void takeoff_run();
void wp_run();
void land_run();
@ -524,6 +526,15 @@ private:
} nav_payload_place;
bool waiting_to_start; // true if waiting for vehicle to be armed or EKF origin before starting mission
// variables to detect mission changes
static const uint8_t mis_change_detect_cmd_max = 3;
struct {
uint32_t last_change_time_ms; // local copy of last time mission was changed
uint16_t curr_cmd_index; // local copy of AP_Mission's current command index
uint8_t cmd_count; // number of commands in the cmd array
AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max]; // local copy of the next few mission commands
} mis_change_detect = {};
};
#if AUTOTUNE_ENABLED == ENABLED

View File

@ -43,6 +43,9 @@ bool ModeAuto::init(bool ignore_checks)
// set flag to start mission
waiting_to_start = true;
// initialise mission change check (ignore results)
check_for_mission_change();
// clear guided limits
copter.mode_guided.limit_clear();
@ -64,8 +67,24 @@ void ModeAuto::run()
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
waiting_to_start = false;
// initialise mission change check (ignore results)
check_for_mission_change();
}
} else {
// check for mission changes
if (check_for_mission_change()) {
// if mission is running restart the current command if it is a waypoint or spline command
if ((copter.mode_auto.mission.state() == AP_Mission::MISSION_RUNNING) && (_mode == Auto_WP)) {
if (mission.restart_current_nav_cmd()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
} else {
// failed to restart mission for some reason
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed but failed to restart command");
}
}
}
mission.update();
}
@ -525,6 +544,55 @@ void ModeAuto::exit_mission()
}
}
// detect external changes to mission
bool ModeAuto::check_for_mission_change()
{
// check if mission has been updated
const uint32_t change_time_ms = mission.last_change_time_ms();
const bool update_time_changed = (change_time_ms != mis_change_detect.last_change_time_ms);;
// check if active command index has changed
const uint16_t curr_cmd_idx = mission.get_current_nav_index();
const bool curr_cmd_idx_changed = (curr_cmd_idx != mis_change_detect.curr_cmd_index);
// no changes if neither mission update time nor active command index has changed
if (!update_time_changed && !curr_cmd_idx_changed) {
return false;
}
// the mission has been updated (but maybe not changed) and/or the current command index has changed
// check the contents of the next three commands to ensure they have not changed
// and update storage so we can detect future changes
bool cmds_changed = false; // true if upcoming command contents have changed
// retrieve cmds from mission and compare with mis_change_detect
uint8_t num_cmds = 0;
uint16_t cmd_idx = curr_cmd_idx;
AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max];
while ((num_cmds < ARRAY_SIZE(cmd)) && mission.get_next_nav_cmd(cmd_idx, cmd[num_cmds])) {
num_cmds++;
if ((num_cmds > mis_change_detect.cmd_count) || (cmd[num_cmds-1] != mis_change_detect.cmd[num_cmds-1])) {
cmds_changed = true;
mis_change_detect.cmd[num_cmds-1] = cmd[num_cmds-1];
}
cmd_idx = cmd[num_cmds-1].index+1;
}
// mission has changed if number of upcoming commands does not match mis_change_detect
if (num_cmds != mis_change_detect.cmd_count) {
cmds_changed = true;
}
// update mis_change_detect with last change time, command index and number of commands
mis_change_detect.last_change_time_ms = change_time_ms;
mis_change_detect.curr_cmd_index = curr_cmd_idx;
mis_change_detect.cmd_count = num_cmds;
// mission has changed if upcoming command contents have changed without the current command index changing
return cmds_changed && !curr_cmd_idx_changed;
}
// do_guided - start guided mode
bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
{
@ -1039,7 +1107,7 @@ Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Lo
ret.lat = default_loc.lat;
ret.lng = default_loc.lng;
}
// use current altitude if not provided
// use default altitude if not provided in cmd
if (ret.alt == 0) {
// set to default_loc's altitude but in command's alt frame
// note that this may use the terrain database
@ -1256,7 +1324,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
wp_nav->get_default_speed_up());
}
// do_nav_wp - initiate move to next waypoint
// do_spline_wp - initiate move to next waypoint
void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
// calculate default location used when lat, lon or alt is zero
@ -1299,7 +1367,9 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
}
}
// get_spline_from_cmd - Calculates the spline type for a given spline waypoint mission command
// calculate locations required to build a spline curve from a mission command
// dest_loc is populated from cmd's location using default_loc in cases where the lat and lon or altitude is zero
// next_dest_loc and nest_dest_loc_is_spline is filled in with the following navigation command's location if it exists. If it does not exist it is set to the dest_loc and false
void ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline)
{
dest_loc = loc_from_cmd(cmd, default_loc);