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         int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
 405         navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
 406
 407         if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) {
 408             static float crossTrackErrorRate;
 409             if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
 410                 static float previousCrossTrackError = 0.0f;
 411                 crossTrackErrorRate = 0.5f * (crossTrackErrorRate + (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousTimeMonitoringUpdate));
 412                 previousCrossTrackError = navCrossTrackError;
 413             }
 414
 415             /* Apply basic adjustment to factor up virtualTargetBearing error based on navCrossTrackError */
 416             float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
 417             adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f));
 418
 419             /* Apply additional fine adjustment based on speed of convergence to try and achieve arrival time of around 15s */
 420             float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, 500.0f);
 421             float rateFactor = limit;
 422             if (crossTrackErrorRate > 0.0f) {
 423                 rateFactor = scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, 30.0f, -limit, limit);
 424             }
 425
 426             /* Determine final adjusted virtualTargetBearing */
 427             uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
 428             adjustmentFactor = constrainf(adjustmentFactor + rateFactor * SIGN(adjustmentFactor), -angleLimit, angleLimit);
 429             virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
 430         }
 431     }
 432
 433     /*
 434      * Calculate NAV heading error
 435      * Units are centidegrees
 436      */
 437     int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
 438
 439     // Forced turn direction
 440     // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
 441     if (ABS(navHeadingError) > 17000) {
 442         forceTurnDirection = true;
 443     }
 444     else if (ABS(navHeadingError) < 9000 && forceTurnDirection) {
 445         forceTurnDirection = false;
 446     }
 447
 448     // If forced turn direction flag is enabled we fix the sign of the direction
 449     if (forceTurnDirection) {
 450         navHeadingError = loiterDirection() * ABS(navHeadingError);
 451     }
 452
 453     // Slow error monitoring (2Hz rate)
 454     if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
 455         // Check if error is decreasing over time
 456         errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
 457
 458         // Save values for next iteration
 459         previousHeadingError = navHeadingError;
 460         previousTimeMonitoringUpdate = currentTimeUs;
 461     }
 462
 463     // Only allow PID integrator to shrink if error is decreasing over time
 464     const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
 465
 466     // Input error in (deg*100), output roll angle (deg*100)
 467     float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros),
 468                                        -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
 469                                         DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
 470                                         pidFlags);
 471
 472     // Apply low-pass filter to prevent rapid correction
 473     rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 474
 475     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
 476     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
 477
 478     /*
 479      * Yaw adjustment
 480      * It is working in relative mode and we aim to keep error at zero
 481      */
 482     if (STATE(FW_HEADING_USE_YAW)) {
 483         posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
 484     } else {
 485         posControl.rcAdjustment[YAW] = 0;
 486     }
 487 }
 488
 489 void applyFixedWingPositionController(timeUs_t currentTimeUs)
 490 {
 491     static timeUs_t previousTimePositionUpdate = 0;         // Occurs @ GPS update rate
 492
 493     // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
 494     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
 495         // If we have new position - update velocity and acceleration controllers
 496         if (posControl.flags.horizontalPositionDataNew) {
 497             const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
 498             previousTimePositionUpdate = currentTimeUs;
 499
 500             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
 501                 // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
 502                 // Account for pilot's roll input (move position target left/right at max of max_manual_speed)
 503                 // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
 504                 // FIXME: verify the above
 505                 calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
 506                 updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
 507                 needToCalculateCircularLoiter = false;
 508             }
 509             else {
 510                 // Position update has not occurred in time (first iteration or glitch), reset altitude controller
 511                 resetFixedWingPositionController();
 512             }
 513
 514             // Indicate that information is no longer usable
 515             posControl.flags.horizontalPositionDataConsumed = true;
 516         }
 517
 518         isRollAdjustmentValid = true;
 519         isYawAdjustmentValid = true;
 520     }
 521     else {
 522         // No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
 523         isRollAdjustmentValid = false;
 524         isYawAdjustmentValid = false;
 525     }
 526 }
 527
 528 int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
 529 {
 530     static timeUs_t previousTimePositionUpdate = 0;         // Occurs @ GPS update rate
 531
 532     // Apply controller only if position source is valid
 533     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
 534         // If we have new position - update velocity and acceleration controllers
 535         if (posControl.flags.horizontalPositionDataNew) {
 536             const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
 537             previousTimePositionUpdate = currentTimeUs;
 538
 539             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
 540                 float velThrottleBoost = ((navConfig()->general.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
 541
 542                 // If we are in the deadband of 50cm/s - don't update speed boost
 543                 if (fabsf(posControl.actualState.velXY - (navConfig()->general.min_ground_speed * 100.0f)) > 50) {
 544                     throttleSpeedAdjustment += velThrottleBoost;
 545                 }
 546
 547                 throttleSpeedAdjustment = constrainf(throttleSpeedAdjustment, 0.0f, 500.0f);
 548             }
 549             else {
 550                 // Position update has not occurred in time (first iteration or glitch), reset altitude controller
 551                 throttleSpeedAdjustment = 0;
 552             }
 553
 554             // Indicate that information is no longer usable
 555             posControl.flags.horizontalPositionDataConsumed = true;
 556         }
 557     }
 558     else {
 559         // No valid pos sensor data, we can't calculate speed
 560         throttleSpeedAdjustment = 0;
 561     }
 562
 563     return throttleSpeedAdjustment;
 564 }
 565
 566 int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
 567 {
 568     static timeUs_t previousTimePitchToThrCorr = 0;
 569     const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
 570     previousTimePitchToThrCorr = currentTimeUs;
 571
 572     static pt1Filter_t pitchToThrFilterState;
 573
 574     // Apply low-pass filter to pitch angle to smooth throttle correction
 575     int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
 576
 577     int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle;
 578
 579 #ifdef USE_FW_AUTOLAND
 580     if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) {
 581         pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f;
 582     }
 583 #endif
 584
 585     if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
 586         // Unfiltered throttle correction outside of pitch deadband
 587         return DECIDEGREES_TO_DEGREES(pitch) * pitchToThrottle;
 588     }
 589     else {
 590         // Filtered throttle correction inside of pitch deadband
 591         return DECIDEGREES_TO_DEGREES(filteredPitch) * pitchToThrottle;
 592     }
 593 }
 594
 595 void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 596 {
 597     int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
 598     int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
 599
 600     if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
 601         // ROLL >0 right, <0 left
 602         int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
 603         rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
 604     }
 605
 606     if (isYawAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
 607         rcCommand[YAW] = posControl.rcAdjustment[YAW];
 608     }
 609
 610     if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
 611         // PITCH >0 dive, <0 climb
 612         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
 613         rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
 614         int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
 615
 616         if (navStateFlags & NAV_CTL_LAND) {
 617         // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
 618             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
 619         } else {
 620             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
 621         }
 622
 623         // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
 624         if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
 625             throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs);
 626             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
 627         }
 628
 629         uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
 630
 631         // Manual throttle increase
 632         if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 633             if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
 634                 correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
 635             } else {
 636                 correctedThrottleValue = getMaxThrottle();
 637             }
 638             isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
 639         } else {
 640             isAutoThrottleManuallyIncreased = false;
 641         }
 642
 643         rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
 644     }
 645
 646 #ifdef USE_FW_AUTOLAND
 647     // Advanced autoland
 648     if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) {
 649         // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
 650         ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 651
 652         if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) {
 653             rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->glidePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
 654         }
 655
 656         if (posControl.navState == NAV_STATE_FW_LANDING_FLARE) {
 657             rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
 658         }
 659     }
 660 #endif
 661     // "Traditional" landing as fallback option
 662     if (navStateFlags & NAV_CTL_LAND) {
 663         int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
 664
 665         if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) ||
 666            (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) {
 667
 668             // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
 669             ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 670
 671             // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
 672             rcCommand[ROLL] = 0;
 673
 674             // Stabilize PITCH angle into shallow dive as specified by the nav_fw_land_dive_angle setting (default value is 2 - defined in navigation.c).
 675             rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
 676         }
 677     }
 678 }
 679
 680 bool isFixedWingAutoThrottleManuallyIncreased(void)
 681 {
 682     return isAutoThrottleManuallyIncreased;
 683 }
 684
 685 bool isFixedWingFlying(void)
 686 {
 687     float airspeed = 0.0f;
 688 #ifdef USE_PITOT
 689     if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
 690         airspeed = getAirspeedEstimate();
 691     }
 692 #endif
 693     bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
 694     bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f;
 695     bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING;
 696
 697     return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition;
 698 }
 699
 700 /*-----------------------------------------------------------
 701  * FixedWing land detector
 702  *-----------------------------------------------------------*/
 703 bool isFixedWingLandingDetected(void)
 704 {
 705     DEBUG_SET(DEBUG_LANDING, 4, 0);
 706     static bool fixAxisCheck = false;
 707
 708     // Basic condition to start looking for landing
 709     bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
 710                           || FLIGHT_MODE(FAILSAFE_MODE)
 711                           || FLIGHT_MODE(NAV_FW_AUTOLAND)
 712                           || (!navigationIsControllingThrottle() && throttleStickIsLow());
 713
 714     if (!startCondition || posControl.flags.resetLandingDetector) {
 715         return fixAxisCheck = posControl.flags.resetLandingDetector = false;
 716     }
 717     DEBUG_SET(DEBUG_LANDING, 4, 1);
 718
 719     static timeMs_t fwLandingTimerStartAt;
 720     static int16_t fwLandSetRollDatum;
 721     static int16_t fwLandSetPitchDatum;
 722     const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f;
 723
 724     const timeMs_t currentTimeMs = millis();
 725
 726     // Check horizontal and vertical velocities are low (cm/s)
 727     bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
 728                         ( posControl.actualState.velXY < (100.0f * sensitivity));
 729     // Check angular rates are low (degs/s)
 730     bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity);
 731     DEBUG_SET(DEBUG_LANDING, 2, velCondition);
 732     DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
 733
 734     if (velCondition && gyroCondition){
 735         DEBUG_SET(DEBUG_LANDING, 4, 2);
 736         DEBUG_SET(DEBUG_LANDING, 5, fixAxisCheck);
 737         if (!fixAxisCheck) {        // capture roll and pitch angles to be used as datums to check for absolute change
 738             fwLandSetRollDatum = attitude.values.roll;  //0.1 deg increments
 739             fwLandSetPitchDatum = attitude.values.pitch;
 740             fixAxisCheck = true;
 741             fwLandingTimerStartAt = currentTimeMs;
 742         } else {
 743             const uint8_t angleLimit = 5 * sensitivity;
 744             bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < angleLimit;
 745             bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < angleLimit;
 746             DEBUG_SET(DEBUG_LANDING, 6, isRollAxisStatic);
 747             DEBUG_SET(DEBUG_LANDING, 7, isPitchAxisStatic);
 748             if (isRollAxisStatic && isPitchAxisStatic) {
 749                 /* Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
 750                  * Conditions need to be held for fixed safety time + optional extra delay.
 751                  * Fixed time increased if velocities invalid to provide extra safety margin against false triggers */
 752                 const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE || posControl.flags.estVelStatus == EST_NONE ? 5000 : 1000;
 753                 timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay;
 754                 return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay;
 755             } else {
 756                 fixAxisCheck = false;
 757             }
 758         }
 759     }
 760     return false;
 761 }
 762
 763 /*-----------------------------------------------------------
 764  * FixedWing emergency landing
 765  *-----------------------------------------------------------*/
 766 void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
 767 {
 768     rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
 769
 770     if (posControl.flags.estAltStatus >= EST_USABLE) {
 771         // target min descent rate at distance 2 x emerg descent rate above takeoff altitude
 772         updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
 773         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 774
 775         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
 776         rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
 777     } else {
 778         rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
 779     }
 780
 781     if (posControl.flags.estPosStatus >= EST_USABLE) {  // Hold position if possible
 782         applyFixedWingPositionController(currentTimeUs);
 783         int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL],
 784                                             -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle),
 785                                             DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
 786         rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
 787         rcCommand[YAW] = 0;
 788     } else {
 789         rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
 790         rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
 791     }
 792 }
 793
 794 /*-----------------------------------------------------------
 795  * Calculate loiter target based on current position and velocity
 796  *-----------------------------------------------------------*/
 797 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
 798 {
 799     // TODO: stub, this should account for velocity and target loiter radius
 800     *pos = navGetCurrentActualPositionAndVelocity()->pos;
 801 }
 802
 803 void resetFixedWingHeadingController(void)
 804 {
 805     updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
 806 }
 807
 808 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 809 {
 810     if (navStateFlags & NAV_CTL_LAUNCH) {
 811         applyFixedWingLaunchController(currentTimeUs);
 812     }
 813     else if (navStateFlags & NAV_CTL_EMERG) {
 814         applyFixedWingEmergencyLandingController(currentTimeUs);
 815     }
 816     else {
 817 #ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY
 818         // Don't apply anything if ground speed is too low (<3m/s)
 819         if (posControl.actualState.velXY > 300) {
 820 #else
 821         if (true) {
 822 #endif
 823             if (navStateFlags & NAV_CTL_ALT) {
 824                 if (getMotorStatus() == MOTOR_STOPPED_USER || FLIGHT_MODE(SOARING_MODE)) {
 825                     // Motor has been stopped by user or soaring mode enabled to override altitude control
 826                     resetFixedWingAltitudeController();
 827                     setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
 828                 } else {
 829                     applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 830                 }
 831             }
 832
 833             if (navStateFlags & NAV_CTL_POS) {
 834                 applyFixedWingPositionController(currentTimeUs);
 835             }
 836
 837         } else {
 838             posControl.rcAdjustment[PITCH] = 0;
 839             posControl.rcAdjustment[ROLL] = 0;
 840         }
 841
 842         if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) {
 843             rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
 844         }
 845
 846         //if (navStateFlags & NAV_CTL_YAW)
 847         if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
 848             applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs);
 849         }
 850
 851         if (FLIGHT_MODE(SOARING_MODE) && navConfig()->general.flags.soaring_motor_stop) {
 852             ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 853         }
 854     }
 855 }
 856
 857 float navigationGetCrossTrackError(void)
 858 {
 859     return navCrossTrackError;
 860 }