Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: get_throttle_out_from_pitch accepts vehicle speed

Browse Source
This commit is contained in:
Randy Mackay 2018-08-08 14:42:09 +09:00
parent 5aa1286344
commit 106b8abab7
2 changed files with 23 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -210,7 +210,15 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
// @Units: Hz // @Units: Hz
// @User: Standard // @User: Standard
AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID), 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
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("_BAL_SPD_FF", 11, AR_AttitudeControl, _pitch_to_throttle_speed_ff, AR_ATTCONTROL_BAL_SPEED_FF),
AP_GROUPEND AP_GROUPEND
}; };
@ -484,10 +492,10 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle, dt); return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle, dt);
} }
// for balancebot // balancebot pitch to throttle controller
// return a throttle output from -1 to +1, given a desired pitch angle // 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 // 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, bool motor_limit_low, bool motor_limit_high, float dt) 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)
{ {
// sanity check dt // sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f); dt = constrain_float(dt, 0.0f, 1.0f);
@ -527,8 +535,11 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool
// get d // get d
const float d = _pitch_to_throttle_pid.get_d(); const float d = _pitch_to_throttle_pid.get_d();
// add feed forward from speed
const float spd_ff = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff;
// constrain and return final output // constrain and return final output
return (ff + p + i + d); return (ff + p + i + d + spd_ff);
} }
// get latest desired pitch in radians for reporting purposes // get latest desired pitch in radians for reporting purposes

10
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -25,6 +25,7 @@
#define AR_ATTCONTROL_PITCH_THR_D 0.03f #define AR_ATTCONTROL_PITCH_THR_D 0.03f
#define AR_ATTCONTROL_PITCH_THR_IMAX 1.0f #define AR_ATTCONTROL_PITCH_THR_IMAX 1.0f
#define AR_ATTCONTROL_PITCH_THR_FILT 10.0f #define AR_ATTCONTROL_PITCH_THR_FILT 10.0f
#define AR_ATTCONTROL_BAL_SPEED_FF 1.0f
#define AR_ATTCONTROL_DT 0.02f #define AR_ATTCONTROL_DT 0.02f
#define AR_ATTCONTROL_TIMEOUT_MS 200 #define AR_ATTCONTROL_TIMEOUT_MS 200
@ -89,10 +90,10 @@ public:
// return a throttle output from -1 to +1 to perform a controlled stop. stopped is set to true once stop has been completed // return a throttle output from -1 to +1 to perform a controlled stop. stopped is set to true once stop has been completed
float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped); float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped);
// for balancebot // balancebot pitch to throttle controller
// return a throttle output from -1 to +1 given a desired pitch angle // 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 // 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, bool motor_limit_low, bool motor_limit_high, float dt); float get_throttle_out_from_pitch(float desired_pitch, float veh_speed_pct, bool motor_limit_low, bool motor_limit_high, float dt);
// get latest desired pitch in radians for reporting purposes // get latest desired pitch in radians for reporting purposes
float get_desired_pitch() const; float get_desired_pitch() const;
@ -137,6 +138,7 @@ private:
AC_PID _steer_rate_pid; // steering rate controller AC_PID _steer_rate_pid; // steering rate controller
AC_PID _throttle_speed_pid; // throttle speed controller AC_PID _throttle_speed_pid; // throttle speed controller
AC_PID _pitch_to_throttle_pid;// balancebot pitch controller AC_PID _pitch_to_throttle_pid;// balancebot pitch controller
AP_Float _pitch_to_throttle_speed_ff; // balancebot feed forward from speed
AP_Float _throttle_accel_max; // speed/throttle control acceleration (and deceleration) maximum in m/s/s. 0 to disable limits 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 AP_Float _throttle_decel_max; // speed/throttle control deceleration maximum in m/s/s. 0 to use ATC_ACCEL_MAX for deceleration