mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 08:45:31 +03:00
fixed bug in updateClimbRateToAltitudeController and added logging
This commit is contained in:
parent
1c218f2301
commit
f0a95f3d22
3 changed files with 8 additions and 2 deletions
|
@ -1871,8 +1871,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
if (STATE(FIXED_WING)) {
|
||||
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
|
||||
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
|
||||
|
||||
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 0, desiredClimbRate);
|
||||
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000);
|
||||
|
||||
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
posControl.desiredState.pos.V.Z = desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.V.Z += desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.V.Z = constrainf(posControl.desiredState.pos.V.Z, // FIXME: calculate sanity limits in a smarter way
|
||||
posControl.actualState.pos.V.Z - 500,
|
||||
posControl.actualState.pos.V.Z + 500);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue