ROVER: stop mission when leaving AUTO

This commit is contained in:
Pierre Kancir 2017-02-16 12:39:00 +01:00 committed by Grant Morphett
parent 1d70a337dd
commit 1bd863e36b

View File

@ -273,9 +273,12 @@ void Rover::set_mode(enum mode mode)
return;
}
// If we are changing out of AUTO mode reset the loiter timer
// If we are changing out of AUTO mode reset the loiter timer and stop current mission
if (control_mode == AUTO) {
loiter_start_time = 0;
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
}
control_mode = mode;