Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: get_throttle_out_from_pitch uses motor limits

Browse Source 
This allows removing I-term build up from throttle hitting 100%
This commit is contained in:
Randy Mackay 2018-08-04 16:07:45 +09:00
parent 65b020eac3
commit d5ef3c2e0f
2 changed files with 20 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -487,20 +487,14 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
// for balancebot
// return a throttle output from -1 to +1, given a desired pitch angle
// desired_pitch is in radians
float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool armed, float dt)
float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool motor_limit_low, bool motor_limit_high, float dt)
{
// reset I term and return if disarmed
if (!armed){
_pitch_to_throttle_pid.reset_I();
return 0.0f;
}
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
// if not called recently, reset input filter
const uint32_t now = AP_HAL::millis();
if ((_balance_last_ms == 0) || ((now - _balance_last_ms) > (AR_ATTCONTROL_TIMEOUT_MS))) {
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();
}
@ -518,8 +512,23 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool
// pitch error is given as input to PID contoller
_pitch_to_throttle_pid.set_input_filter_all(pitch_error);
// return output of PID controller
return constrain_float(_pitch_to_throttle_pid.get_pid(), -1.0f, +1.0f);
// get feed-forward
const float ff = _pitch_to_throttle_pid.get_ff(desired_pitch);
// get p
const float p = _pitch_to_throttle_pid.get_p();
// get i unless non-skid-steering rover at low speed or steering output has hit a limit
float i = _pitch_to_throttle_pid.get_integrator();
if ((is_negative(pitch_error) && !motor_limit_low) || (is_positive(pitch_error) && !motor_limit_high)) {
i = _pitch_to_throttle_pid.get_i();
}
// get d
const float d = _pitch_to_throttle_pid.get_d();
// constrain and return final output
return (ff + p + i + d);
}
// get latest desired pitch in radians for reporting purposes

2
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -92,7 +92,7 @@ public:
// for balancebot
// return a throttle output from -1 to +1 given a desired pitch angle
// desired_pitch is in radians
float get_throttle_out_from_pitch(float desired_pitch, bool armed, float dt);
float get_throttle_out_from_pitch(float desired_pitch, bool motor_limit_low, bool motor_limit_high, float dt);
// get latest desired pitch in radians for reporting purposes
float get_desired_pitch() const;