Browse Source

Plane: fixed stick mixing and throttle nudge in QRTL approach

thanks to Henry for noticing this
gps-1.3.1
Andrew Tridgell 3 years ago committed by Peter Barker
parent
commit
336b666f83
  1. 1
      ArduPlane/Attitude.cpp
  2. 2
      ArduPlane/mode.h
  3. 12
      ArduPlane/mode_qrtl.cpp

1
ArduPlane/Attitude.cpp

@ -258,7 +258,6 @@ void Plane::stabilize_stick_mixing_fbw() @@ -258,7 +258,6 @@ void Plane::stabilize_stick_mixing_fbw()
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
control_mode == &mode_qacro ||
#if QAUTOTUNE_ENABLED
control_mode == &mode_qautotune ||

2
ArduPlane/mode.h

@ -592,6 +592,8 @@ public: @@ -592,6 +592,8 @@ public:
bool update_target_altitude() override;
bool allows_throttle_nudging() const override;
protected:
bool _enter() override;

12
ArduPlane/mode_qrtl.cpp

@ -49,6 +49,12 @@ void ModeQRTL::run() @@ -49,6 +49,12 @@ void ModeQRTL::run()
// start landing logic
quadplane.verify_vtol_land();
}
// when in approach allow stick mixing
if (quadplane.poscontrol.get_state() == QuadPlane::QPOS_AIRBRAKE ||
quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) {
plane.stabilize_stick_mixing_fbw();
}
}
/*
@ -83,4 +89,10 @@ bool ModeQRTL::update_target_altitude() @@ -83,4 +89,10 @@ bool ModeQRTL::update_target_altitude()
return true;
}
// only nudge during approach
bool ModeQRTL::allows_throttle_nudging() const
{
return plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH;
}
#endif

Loading…
Cancel
Save