fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW attitude controller: improve readability and fix euler rate sp vs. body rate sp

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-19 09:40:48 +02:00
parent e0a998e6ad
commit 15c95a7b6a
8 changed files with 64 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/ecl_pitch_controller.cpp
+6 -5
View File
@@ -49,20 +49,21 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed))) {
return _rate_setpoint;
return _body_rate_setpoint;
}
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
float euler_rate_setpoint = pitch_error / _tc;
_euler_rate_setpoint = pitch_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = cosf(ctl_data.roll) * euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
_body_rate_setpoint = cosf(ctl_data.roll) * _euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
return _rate_setpoint;
return _body_rate_setpoint;
}