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