dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

TECS: make sure to constrain pitch to current min/max pitch 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-12-07 10:39:10 +01:00
parent 5928d7f067
commit 5d433ddef7
1 changed files with 1 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/tecs/TECS.cpp
View File

@ -419,6 +419,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific
const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON);
_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
_pitch_setpoint + pitch_increment);
_pitch_setpoint = constrain(_pitch_setpoint, param.pitch_min, param.pitch_max);
//Debug Output
_debug_output.energy_balance_rate_estimate = seb_rate.estimate;