AC_PosControl: Add monitoring and reporting of forward accel saturation

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -656,6 +656,13 @@ void AC_PosControl::update_xy_controller()
     if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) {
         // _accel_target was not limited so we can zero the xy limit vector
         _limit_vector.xy().zero();
+    } else {
+        // Check for pitch limiting in the forward direction
+        const float accel_fwd_unlimited = _limit_vector.x * _ahrs.cos_yaw() + _limit_vector.y * _ahrs.sin_yaw();
+        const float pitch_target_unlimited = accel_to_angle(- MIN(accel_fwd_unlimited, accel_max) * 0.01f) * 100;
+        const float accel_fwd_limited = _accel_target.x * _ahrs.cos_yaw() + _accel_target.y * _ahrs.sin_yaw();
+        const float pitch_target_limited = accel_to_angle(- accel_fwd_limited * 0.01f) * 100;
+        _fwd_pitch_is_limited = is_negative(pitch_target_unlimited) && pitch_target_unlimited < pitch_target_limited;
     }
 
     // update angle targets that will be passed to stabilize controller

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -394,6 +394,9 @@ public:
     // get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up
     float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; }
 
+    /// returns true when the forward pitch demand is limited by the maximum allowed tilt
+    bool get_fwd_pitch_is_limited() const { return _fwd_pitch_is_limited; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
@@ -462,6 +465,8 @@ protected:
     Vector3f    _accel_target;          // acceleration target in NEU cm/s/s
     Vector3f    _limit_vector;          // the direction that the position controller is limited, zero when not limited
 
+    bool        _fwd_pitch_is_limited;     // true when the forward pitch demand is being limited to meet acceleration limits
+
     float       _pos_offset_target_z;   // vertical position offset target, frame NEU in cm relative to the EKF origin
     float       _pos_offset_z;          // vertical position offset, frame NEU in cm relative to the EKF origin
     float       _vel_offset_z;          // vertical velocity offset in NEU cm/s calculated by pos_to_rate step