diff --git a/Rover/Parameters.h b/Rover/Parameters.h index e176a72f0c..e2d8d9849c 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -384,8 +384,10 @@ public: // windvane AP_WindVane windvane; +#if AP_MISSION_ENABLED // mission behave - AP_Int8 mis_done_behave; + AP_Enum mis_done_behave; +#endif // balance both pitch trim AP_Float bal_pitch_trim; diff --git a/Rover/mode.h b/Rover/mode.h index f166843ed0..075dc1b34b 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -282,11 +282,11 @@ public: FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command_callback, bool, const AP_Mission::Mission_Command&), FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)}; - enum Mis_Done_Behave { - MIS_DONE_BEHAVE_HOLD = 0, - MIS_DONE_BEHAVE_LOITER = 1, - MIS_DONE_BEHAVE_ACRO = 2, - MIS_DONE_BEHAVE_MANUAL = 3 + enum class DoneBehaviour : uint8_t { + HOLD = 0, + LOITER = 1, + ACRO = 2, + MANUAL = 3, }; protected: diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 387349dfda..fa1ea8ece2 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -626,16 +626,25 @@ void ModeAuto::exit_mission() // send message gcs().send_text(MAV_SEVERITY_NOTICE, "Mission Complete"); - if (g2.mis_done_behave == MIS_DONE_BEHAVE_LOITER && start_loiter()) { - return; - } - - if (g2.mis_done_behave == MIS_DONE_BEHAVE_ACRO && rover.set_mode(rover.mode_acro, ModeReason::MISSION_END)) { - return; - } - - if (g2.mis_done_behave == MIS_DONE_BEHAVE_MANUAL && rover.set_mode(rover.mode_manual, ModeReason::MISSION_END)) { - return; + switch ((DoneBehaviour)g2.mis_done_behave) { + case DoneBehaviour::HOLD: + // the default "start_stop" behaviour is used + break; + case DoneBehaviour::LOITER: + if (start_loiter()) { + return; + } + break; + case DoneBehaviour::ACRO: + if (rover.set_mode(rover.mode_acro, ModeReason::MISSION_END)) { + return; + } + break; + case DoneBehaviour::MANUAL: + if (rover.set_mode(rover.mode_manual, ModeReason::MISSION_END)) { + return; + } + break; } start_stop(); diff --git a/Rover/system.cpp b/Rover/system.cpp index b00c2cae1e..4c45b7796a 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -145,7 +145,7 @@ void Rover::init_ardupilot() // boat should loiter after completing a mission to avoid drifting off if (is_boat()) { - rover.g2.mis_done_behave.set_default(ModeAuto::Mis_Done_Behave::MIS_DONE_BEHAVE_LOITER); + rover.g2.mis_done_behave.set_default(uint8_t(ModeAuto::DoneBehaviour::LOITER)); } // flag that initialisation has completed