AR_AttitudeControl: balancebot gets pitch limit protection

libraries/APM_Control/AR_AttitudeControl.cpp

@@ -40,6 +40,9 @@
 #define AR_ATTCONTROL_PITCH_THR_IMAX    1.0f
 #define AR_ATTCONTROL_PITCH_THR_FILT    10.0f
 #define AR_ATTCONTROL_BAL_PITCH_FF      0.4f
+#define AR_ATTCONTROL_PITCH_LIM_TC      0.5f        // pitch limit default time constant
+#define AR_ATTCONTROL_PITCH_RELAX_RATIO 0.5f        // pitch limit relaxed 2x slower than it is limited
+#define AR_ATTCONTROL_PITCH_LIM_THR_THRESH  0.60    // pitch limiting starts if throttle exceeds 60%
 #define AR_ATTCONTROL_DT                0.02f
 #define AR_ATTCONTROL_TIMEOUT_MS        200
 #define AR_ATTCONTROL_HEEL_SAIL_P       1.0f
@@ -443,6 +446,22 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_TURN_MAX_G", 13, AR_AttitudeControl, _turn_lateral_G_max, 0.6f),
 
+    // @Param: _BAL_LIM_TC
+    // @DisplayName: Pitch control limit time constant
+    // @Description: Pitch control limit time constant to protect against falling.  Lower values limit pitch more quickly, higher values limit more slowly.  Set to 0 to disable
+    // @Range: 0.0 5.0
+    // @Increment: 0.01
+    // @User: Standard
+    AP_GROUPINFO("_BAL_LIM_TC", 14, AR_AttitudeControl, _pitch_limit_tc, AR_ATTCONTROL_PITCH_LIM_TC),
+
+    // @Param: _BAL_LIM_THR
+    // @DisplayName: Pitch control limit throttle threshold
+    // @Description: Pitch control limit throttle threshold.  Pitch angle will be limited if throttle crosses this threshold (from 0 to 1)
+    // @Range: 0.0 1.0
+    // @Increment: 0.01
+    // @User: Standard
+    AP_GROUPINFO("_BAL_LIM_THR", 15, AR_AttitudeControl, _pitch_limit_throttle_thresh, AR_ATTCONTROL_PITCH_LIM_THR_THRESH),
+
     AP_GROUPEND
 };
 
@@ -716,8 +735,9 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
 
 // balancebot pitch to throttle controller
 // returns a throttle output from -1 to +1 given a desired pitch angle (in radians)
+// pitch_max should be the user defined max pitch angle (in radians)
 // motor_limit should be true if the motors have hit their upper or lower limit
-float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool motor_limit, float dt)
+float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float pitch_max, bool motor_limit, float dt)
 {
     // sanity check dt
     dt = constrain_float(dt, 0.0f, 1.0f);
@@ -727,9 +747,20 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool
     if ((_balance_last_ms == 0) || ((now - _balance_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
         _pitch_to_throttle_pid.reset_filter();
         _pitch_to_throttle_pid.reset_I();
+        _pitch_limit_low = -pitch_max;
+        _pitch_limit_high = pitch_max;
     }
     _balance_last_ms = now;
 
+    // limit desired pitch to protect against falling
+    const bool pitch_limit_active = (_pitch_limit_tc >= 0.01) && (_pitch_limit_throttle_thresh > 0);
+    if (pitch_limit_active) {
+        desired_pitch = constrain_float(desired_pitch, _pitch_limit_low, _pitch_limit_high);
+        _pitch_limited = (desired_pitch <= _pitch_limit_low || desired_pitch >= _pitch_limit_high);
+    } else {
+        _pitch_limited = false;
+    }
+
     // set PID's dt
     _pitch_to_throttle_pid.set_dt(dt);
 
@@ -741,6 +772,25 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool
     output += _pitch_to_throttle_pid.update_all(desired_pitch, pitch_rad, motor_limit);
     output += _pitch_to_throttle_pid.get_ff();
 
+    // update pitch limits for next iteration
+    // note: pitch is positive when leaning backwards, negative when leaning forward
+    if (pitch_limit_active) {
+        const float pitch_limit_incr = 1.0/_pitch_limit_tc * dt * pitch_max;
+        const float pitch_relax_incr = pitch_limit_incr * AR_ATTCONTROL_PITCH_RELAX_RATIO;
+        if (output <= -_pitch_limit_throttle_thresh) {
+            // very low negative throttle output means we must lower pitch_high (e.g. reduce leaning backwards)
+            _pitch_limit_high = MAX(_pitch_limit_high - pitch_limit_incr, 0);
+        } else {
+            _pitch_limit_high = MIN(_pitch_limit_high + pitch_relax_incr, pitch_max);
+        }
+        if (output >= _pitch_limit_throttle_thresh) {
+            // very high positive throttle output means we must raise pitch_low (reduce leaning forwards)
+            _pitch_limit_low = MIN(_pitch_limit_low + pitch_limit_incr, 0);
+        } else {
+            _pitch_limit_low = MAX(_pitch_limit_low - pitch_relax_incr, -pitch_max);
+        }
+    }
+
     // constrain and return final output
     return output;
 }

libraries/APM_Control/AR_AttitudeControl.h

@@ -68,8 +68,13 @@ public:
 
     // balancebot pitch to throttle controller
     // returns a throttle output from -1 to +1 given a desired pitch angle (in radians)
+    // pitch_max should be the user defined max pitch angle (in radians)
     // motor_limit should be true if the motors have hit their upper or lower limit
-    float get_throttle_out_from_pitch(float desired_pitch, bool motor_limit, float dt);
+    float get_throttle_out_from_pitch(float desired_pitch, float pitch_max, bool motor_limit, float dt);
+
+    // returns true if the pitch angle has been limited to prevent falling over
+    // pitch limit protection is implemented within get_throttle_out_from_pitch
+    bool pitch_limited() const { return _pitch_limited; }
 
     // get latest desired pitch in radians for reporting purposes
     float get_desired_pitch() const;
@@ -122,6 +127,8 @@ private:
     AC_PID   _throttle_speed_pid;   // throttle speed controller
     AC_PID   _pitch_to_throttle_pid;// balancebot pitch controller
     AP_Float _pitch_to_throttle_ff; // balancebot feed forward from current pitch angle
+    AP_Float _pitch_limit_tc;       // balancebot pitch limit protection time constant
+    AP_Float _pitch_limit_throttle_thresh;  // balancebot pitch limit throttle threshold (in the range 0 to 1.0)
 
     AP_Float _throttle_accel_max;   // speed/throttle control acceleration (and deceleration) maximum in m/s/s.  0 to disable limits
     AP_Float _throttle_decel_max;    // speed/throttle control deceleration maximum in m/s/s. 0 to use ATC_ACCEL_MAX for deceleration
@@ -146,7 +153,10 @@ private:
     AP_PIDInfo _throttle_speed_pid_info;   // local copy of throttle_speed controller's PID info to allow reporting of unusual FF
 
     // balancebot pitch control
-    uint32_t _balance_last_ms = 0;
+    uint32_t _balance_last_ms = 0;  // system time that get_throttle_out_from_pitch was last called
+    float _pitch_limit_low = 0;     // min desired pitch (in radians) used to protect against falling over
+    float _pitch_limit_high = 0;    // max desired pitch (in radians) used to protect against falling over
+    bool _pitch_limited = false;    // true if pitch was limited on last call to get_throttle_out_from_pitch
 
     // Sailboat heel control
     AC_PID   _sailboat_heel_pid;    // Sailboat heel angle pid controller