mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
Plane: remove duplicate last mode reason
This commit is contained in:
parent
0677965524
commit
90f2bf3ba2
ArduPlane
@ -1057,7 +1057,6 @@ private:
|
||||
bool set_mode(Mode& new_mode, const ModeReason reason);
|
||||
bool set_mode(const uint8_t mode, const ModeReason reason) override;
|
||||
bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason);
|
||||
ModeReason _last_reason;
|
||||
void check_long_failsafe();
|
||||
void check_short_failsafe();
|
||||
void startup_INS_ground(void);
|
||||
|
@ -200,11 +200,11 @@ void Plane::failsafe_short_off_event(ModeReason reason)
|
||||
// We're back in radio contact
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Short Failsafe Cleared");
|
||||
failsafe.state = FAILSAFE_NONE;
|
||||
//restore entry mode if desired but check that our current mode is still due to failsafe
|
||||
if ( _last_reason == ModeReason::RADIO_FAILSAFE) {
|
||||
// restore entry mode if desired but check that our current mode is still due to failsafe
|
||||
if (control_mode_reason == ModeReason::RADIO_FAILSAFE) {
|
||||
set_mode_by_number(failsafe.saved_mode_number, ModeReason::RADIO_FAILSAFE_RECOVERY);
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"Flight mode %s restored",control_mode->name());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Plane::failsafe_long_off_event(ModeReason reason)
|
||||
|
@ -204,14 +204,11 @@ void Plane::startup_ground(void)
|
||||
|
||||
bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
|
||||
{
|
||||
// update last reason
|
||||
const ModeReason last_reason = _last_reason;
|
||||
_last_reason = reason;
|
||||
|
||||
if (control_mode == &new_mode) {
|
||||
// don't switch modes if we are already in the correct mode.
|
||||
// only make happy noise if using a difent method to switch, this stops beeping for repeated change mode requests from GCS
|
||||
if ((reason != last_reason) && (reason != ModeReason::INITIALISED)) {
|
||||
if ((reason != control_mode_reason) && (reason != ModeReason::INITIALISED)) {
|
||||
AP_Notify::events.user_mode_change = 1;
|
||||
}
|
||||
return true;
|
||||
|
Loading…
Reference in New Issue
Block a user