src/main/navigation/navigation.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 #include <stdbool.h>
  21
  22 #include "common/axis.h"
  23 #include "common/filter.h"
  24 #include "common/maths.h"
  25 #include "common/vector.h"
  26 #include "common/fp_pid.h"
  27
  28 #include "config/feature.h"
  29
  30 #include "flight/failsafe.h"
  31
  32 #include "io/gps.h"
  33
  34 /* GPS Home location data */
  35 extern gpsLocation_t        GPS_home;
  36 extern uint32_t             GPS_distanceToHome;        // distance to home point in meters
  37 extern int16_t              GPS_directionToHome;       // direction to home point in degrees
  38 extern fpVector3_t          original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
  39
  40 extern bool autoThrottleManuallyIncreased;
  41
  42 /* Navigation system updates */
  43 void onNewGPSData(void);
  44 #if defined(USE_SAFE_HOME)
  45
  46 #define MAX_SAFE_HOMES 8
  47
  48 typedef struct {
  49     uint8_t enabled;
  50     int32_t lat;
  51     int32_t lon;
  52 } navSafeHome_t;
  53
  54 typedef enum {
  55     SAFEHOME_USAGE_OFF = 0,    // Don't use safehomes
  56     SAFEHOME_USAGE_RTH = 1,    // Default - use safehome for RTH
  57     SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only
  58 } safehomeUsageMode_e;
  59
  60 PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
  61
  62 extern int8_t safehome_index;                    // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
  63 extern uint32_t safehome_distance;               // distance to the nearest safehome
  64 extern bool safehome_applied;                    // whether the safehome has been applied to home.
  65
  66 void resetSafeHomes(void);                       // remove all safehomes
  67 bool findNearestSafeHome(void);                  // Find nearest safehome
  68
  69 #endif // defined(USE_SAFE_HOME)
  70
  71 #ifndef NAV_MAX_WAYPOINTS
  72 #define NAV_MAX_WAYPOINTS 15
  73 #endif
  74
  75 #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2       // low-pass filter on XY-acceleration target
  76
  77 enum {
  78     NAV_GPS_ATTI    = 0,                    // Pitch/roll stick controls attitude (pitch/roll lean angles)
  79     NAV_GPS_CRUISE  = 1                     // Pitch/roll stick controls velocity (forward/right speed)
  80 };
  81
  82 enum {
  83     NAV_LOITER_RIGHT = 0,                    // Loitering direction right
  84     NAV_LOITER_LEFT  = 1,                    // Loitering direction left
  85     NAV_LOITER_YAW   = 2
  86 };
  87
  88 enum {
  89     NAV_RTH_NO_ALT                       = 0, // Maintain current altitude
  90     NAV_RTH_EXTRA_ALT                    = 1, // Maintain current altitude + predefined safety margin
  91     NAV_RTH_CONST_ALT                    = 2, // Climb/descend to predefined altitude
  92     NAV_RTH_MAX_ALT                      = 3, // Track maximum altitude and climb to it when RTH
  93     NAV_RTH_AT_LEAST_ALT                 = 4, // Climb to predefined altitude if below it
  94     NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT  = 5, // Climb to predefined altitude if below it,
  95                                               // descend linearly to reach home at predefined altitude if above it
  96 };
  97
  98 enum {
  99     NAV_RTH_CLIMB_STAGE_AT_LEAST        = 0, // Will climb to the lesser of rth_climb_first_stage_altitude or rth_altitude, before turning
 100     NAV_RTH_CLIMB_STAGE_EXTRA           = 1, // Will climb the lesser of rth_climb_first_stage_altitude above the current altitude or to nav_rth_altitude, before turning
 101 };
 102
 103 enum {
 104     NAV_HEADING_CONTROL_NONE = 0,
 105     NAV_HEADING_CONTROL_AUTO,
 106     NAV_HEADING_CONTROL_MANUAL
 107 };
 108
 109 typedef enum {
 110     NAV_RESET_NEVER = 0,
 111     NAV_RESET_ON_FIRST_ARM,
 112     NAV_RESET_ON_EACH_ARM,
 113 } nav_reset_type_e;
 114
 115 typedef enum {
 116     NAV_RTH_ALLOW_LANDING_NEVER = 0,
 117     NAV_RTH_ALLOW_LANDING_ALWAYS = 1,
 118     NAV_RTH_ALLOW_LANDING_FS_ONLY = 2, // Allow landing only if RTH was triggered by failsafe
 119 } navRTHAllowLanding_e;
 120
 121 typedef enum {
 122     NAV_EXTRA_ARMING_SAFETY_OFF = 0,
 123     NAV_EXTRA_ARMING_SAFETY_ON = 1,
 124     NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS = 2, // Allow disabling by holding THR+YAW low
 125 } navExtraArmingSafety_e;
 126
 127 typedef enum {
 128     NAV_ARMING_BLOCKER_NONE = 0,
 129     NAV_ARMING_BLOCKER_MISSING_GPS_FIX = 1,
 130     NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE = 2,
 131     NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR = 3,
 132     NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR = 4,
 133 } navArmingBlocker_e;
 134
 135 typedef enum {
 136     NOMS_OFF_ALWAYS,
 137     NOMS_OFF,
 138     NOMS_AUTO_ONLY,
 139     NOMS_ALL_NAV
 140 } navOverridesMotorStop_e;
 141
 142 typedef enum {
 143     OFF,
 144     ON,
 145     ON_FW_SPIRAL,
 146 } navRTHClimbFirst_e;
 147
 148 typedef enum {  // keep aligned with fixedWingLaunchState_t
 149     FW_LAUNCH_DETECTED = 4,
 150     FW_LAUNCH_ABORTED = 9,
 151     FW_LAUNCH_FLYING = 10,
 152 } navFwLaunchStatus_e;
 153
 154 typedef enum {
 155     WP_PLAN_WAIT,
 156     WP_PLAN_SAVE,
 157     WP_PLAN_OK,
 158     WP_PLAN_FULL,
 159 } wpMissionPlannerStatus_e;
 160
 161 typedef enum {
 162     WP_MISSION_START,
 163     WP_MISSION_RESUME,
 164     WP_MISSION_SWITCH,
 165 } navMissionRestart_e;
 166
 167 typedef struct positionEstimationConfig_s {
 168     uint8_t automatic_mag_declination;
 169     uint8_t reset_altitude_type; // from nav_reset_type_e
 170     uint8_t reset_home_type; // nav_reset_type_e
 171     uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
 172     uint8_t use_gps_velned;
 173     uint8_t allow_dead_reckoning;
 174
 175     uint16_t max_surface_altitude;
 176
 177     float w_z_baro_p;   // Weight (cutoff frequency) for barometer altitude measurements
 178
 179     float w_z_surface_p;  // Weight (cutoff frequency) for surface altitude measurements
 180     float w_z_surface_v;  // Weight (cutoff frequency) for surface velocity measurements
 181
 182     float w_z_gps_p;    // GPS altitude data is very noisy and should be used only on airplanes
 183     float w_z_gps_v;    // Weight (cutoff frequency) for GPS climb rate measurements
 184
 185     float w_xy_gps_p;   // Weight (cutoff frequency) for GPS position measurements
 186     float w_xy_gps_v;   // Weight (cutoff frequency) for GPS velocity measurements
 187
 188     float w_xy_flow_p;
 189     float w_xy_flow_v;
 190
 191     float w_z_res_v;    // When velocity sources lost slowly decrease estimated velocity with this weight
 192     float w_xy_res_v;
 193
 194     float w_acc_bias;   // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
 195     float w_xyz_acc_p;
 196
 197     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
 198     float baro_epv;     // Baro position error
 199
 200     uint8_t use_gps_no_baro;
 201 } positionEstimationConfig_t;
 202
 203 PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);
 204
 205 typedef struct navConfig_s {
 206
 207     struct {
 208         struct {
 209             uint8_t use_thr_mid_for_althold;    // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate
 210             uint8_t extra_arming_safety;        // from navExtraArmingSafety_e
 211             uint8_t user_control_mode;          // NAV_GPS_ATTI or NAV_GPS_CRUISE
 212             uint8_t rth_alt_control_mode;       // Controls the logic for choosing the RTH altitude
 213             uint8_t rth_climb_first;            // Controls the logic for initial RTH climbout
 214             uint8_t rth_climb_first_stage_mode;  // To determine how rth_climb_first_stage_altitude is used
 215             uint8_t rth_tail_first;             // Return to home tail first
 216             uint8_t disarm_on_landing;          //
 217             uint8_t rth_allow_landing;          // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e.
 218             uint8_t rth_climb_ignore_emerg;     // Option to ignore GPS loss on initial climb stage of RTH
 219             uint8_t rth_alt_control_override;   // Override RTH Altitude and Climb First settings using Pitch and Roll stick
 220             uint8_t nav_overrides_motor_stop;   // Autonomous modes override motor_stop setting and user command to stop motor
 221             uint8_t safehome_usage_mode;        // Controls when safehomes are used
 222             uint8_t soaring_motor_stop;         // stop motor when Soaring mode enabled
 223             uint8_t mission_planner_reset;      // Allow WP Mission Planner reset using mode toggle (resets WPs to 0)
 224             uint8_t waypoint_mission_restart;   // Waypoint mission restart action
 225         } flags;
 226
 227         uint8_t  pos_failure_timeout;               // Time to wait before switching to emergency landing (0 - disable)
 228         uint16_t waypoint_radius;                   // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
 229         uint16_t waypoint_safe_distance;            // Waypoint mission sanity check distance
 230 #ifdef USE_MULTI_MISSION
 231         uint8_t  waypoint_multi_mission_index;      // Index of mission to be loaded in multi mission entry
 232 #endif
 233         bool     waypoint_load_on_boot;             // load waypoints automatically during boot
 234         uint16_t auto_speed;                        // autonomous navigation speed cm/sec
 235         uint16_t max_auto_speed;                    // maximum allowed autonomous navigation speed cm/sec
 236         uint16_t max_auto_climb_rate;               // max vertical speed limitation cm/sec
 237         uint16_t max_manual_speed;                  // manual velocity control max horizontal speed
 238         uint16_t max_manual_climb_rate;             // manual velocity control max vertical speed
 239         uint16_t land_minalt_vspd;                  // Final RTH landing descent rate under minalt
 240         uint16_t land_maxalt_vspd;                  // RTH landing descent rate target at maxalt
 241         uint16_t land_slowdown_minalt;              // Altitude to stop lowering descent rate during RTH descend
 242         uint16_t land_slowdown_maxalt;              // Altitude to start lowering descent rate during RTH descend
 243         uint16_t emerg_descent_rate;                // emergency landing descent rate
 244         uint16_t rth_altitude;                      // altitude to maintain when RTH is active (depends on rth_alt_control_mode) (cm)
 245         uint16_t rth_home_altitude;                 // altitude to go to during RTH after the craft reached home (cm)
 246         uint16_t rth_climb_first_stage_altitude;    // Altitude to reach before transitioning from climb first to turn first
 247         uint16_t min_rth_distance;                  // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland
 248         uint16_t rth_abort_threshold;               // Initiate emergency landing if during RTH we get this much [cm] away from home
 249         uint16_t max_terrain_follow_altitude;       // Max altitude to be used in SURFACE TRACKING mode
 250         uint16_t safehome_max_distance;             // Max distance that a safehome is from the arming point
 251         uint16_t max_altitude;                      // Max altitude when in AltHold mode (not Surface Following)
 252     } general;
 253
 254     struct {
 255         uint8_t  max_bank_angle;                // multicopter max banking angle (deg)
 256         uint16_t auto_disarm_delay;             // multicopter safety delay for landing detector
 257
 258 #ifdef USE_MR_BRAKING_MODE
 259         uint16_t braking_speed_threshold;       // above this speed braking routine might kick in
 260         uint16_t braking_disengage_speed;       // below this speed braking will be disengaged
 261         uint16_t braking_timeout;               // Timeout for braking mode
 262         uint8_t  braking_boost_factor;          // Acceleration boost multiplier at max speed
 263         uint16_t braking_boost_timeout;         // Timeout for boost mode
 264         uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage
 265         uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage
 266         uint8_t  braking_bank_angle;            // Max angle [deg] that MR is allowed duing braking boost phase
 267 #endif
 268
 269         uint8_t posDecelerationTime;            // Brake time parameter
 270         uint8_t posResponseExpo;                // Position controller expo (taret vel expo for MC)
 271         bool slowDownForTurning;             // Slow down during WP missions when changing heading on next waypoint
 272     } mc;
 273
 274     struct {
 275         uint8_t  max_bank_angle;             // Fixed wing max banking angle (deg)
 276         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
 277         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
 278         uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
 279         uint8_t control_smoothness;          // The amount of smoothing to apply to controls for navigation
 280         uint16_t pitch_to_throttle_smooth;    // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
 281         uint8_t  pitch_to_throttle_thresh;   // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
 282         uint16_t loiter_radius;              // Loiter radius when executing PH on a fixed wing
 283         int8_t land_dive_angle;
 284         uint16_t launch_velocity_thresh;     // Velocity threshold for swing launch detection
 285         uint16_t launch_accel_thresh;        // Acceleration threshold for launch detection (cm/s/s)
 286         uint16_t launch_time_thresh;         // Time threshold for launch detection (ms)
 287         uint16_t launch_motor_timer;         // Time to wait before setting launch_throttle (ms)
 288         uint16_t launch_idle_motor_timer;    // Time to wait before motor starts at_idle throttle (ms)
 289         uint16_t launch_motor_spinup_time;   // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
 290         uint16_t launch_end_time;            // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
 291         uint16_t launch_min_time;                  // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
 292         uint16_t launch_timeout;             // Launch timeout to disable launch mode and swith to normal flight (ms)
 293         uint16_t launch_max_altitude;        // cm, altitude where to consider launch ended
 294         uint8_t  launch_climb_angle;         // Target climb angle for launch (deg)
 295         uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
 296         uint8_t  cruise_yaw_rate;            // Max yaw rate (dps) when CRUISE MODE is enabled
 297         bool     allow_manual_thr_increase;
 298         bool     useFwNavYawControl;
 299         uint8_t  yawControlDeadband;
 300         uint8_t  soaring_pitch_deadband;     // soaring mode pitch angle deadband (deg)
 301     } fw;
 302 } navConfig_t;
 303
 304 PG_DECLARE(navConfig_t, navConfig);
 305
 306 typedef struct gpsOrigin_s {
 307     bool    valid;
 308     float   scale;
 309     int32_t lat;    // Lattitude * 1e+7
 310     int32_t lon;    // Longitude * 1e+7
 311     int32_t alt;    // Altitude in centimeters (meters * 100)
 312 } gpsOrigin_t;
 313
 314 typedef enum {
 315     NAV_WP_ACTION_WAYPOINT  = 0x01,
 316     NAV_WP_ACTION_HOLD_TIME = 0x03,
 317     NAV_WP_ACTION_RTH       = 0x04,
 318     NAV_WP_ACTION_SET_POI   = 0x05,
 319     NAV_WP_ACTION_JUMP      = 0x06,
 320     NAV_WP_ACTION_SET_HEAD  = 0x07,
 321     NAV_WP_ACTION_LAND      = 0x08
 322 } navWaypointActions_e;
 323
 324 typedef enum {
 325     NAV_WP_HEAD_MODE_NONE  = 0,
 326     NAV_WP_HEAD_MODE_POI   = 1,
 327     NAV_WP_HEAD_MODE_FIXED = 2
 328 } navWaypointHeadings_e;
 329
 330 typedef enum {
 331     NAV_WP_FLAG_HOME = 0x48,
 332     NAV_WP_FLAG_LAST = 0xA5
 333 } navWaypointFlags_e;
 334
 335 typedef struct {
 336     uint8_t action;
 337     int32_t lat;
 338     int32_t lon;
 339     int32_t alt;
 340     int16_t p1, p2, p3;
 341     uint8_t flag;
 342 } navWaypoint_t;
 343
 344 typedef struct {
 345     navWaypointHeadings_e  mode;
 346     uint32_t heading; // fixed heading * 100 (SET_HEAD)
 347     fpVector3_t poi_pos; // POI location in local coordinates (SET_POI)
 348 } navWapointHeading_t;
 349
 350 typedef struct radar_pois_s {
 351     gpsLocation_t gps;
 352     uint8_t state;
 353     uint16_t heading; // °
 354     uint16_t speed; // cm/s
 355     uint8_t lq; // from 0 t o 4
 356     uint16_t distance; // m
 357     int16_t altitude; // m
 358     int16_t direction; // °
 359 } radar_pois_t;
 360
 361 #define RADAR_MAX_POIS 5
 362
 363 extern radar_pois_t radar_pois[RADAR_MAX_POIS];
 364
 365 typedef struct {
 366     fpVector3_t pos;
 367     int32_t     yaw;             // deg * 100
 368 } navWaypointPosition_t;
 369
 370 typedef struct navDestinationPath_s {
 371     uint32_t distance; // meters * 100
 372     int32_t bearing; // deg * 100
 373 } navDestinationPath_t;
 374
 375 typedef struct navigationPIDControllers_s {
 376     /* Multicopter PIDs */
 377     pidController_t   pos[XYZ_AXIS_COUNT];
 378     pidController_t vel[XYZ_AXIS_COUNT];
 379     pidController_t surface;
 380
 381     /* Fixed-wing PIDs */
 382     pidController_t fw_alt;
 383     pidController_t fw_nav;
 384     pidController_t fw_heading;
 385 } navigationPIDControllers_t;
 386
 387 /* MultiWii-compatible params for telemetry */
 388 typedef enum {
 389     MW_GPS_MODE_NONE = 0,
 390     MW_GPS_MODE_HOLD,
 391     MW_GPS_MODE_RTH,
 392     MW_GPS_MODE_NAV,
 393     MW_GPS_MODE_EMERG = 15
 394 } navSystemStatus_Mode_e;
 395
 396 typedef enum {
 397     MW_NAV_STATE_NONE = 0,                // None
 398     MW_NAV_STATE_RTH_START,               // RTH Start
 399     MW_NAV_STATE_RTH_ENROUTE,             // RTH Enroute
 400     MW_NAV_STATE_HOLD_INFINIT,            // PosHold infinit
 401     MW_NAV_STATE_HOLD_TIMED,              // PosHold timed
 402     MW_NAV_STATE_WP_ENROUTE,              // WP Enroute
 403     MW_NAV_STATE_PROCESS_NEXT,            // Process next
 404     MW_NAV_STATE_DO_JUMP,                 // Jump
 405     MW_NAV_STATE_LAND_START,              // Start Land (unused)
 406     MW_NAV_STATE_LAND_IN_PROGRESS,        // Land in Progress
 407     MW_NAV_STATE_LANDED,                  // Landed
 408     MW_NAV_STATE_LAND_SETTLE,             // Settling before land
 409     MW_NAV_STATE_LAND_START_DESCENT,      // Start descent
 410     MW_NAV_STATE_HOVER_ABOVE_HOME,        // Hover/Loitering above home
 411     MW_NAV_STATE_EMERGENCY_LANDING,       // Emergency landing
 412     MW_NAV_STATE_RTH_CLIMB,               // RTH Climb safe altitude
 413 } navSystemStatus_State_e;
 414
 415 typedef enum {
 416     MW_NAV_ERROR_NONE = 0,            //All systems clear
 417     MW_NAV_ERROR_TOOFAR,              //Next waypoint distance is more than safety distance
 418     MW_NAV_ERROR_SPOILED_GPS,         //GPS reception is compromised - Nav paused - copter is adrift !
 419     MW_NAV_ERROR_WP_CRC,              //CRC error reading WP data from EEPROM - Nav stopped
 420     MW_NAV_ERROR_FINISH,              //End flag detected, navigation finished
 421     MW_NAV_ERROR_TIMEWAIT,            //Waiting for poshold timer
 422     MW_NAV_ERROR_INVALID_JUMP,        //Invalid jump target detected, aborting
 423     MW_NAV_ERROR_INVALID_DATA,        //Invalid mission step action code, aborting, copter is adrift
 424     MW_NAV_ERROR_WAIT_FOR_RTH_ALT,    //Waiting to reach RTH Altitude
 425     MW_NAV_ERROR_GPS_FIX_LOST,        //Gps fix lost, aborting mission
 426     MW_NAV_ERROR_DISARMED,            //NAV engine disabled due disarm
 427     MW_NAV_ERROR_LANDING              //Landing
 428 } navSystemStatus_Error_e;
 429
 430 typedef enum {
 431     MW_NAV_FLAG_ADJUSTING_POSITION  = 1 << 0,
 432     MW_NAV_FLAG_ADJUSTING_ALTITUDE  = 1 << 1,
 433 } navSystemStatus_Flags_e;
 434
 435 typedef struct {
 436     navSystemStatus_Mode_e  mode;
 437     navSystemStatus_State_e state;
 438     navSystemStatus_Error_e error;
 439     navSystemStatus_Flags_e flags;
 440     uint8_t                 activeWpNumber;
 441     navWaypointActions_e    activeWpAction;
 442 } navSystemStatus_t;
 443
 444 void navigationUsePIDs(void);
 445 void navigationInit(void);
 446
 447 /* Position estimator update functions */
 448 void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs);
 449 void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs);
 450 void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt);
 451 void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs);
 452
 453 /* Navigation system updates */
 454 void updateWaypointsAndNavigationMode(void);
 455 void updatePositionEstimator(void);
 456 void applyWaypointNavigationAndAltitudeHold(void);
 457
 458 /* Functions to signal navigation requirements to main loop */
 459 bool navigationRequiresAngleMode(void);
 460 bool navigationRequiresThrottleTiltCompensation(void);
 461 bool navigationRequiresTurnAssistance(void);
 462 int8_t navigationGetHeadingControlState(void);
 463 // Returns wether arming is blocked by the navigation system.
 464 // If usedBypass is provided, it will indicate wether any checks
 465 // were bypassed due to user input.
 466 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass);
 467 bool navigationPositionEstimateIsHealthy(void);
 468 bool navIsCalibrationComplete(void);
 469 bool navigationTerrainFollowingEnabled(void);
 470
 471 /* Access to estimated position and velocity */
 472 typedef struct {
 473     uint8_t altStatus;
 474     uint8_t posStatus;
 475     uint8_t velStatus;
 476     uint8_t aglStatus;
 477     fpVector3_t pos;
 478     fpVector3_t vel;
 479     float agl;
 480 } navPositionAndVelocity_t;
 481
 482 float getEstimatedActualVelocity(int axis);
 483 float getEstimatedActualPosition(int axis);
 484 uint32_t getTotalTravelDistance(void);
 485 void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos);
 486
 487 /* Waypoint list access functions */
 488 int getWaypointCount(void);
 489 bool isWaypointListValid(void);
 490 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData);
 491 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData);
 492 void resetWaypointList(void);
 493 bool loadNonVolatileWaypointList(bool clearIfLoaded);
 494 bool saveNonVolatileWaypointList(void);
 495 #ifdef USE_MULTI_MISSION
 496 void selectMultiMissionIndex(int8_t increment);
 497 void setMultiMissionOnArm(void);
 498 #endif
 499 float getFinalRTHAltitude(void);
 500 int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);
 501
 502 /* Geodetic functions */
 503 typedef enum {
 504     GEO_ALT_ABSOLUTE,
 505     GEO_ALT_RELATIVE
 506 } geoAltitudeConversionMode_e;
 507
 508 typedef enum {
 509     GEO_ORIGIN_SET,
 510     GEO_ORIGIN_RESET_ALTITUDE
 511 } geoOriginResetMode_e;
 512
 513 typedef enum {
 514     NAV_WP_TAKEOFF_DATUM,
 515     NAV_WP_MSL_DATUM
 516 } geoAltitudeDatumFlag_e;
 517
 518 // geoSetOrigin stores the location provided in llh as a GPS origin in the
 519 // provided origin parameter. resetMode indicates wether all origin coordinates
 520 // should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving
 521 // other fields untouched (GEO_ORIGIN_RESET_ALTITUDE).
 522 void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode);
 523 // geoConvertGeodeticToLocal converts the geodetic location given in llh to
 524 // the local coordinate space and stores the result in pos. The altConv
 525 // indicates wether the altitude in llh is relative to the default GPS
 526 // origin (GEO_ALT_RELATIVE) or absolute (e.g. Earth frame)
 527 // (GEO_ALT_ABSOLUTE). If origin is invalid pos is set to
 528 // (0, 0, 0) and false is returned. It returns true otherwise.
 529 bool geoConvertGeodeticToLocal(fpVector3_t *pos, const gpsOrigin_t *origin, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv);
 530 // geoConvertGeodeticToLocalOrigin calls geoConvertGeodeticToLocal with the
 531 // default GPS origin.
 532 bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv);
 533 // geoConvertLocalToGeodetic converts a local point as provided in pos to
 534 // geodetic coordinates using the provided GPS origin. It returns wether
 535 // the provided origin is valid and the conversion could be performed.
 536 bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
 537 float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
 538 // Select absolute or relative altitude based on WP mission flag setting
 539 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
 540
 541 /* Distance/bearing calculation */
 542 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
 543 uint32_t distanceToFirstWP(void);
 544
 545 /* Failsafe-forced RTH mode */
 546 void activateForcedRTH(void);
 547 void abortForcedRTH(void);
 548 rthState_e getStateOfForcedRTH(void);
 549
 550 /* Failsafe-forced Emergency Landing mode */
 551 void activateForcedEmergLanding(void);
 552 void abortForcedEmergLanding(void);
 553 emergLandState_e getStateOfForcedEmergLanding(void);
 554
 555 /* Getter functions which return data about the state of the navigation system */
 556 bool navigationInAutomaticThrottleMode(void);
 557 bool navigationIsControllingThrottle(void);
 558 bool isFixedWingAutoThrottleManuallyIncreased(void);
 559 bool navigationIsFlyingAutonomousMode(void);
 560 bool navigationIsExecutingAnEmergencyLanding(void);
 561 bool navigationIsControllingAltitude(void);
 562 /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
 563  * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
 564  */
 565 bool navigationRTHAllowsLanding(void);
 566 bool isWaypointMissionRTHActive(void);
 567
 568 bool rthClimbStageActiveAndComplete(void);
 569
 570 bool isNavLaunchEnabled(void);
 571 uint8_t fixedWingLaunchStatus(void);
 572 const char * fixedWingLaunchStateMessage(void);
 573
 574 float calculateAverageSpeed(void);
 575
 576 const navigationPIDControllers_t* getNavigationPIDControllers(void);
 577
 578 int32_t navigationGetHeadingError(void);
 579 int32_t getCruiseHeadingAdjustment(void);
 580 bool isAdjustingPosition(void);
 581 bool isAdjustingHeading(void);
 582
 583 /* Returns the heading recorded when home position was acquired.
 584  * Note that the navigation system uses deg*100 as unit and angles
 585  * are in the [0, 360 * 100) interval.
 586  */
 587 int32_t navigationGetHomeHeading(void);
 588
 589 /* Compatibility data */
 590 extern navSystemStatus_t    NAV_Status;
 591
 592 extern int16_t navCurrentState;
 593 extern int16_t navActualVelocity[3];
 594 extern int16_t navDesiredVelocity[3];
 595 extern int32_t navTargetPosition[3];
 596 extern int32_t navLatestActualPosition[3];
 597 extern int16_t navActualSurface;
 598 extern uint16_t navFlags;
 599 extern uint16_t navEPH;
 600 extern uint16_t navEPV;
 601 extern int16_t navAccNEU[3];