Plane: use a method on Mode for auto-navigation-mode

Avoids the state getting stale, which it will with a failed attempt to
go into qautotune, for example.
This commit is contained in:
Peter Barker 2020-12-23 15:56:54 +11:00 committed by Peter Barker
parent a1ea1306a1
commit f6cb1b5ad6
22 changed files with 20 additions and 24 deletions

View File

@ -54,7 +54,7 @@ float Plane::get_speed_scaler(void)
*/ */
bool Plane::stick_mixing_enabled(void) bool Plane::stick_mixing_enabled(void)
{ {
if (auto_throttle_mode && auto_navigation_mode) { if (auto_throttle_mode && plane.control_mode->does_auto_navigation()) {
// we're in an auto mode. Check the stick mixing flag // we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED && if (g.stick_mixing != STICK_MIXING_DISABLED &&
g.stick_mixing != STICK_MIXING_VTOL_YAW && g.stick_mixing != STICK_MIXING_VTOL_YAW &&

View File

@ -576,10 +576,6 @@ private:
// we need to run the speed/height controller // we need to run the speed/height controller
bool auto_throttle_mode:1; bool auto_throttle_mode:1;
// true if we are in an auto-navigation mode, which controls whether control input is ignored
// with STICK_MIXING=0
bool auto_navigation_mode:1;
// this controls throttle suppression in auto modes // this controls throttle suppression in auto modes
bool throttle_suppressed; bool throttle_suppressed;

View File

@ -73,7 +73,7 @@ bool Mode::enter()
// start with throttle suppressed in auto_throttle modes // start with throttle suppressed in auto_throttle modes
plane.throttle_suppressed = plane.auto_throttle_mode; plane.throttle_suppressed = plane.auto_throttle_mode;
#if HAL_ADSB_ENABLED #if HAL_ADSB_ENABLED
plane.adsb.set_is_auto_mode(plane.auto_navigation_mode); plane.adsb.set_is_auto_mode(does_auto_navigation());
#endif #endif
// reset steering integrator on mode change // reset steering integrator on mode change

View File

@ -87,6 +87,10 @@ public:
// depending on airspeed_nudge_cm // depending on airspeed_nudge_cm
virtual bool allows_throttle_nudging() const { return false; } virtual bool allows_throttle_nudging() const { return false; }
// true if the mode sets the vehicle destination, which controls
// whether control input is ignored with STICK_MIXING=0
virtual bool does_auto_navigation() const { return false; }
protected: protected:
// subclasses override this to perform checks before entering the mode // subclasses override this to perform checks before entering the mode
@ -128,6 +132,8 @@ public:
bool allows_throttle_nudging() const override { return true; } bool allows_throttle_nudging() const override { return true; }
bool does_auto_navigation() const override { return true; }
protected: protected:
bool _enter() override; bool _enter() override;
@ -169,6 +175,8 @@ public:
bool allows_throttle_nudging() const override { return true; } bool allows_throttle_nudging() const override { return true; }
bool does_auto_navigation() const override { return true; }
protected: protected:
bool _enter() override; bool _enter() override;
@ -185,6 +193,8 @@ public:
// methods that affect movement of the vehicle in this mode // methods that affect movement of the vehicle in this mode
void update() override; void update() override;
bool does_auto_navigation() const override { return true; }
protected: protected:
bool _enter() override; bool _enter() override;
@ -206,6 +216,8 @@ public:
bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc); bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
bool isHeadingLinedUp_cd(const int32_t bearing_cd); bool isHeadingLinedUp_cd(const int32_t bearing_cd);
bool does_auto_navigation() const override { return true; }
protected: protected:
bool _enter() override; bool _enter() override;
@ -244,6 +256,8 @@ public:
bool allows_throttle_nudging() const override { return true; } bool allows_throttle_nudging() const override { return true; }
bool does_auto_navigation() const override { return true; }
protected: protected:
bool _enter() override; bool _enter() override;
@ -536,6 +550,8 @@ public:
bool allows_throttle_nudging() const override { return true; } bool allows_throttle_nudging() const override { return true; }
bool does_auto_navigation() const override { return true; }
// var_info for holding parameter information // var_info for holding parameter information
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -571,6 +587,8 @@ public:
bool allows_throttle_nudging() const override { return true; } bool allows_throttle_nudging() const override { return true; }
bool does_auto_navigation() const override { return true; }
protected: protected:
bool exit_heading_aligned() const; bool exit_heading_aligned() const;

View File

@ -4,7 +4,6 @@
bool ModeAcro::_enter() bool ModeAcro::_enter()
{ {
plane.auto_throttle_mode = false; plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
plane.acro_state.locked_roll = false; plane.acro_state.locked_roll = false;
plane.acro_state.locked_pitch = false; plane.acro_state.locked_pitch = false;

View File

@ -4,7 +4,6 @@
bool ModeAuto::_enter() bool ModeAuto::_enter()
{ {
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
if (plane.quadplane.available() && plane.quadplane.enable == 2) { if (plane.quadplane.available() && plane.quadplane.enable == 2) {
plane.auto_state.vtol_mode = true; plane.auto_state.vtol_mode = true;
} else { } else {

View File

@ -4,7 +4,6 @@
bool ModeAutoTune::_enter() bool ModeAutoTune::_enter()
{ {
plane.auto_throttle_mode = false; plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
plane.autotune_start(); plane.autotune_start();
return true; return true;

View File

@ -5,7 +5,6 @@ bool ModeCircle::_enter()
{ {
// the altitude to circle at is taken from the current altitude // the altitude to circle at is taken from the current altitude
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.next_WP_loc.alt = plane.current_loc.alt; plane.next_WP_loc.alt = plane.current_loc.alt;
return true; return true;

View File

@ -4,7 +4,6 @@
bool ModeCruise::_enter() bool ModeCruise::_enter()
{ {
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false;
locked_heading = false; locked_heading = false;
lock_timer_ms = 0; lock_timer_ms = 0;

View File

@ -4,7 +4,6 @@
bool ModeFBWA::_enter() bool ModeFBWA::_enter()
{ {
plane.auto_throttle_mode = false; plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true; return true;
} }

View File

@ -4,7 +4,6 @@
bool ModeFBWB::_enter() bool ModeFBWB::_enter()
{ {
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false;
#if HAL_SOARING_ENABLED #if HAL_SOARING_ENABLED
// for ArduSoar soaring_controller // for ArduSoar soaring_controller

View File

@ -4,7 +4,6 @@
bool ModeGuided::_enter() bool ModeGuided::_enter()
{ {
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.guided_throttle_passthru = false; plane.guided_throttle_passthru = false;
/* /*
when entering guided mode we set the target as the current when entering guided mode we set the target as the current

View File

@ -4,7 +4,6 @@
bool ModeInitializing::_enter() bool ModeInitializing::_enter()
{ {
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false;
return true; return true;
} }

View File

@ -4,7 +4,6 @@
bool ModeLoiter::_enter() bool ModeLoiter::_enter()
{ {
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.do_loiter_at_location(); plane.do_loiter_at_location();
plane.loiter_angle_reset(); plane.loiter_angle_reset();

View File

@ -4,7 +4,6 @@
bool ModeManual::_enter() bool ModeManual::_enter()
{ {
plane.auto_throttle_mode = false; plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true; return true;
} }

View File

@ -3,7 +3,6 @@
bool ModeQAcro::_enter() bool ModeQAcro::_enter()
{ {
plane.auto_navigation_mode = false;
if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) {
plane.control_mode = plane.previous_mode; plane.control_mode = plane.previous_mode;
} else { } else {

View File

@ -3,7 +3,6 @@
bool ModeQStabilize::_enter() bool ModeQStabilize::_enter()
{ {
plane.auto_navigation_mode = false;
if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) {
plane.control_mode = plane.previous_mode; plane.control_mode = plane.previous_mode;
} else { } else {

View File

@ -4,7 +4,6 @@
bool ModeRTL::_enter() bool ModeRTL::_enter()
{ {
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.prev_WP_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc;
plane.do_RTL(plane.get_RTL_altitude()); plane.do_RTL(plane.get_RTL_altitude());
plane.rtl.done_climb = false; plane.rtl.done_climb = false;

View File

@ -4,7 +4,6 @@
bool ModeStabilize::_enter() bool ModeStabilize::_enter()
{ {
plane.auto_throttle_mode = false; plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true; return true;
} }

View File

@ -53,7 +53,6 @@ bool ModeTakeoff::_enter()
{ {
// the altitude to circle at is taken from the current altitude // the altitude to circle at is taken from the current altitude
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
takeoff_started = false; takeoff_started = false;

View File

@ -10,7 +10,6 @@ bool ModeThermal::_enter()
} }
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.do_loiter_at_location(); plane.do_loiter_at_location();
plane.loiter_angle_reset(); plane.loiter_angle_reset();

View File

@ -4,7 +4,6 @@
bool ModeTraining::_enter() bool ModeTraining::_enter()
{ {
plane.auto_throttle_mode = false; plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true; return true;
} }