Rover: balancebot pitch control feedforward uses current pitch angle

Rover/Parameters.cpp

@@ -502,10 +502,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @DisplayName: BalanceBot Maximum Pitch
     // @Description: Pitch angle in degrees at 100% throttle
     // @Units: deg
-    // @Range: 0 5
+    // @Range: 0 15
     // @Increment: 0.1
     // @User: Standard
-    AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 2),
+    AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 10),
 
     // @Param: CRASH_ANGLE
     // @DisplayName: Crash Angle

Rover/balance_bot.cpp

@@ -7,15 +7,8 @@ void Rover::balancebot_pitch_control(float &throttle)
     // calculate desired pitch angle
     const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max) + radians(g2.bal_pitch_trim);
 
-    // calculate speed from wheel encoders
-    float veh_speed_pct = 0.0f;
-    if (g2.wheel_encoder.enabled(0) && g2.wheel_encoder.enabled(1) && is_positive(g2.wheel_rate_control.get_rate_max_rads())) {
-        veh_speed_pct = (g2.wheel_encoder.get_rate(0) + g2.wheel_encoder.get_rate(1)) / (2.0f * g2.wheel_rate_control.get_rate_max_rads()) * 100.0f;
-        veh_speed_pct = constrain_float(veh_speed_pct, -100.0f, 100.0f);
-    }
-
     // calculate required throttle using PID controller
-    throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, veh_speed_pct, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
+    throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, radians(g2.bal_pitch_max), g2.motors.limit.throttle_lower || g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
 }
 
 // returns true if vehicle is a balance bot

Rover/mode.cpp

@@ -317,7 +317,9 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
             bool stopped;
             throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped);
         } else {
-            throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
+            bool motor_lim_low = g2.motors.limit.throttle_lower || attitude_control.pitch_limited();
+            bool motor_lim_high = g2.motors.limit.throttle_upper || attitude_control.pitch_limited();
+            throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, motor_lim_low, motor_lim_high, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
         }
 
         // if vehicle is balance bot, calculate actual throttle required for balancing

Tools/Frame_params/ArduRoller-balancebot.param

@@ -10,7 +10,6 @@ ATC_BAL_FLTE,0
 ATC_BAL_I,7
 ATC_BAL_IMAX,1
 ATC_BAL_P,5
-ATC_BAL_SPD_FF,1.0
 ATC_BRAKE,1
 ATC_STR_ACC_MAX,120
 BAL_PITCH_MAX,10

Tools/autotest/balancebot.py

@@ -60,7 +60,6 @@ def TestWheelEncoder(self):
         '''make sure wheel encoders are generally working'''
         ex = None
         try:
-            self.set_parameter("ATC_BAL_SPD_FF", 0)
             self.set_parameter("WENC_TYPE", 10)
             self.set_parameter("AHRS_EKF_TYPE", 10)
             self.reboot_sitl()

Tools/autotest/default_params/balancebot.parm

@@ -1,7 +1,6 @@
-ATC_BAL_P 18.0
-ATC_BAL_I 80.0
-ATC_BAL_D 0.005
-ATC_BAL_SPD_FF 0.000000
+ATC_BAL_P 3.5
+ATC_BAL_I 5.0
+ATC_BAL_D 0.06
 ATC_SPEED_P 0.9
 ATC_SPEED_I 0.5
 AHRS_EKF_TYPE 10

libraries/APM_Control/AR_AttitudeControl.cpp

@@ -39,7 +39,10 @@
 #define AR_ATTCONTROL_PITCH_THR_D       0.03f
 #define AR_ATTCONTROL_PITCH_THR_IMAX    1.0f
 #define AR_ATTCONTROL_PITCH_THR_FILT    10.0f
-#define AR_ATTCONTROL_BAL_SPEED_FF      1.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
@@ -319,44 +322,44 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
 
     // @Param: _BAL_FLTT
     // @DisplayName: Pitch control Target filter frequency in Hz
-    // @Description: Target filter frequency in Hz
+    // @Description: Pitch control Target filter frequency in Hz
     // @Range: 0.000 100.000
     // @Increment: 0.1
     // @Units: Hz
     // @User: Standard
 
     // @Param: _BAL_FLTE
     // @DisplayName: Pitch control Error filter frequency in Hz
-    // @Description: Error filter frequency in Hz
+    // @Description: Pitch control Error filter frequency in Hz
     // @Range: 0.000 100.000
     // @Increment: 0.1
     // @Units: Hz
     // @User: Standard
 
     // @Param: _BAL_FLTD
     // @DisplayName: Pitch control Derivative term filter frequency in Hz
-    // @Description: Derivative filter frequency in Hz
+    // @Description: Pitch control Derivative filter frequency in Hz
     // @Range: 0.000 100.000
     // @Increment: 0.1
     // @Units: Hz
     // @User: Standard
 
     // @Param: _BAL_SMAX
     // @DisplayName: Pitch control slew rate limit
-    // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
+    // @Description: Pitch control upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
     // @Range: 0 200
     // @Increment: 0.5
     // @User: Advanced
 
     AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID),
 
-    // @Param: _BAL_SPD_FF
-    // @DisplayName: Pitch control feed forward from speed
-    // @Description: Pitch control feed forward from speed
-    // @Range: 0.0 10.0
+    // @Param: _BAL_PIT_FF
+    // @DisplayName: Pitch control feed forward from current pitch angle
+    // @Description: Pitch control feed forward from current pitch angle
+    // @Range: 0.0 1.0
     // @Increment: 0.01
     // @User: Standard
-    AP_GROUPINFO("_BAL_SPD_FF", 11, AR_AttitudeControl, _pitch_to_throttle_speed_ff, AR_ATTCONTROL_BAL_SPEED_FF),
+    AP_GROUPINFO("_BAL_PIT_FF", 11, AR_AttitudeControl, _pitch_to_throttle_ff, AR_ATTCONTROL_BAL_PITCH_FF),
 
     // @Param: _SAIL_P
     // @DisplayName: Sail Heel control P gain
@@ -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
 };
 
@@ -715,9 +734,10 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
 }
 
 // balancebot pitch to throttle controller
-// returns a throttle output from -100 to +100 given a desired pitch angle and vehicle's current speed (from wheel encoders)
-// desired_pitch is in radians, veh_speed_pct is supplied as a percentage (-100 to +100) of vehicle's top speed
-float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float vehicle_speed_pct, bool motor_limit_low, bool motor_limit_high, float dt)
+// 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, float pitch_max, bool motor_limit, float dt)
 {
     // sanity check dt
     dt = constrain_float(dt, 0.0f, 1.0f);
@@ -727,17 +747,50 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float
     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);
 
-    // add feed forward from speed
-    float output = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff;
-    output += _pitch_to_throttle_pid.update_all(desired_pitch, AP::ahrs().pitch, (motor_limit_low || motor_limit_high));
+    // initialise output to feed forward from current pitch angle
+    const float pitch_rad = AP::ahrs().pitch;
+    float output = sinf(pitch_rad) * _pitch_to_throttle_ff;
+
+    // add regular PID control
+    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

@@ -67,9 +67,14 @@ class AR_AttitudeControl {
     float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped);
 
     // balancebot pitch to throttle controller
-    // returns a throttle output from -100 to +100 given a desired pitch angle and vehicle's current speed (from wheel encoders)
-    // desired_pitch is in radians, veh_speed_pct is supplied as a percentage (-100 to +100) of vehicle's top speed
-    float get_throttle_out_from_pitch(float desired_pitch, float veh_speed_pct, bool motor_limit_low, bool motor_limit_high, float dt);
+    // 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, 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;
@@ -121,7 +126,9 @@ class AR_AttitudeControl {
     AC_PID   _steer_rate_pid;       // steering rate controller
     AC_PID   _throttle_speed_pid;   // throttle speed controller
     AC_PID   _pitch_to_throttle_pid;// balancebot pitch controller
-    AP_Float _pitch_to_throttle_speed_ff;   // balancebot feed forward from speed
+    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 @@ class AR_AttitudeControl {
     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