Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 06:15:14 +03:00
Code Issues Wiki Activity

Minor fixes

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-08-06 23:19:35 +10:00
parent 9524fd7f7e
commit 89e86bbc8b
1 changed files with 3 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -121,14 +121,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
const float pitchGainInv = GRAVITY_MSS; // GRAVITY_MSS * airspeed; const float pitchGainInv = GRAVITY_MSS; // GRAVITY_MSS * airspeed;
// Here we use negative values for dive for better clarity // Here we use negative values for dive for better clarity
int maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
int minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// PID controller to translate energy balance error [J] into pitch angle [decideg] // PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0) / pitchGainInv; float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0) / pitchGainInv;
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
// Calculate climb angle ( >0 - climb, <0 - dive) // Reconstrain pitch angle ( >0 - climb, <0 - dive)
targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
posControl.rcAdjustment[PITCH] = targetPitchAngle; posControl.rcAdjustment[PITCH] = targetPitchAngle;
} }