fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 04:53:34 +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-02-12 11:40:14 +01:00
committed by Lorenz Meier
parent c4cc02b3f2
commit e1f9874c82
1 changed files with 7 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
attitude_fw/ecl_wheel_controller.cpp
+7 -12
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 //xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max); float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */ /* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
_rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained);
integrator_constrained;
/*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
(double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/
return math::constrain(_last_output, -1.0f, 1.0f); 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; return _rate_setpoint;
} }
/* Calculate the error */ /* Calculate the error */
float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
/* shortest angle (wrap around) */ /* 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; yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
/*warnx("yaw_error: %.4f", (double)yaw_error);*/
/* Apply P controller: rate setpoint from current error and time constant */ /* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = yaw_error / _tc; _rate_setpoint = yaw_error / _tc;