Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: balancebot pitch feedforward uses current pitch angle

This commit is contained in:
Randy Mackay 2022-11-07 20:03:56 +09:00
parent 30b2e03a85
commit 3d89bd9f14
2 changed files with 19 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -39,7 +39,7 @@
#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_DT 0.02f
#define AR_ATTCONTROL_TIMEOUT_MS 200
#define AR_ATTCONTROL_HEEL_SAIL_P 1.0f
@ -350,13 +350,13 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
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
@ -715,9 +715,9 @@ 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)
// 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)
{
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
@ -733,9 +733,12 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float
// 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();
// constrain and return final output

8
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -67,9 +67,9 @@ public:
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)
// 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);
// get latest desired pitch in radians for reporting purposes
float get_desired_pitch() const;
@ -121,7 +121,7 @@ private:
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 _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