Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

fixed bug in updateClimbRateToAltitudeController and added logging

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2017-05-22 11:44:46 +02:00
parent 1c218f2301
commit f0a95f3d22
3 changed files with 8 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.h
View file

@ -49,5 +49,6 @@ typedef enum {
DEBUG_GYRO, DEBUG_GYRO,
DEBUG_NOTCH, DEBUG_NOTCH,
DEBUG_NAV_LANDING_DETECTOR, DEBUG_NAV_LANDING_DETECTOR,
DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

3
src/main/fc/cli.c
View file

@ -324,7 +324,8 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"NONE", "NONE",
"GYRO", "GYRO",
"NOTCH", "NOTCH",
"NAV_LANDING" "NAV_LANDING",
"FW_ALTITUDE"
}; };
#ifdef TELEMETRY_LTM #ifdef TELEMETRY_LTM

6
src/main/navigation/navigation.c
View file

@ -1871,8 +1871,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); 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)) { 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.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,
posControl.actualState.pos.V.Z + 500); posControl.actualState.pos.V.Z + 500);