src/main/navigation/navigation_fixedwing.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #include "build/build_config.h"
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/filter.h"
  30
  31 #include "drivers/time.h"
  32
  33 #include "sensors/sensors.h"
  34 #include "sensors/acceleration.h"
  35 #include "sensors/boardalignment.h"
  36 #include "sensors/gyro.h"
  37 #include "sensors/pitotmeter.h"
  38
  39 #include "flight/pid.h"
  40 #include "flight/imu.h"
  41 #include "flight/mixer.h"
  42 #include "flight/mixer_profile.h"
  43
  44 #include "fc/config.h"
  45 #include "fc/controlrate_profile.h"
  46 #include "fc/rc_controls.h"
  47 #include "fc/rc_modes.h"
  48 #include "fc/runtime_config.h"
  49
  50 #include "navigation/navigation.h"
  51 #include "navigation/navigation_private.h"
  52
  53 #include "programming/logic_condition.h"
  54
  55 #include "rx/rx.h"
  56
  57 #include "sensors/battery.h"
  58
  59 // Base frequencies for smoothing pitch and roll
  60 #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f
  61 #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ     10.0f
  62
  63 // If we are going slower than the minimum ground speed (navConfig()->general.min_ground_speed) - boost throttle to fight against the wind
  64 #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN        1.5f
  65
  66 // If this is enabled navigation won't be applied if velocity is below 3 m/s
  67 //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY
  68
  69 static bool isPitchAdjustmentValid = false;
  70 static bool isRollAdjustmentValid = false;
  71 static bool isYawAdjustmentValid = false;
  72 static float throttleSpeedAdjustment = 0;
  73 static bool isAutoThrottleManuallyIncreased = false;
  74 static float navCrossTrackError;
  75 static int8_t loiterDirYaw = 1;
  76 bool needToCalculateCircularLoiter;
  77
  78 // Calculates the cutoff frequency for smoothing out roll/pitch commands
  79 // control_smoothness valid range from 0 to 9
  80 // resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
  81 static float getSmoothnessCutoffFreq(float baseFreq)
  82 {
  83     uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
  84     return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
  85 }
  86
  87 // Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection
  88 // pitch_to_throttle_smooth valid range from 0 to 9
  89 // resulting cutoff_freq ranging from baseFreq downwards to ~0.01Hz
  90 static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq)
  91 {
  92     uint16_t smoothness = 10 - navConfig()->fw.pitch_to_throttle_smooth;
  93     return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f;
  94 }
  95
  96 /*-----------------------------------------------------------
  97  * Altitude controller
  98  *-----------------------------------------------------------*/
  99 void setupFixedWingAltitudeController(void)
 100 {
 101     // TODO
 102 }
 103
 104 void resetFixedWingAltitudeController(void)
 105 {
 106     navPidReset(&posControl.pids.fw_alt);
 107     posControl.rcAdjustment[PITCH] = 0;
 108     isPitchAdjustmentValid = false;
 109     throttleSpeedAdjustment = 0;
 110 }
 111
 112 bool adjustFixedWingAltitudeFromRCInput(void)
 113 {
 114     int16_t rcAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband, -500, 500);
 115
 116     if (rcAdjustment) {
 117         // set velocity proportional to stick movement
 118         float rcClimbRate = -rcAdjustment * navConfig()->fw.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
 119         updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
 120         return true;
 121     }
 122     else {
 123         // Adjusting finished - reset desired position to stay exactly where pilot released the stick
 124         if (posControl.flags.isAdjustingAltitude) {
 125             updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
 126         }
 127         return false;
 128     }
 129 }
 130
 131 // Position to velocity controller for Z axis
 132 static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 133 {
 134     static pt1Filter_t velzFilterState;
 135
 136     float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
 137
 138     // Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention)
 139     if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) {
 140         desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate;
 141     }
 142
 143     // Here we use negative values for dive for better clarity
 144     const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
 145     const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
 146
 147     // PID controller to translate desired climb rate error into pitch angle [decideg]
 148     float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;
 149     float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
 150
 151     // Apply low-pass filter to prevent rapid correction
 152     targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 153
 154     // Reconstrain pitch angle (> 0 - climb, < 0 - dive)
 155     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
 156     posControl.rcAdjustment[PITCH] = targetPitchAngle;
 157
 158     posControl.desiredState.vel.z = desiredClimbRate;
 159     navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767);
 160 }
 161
 162 void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
 163 {
 164     static timeUs_t previousTimePositionUpdate = 0;         // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
 165
 166     if ((posControl.flags.estAltStatus >= EST_USABLE)) {
 167         if (posControl.flags.verticalPositionDataNew) {
 168             const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
 169             previousTimePositionUpdate = currentTimeUs;
 170
 171             // Check if last correction was not too long ago
 172             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
 173                 updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
 174             }
 175             else {
 176                 // Position update has not occurred in time (first iteration or glitch), reset altitude controller
 177                 resetFixedWingAltitudeController();
 178             }
 179
 180             // Indicate that information is no longer usable
 181             posControl.flags.verticalPositionDataConsumed = true;
 182         }
 183
 184         isPitchAdjustmentValid = true;
 185     }
 186     else {
 187         // No valid altitude sensor data, don't adjust pitch automatically, rcCommand[PITCH] is passed through to PID controller
 188         isPitchAdjustmentValid = false;
 189     }
 190 }
 191
 192 /*-----------------------------------------------------------
 193  * Adjusts desired heading from pilot's input
 194  *-----------------------------------------------------------*/
 195 bool adjustFixedWingHeadingFromRCInput(void)
 196 {
 197     if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
 198         return true;
 199     }
 200
 201     return false;
 202 }
 203
 204 /*-----------------------------------------------------------
 205  * XY-position controller for multicopter aircraft
 206  *-----------------------------------------------------------*/
 207 static fpVector3_t virtualDesiredPosition;
 208 static pt1Filter_t fwPosControllerCorrectionFilterState;
 209
 210 /*
 211  * TODO Currently this function resets both FixedWing and Rover & Boat position controller
 212  */
 213 void resetFixedWingPositionController(void)
 214 {
 215     virtualDesiredPosition.x = 0;
 216     virtualDesiredPosition.y = 0;
 217     virtualDesiredPosition.z = 0;
 218
 219     navPidReset(&posControl.pids.fw_nav);
 220     navPidReset(&posControl.pids.fw_heading);
 221     posControl.rcAdjustment[ROLL] = 0;
 222     posControl.rcAdjustment[YAW] = 0;
 223     isRollAdjustmentValid = false;
 224     isYawAdjustmentValid = false;
 225
 226     pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
 227 }
 228
 229 static int8_t loiterDirection(void) {
 230     int8_t dir = 1; //NAV_LOITER_RIGHT
 231
 232     if (navConfig()->fw.loiter_direction == NAV_LOITER_LEFT) {
 233         dir = -1;
 234     }
 235
 236     if (navConfig()->fw.loiter_direction == NAV_LOITER_YAW) {
 237
 238         if (rcCommand[YAW] < -250) {
 239             loiterDirYaw = 1; //RIGHT //yaw is contrariwise
 240         }
 241
 242         if (rcCommand[YAW] > 250) {
 243             loiterDirYaw = -1; //LEFT  //see annexCode in fc_core.c
 244         }
 245
 246         dir = loiterDirYaw;
 247     }
 248
 249     if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) {
 250         dir *= -1;
 251     }
 252
 253     return dir;
 254 }
 255
 256 static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 257 {
 258     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) {
 259         return;
 260     }
 261
 262     float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
 263     float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
 264
 265     float distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
 266
 267     // Limit minimum forward velocity to 1 m/s
 268     float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
 269
 270     uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
 271     fpVector3_t loiterCenterPos = posControl.desiredState.pos;
 272     int8_t loiterTurnDirection = loiterDirection();
 273
 274     // Detemine if a circular loiter is required.
 275     // For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
 276     #define TAN_15DEG    0.26795f
 277
 278     bool loiterApproachActive = isNavHoldPositionActive() &&
 279                                 distanceToActualTarget <= (navLoiterRadius / TAN_15DEG) &&
 280                                 distanceToActualTarget > 50.0f;
 281     needToCalculateCircularLoiter = loiterApproachActive || (navGetCurrentStateFlags() & NAV_CTL_HOLD);
 282
 283     //if vtol landing is required, fly straight to homepoint
 284     if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){
 285         needToCalculateCircularLoiter = false;
 286     }
 287
 288     /* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP
 289      * Works for turns > 30 degs and < 160 degs.
 290      * Option 1 switches to loiter path around waypoint using navLoiterRadius.
 291      * Loiter centered on point inside turn at required distance from waypoint and
 292      * on a bearing midway between current and next waypoint course bearings.
 293      * Option 2 simply uses a normal turn once the turn initiation point is reached */
 294     int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle);
 295     posControl.flags.wpTurnSmoothingActive = false;
 296     if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
 297         // turnStartFactor adjusts start of loiter based on turn angle
 298         float turnStartFactor;
 299         if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) {     // passes through WP
 300             turnStartFactor = waypointTurnAngle / 6000.0f;
 301         } else {    // // cut inside turn missing WP
 302             turnStartFactor = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 1.0f, 2.0f);
 303         }
 304         // velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time
 305         if (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) {
 306             if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) {
 307                 int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.bearing + 18000);
 308                 loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
 309                 loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
 310
 311                 posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
 312                 posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
 313
 314                 // turn direction to next waypoint
 315                 loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1;  // 1 = right
 316
 317                 needToCalculateCircularLoiter = true;
 318             }
 319             posControl.flags.wpTurnSmoothingActive = true;
 320         }
 321     }
 322
 323     // We are closing in on a waypoint, calculate circular loiter if required
 324     if (needToCalculateCircularLoiter) {
 325         float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterTurnDirection * 45.0f);
 326         float loiterTargetX = loiterCenterPos.x + navLoiterRadius * cos_approx(loiterAngle);
 327         float loiterTargetY = loiterCenterPos.y + navLoiterRadius * sin_approx(loiterAngle);
 328
 329         // We have temporary loiter target. Recalculate distance and position error
 330         posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
 331         posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
 332         distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
 333     }
 334
 335     // Calculate virtual waypoint
 336     virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget);
 337     virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY * (trackingDistance / distanceToActualTarget);
 338
 339     // Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
 340     if (posControl.flags.isAdjustingPosition) {
 341         int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
 342
 343         if (rcRollAdjustment) {
 344             float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod;
 345
 346             // Rotate this target shift from body frame to to earth frame and apply to position target
 347             virtualDesiredPosition.x += -rcShiftY * posControl.actualState.sinYaw;
 348             virtualDesiredPosition.y +=  rcShiftY * posControl.actualState.cosYaw;
 349         }
 350     }
 351 }
 352
 353 bool adjustFixedWingPositionFromRCInput(void)
 354 {
 355     int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
 356     return (rcRollAdjustment);
 357 }
 358
 359 float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing) {
 360     static float limit = 0.0f;
 361
 362     if (limit == 0.0f) {
 363         limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
 364     }
 365
 366     const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
 367
 368     const float yawAdjustment = navPidApply2(
 369         &posControl.pids.fw_heading,
 370         0,
 371         applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
 372         US2S(deltaMicros),
 373         -limit,
 374         limit,
 375         yawPidFlags
 376         ) * 0.01f;
 377
 378     DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
 379     DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
 380     DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
 381     DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
 382     DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
 383
 384     return yawAdjustment;
 385 }
 386
 387 static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
 388 {
 389     static timeUs_t previousTimeMonitoringUpdate;
 390     static int32_t previousHeadingError;
 391     static bool errorIsDecreasing;
 392     static bool forceTurnDirection = false;
 393     int32_t virtualTargetBearing;
 394
 395     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) {
 396         virtualTargetBearing = posControl.desiredState.yaw;
 397     } else {
 398         // We have virtual position target, calculate heading error
 399         virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
 400     }
 401
 402     /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
 403     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
 404         // courseVirtualCorrection initially used to determine current position relative to course line for later use
 405         int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
 406         navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
 407
 408         // tracking only active when certain distance and heading conditions are met
 409         if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
 410             int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog);
 411
 412             // captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
 413             // Closing distance threashold based on speed and an assumed 1 second response time.
 414             float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
 415
 416             // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
 417             // initial courseCorrectionFactor based on distance to course line
 418             float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
 419             courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
 420
 421             // course heading alignment factor
 422             float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f);
 423             courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
 424
 425             // final courseCorrectionFactor combining distance and heading factors
 426             courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
 427
 428             // final courseVirtualCorrection value
 429             courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
 430             virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection);
 431         }
 432     }
 433
 434     /*
 435      * Calculate NAV heading error
 436      * Units are centidegrees
 437      */
 438     int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
 439
 440     // Forced turn direction
 441     // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
 442     if (ABS(navHeadingError) > 17000) {
 443         forceTurnDirection = true;
 444     }
 445     else if (ABS(navHeadingError) < 9000 && forceTurnDirection) {
 446         forceTurnDirection = false;
 447     }
 448
 449     // If forced turn direction flag is enabled we fix the sign of the direction
 450     if (forceTurnDirection) {
 451         navHeadingError = loiterDirection() * ABS(navHeadingError);
 452     }
 453
 454     // Slow error monitoring (2Hz rate)
 455     if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
 456         // Check if error is decreasing over time
 457         errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
 458
 459         // Save values for next iteration
 460         previousHeadingError = navHeadingError;
 461         previousTimeMonitoringUpdate = currentTimeUs;
 462     }
 463
 464     // Only allow PID integrator to shrink if error is decreasing over time
 465     const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
 466
 467     // Input error in (deg*100), output roll angle (deg*100)
 468     float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros),
 469                                        -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
 470                                         DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
 471                                         pidFlags);
 472
 473     // Apply low-pass filter to prevent rapid correction
 474     rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 475
 476     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
 477     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
 478
 479     /*
 480      * Yaw adjustment
 481      * It is working in relative mode and we aim to keep error at zero
 482      */
 483     if (STATE(FW_HEADING_USE_YAW)) {
 484         posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
 485     } else {
 486         posControl.rcAdjustment[YAW] = 0;
 487     }
 488 }
 489
 490 void applyFixedWingPositionController(timeUs_t currentTimeUs)
 491 {
 492     static timeUs_t previousTimePositionUpdate = 0;         // Occurs @ GPS update rate
 493
 494     // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
 495     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
 496         // If we have new position - update velocity and acceleration controllers
 497         if (posControl.flags.horizontalPositionDataNew) {
 498             const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
 499             previousTimePositionUpdate = currentTimeUs;
 500
 501             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
 502                 // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
 503                 // Account for pilot's roll input (move position target left/right at max of max_manual_speed)
 504                 // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
 505                 // FIXME: verify the above
 506                 calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
 507                 updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
 508                 needToCalculateCircularLoiter = false;
 509             }
 510             else {
 511                 // Position update has not occurred in time (first iteration or glitch), reset altitude controller
 512                 resetFixedWingPositionController();
 513             }
 514
 515             // Indicate that information is no longer usable
 516             posControl.flags.horizontalPositionDataConsumed = true;
 517         }
 518
 519         isRollAdjustmentValid = true;
 520         isYawAdjustmentValid = true;
 521     }
 522     else {
 523         // No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
 524         isRollAdjustmentValid = false;
 525         isYawAdjustmentValid = false;
 526     }
 527 }
 528
 529 int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
 530 {
 531     static timeUs_t previousTimePositionUpdate = 0;         // Occurs @ GPS update rate
 532
 533     // Apply controller only if position source is valid
 534     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
 535         // If we have new position - update velocity and acceleration controllers
 536         if (posControl.flags.horizontalPositionDataNew) {
 537             const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
 538             previousTimePositionUpdate = currentTimeUs;
 539
 540             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
 541                 float velThrottleBoost = ((navConfig()->general.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
 542
 543                 // If we are in the deadband of 50cm/s - don't update speed boost
 544                 if (fabsf(posControl.actualState.velXY - (navConfig()->general.min_ground_speed * 100.0f)) > 50) {
 545                     throttleSpeedAdjustment += velThrottleBoost;
 546                 }
 547
 548                 throttleSpeedAdjustment = constrainf(throttleSpeedAdjustment, 0.0f, 500.0f);
 549             }
 550             else {
 551                 // Position update has not occurred in time (first iteration or glitch), reset altitude controller
 552                 throttleSpeedAdjustment = 0;
 553             }
 554
 555             // Indicate that information is no longer usable
 556             posControl.flags.horizontalPositionDataConsumed = true;
 557         }
 558     }
 559     else {
 560         // No valid pos sensor data, we can't calculate speed
 561         throttleSpeedAdjustment = 0;
 562     }
 563
 564     return throttleSpeedAdjustment;
 565 }
 566
 567 int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
 568 {
 569     static timeUs_t previousTimePitchToThrCorr = 0;
 570     const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
 571     previousTimePitchToThrCorr = currentTimeUs;
 572
 573     static pt1Filter_t pitchToThrFilterState;
 574
 575     // Apply low-pass filter to pitch angle to smooth throttle correction
 576     int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
 577
 578     int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle;
 579
 580 #ifdef USE_FW_AUTOLAND
 581     if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) {
 582         pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f;
 583     }
 584 #endif
 585
 586     if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
 587         // Unfiltered throttle correction outside of pitch deadband
 588         return DECIDEGREES_TO_DEGREES(pitch) * pitchToThrottle;
 589     }
 590     else {
 591         // Filtered throttle correction inside of pitch deadband
 592         return DECIDEGREES_TO_DEGREES(filteredPitch) * pitchToThrottle;
 593     }
 594 }
 595
 596 void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 597 {
 598     int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
 599     int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
 600
 601     if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
 602         // ROLL >0 right, <0 left
 603         int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
 604         rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
 605     }
 606
 607     if (isYawAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
 608         rcCommand[YAW] = posControl.rcAdjustment[YAW];
 609     }
 610
 611     if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
 612         // PITCH >0 dive, <0 climb
 613         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
 614         rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
 615         int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
 616
 617         if (navStateFlags & NAV_CTL_LAND) {
 618         // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
 619             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
 620         } else {
 621             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
 622         }
 623
 624         // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
 625         if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
 626             throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs);
 627             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
 628         }
 629
 630         uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
 631
 632         // Manual throttle increase
 633         if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 634             if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
 635                 correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
 636             } else {
 637                 correctedThrottleValue = getMaxThrottle();
 638             }
 639             isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
 640         } else {
 641             isAutoThrottleManuallyIncreased = false;
 642         }
 643
 644         rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
 645     }
 646
 647 #ifdef USE_FW_AUTOLAND
 648     // Advanced autoland
 649     if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) {
 650         // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
 651         ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 652
 653         if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) {
 654             rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->glidePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
 655         }
 656
 657         if (posControl.navState == NAV_STATE_FW_LANDING_FLARE) {
 658             rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
 659         }
 660     }
 661 #endif
 662     // "Traditional" landing as fallback option
 663     if (navStateFlags & NAV_CTL_LAND) {
 664         int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
 665
 666         if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) ||
 667            (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) {
 668
 669             // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
 670             ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 671
 672             // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
 673             rcCommand[ROLL] = 0;
 674
 675             // Stabilize PITCH angle into shallow dive as specified by the nav_fw_land_dive_angle setting (default value is 2 - defined in navigation.c).
 676             rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
 677         }
 678     }
 679 }
 680
 681 bool isFixedWingAutoThrottleManuallyIncreased(void)
 682 {
 683     return isAutoThrottleManuallyIncreased;
 684 }
 685
 686 bool isFixedWingFlying(void)
 687 {
 688     float airspeed = 0.0f;
 689 #ifdef USE_PITOT
 690     if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
 691         airspeed = getAirspeedEstimate();
 692     }
 693 #endif
 694     bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
 695     bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f;
 696     bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING;
 697
 698     return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition;
 699 }
 700
 701 /*-----------------------------------------------------------
 702  * FixedWing land detector
 703  *-----------------------------------------------------------*/
 704 bool isFixedWingLandingDetected(void)
 705 {
 706     DEBUG_SET(DEBUG_LANDING, 4, 0);
 707     static bool fixAxisCheck = false;
 708
 709     // Basic condition to start looking for landing
 710     bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
 711                           || FLIGHT_MODE(FAILSAFE_MODE)
 712                           || FLIGHT_MODE(NAV_FW_AUTOLAND)
 713                           || (!navigationIsControllingThrottle() && throttleStickIsLow());
 714
 715     if (!startCondition || posControl.flags.resetLandingDetector) {
 716         return fixAxisCheck = posControl.flags.resetLandingDetector = false;
 717     }
 718     DEBUG_SET(DEBUG_LANDING, 4, 1);
 719
 720     static timeMs_t fwLandingTimerStartAt;
 721     static int16_t fwLandSetRollDatum;
 722     static int16_t fwLandSetPitchDatum;
 723     const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f;
 724
 725     const timeMs_t currentTimeMs = millis();
 726
 727     // Check horizontal and vertical velocities are low (cm/s)
 728     bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
 729                         ( posControl.actualState.velXY < (100.0f * sensitivity));
 730     // Check angular rates are low (degs/s)
 731     bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity);
 732     DEBUG_SET(DEBUG_LANDING, 2, velCondition);
 733     DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
 734
 735     if (velCondition && gyroCondition){
 736         DEBUG_SET(DEBUG_LANDING, 4, 2);
 737         DEBUG_SET(DEBUG_LANDING, 5, fixAxisCheck);
 738         if (!fixAxisCheck) {        // capture roll and pitch angles to be used as datums to check for absolute change
 739             fwLandSetRollDatum = attitude.values.roll;  //0.1 deg increments
 740             fwLandSetPitchDatum = attitude.values.pitch;
 741             fixAxisCheck = true;
 742             fwLandingTimerStartAt = currentTimeMs;
 743         } else {
 744             const uint8_t angleLimit = 5 * sensitivity;
 745             bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < angleLimit;
 746             bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < angleLimit;
 747             DEBUG_SET(DEBUG_LANDING, 6, isRollAxisStatic);
 748             DEBUG_SET(DEBUG_LANDING, 7, isPitchAxisStatic);
 749             if (isRollAxisStatic && isPitchAxisStatic) {
 750                 /* Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
 751                  * Conditions need to be held for fixed safety time + optional extra delay.
 752                  * Fixed time increased if velocities invalid to provide extra safety margin against false triggers */
 753                 const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE || posControl.flags.estVelStatus == EST_NONE ? 5000 : 1000;
 754                 timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay;
 755                 return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay;
 756             } else {
 757                 fixAxisCheck = false;
 758             }
 759         }
 760     }
 761     return false;
 762 }
 763
 764 /*-----------------------------------------------------------
 765  * FixedWing emergency landing
 766  *-----------------------------------------------------------*/
 767 void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
 768 {
 769     rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
 770
 771     if (posControl.flags.estAltStatus >= EST_USABLE) {
 772         // target min descent rate at distance 2 x emerg descent rate above takeoff altitude
 773         updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
 774         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 775
 776         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
 777         rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
 778     } else {
 779         rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
 780     }
 781
 782     if (posControl.flags.estPosStatus >= EST_USABLE) {  // Hold position if possible
 783         applyFixedWingPositionController(currentTimeUs);
 784         int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL],
 785                                             -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle),
 786                                             DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
 787         rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
 788         rcCommand[YAW] = 0;
 789     } else {
 790         rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
 791         rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
 792     }
 793 }
 794
 795 /*-----------------------------------------------------------
 796  * Calculate loiter target based on current position and velocity
 797  *-----------------------------------------------------------*/
 798 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
 799 {
 800     // TODO: stub, this should account for velocity and target loiter radius
 801     *pos = navGetCurrentActualPositionAndVelocity()->pos;
 802 }
 803
 804 void resetFixedWingHeadingController(void)
 805 {
 806     updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
 807 }
 808
 809 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 810 {
 811     if (navStateFlags & NAV_CTL_LAUNCH) {
 812         applyFixedWingLaunchController(currentTimeUs);
 813     }
 814     else if (navStateFlags & NAV_CTL_EMERG) {
 815         applyFixedWingEmergencyLandingController(currentTimeUs);
 816     }
 817     else {
 818 #ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY
 819         // Don't apply anything if ground speed is too low (<3m/s)
 820         if (posControl.actualState.velXY > 300) {
 821 #else
 822         if (true) {
 823 #endif
 824             if (navStateFlags & NAV_CTL_ALT) {
 825                 if (getMotorStatus() == MOTOR_STOPPED_USER || FLIGHT_MODE(SOARING_MODE)) {
 826                     // Motor has been stopped by user or soaring mode enabled to override altitude control
 827                     resetFixedWingAltitudeController();
 828                     setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
 829                 } else {
 830                     applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 831                 }
 832             }
 833
 834             if (navStateFlags & NAV_CTL_POS) {
 835                 applyFixedWingPositionController(currentTimeUs);
 836             }
 837
 838         } else {
 839             posControl.rcAdjustment[PITCH] = 0;
 840             posControl.rcAdjustment[ROLL] = 0;
 841         }
 842
 843         if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) {
 844             rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
 845         }
 846
 847         //if (navStateFlags & NAV_CTL_YAW)
 848         if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
 849             applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs);
 850         }
 851
 852         if (FLIGHT_MODE(SOARING_MODE) && navConfig()->general.flags.soaring_motor_stop) {
 853             ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 854         }
 855     }
 856 }
 857
 858 float navigationGetCrossTrackError(void)
 859 {
 860     return navCrossTrackError;
 861 }