Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: incorporate mission change detector

This commit is contained in:
Randy Mackay 2021-11-15 19:27:35 +09:00
parent 315c5fb7d7
commit 4c6bccc8d5
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Rover/mode.h
View File

@ -5,6 +5,7 @@
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Mission/AP_Mission.h> #include <AP_Mission/AP_Mission.h>
#include <AP_Mission/AP_Mission_ChangeDetector.h>
#include <AR_WPNav/AR_WPNav.h> #include <AR_WPNav/AR_WPNav.h>
#include "defines.h" #include "defines.h"
@ -383,6 +384,9 @@ private:
float arg2; // 2nd argument provided by mission command float arg2; // 2nd argument provided by mission command
} nav_scripting; } nav_scripting;
#endif #endif
// Mission change detector
AP_Mission_ChangeDetector mis_change_detector;
}; };

16
Rover/mode_auto.cpp
View File

@ -53,8 +53,24 @@ void ModeAuto::update()
// start/resume the mission (based on MIS_RESTART parameter) // start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume(); mission.start_or_resume();
waiting_to_start = false; waiting_to_start = false;
// initialise mission change check
IGNORE_RETURN(mis_change_detector.check_for_mission_change());
} }
} else { } else {
// check for mission changes
if (mis_change_detector.check_for_mission_change()) {
// if mission is running restart the current command if it is a waypoint command
if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == AutoSubMode::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(); mission.update();
} }