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

yaw controller: computed desired yaw rate based on coordinated turn constraint

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 4057defa53
commit f300877db4
1 changed files with 17 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
attitude_fw/ecl_yaw_controller.cpp
+17 -12
View File
@@ -89,23 +89,28 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
return _rate_setpoint; return _rate_setpoint;
} }
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ float constrained_roll;
_rate_setpoint = 0.0f; /* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v + } else {
ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) { // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) + //note: the ranges are extended by 10 deg here to avoid numeric resolution effects
ctl_data.speed_body_w * sinf(ctl_data.pitch)); if (ctl_data.roll > 0.0f) {
/* right hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
if (fabsf(denumerator) > FLT_EPSILON) { } else {
_rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint + /* left hemisphere */
9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) + constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f));
ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) /
denumerator;
} }
} }
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
/* limit the rate */ //XXX: move to body angluar rates /* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) { if (_max_rate > 0.01f) {
@@ -208,4 +213,4 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
return control_bodyrate(ctl_data); return control_bodyrate(ctl_data);
} }