mirror of https://github.com/ArduPilot/ardupilot
Copter: integrate AP_Mission_ChangeDetector
This commit is contained in:
parent
c0855a0d1e
commit
d5461f2225
|
@ -41,6 +41,7 @@
|
|||
#include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
|
||||
#include <AP_AHRS/AP_AHRS.h> // AHRS (Attitude Heading Reference System) interface library for ArduPilot
|
||||
#include <AP_Mission/AP_Mission.h> // Mission command library
|
||||
#include <AP_Mission/AP_Mission_ChangeDetector.h> // Mission command change detection library
|
||||
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
|
||||
#include <AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h> // 6DoF Attitude control library
|
||||
#include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
|
||||
|
|
|
@ -441,6 +441,9 @@ public:
|
|||
FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
|
||||
FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)};
|
||||
|
||||
// Mission change detector
|
||||
AP_Mission_ChangeDetector mis_change_detector;
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return auto_RTL? "AUTO RTL" : "AUTO"; }
|
||||
|
@ -575,15 +578,6 @@ private:
|
|||
|
||||
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 = {};
|
||||
|
||||
// True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode
|
||||
bool auto_RTL;
|
||||
};
|
||||
|
|
|
@ -45,7 +45,7 @@ bool ModeAuto::init(bool ignore_checks)
|
|||
waiting_to_start = true;
|
||||
|
||||
// initialise mission change check (ignore results)
|
||||
check_for_mission_change();
|
||||
IGNORE_RETURN(mis_change_detector.check_for_mission_change());
|
||||
|
||||
// clear guided limits
|
||||
copter.mode_guided.limit_clear();
|
||||
|
@ -91,13 +91,13 @@ void ModeAuto::run()
|
|||
waiting_to_start = false;
|
||||
|
||||
// initialise mission change check (ignore results)
|
||||
check_for_mission_change();
|
||||
IGNORE_RETURN(mis_change_detector.check_for_mission_change());
|
||||
}
|
||||
} else {
|
||||
// check for mission changes
|
||||
if (check_for_mission_change()) {
|
||||
if (mis_change_detector.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 == SubMode::WP)) {
|
||||
if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_mode == SubMode::WP)) {
|
||||
if (mission.restart_current_nav_cmd()) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
|
||||
} else {
|
||||
|
@ -606,55 +606,6 @@ 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)
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue