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