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