fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:03:33 +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_yaw_controller.cpp
+9 -10
View File
@@ -48,10 +48,9 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint))) {
return _rate_setpoint;
return _body_rate_setpoint;
}
float constrained_roll;
@@ -82,18 +81,18 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
float euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
_euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = -sinf(ctl_data.roll) * euler_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * euler_rate_setpoint;
_body_rate_setpoint = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
}
if (!PX4_ISFINITE(_rate_setpoint)) {
if (!PX4_ISFINITE(_body_rate_setpoint)) {
PX4_WARN("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
_body_rate_setpoint = 0.0f;
}
return _rate_setpoint;
return _body_rate_setpoint;
}