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

FW attitude controller: constrain rates correctly

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-08-12 14:17:43 +02:00
parent 15c95a7b6a
commit cd080289c6
6 changed files with 9 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/fw_att_control/ecl_controller.cpp
View File

@ -103,11 +103,6 @@ void ECL_Controller::set_max_rate(float max_rate)
_max_rate = max_rate;
}
void ECL_Controller::set_bodyrate_setpoint(float rate)
{
_body_rate_setpoint = math::constrain(rate, -_max_rate, _max_rate);
}
float ECL_Controller::get_euler_rate_setpoint()
{
return _euler_rate_setpoint;

1
src/modules/fw_att_control/ecl_controller.h
View File

@ -87,7 +87,6 @@ public:
void set_k_ff(float k_ff);
void set_integrator_max(float max);
void set_max_rate(float max_rate);
void set_bodyrate_setpoint(float rate);
/* Getters */
float get_euler_rate_setpoint();

5
src/modules/fw_att_control/ecl_pitch_controller.cpp
View File

@ -62,8 +62,9 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
_euler_rate_setpoint = pitch_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_body_rate_setpoint = cosf(ctl_data.roll) * _euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate);
return _body_rate_setpoint;
}

5
src/modules/fw_att_control/ecl_pitch_controller.h
View File

@ -73,11 +73,6 @@ public:
_max_rate_neg = max_rate_neg;
}
void set_bodyrate_setpoint(float rate)
{
_body_rate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate);
}
protected:
float _max_rate_neg{0.0f};
};

4
src/modules/fw_att_control/ecl_roll_controller.cpp
View File

@ -61,7 +61,9 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData
_euler_rate_setpoint = roll_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_body_rate_setpoint = _euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.euler_yaw_rate_setpoint;
const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) *
ctl_data.euler_yaw_rate_setpoint;
_body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);
return _body_rate_setpoint;
}

5
src/modules/fw_att_control/ecl_yaw_controller.cpp
View File

@ -85,8 +85,9 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
/* Transform setpoint to body angular rates (jacobian) */
_body_rate_setpoint = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
_body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate);
}
if (!PX4_ISFINITE(_body_rate_setpoint)) {