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