src/main/navigation/navigation_private.h

   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 #pragma once
  19
  20 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR    1.113195f  // MagicEarthNumber from APM
  21
  22 #include "common/axis.h"
  23 #include "common/maths.h"
  24 #include "common/filter.h"
  25 #include "common/time.h"
  26 #include "common/vector.h"
  27 #include "fc/runtime_config.h"
  28 #include "navigation/navigation.h"
  29
  30 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
  31 #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
  32 #define NAV_FW_CONTROL_MONITORING_RATE      2
  33 #define NAV_DTERM_CUT_HZ                    10.0f
  34 #define NAV_VEL_Z_DERIVATIVE_CUT_HZ         5.0f
  35 #define NAV_VEL_Z_ERROR_CUT_HZ              5.0f
  36 #define NAV_ACCELERATION_XY_MAX             980.0f  // cm/s/s       // approx 45 deg lean angle
  37
  38 #define INAV_SURFACE_MAX_DISTANCE           40
  39
  40 #define MC_POS_CONTROL_JERK_LIMIT_CMSSS     1700.0f // jerk limit on horizontal acceleration (cm/s^3)
  41
  42 #define MC_LAND_CHECK_VEL_XY_MOVING         100.0f  // cm/s
  43 #define MC_LAND_CHECK_VEL_Z_MOVING          100.0f  // cm/s
  44 #define MC_LAND_THR_STABILISE_DELAY         1       // seconds
  45 #define MC_LAND_DESCEND_THROTTLE            40      // RC pwm units (us)
  46 #define MC_LAND_SAFE_SURFACE                5.0f    // cm
  47
  48 #define NAV_RTH_TRACKBACK_POINTS            50      // max number RTH trackback points
  49
  50 #define MAX_POSITION_UPDATE_INTERVAL_US     HZ2US(MIN_POSITION_UPDATE_RATE_HZ)        // convenience macro
  51 _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");
  52
  53 typedef enum {
  54     NAV_POS_UPDATE_NONE                 = 0,
  55     NAV_POS_UPDATE_Z                    = 1 << 1,
  56     NAV_POS_UPDATE_XY                   = 1 << 0,
  57     NAV_POS_UPDATE_HEADING              = 1 << 2,
  58     NAV_POS_UPDATE_BEARING              = 1 << 3,
  59     NAV_POS_UPDATE_BEARING_TAIL_FIRST   = 1 << 4,
  60 } navSetWaypointFlags_t;
  61
  62 typedef enum {
  63     ROC_TO_ALT_RESET,
  64     ROC_TO_ALT_CONSTANT,
  65     ROC_TO_ALT_TARGET
  66 } climbRateToAltitudeControllerMode_e;
  67
  68 typedef enum {
  69     EST_NONE = 0,       // No valid sensor present
  70     EST_USABLE = 1,     // Estimate is usable but may be inaccurate
  71     EST_TRUSTED = 2     // Estimate is usable and based on actual sensor data
  72 } navigationEstimateStatus_e;
  73
  74 typedef enum {
  75     NAV_HOME_INVALID = 0,
  76     NAV_HOME_VALID_XY = 1 << 0,
  77     NAV_HOME_VALID_Z = 1 << 1,
  78     NAV_HOME_VALID_HEADING = 1 << 2,
  79     NAV_HOME_VALID_ALL = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z | NAV_HOME_VALID_HEADING,
  80 } navigationHomeFlags_t;
  81
  82 typedef struct navigationFlags_s {
  83     bool horizontalPositionDataNew;
  84     bool verticalPositionDataNew;
  85
  86     bool horizontalPositionDataConsumed;
  87     bool verticalPositionDataConsumed;
  88
  89     navigationEstimateStatus_e estAltStatus;        // Indicates that we have a working altitude sensor (got at least one valid reading from it)
  90     navigationEstimateStatus_e estPosStatus;        // Indicates that GPS is working (or not)
  91     navigationEstimateStatus_e estVelStatus;        // Indicates that GPS is working (or not)
  92     navigationEstimateStatus_e estAglStatus;
  93     navigationEstimateStatus_e estHeadingStatus;    // Indicate valid heading - wither mag or GPS at certain speed on airplane
  94     bool gpsCfEstimatedAltitudeMismatch;            // Indicates a mismatch between GPS altitude and estimated altitude
  95
  96     bool isAdjustingPosition;
  97     bool isAdjustingAltitude;
  98     bool isAdjustingHeading;
  99
 100     // Behaviour modifiers
 101     bool isGCSAssistedNavigationEnabled;    // Does INAV accept WP#255 - follow-me etc.
 102     bool isGCSAssistedNavigationReset;      // GCS control was disabled - indicate that so code could take action accordingly
 103     bool isTerrainFollowEnabled;            // Does INAV use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings)
 104
 105     // Failsafe actions
 106     bool forcedRTHActivated;
 107     bool forcedEmergLandingActivated;
 108
 109     /* Landing detector */
 110     bool resetLandingDetector;
 111
 112     bool wpMissionPlannerActive;            // Activation status of WP mission planner
 113     bool rthTrackbackActive;                // Activation status of RTH trackback
 114     bool wpTurnSmoothingActive;             // Activation status WP turn smoothing
 115     bool manualEmergLandActive;             // Activation status of manual emergency landing
 116 } navigationFlags_t;
 117
 118 typedef struct {
 119     fpVector3_t pos;
 120     fpVector3_t vel;
 121 } navEstimatedPosVel_t;
 122
 123 typedef struct {
 124     // Local estimated states
 125     navEstimatedPosVel_t    abs;
 126     navEstimatedPosVel_t    agl;
 127     int32_t                 yaw;
 128     int32_t                 cog;
 129
 130     // Service values
 131     float                   sinYaw;
 132     float                   cosYaw;
 133     float                   surfaceMin;
 134     float                   velXY;
 135 } navigationEstimatedState_t;
 136
 137 typedef struct {
 138     fpVector3_t pos;
 139     fpVector3_t vel;
 140     int32_t     yaw;
 141 } navigationDesiredState_t;
 142
 143 typedef enum {
 144     NAV_FSM_EVENT_NONE = 0,
 145     NAV_FSM_EVENT_TIMEOUT,
 146
 147     NAV_FSM_EVENT_SUCCESS,
 148     NAV_FSM_EVENT_ERROR,
 149
 150     NAV_FSM_EVENT_SWITCH_TO_IDLE,
 151     NAV_FSM_EVENT_SWITCH_TO_ALTHOLD,
 152     NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D,
 153     NAV_FSM_EVENT_SWITCH_TO_RTH,
 154     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT,
 155     NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING,
 156     NAV_FSM_EVENT_SWITCH_TO_LAUNCH,
 157     NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD,
 158     NAV_FSM_EVENT_SWITCH_TO_CRUISE,
 159     NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ,
 160     NAV_FSM_EVENT_SWITCH_TO_MIXERAT,
 161     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING,
 162
 163     NAV_FSM_EVENT_STATE_SPECIFIC_1,             // State-specific event
 164     NAV_FSM_EVENT_STATE_SPECIFIC_2,             // State-specific event
 165     NAV_FSM_EVENT_STATE_SPECIFIC_3,             // State-specific event
 166     NAV_FSM_EVENT_STATE_SPECIFIC_4,             // State-specific event
 167     NAV_FSM_EVENT_STATE_SPECIFIC_5,             // State-specific event
 168
 169     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_1,
 170     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
 171     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_1,
 172     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_2,
 173     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_3,
 174     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_1,
 175     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2,
 176     NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
 177     NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
 178     NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5,
 179
 180     NAV_FSM_EVENT_COUNT,
 181 } navigationFSMEvent_t;
 182
 183 // This enum is used to keep values in blackbox logs stable, so we can
 184 // freely change navigationFSMState_t.
 185 typedef enum {
 186     NAV_PERSISTENT_ID_UNDEFINED                                 = 0,
 187
 188     NAV_PERSISTENT_ID_IDLE                                      = 1,
 189
 190     NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE                        = 2,
 191     NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS                       = 3,
 192
 193     NAV_PERSISTENT_ID_UNUSED_1                                  = 4,  // was NAV_STATE_POSHOLD_2D_INITIALIZE
 194     NAV_PERSISTENT_ID_UNUSED_2                                  = 5,  // was NAV_STATE_POSHOLD_2D_IN_PROGRESS
 195
 196     NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE                     = 6,
 197     NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS                    = 7,
 198
 199     NAV_PERSISTENT_ID_RTH_INITIALIZE                            = 8,
 200     NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT                     = 9,
 201     NAV_PERSISTENT_ID_RTH_HEAD_HOME                             = 10,
 202     NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING                = 11,
 203     NAV_PERSISTENT_ID_RTH_LANDING                               = 12,
 204     NAV_PERSISTENT_ID_RTH_FINISHING                             = 13,
 205     NAV_PERSISTENT_ID_RTH_FINISHED                              = 14,
 206
 207     NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE                       = 15,
 208     NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION                       = 16,
 209     NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS                      = 17,
 210     NAV_PERSISTENT_ID_WAYPOINT_REACHED                          = 18,
 211     NAV_PERSISTENT_ID_WAYPOINT_NEXT                             = 19,
 212     NAV_PERSISTENT_ID_WAYPOINT_FINISHED                         = 20,
 213     NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND                         = 21,
 214
 215     NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE              = 22,
 216     NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS             = 23,
 217     NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED                = 24,
 218
 219     NAV_PERSISTENT_ID_LAUNCH_INITIALIZE                         = 25,
 220     NAV_PERSISTENT_ID_LAUNCH_WAIT                               = 26,
 221     NAV_PERSISTENT_ID_UNUSED_3                                  = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY
 222     NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS                        = 28,
 223
 224     NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE                    = 29,
 225     NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS                   = 30,
 226     NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING                     = 31,
 227
 228     NAV_PERSISTENT_ID_CRUISE_INITIALIZE                         = 32,
 229     NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS                        = 33,
 230     NAV_PERSISTENT_ID_CRUISE_ADJUSTING                          = 34,
 231
 232     NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME                        = 35,
 233     NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME                      = 36,
 234     NAV_PERSISTENT_ID_UNUSED_4                                  = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME
 235     NAV_PERSISTENT_ID_RTH_TRACKBACK                             = 38,
 236
 237     NAV_PERSISTENT_ID_MIXERAT_INITIALIZE                        = 39,
 238     NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS                       = 40,
 239     NAV_PERSISTENT_ID_MIXERAT_ABORT                             = 41,
 240     NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER                = 42,
 241     NAV_PERSISTENT_ID_FW_LANDING_LOITER                         = 43,
 242     NAV_PERSISTENT_ID_FW_LANDING_APPROACH                       = 44,
 243     NAV_PERSISTENT_ID_FW_LANDING_GLIDE                          = 45,
 244     NAV_PERSISTENT_ID_FW_LANDING_FLARE                          = 46,
 245     NAV_PERSISTENT_ID_FW_LANDING_ABORT                          = 47,
 246     NAV_PERSISTENT_ID_FW_LANDING_FINISHED                       = 48,
 247 } navigationPersistentId_e;
 248
 249 typedef enum {
 250     NAV_STATE_UNDEFINED = 0,
 251
 252     NAV_STATE_IDLE,
 253
 254     NAV_STATE_ALTHOLD_INITIALIZE,
 255     NAV_STATE_ALTHOLD_IN_PROGRESS,
 256
 257     NAV_STATE_POSHOLD_3D_INITIALIZE,
 258     NAV_STATE_POSHOLD_3D_IN_PROGRESS,
 259
 260     NAV_STATE_RTH_INITIALIZE,
 261     NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
 262     NAV_STATE_RTH_TRACKBACK,
 263     NAV_STATE_RTH_HEAD_HOME,
 264     NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
 265     NAV_STATE_RTH_HOVER_ABOVE_HOME,
 266     NAV_STATE_RTH_LANDING,
 267     NAV_STATE_RTH_FINISHING,
 268     NAV_STATE_RTH_FINISHED,
 269
 270     NAV_STATE_WAYPOINT_INITIALIZE,
 271     NAV_STATE_WAYPOINT_PRE_ACTION,
 272     NAV_STATE_WAYPOINT_IN_PROGRESS,
 273     NAV_STATE_WAYPOINT_REACHED,
 274     NAV_STATE_WAYPOINT_HOLD_TIME,
 275     NAV_STATE_WAYPOINT_NEXT,
 276     NAV_STATE_WAYPOINT_FINISHED,
 277     NAV_STATE_WAYPOINT_RTH_LAND,
 278
 279     NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
 280     NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
 281     NAV_STATE_EMERGENCY_LANDING_FINISHED,
 282
 283     NAV_STATE_LAUNCH_INITIALIZE,
 284     NAV_STATE_LAUNCH_WAIT,
 285     NAV_STATE_LAUNCH_IN_PROGRESS,
 286
 287     NAV_STATE_COURSE_HOLD_INITIALIZE,
 288     NAV_STATE_COURSE_HOLD_IN_PROGRESS,
 289     NAV_STATE_COURSE_HOLD_ADJUSTING,
 290     NAV_STATE_CRUISE_INITIALIZE,
 291     NAV_STATE_CRUISE_IN_PROGRESS,
 292     NAV_STATE_CRUISE_ADJUSTING,
 293
 294     NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
 295     NAV_STATE_FW_LANDING_LOITER,
 296     NAV_STATE_FW_LANDING_APPROACH,
 297     NAV_STATE_FW_LANDING_GLIDE,
 298     NAV_STATE_FW_LANDING_FLARE,
 299     NAV_STATE_FW_LANDING_FINISHED,
 300     NAV_STATE_FW_LANDING_ABORT,
 301
 302     NAV_STATE_MIXERAT_INITIALIZE,
 303     NAV_STATE_MIXERAT_IN_PROGRESS,
 304     NAV_STATE_MIXERAT_ABORT,
 305
 306     NAV_STATE_COUNT,
 307 } navigationFSMState_t;
 308
 309 typedef enum {
 310     /* Navigation controllers */
 311     NAV_CTL_ALT             = (1 << 0),     // Altitude controller
 312     NAV_CTL_POS             = (1 << 1),     // Position controller
 313     NAV_CTL_YAW             = (1 << 2),
 314     NAV_CTL_EMERG           = (1 << 3),
 315     NAV_CTL_LAUNCH          = (1 << 4),
 316
 317     /* Navigation requirements for flight modes and controllers */
 318     NAV_REQUIRE_ANGLE       = (1 << 5),
 319     NAV_REQUIRE_ANGLE_FW    = (1 << 6),
 320     NAV_REQUIRE_MAGHOLD     = (1 << 7),
 321     NAV_REQUIRE_THRTILT     = (1 << 8),
 322
 323     /* Navigation autonomous modes */
 324     NAV_AUTO_RTH            = (1 << 9),
 325     NAV_AUTO_WP             = (1 << 10),
 326
 327     /* Adjustments for navigation modes from RC input */
 328     NAV_RC_ALT              = (1 << 11),
 329     NAV_RC_POS              = (1 << 12),
 330     NAV_RC_YAW              = (1 << 13),
 331
 332     /* Additional flags */
 333     NAV_CTL_LAND            = (1 << 14),
 334     NAV_AUTO_WP_DONE        = (1 << 15),    //Waypoint mission reached the last waypoint and is idling
 335
 336     NAV_MIXERAT             = (1 << 16),    //MIXERAT in progress
 337 } navigationFSMStateFlags_t;
 338
 339 typedef struct {
 340     navigationPersistentId_e            persistentId;
 341     navigationFSMEvent_t                (*onEntry)(navigationFSMState_t previousState);
 342     uint32_t                            timeoutMs;
 343     navSystemStatus_State_e             mwState;
 344     navSystemStatus_Error_e             mwError;
 345     navigationFSMStateFlags_t           stateFlags;
 346     flightModeFlags_e                   mapToFlightModes;
 347     navigationFSMState_t                onEvent[NAV_FSM_EVENT_COUNT];
 348 } navigationFSMStateDescriptor_t;
 349
 350 typedef struct {
 351     timeMs_t        lastCheckTime;
 352     bool            rthSanityOK;
 353     float           minimalDistanceToHome;
 354 } rthSanityChecker_t;
 355
 356 typedef struct {
 357     int32_t                     course;
 358     int32_t                     previousCourse;
 359     timeMs_t                    lastCourseAdjustmentTime;
 360     float                       multicopterSpeed;
 361 } navCruise_t;
 362
 363 typedef struct {
 364     navigationHomeFlags_t   homeFlags;
 365     navWaypointPosition_t   homePosition;           // Original home position and base altitude
 366     float                   rthInitialAltitude;     // Altitude at start of RTH, can include added margins and extra height
 367     float                   rthClimbStageAltitude;  // Altitude at end of the climb phase
 368     float                   rthFinalAltitude;       // Altitude at end of RTH approach
 369     float                   rthInitialDistance;     // Distance when starting flight home
 370     fpVector3_t             homeTmpWaypoint;        // Temporary storage for home target
 371     fpVector3_t             originalHomePosition;   // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
 372     bool                    rthLinearDescentActive; // Activation status of Linear Descent
 373 } rthState_t;
 374
 375 #ifdef USE_FW_AUTOLAND
 376 typedef enum {
 377     FW_AUTOLAND_WP_TURN,
 378     FW_AUTOLAND_WP_FINAL_APPROACH,
 379     FW_AUTOLAND_WP_LAND,
 380     FW_AUTOLAND_WP_COUNT,
 381 } fwAutolandWaypoint_t;
 382
 383 typedef struct {
 384     timeUs_t loiterStartTime;
 385     fpVector3_t landWaypoints[FW_AUTOLAND_WP_COUNT];
 386     fpVector3_t landPos;
 387     int32_t landPosHeading;
 388     int32_t landingDirection;
 389     int32_t landAproachAltAgl;
 390     int32_t landAltAgl;
 391     uint8_t approachSettingIdx;
 392     fwAutolandWaypoint_t landCurrentWp;
 393     bool landAborted;
 394     bool landWp;
 395     fwAutolandState_t landState;
 396 } fwLandState_t;
 397 #endif
 398
 399 typedef enum {
 400     RTH_HOME_ENROUTE_INITIAL,       // Initial position for RTH approach
 401     RTH_HOME_ENROUTE_PROPORTIONAL,  // Prorpotional position for RTH approach
 402     RTH_HOME_ENROUTE_FINAL,         // Final position for RTH approach
 403     RTH_HOME_FINAL_HOVER,           // Final hover altitude (if rth_home_altitude is set)
 404     RTH_HOME_FINAL_LAND,            // Home position and altitude
 405 } rthTargetMode_e;
 406
 407 typedef struct {
 408     fpVector3_t nearestSafeHome;    // The nearestSafeHome found during arming
 409     uint32_t    distance;           // distance to the nearest safehome
 410     int8_t      index;              // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
 411     bool        isApplied;          // whether the safehome has been applied to home
 412 } safehomeState_t;
 413
 414 typedef struct {
 415     /* Flags and navigation system state */
 416     navigationFSMState_t        navState;
 417     navigationPersistentId_e    navPersistentId;
 418
 419     navigationFlags_t           flags;
 420
 421     /* Navigation PID controllers + pre-computed flight parameters */
 422     navigationPIDControllers_t  pids;
 423     float                       posDecelerationTime;
 424     float                       posResponseExpo;
 425
 426     /* Local system state, both actual (estimated) and desired (target setpoint)*/
 427     navigationEstimatedState_t  actualState;
 428     navigationDesiredState_t    desiredState;   // waypoint coordinates + velocity
 429
 430     uint32_t                    lastValidPositionTimeMs;
 431     uint32_t                    lastValidAltitudeTimeMs;
 432
 433     /* INAV GPS origin (position where GPS fix was first acquired) */
 434     gpsOrigin_t                 gpsOrigin;
 435
 436     /* Home/RTH parameters - NEU coordinates (geodetic position of home (LLH) is stored in GPS_home variable) */
 437     rthSanityChecker_t          rthSanityChecker;
 438     rthState_t                  rthState;
 439     uint32_t                    homeDistance;   // cm
 440     int32_t                     homeDirection;  // deg*100
 441     timeMs_t                    landingDelay;
 442
 443     /* Safehome parameters */
 444     safehomeState_t             safehomeState;
 445
 446     /* Cruise */
 447     navCruise_t                 cruise;
 448
 449     /* Waypoint list */
 450     navWaypoint_t               waypointList[NAV_MAX_WAYPOINTS];
 451     bool                        waypointListValid;
 452     int8_t                      waypointCount;              // number of WPs in loaded mission
 453     int8_t                      startWpIndex;               // index of first waypoint in mission
 454     int8_t                      geoWaypointCount;           // total geospatial WPs in mission
 455     bool                        wpMissionRestart;           // mission restart from first waypoint
 456
 457     /* WP Mission planner */
 458     int8_t                      wpMissionPlannerStatus;     // WP save status for setting in flight WP mission planner
 459     int8_t                      wpPlannerActiveWPIndex;
 460 #ifdef USE_MULTI_MISSION
 461     /* Multi Missions */
 462     int8_t                      multiMissionCount;          // number of missions in multi mission entry
 463     int8_t                      loadedMultiMissionIndex;    // index of selected multi mission
 464     int8_t                      totalMultiMissionWpCount;   // total number of waypoints in all multi missions
 465 #endif
 466     navWaypointPosition_t       activeWaypoint;             // Local position, current bearing and turn angle to next WP, filled on waypoint activation
 467     int8_t                      activeWaypointIndex;
 468     float                       wpInitialAltitude;          // Altitude at start of WP
 469     float                       wpInitialDistance;          // Distance when starting flight to WP
 470     float                       wpDistance;                 // Distance to active WP
 471     timeMs_t                    wpReachedTime;              // Time the waypoint was reached
 472     bool                        wpAltitudeReached;          // WP altitude achieved
 473
 474     /* RTH Trackback */
 475     fpVector3_t                 rthTBPointsList[NAV_RTH_TRACKBACK_POINTS];
 476     int8_t                      rthTBLastSavedIndex;        // last trackback point index saved
 477     int8_t                      activeRthTBPointIndex;
 478     int8_t                      rthTBWrapAroundCounter;     // stores trackpoint array overwrite index position
 479
 480 #ifdef USE_FW_AUTOLAND
 481     /* Fixedwing autoland */
 482     fwLandState_t fwLandState;
 483 #endif
 484
 485     /* Internals & statistics */
 486     int16_t                     rcAdjustment[4];
 487     float                       totalTripDistance;
 488 } navigationPosControl_t;
 489
 490 typedef struct {
 491     float dTermAttenuation;
 492     float dTermAttenuationStart;
 493     float dTermAttenuationEnd;
 494     float breakingBoostFactor;
 495 } multicopterPosXyCoefficients_t;
 496
 497 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
 498 PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
 499 #endif
 500
 501 extern navigationPosControl_t posControl;
 502 extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 503
 504 /* Internally used functions */
 505 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
 506
 507 bool isThrustFacingDownwards(void);
 508 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
 509 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
 510
 511 bool isLandingDetected(void);
 512 void resetLandingDetector(void);
 513 bool isFlightDetected(void);
 514 bool isFixedWingFlying(void);
 515 bool isMulticopterFlying(void);
 516
 517 navigationFSMStateFlags_t navGetCurrentStateFlags(void);
 518 flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state);
 519
 520 void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
 521 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
 522 void setDesiredSurfaceOffset(float surfaceOffset);
 523 void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);   // NOT USED
 524 void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode);
 525
 526 bool isNavHoldPositionActive(void);
 527 bool isLastMissionWaypoint(void);
 528 float getActiveSpeed(void);
 529 bool isWaypointNavTrackingActive(void);
 530
 531 void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
 532 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
 533 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
 534
 535 bool checkForPositionSensorTimeout(void);
 536
 537 bool isGPSGlitchDetected(void);
 538
 539 /* Multicopter-specific functions */
 540 void setupMulticopterAltitudeController(void);
 541
 542 void resetMulticopterAltitudeController(void);
 543 void resetMulticopterPositionController(void);
 544 void resetMulticopterHeadingController(void);
 545 void resetMulticopterBrakingMode(void);
 546
 547 bool adjustMulticopterAltitudeFromRCInput(void);
 548 bool adjustMulticopterHeadingFromRCInput(void);
 549 bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment);
 550
 551 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 552
 553 bool isMulticopterLandingDetected(void);
 554
 555 void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
 556
 557 /* Fixed-wing specific functions */
 558 void setupFixedWingAltitudeController(void);
 559
 560 void resetFixedWingAltitudeController(void);
 561 void resetFixedWingPositionController(void);
 562 void resetFixedWingHeadingController(void);
 563
 564 bool adjustFixedWingAltitudeFromRCInput(void);
 565 bool adjustFixedWingHeadingFromRCInput(void);
 566 bool adjustFixedWingPositionFromRCInput(void);
 567
 568 void applyFixedWingPositionController(timeUs_t currentTimeUs);
 569 float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing);
 570 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 571
 572 bool isFixedWingLandingDetected(void);
 573
 574 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
 575
 576 /* Fixed-wing launch controller */
 577 void resetFixedWingLaunchController(timeUs_t currentTimeUs);
 578 void enableFixedWingLaunchController(timeUs_t currentTimeUs);
 579 void abortFixedWingLaunch(void);
 580 void applyFixedWingLaunchController(timeUs_t currentTimeUs);
 581
 582 /*
 583  * Rover specific functions
 584  */
 585 void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);