Browse Source

AC_AttitudeControl: Add minimum angle limit for get_althold_lean_angle_max

mission-4.1.18
Leonard Hall 7 years ago committed by Randy Mackay
parent
commit
706ff85be7
  1. 2
      libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
  2. 1
      libraries/AC_AttitudeControl/AC_AttitudeControl.h

2
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@ -903,7 +903,7 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits) @@ -903,7 +903,7 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
float AC_AttitudeControl::get_althold_lean_angle_max() const
{
// convert to centi-degrees for public interface
return ToDeg(_althold_lean_angle_max) * 100.0f;
return MAX(ToDeg(_althold_lean_angle_max), AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN) * 100.0f;
}
// Proportional controller with piecewise sqrt sections to constrain second derivative

1
libraries/AC_AttitudeControl/AC_AttitudeControl.h

@ -34,6 +34,7 @@ @@ -34,6 +34,7 @@
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0f // Min lean angle so that vehicle can maintain limited control
#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix default
#define AC_ATTITUDE_CONTROL_MAN_DEFAULT 0.5f // manual throttle mix default

Loading…
Cancel
Save