mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 12:14:10 -04:00
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:
parent
a1ea1306a1
commit
f6cb1b5ad6
@ -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 &&
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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 {
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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();
|
||||||
|
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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 {
|
||||||
|
@ -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 {
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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();
|
||||||
|
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user