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

FW: Remove limitation of minimum ground speed for navigation

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-03-24 10:55:58 +10:00
parent 8860c9034b
commit c801ce80fa
1 changed files with 10 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/flight/navigation_rewrite_fixedwing.c
View file

@ -385,12 +385,15 @@ void resetFixedWingHeadingController(void)
magHold = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw); magHold = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw);
} }
//#define NAV_FW_LIMIT_MIN_FLY_VELOCITY
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime) void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime)
{ {
if (navStateFlags & NAV_CTL_EMERG) { if (navStateFlags & NAV_CTL_EMERG) {
applyFixedWingEmergencyLandingController(); applyFixedWingEmergencyLandingController();
} }
else { else {
#ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY
// Don't apply anything if ground speed is too low (<3m/s) // Don't apply anything if ground speed is too low (<3m/s)
float forwardVelocitySquared = sq(posControl.actualState.vel.V.X) + sq(posControl.actualState.vel.V.Y); float forwardVelocitySquared = sq(posControl.actualState.vel.V.X) + sq(posControl.actualState.vel.V.Y);
if (forwardVelocitySquared > (300 * 300)) { if (forwardVelocitySquared > (300 * 300)) {
@ -405,6 +408,13 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
posControl.rcAdjustment[ROLL] = 0; posControl.rcAdjustment[ROLL] = 0;
posControl.rcAdjustment[THROTTLE] = 0; posControl.rcAdjustment[THROTTLE] = 0;
} }
#else
if (navStateFlags & NAV_CTL_ALT)
applyFixedWingAltitudeController(currentTime);
if (navStateFlags & NAV_CTL_POS)
applyFixedWingPositionController(currentTime);
#endif
//if (navStateFlags & NAV_CTL_YAW) //if (navStateFlags & NAV_CTL_YAW)
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS))