Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: re-order pitch control to match other similar methods

Browse Source
This commit is contained in:
Randy Mackay 2018-08-04 15:52:38 +09:00
parent f6cc934678
commit 65b020eac3
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -489,7 +489,6 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
// desired_pitch is in radians
float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool armed, float dt)
{
// reset I term and return if disarmed
if (!armed){
_pitch_to_throttle_pid.reset_I();
@ -499,25 +498,26 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
const uint32_t now = AP_HAL::millis();
// 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))) {
_pitch_to_throttle_pid.reset_filter();
} else {
_pitch_to_throttle_pid.set_dt(dt);
_pitch_to_throttle_pid.reset_I();
}
_balance_last_ms = now;
// calculate pitch error
const float pitch_error = desired_pitch - _ahrs.pitch;
// pitch error is given as input to PID contoller
_pitch_to_throttle_pid.set_input_filter_all(pitch_error);
// set PID's dt
_pitch_to_throttle_pid.set_dt(dt);
// record desired speed for logging purposes only
_pitch_to_throttle_pid.set_desired_rate(desired_pitch);
// 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);
}