fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 15:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

l1 position controller: make sure that a NAN roll setpoint

Browse Source 
does not keep the roll setpoint state at NAN forever

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2018-08-20 21:29:42 +02:00 committed by Lorenz Meier
parent 34cd1366b7
commit aa134d5a61
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
l1/ecl_l1_pos_controller.cpp
View File

@ -51,16 +51,14 @@ void ECL_L1_Pos_Controller::update_roll_setpoint()
float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);
// no slew rate limiting active
if (_dt <= 0.0f || _roll_slew_rate <= 0.0f) {
_roll_setpoint = roll_new;
return;
if (_dt > 0.0f && _roll_slew_rate > 0.0f) {
// slew rate limiting active
roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt);
}
// slew rate limiting active
roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt);
_roll_setpoint = roll_new;
if (ISFINITE(roll_new)) {
_roll_setpoint = roll_new;
}
}