fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

wheel controller: fix computation and scaling of integrator

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2017-01-26 14:06:14 +01:00 committed by Lorenz Meier
parent c4cc02b3f2
commit e1f9874c82
1 changed files with 7 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
attitude_fw/ecl_wheel_controller.cpp
View File

@ -102,13 +102,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
_rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
integrator_constrained;
/*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
(double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/
/* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@ -122,11 +118,10 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
return _rate_setpoint;
}
/* Calculate the error */
float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
/* shortest angle (wrap around) */
yaw_error = (float)fmodf((float)fmodf((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
/*warnx("yaw_error: %.4f", (double)yaw_error);*/
/* Calculate the error */
float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
/* shortest angle (wrap around) */
yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = yaw_error / _tc;