AR_AttitudeControl: balancebot gets pitch limit protection

This commit is contained in:
Randy Mackay 2022-11-10 12:00:36 +09:00
parent ef7bb2d4e0
commit 71ae3f0a13
2 changed files with 63 additions and 3 deletions

View File

@ -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;
}

View File

@ -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