7.1 mergeback

.gitignore

@@ -36,3 +36,4 @@ make/local.mk
 launch.json
 .vscode/tasks.json
 .vscode/c_cpp_properties.json
+/cmake-build-debug/

.vscode/settings.json

@@ -3,6 +3,8 @@
         "chrono": "c",
         "cmath": "c",
         "ranges": "c",
+        "navigation.h": "c",
+        "rth_trackback.h": "c"
         "platform.h": "c",
         "timer.h": "c",
         "bus.h": "c"

CMakeLists.txt

@@ -51,7 +51,7 @@ else()
     endif()
 endif()
 
-project(INAV VERSION 7.1.0)
+project(INAV VERSION 8.0.0)
 
 enable_language(ASM)
 

cmake/arm-none-eabi-checks.cmake

@@ -2,21 +2,18 @@ include(gcc)
 set(arm_none_eabi_triplet "arm-none-eabi")
 
 # Keep version in sync with the distribution files below
-set(arm_none_eabi_gcc_version "10.3.1")
+set(arm_none_eabi_gcc_version "13.2.1")
-set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu-rm/10.3-2021.10/gcc-arm-none-eabi-10.3-2021.10")
+# This is the output directory "pretty" name and URI name prefix
+set(base_dir_name "arm-gnu-toolchain-13.2.rel1")
+# This is the name inside the archive, which is no longer evincible from URI, alas
+set(archive_base_dir_name "arm-gnu-toolchain-13.2.Rel1")
+set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/${base_dir_name}")
 # suffix and checksum
-set(arm_none_eabi_win32 "win32.zip" 2bc8f0c4c4659f8259c8176223eeafc1)
+set(arm_none_eabi_win32 "mingw-w64-i686-arm-none-eabi.zip" 7fd677088038cdf82f33f149e2e943ee)
-set(arm_none_eabi_linux_amd64 "x86_64-linux.tar.bz2" 2383e4eb4ea23f248d33adc70dc3227e)
+set(arm_none_eabi_linux_amd64 "x86_64-arm-none-eabi.tar.xz" 791754852f8c18ea04da7139f153a5b7)
-set(arm_none_eabi_linux_aarch64 "aarch64-linux.tar.bz2" 3fe3d8bb693bd0a6e4615b6569443d0d)
+set(arm_none_eabi_linux_aarch64 "aarch64-arm-none-eabi.tar.xz" 5a08122e6d4caf97c6ccd1d29e62599c)
-set(arm_none_eabi_gcc_macos "mac.tar.bz2" 7f2a7b7b23797302a9d6182c6e482449)
+set(arm_none_eabi_darwin_amd64 "darwin-x86_64-arm-none-eabi.tar.xz" 41d49840b0fc676d2ae35aab21a58693)
-
+set(arm_none_eabi_darwin_aarch64 "darwin-arm64-arm-none-eabi.tar.xz" 2c43e9d72206c1f81227b0a685df5ea6)
-function(arm_none_eabi_gcc_distname var)
-    string(REPLACE "/" ";" url_parts ${arm_none_eabi_base_url})
-    list(LENGTH url_parts n)
-    math(EXPR last "${n} - 1")
-    list(GET url_parts ${last} basename)
-    set(${var} ${basename} PARENT_SCOPE)
-endfunction()
 
 function(host_uname_machine var)
     # We need to call uname -m manually, since at the point
@@ -47,7 +44,14 @@ function(arm_none_eabi_gcc_install)
             message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}")
         endif()
     elseif(CMAKE_HOST_SYSTEM_NAME STREQUAL "Darwin")
-        set(dist ${arm_none_eabi_gcc_macos})
+        host_uname_machine(machine)
+        if(machine STREQUAL "x86_64" OR machine STREQUAL "amd64")
+            set(dist ${arm_none_eabi_darwin_amd64})
+        elseif(machine STREQUAL "aarch64")
+            set(dist ${arm_none_eabi_darwin_aarch64})
+        else()
+            message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}")
+        endif()
     endif()
 
     if(dist STREQUAL "")
@@ -83,11 +87,27 @@ function(arm_none_eabi_gcc_install)
     if(NOT status EQUAL 0)
         message(FATAL_ERROR "error extracting ${basename}: ${status}")
     endif()
+    string(REPLACE "." ";" url_parts ${dist_suffix})
+    list(GET url_parts 0 host_dir_name)
+    set(dir_name "${archive_base_dir_name}-${host_dir_name}")
+    file(REMOVE_RECURSE "${TOOLS_DIR}/${base_dir_name}")
+    file(RENAME  "${TOOLS_DIR}/${dir_name}" "${TOOLS_DIR}/${base_dir_name}")
+    # This is **somewhat ugly**
+    # the newlib distributed by ARM generates suprious warnings from re-entrant POSIX functions
+    # that INAV doesn't use. These "harmless" warnings can be surpressed by removing the
+    # errant section from the only libnosys used by INAV ...
+    # So look the other way ... while this is "fixed"
+    execute_process(COMMAND arm-none-eabi-objcopy -w -R .gnu.warning.* "${TOOLS_DIR}/${base_dir_name}/arm-none-eabi/lib/thumb/v7e-m+fp/hard/libnosys.a"
+      RESULT_VARIABLE status
+      WORKING_DIRECTORY ${TOOLS_DIR}
+    )
+    if(NOT status EQUAL 0)
+        message(FATAL_ERROR "error fixing libnosys.a: ${status}")
+    endif()
 endfunction()
 
 function(arm_none_eabi_gcc_add_path)
-    arm_none_eabi_gcc_distname(dist_name)
+    set(gcc_path "${TOOLS_DIR}/${base_dir_name}/bin")
-    set(gcc_path "${TOOLS_DIR}/${dist_name}/bin")
     if(CMAKE_HOST_SYSTEM MATCHES ".*Windows.*")
         set(sep "\\;")
     else()
@@ -110,7 +130,7 @@ function(arm_none_eabi_gcc_check)
     message("-- found ${prog} ${version} at ${prog_path}")
     if(COMPILER_VERSION_CHECK AND NOT arm_none_eabi_gcc_version STREQUAL version)
         message("-- expecting ${prog} version ${arm_none_eabi_gcc_version}, but got version ${version} instead")
-        arm_none_eabi_gcc_install()
+	arm_none_eabi_gcc_install()
         return()
     endif()
 endfunction()

cmake/at32.cmake

@@ -9,7 +9,7 @@ option(SEMIHOSTING "Enable semihosting")
 message("-- DEBUG_HARDFAULTS: ${DEBUG_HARDFAULTS}, SEMIHOSTING: ${SEMIHOSTING}")
 
 set(CMSIS_DIR "${MAIN_LIB_DIR}/lib/main/AT32F43x/Drivers/CMSIS")
-set(CMSIS_INCLUDE_DIR "${CMSIS_DIR}/cm4/core_support") 
+set(CMSIS_INCLUDE_DIR "${CMSIS_DIR}/cm4/core_support")
 # DSP use common
 set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP")
 set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include")
@@ -50,8 +50,8 @@ main_sources(AT32_ASYNCFATFS_SRC
 )
 
 main_sources(AT32_MSC_SRC
-    msc/at32_msc_diskio.c 
+    msc/at32_msc_diskio.c
-    msc/emfat.c 
+    msc/emfat.c
     msc/emfat_file.c
 )
 
@@ -92,6 +92,7 @@ set(AT32_LINK_OPTIONS
     -Wl,--cref
     -Wl,--no-wchar-size-warning
     -Wl,--print-memory-usage
+    -Wl,--no-warn-rwx-segments
 )
 # Get target features
 macro(get_at32_target_features output_var dir target_name)
@@ -264,7 +265,7 @@ function(add_at32_executable)
     endif()
 endfunction()
 
-#  Main function of AT32 
+#  Main function of AT32
 function(target_at32)
     if(NOT arm-none-eabi STREQUAL TOOLCHAIN)
         return()

docs/ADSB.md

@@ -0,0 +1,17 @@
+# ADS-B
+
+[Automatic Dependent Surveillance Broadcast](https://en.wikipedia.org/wiki/Automatic_Dependent_Surveillance%E2%80%93Broadcast)
+is an air traffic surveillance technology that enables aircraft to be accurately tracked by air traffic controllers and other pilots without the need for conventional radar.
+
+## Current state
+
+OSD can be configured to shows the closest aircraft.
+
+## Hardware
+
+All ADSB receivers which can send Mavlink [ADSB_VEHICLE](https://mavlink.io/en/messages/common.html#ADSB_VEHICLE) message are supported 
+
+* [PINGRX](https://uavionix.com/product/pingrx-pro/) (not tested)
+* [TT-SC1](https://www.aerobits.pl/product/aero/) (tested)
+
+

docs/Boards.md

@@ -12,6 +12,9 @@ These boards are well tested with INAV and are known to be of good quality and r
 | [Holybro Kakute H7](https://inavflight.com/shop/s/bg/1914066)       | H7         | KAKUTEH7               | All  | All     | All            | All       | All      | SERIAL, SD     |
 
 It's possible to find more supported and tested boards [here](https://github.com/iNavFlight/inav/wiki/Welcome-to-INAV,-useful-links-and-products)
+
+There is also a [full list of all supported boards](https://github.com/iNavFlight/inav/wiki/Boards,-Targets-and-PWM-allocations).
+
 ### Boards documentation
 
 See the [docs/boards](https://github.com/iNavFlight/inav/tree/master/docs/boards) folder for additional information regards to many targets in INAV, to example help in finding pinout and features. _Feel free to help improve the docs._

docs/ESC and servo outputs.md

@@ -28,23 +28,8 @@ While motors are usually ordered sequentially, here is no standard output layout
 
 ## Modifying output mapping
 
-### Modifying all outputs at the same time
-
-Since INAV 5, it has been possible to force *ALL* outputs to be `MOTORS` or `SERVOS`.
-
-Traditional ESCs usually can be controlled via a servo output, but would require calibration.
-
-This can be done with the `output_mode` CLI setting. Allowed values:
-
-* `AUTO` assigns outputs according to the default mapping
-* `SERVOS` assigns all outputs to servos
-* `MOTORS` assigns all outputs to motors
-
-### Modifying only some outputs
-
 INAV 7 introduced extra functionality that let you force only some outputs to be either *MOTORS* or *SERVOS*, with some restrictions dictated by the hardware.
 
-The mains restrictions is that outputs need to be associated with timers, which are usually shared between multiple outputs. Two outputs on the same timer need to have the same function.
+The main restrictions is that outputs are associated with timers, which can be shared between multiple outputs and  two outputs on the same timer need to have the same function.
 
 The easiest way to modify outputs, is to use the Mixer tab in the Configurator, as it will clearly show you which timer is used by all outputs, but you can also use `timer_output_mode` on the cli.
-This can be used in conjunction to the previous method, in that cass all outputs will follow `output_mode` and `timer_output_mode` overrides are applied after that.

docs/GPS_fix_estimation.md

@@ -0,0 +1,132 @@
+# GPS Fix estimation (dead reconing, RTH without GPS) for fixed wing
+
+Video demonstration
+
+[![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/wzvgRpXCS4U/0.jpg)](https://www.youtube.com/watch?v=wzvgRpXCS4U)
+
+There is possibility to allow plane to estimate it's position when GPS fix is lost.
+The main purpose is RTH without GPS.
+It works for fixed wing only.
+
+Plane should have the following sensors:
+- acceleromenter, gyroscope
+- barometer
+- GPS
+- magnethometer (optional, highly recommended)
+- pitot (optional)
+
+By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost.
+
+GPS fix estimation allows to recover plane using magnetometer and baromener only.
+
+GPS Fix is also estimated on GPS Sensor timeouts (hardware failures).
+
+Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated.
+
+# How it works ?
+
+In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints.
+
+Without GPS fix, plane has nose heading from magnetometer and height from barometer only.
+
+To navigate without GPS fix, we make the following assumptions:
+- plane is flying in the direction where nose is pointing
+- (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings
+
+It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available).
+
+From estimated heading direction and speed, plane is able to **roughly** estimate it's position.
+
+It is assumed, that plane will fly in roughly estimated direction to home position untill either GPS fix or RC signal is recovered.
+
+*Plane has to acquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*.
+
+# Estimation without magnethometer
+
+Without magnethometer, navigation accuracy is very poor. The problem is heading drift. 
+
+The longer plane flies without magnethometer or GPS, the bigger is course estimation error.
+
+After few minutes and few turns, "North" direction estimation can be completely broken.
+In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with occasional GPS outages.
+
+![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da)
+
+(purple line - estimated position, black line - real position).
+
+It is recommened to use GPS fix estimation without magnethometer as last resort only. For example, if plane is flying above lake, landing means loss of plane. With GPS Fix estimation, plane will try to do RTH in very rought direction, instead of landing.
+
+It is up to user to estimate the risk of fly-away.
+
+
+# Settings
+
+GPS Fix estimation is enabled with CLI command:
+
+```set inav_allow_gps_fix_estimation=ON```
+
+Also you have to specify cruise airspeed of the plane.
+
+To find out cruise airspeed, make a test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed.
+
+Cruise airspeed is specified in cm/s.
+
+To convert km/h to m/s, multiply by 27.77.
+
+
+Example: 100 km/h = 100 * 27.77 = 2777 cm/s
+
+```set fw_reference_airspeed=2777```
+
+*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Increase cruise speed with throttle"  - do not use it without GPS Fix.*
+
+*If pitot is available, pitot sensor data will be used instead of constant. It is not necessary to specify fw_reference_airspeed. However, it is still adviced to specify for the case of pitot failure.*
+
+*Note related command: to continue mission without RC signal, see command ```set failsafe_mission_delay=-1```.*
+
+**After entering CLI command, make sure that settings are saved:**
+
+```save```
+
+# Disabling GPS sensor from RC controller
+
+![](Screenshots/programming_disable_gps_sensor_fix.png) 
+
+For testing purposes, it is possible to disable GPS sensor fix from RC controller in programming tab:
+
+*GPS can be disabled only after: 1) initial GPS fix is acquired 2) in ARMED mode.*
+
+# Allowing wp missions with GPS Fix estimation
+
+```failsafe_gps_fix_estimation_delay```
+
+Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. RTH is trigerred regradless of failsafe procedure selected in configurator.
+
+# Expected error (mag + baro)
+
+Realistic expected error is up to 200m per 1km of flight path. In tests, 500m drift per 5km path was seen. 
+
+To dicrease drift:
+- fly one large circle with GPS available to get good wind estimation
+- use airspeed sensor. If airspeed sensor is not installed, fly in cruise mode without throttle override.
+- do smooth, large turns
+- make sure compass is pointing in nose direction precicely
+- calibrate compass correctly
+
+This video shows real world test where GPS was disabled occasionally. Wind is 10km/h south-west:
+
+
+https://github.com/RomanLut/inav/assets/11955117/0599a3c3-df06-4d40-a32a-4d8f96140592
+
+
+Purple line shows estimated position. Black line shows real position. "EST ERR" sensor shows estimation error in metters. Estimation is running when satellite icon displays "ES". Estimated position snaps to real position when GPS fix is reaquired.
+
+
+# Is it possible to implement this for multirotor ?
+
+There are some ideas, but there is no solution now. We can not make assumptions with multirotor which we can make with a fixed wing.
+
+
+# Links
+
+INAV HITL  https://github.com/RomanLut/INAV-X-Plane-HITL

docs/SITL/SITL.md

@@ -79,7 +79,7 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/)
 For SBUS, the command line arguments of the python script are:
 ```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000```
 
-### Telemtry
+### Telemetry
 
 LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP.
 

docs/Settings.md

@@ -1012,6 +1012,16 @@ Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active
 
 ---
 
+### failsafe_gps_fix_estimation_delay
+
+Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 7 | -1 | 600 |
+
+---
+
 ### failsafe_lights
 
 Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF].
@@ -1772,6 +1782,16 @@ Defines if INAV will dead-reckon over short GPS outages. May also be useful for
 
 ---
 
+### inav_allow_gps_fix_estimation
+
+Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### inav_auto_mag_decl
 
 Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored.
@@ -2762,16 +2782,6 @@ Craft name
 
 ---
 
-### nav_auto_climb_rate
-
-Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 500 | 10 | 2000 |
-
----
-
 ### nav_auto_disarm_delay
 
 Delay before craft disarms when `nav_disarm_on_landing` is set (ms)
@@ -2798,7 +2808,7 @@ Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 20 | 0 | 120 |
+| 60 | 0 | 120 |
 
 ---
 
@@ -3162,6 +3172,16 @@ PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]
 
 ---
 
+### nav_fw_manual_climb_rate
+
+Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 300 | 10 | 2500 |
+
+---
+
 ### nav_fw_max_thr
 
 Maximum throttle for flying wing in GPS assisted modes
@@ -3432,16 +3452,6 @@ Allows immediate landing detection based on G bump at touchdown when set to ON.
 
 ---
 
-### nav_manual_climb_rate
-
-Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 200 | 10 | 2000 |
-
----
-
 ### nav_manual_speed
 
 Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]
@@ -3492,6 +3502,16 @@ If set to STICK the FC remembers the throttle stick position when enabling ALTHO
 
 ---
 
+### nav_mc_auto_climb_rate
+
+Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 10 | 2000 |
+
+---
+
 ### nav_mc_bank_angle
 
 Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude
@@ -3602,6 +3622,16 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
 
 ---
 
+### nav_mc_manual_climb_rate
+
+Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 200 | 10 | 2000 |
+
+---
+
 ### nav_mc_pos_deceleration_time
 
 Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting
@@ -4072,6 +4102,36 @@ _// TODO_
 
 ---
 
+### osd_adsb_distance_alert
+
+Distance inside which ADSB data flashes for proximity warning
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3000 | 1 | 64000 |
+
+---
+
+### osd_adsb_distance_warning
+
+Distance in meters of ADSB aircraft that is displayed
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 20000 | 1 | 64000 |
+
+---
+
+### osd_adsb_ignore_plane_above_me_limit
+
+Ignore adsb planes above, limit, 0 disabled (meters)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 64000 |
+
+---
+
 ### osd_ahi_bordered
 
 Shows a border/corners around the AHI region (pixel OSD only)
@@ -5928,7 +5988,7 @@ The percentage of the throttle range (`max_throttle` - `min_command`) above `min
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 15 | 0 | 30 |
+| 8 | 0 | 30 |
 
 ---
 

docs/boards/KAKUTEH7WING.md

@@ -0,0 +1,10 @@
+# Board - [KAKUTEH7WING](https://holybro.com/products/kakute-h743-wing)
+
+[manufacturer manual](https://cdn.shopify.com/s/files/1/0604/5905/7341/files/Holybro_KakuteH743-Wing_Manual_v1.0_C.pdf?v=1693393206)
+
+Note that this board has two i2c plugs.
+The six-pin plug should be used for GPS and compass.
+The 4-pin plug should be used for other i2c sesors such as i2c pitot (airspeed sensor), rangefinder, and external
+temperature sensors.
+
+![KAKUTEH7WING wiring diagram](../assets/images/KAKUTEH7WING-wiring-diagram.webp)

docs/boards/SPEEDYBEEF405V3.md

@@ -1,6 +1,6 @@
 # SpeedyBee F405 V3
 
-> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead
+> Notice: DSHOT on SpeedyBe F405 V3 requires INAV 7.0.0 or later. If you are using an older version, use MULTISHOT instead.
 
 > Notice: The analog OSD and the SD card (blackbox) share the same SPI bus, which can cause problems when trying to use analog OSD and blackbox at the same time.
 

docs/development/serial_printf_debugging.md

@@ -100,7 +100,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa
 
 * [msp-tool](https://github.com/fiam/msp-tool)
 * [mwp](https://github.com/stronnag/mwptools)
-* [dbg-tool](https://github.com/stronnag/mwptools)
+* [dbg-tool](https://codeberg.org/stronnag/dbg-tool)
 * [INAV Configurator](https://github.com/iNavFlight/inav-configurator)
 
 For example, with the final lines of `src/main/fc/fc_init.c` set to:

docs/development/wp_mission_schema/README.md

@@ -13,7 +13,16 @@ The format is defined the XSD schema here.
 * The `mwp` tag was introduced by the eponymous mission planner. Other mission planners may consider that reusing some of the tags (`cx`, `cy` - centre location, `zoom` TMS zoom level, `home-x`, `home-y` - home location) is useful.
 * `meta` may be used as a synonym for `mwp`.
 * The `version` tag may be intepreted by mission planners as they see fit. For example, the (obsolete) Android 'ez-gui' application requires '2.3-pre8'. For multi-mission files it is recommended to use another `version`.
-* the `mwp` / `meta` element may be interleaved with `missionitem` in a multi-mission file to provide mission segment specific home, centre locations and zoom.
+* The `mwp` / `meta` element may be interleaved with `missionitem` in a multi-mission file to provide mission segment specific home, centre locations and zoom.
+* The `fwapproach` element defines INAV 7.1.0 and later Autoland parameters for the mission.
+
+## Validation
+
+You can check that your files validate using the open source `xmlint` tool.
+
+```
+xmllint --schema docs/development/wp_mission_schema/mw-mission.xsd  test.mission --noout
+```
 
 ## Examples
 

docs/development/wp_mission_schema/mw-mission.xsd

@@ -12,6 +12,7 @@
         <xs:choice minOccurs="0" maxOccurs="unbounded">
           <xs:element ref="missionitem"/>
           <xs:element ref="mwp"/>
+          <xs:element ref="fwapproach"/>
         </xs:choice>
       </xs:sequence>
     </xs:complexType>
@@ -21,6 +22,25 @@
       <xs:attribute name="value" use="required"/>
     </xs:complexType>
   </xs:element>
+  <xs:element name="fwapproach">
+    <xs:complexType>
+      <xs:attribute name="no" use="required" type="xs:integer"/>
+      <xs:attribute name="index" use="required" type="xs:integer"/>
+      <xs:attribute name="approachalt" use="required" type="xs:integer"/>
+      <xs:attribute name="landalt" use="required" type="xs:integer"/>
+      <xs:attribute name="landheading1" use="required" type="xs:integer"/>
+      <xs:attribute name="landheading2" use="required" type="xs:integer"/>
+      <xs:attribute name="approachdirection" use="required">
+	<xs:simpleType>
+	  <xs:restriction base="xs:token">
+	    <xs:enumeration value="left"/>
+	    <xs:enumeration value="right"/>
+	  </xs:restriction>
+	</xs:simpleType>
+      </xs:attribute>
+      <xs:attribute name="sealevelref" use="required" type="xs:boolean"/>
+    </xs:complexType>
+  </xs:element>
   <xs:element name="missionitem">
     <xs:complexType>
       <xs:attribute name="action" use="required">

lib/main/MAVLink/common/common.h

@@ -48,7 +48,7 @@ typedef enum FIRMWARE_VERSION_TYPE
 } FIRMWARE_VERSION_TYPE;
 #endif
 
-/** @brief Flags to report failure cases over the high latency telemtry. */
+/** @brief Flags to report failure cases over the high latency telemetry. */
 #ifndef HAVE_ENUM_HL_FAILURE_FLAG
 #define HAVE_ENUM_HL_FAILURE_FLAG
 typedef enum HL_FAILURE_FLAG

src/main/CMakeLists.txt

@@ -344,6 +344,7 @@ main_sources(COMMON_SRC
     flight/ez_tune.c
     flight/ez_tune.h
 
+    io/adsb.c
     io/beeper.c
     io/beeper.h
     io/servo_sbus.c
@@ -560,6 +561,8 @@ main_sources(COMMON_SRC
     navigation/navigation_rover_boat.c
     navigation/sqrt_controller.c
     navigation/sqrt_controller.h
+    navigation/rth_trackback.c
+    navigation/rth_trackback.h
 
     sensors/barometer.c
     sensors/barometer.h

src/main/cms/cms_menu_navigation.c

@@ -45,8 +45,9 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
     OSD_SETTING_ENTRY("MC NAV SPEED", SETTING_NAV_AUTO_SPEED),
     OSD_SETTING_ENTRY("MC MAX NAV SPEED", SETTING_NAV_MAX_AUTO_SPEED),
     OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED),
-    OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
+    OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_MC_AUTO_CLIMB_RATE),
-    OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
+    OSD_SETTING_ENTRY("MAX MC AH CLIMB RATE", SETTING_NAV_MC_MANUAL_CLIMB_RATE),
+    OSD_SETTING_ENTRY("MAX FW AH CLIMB RATE", SETTING_NAV_FW_MANUAL_CLIMB_RATE),
     OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
     OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE),
     OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),

src/main/common/maths.h

@@ -36,7 +36,7 @@
 #define RAD (M_PIf / 180.0f)
 
 #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
-#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f)
+#define CENTIDEGREES_TO_DEGREES(angle) ((angle) * 0.01f)
 
 #define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f)
 #define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10)
@@ -54,11 +54,11 @@
 #define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD)
 
 #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
-#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
+#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD)
 
 #define CENTIMETERS_TO_CENTIFEET(cm)    (cm / 0.3048f)
 #define CENTIMETERS_TO_FEET(cm)         (cm / 30.48f)
-#define CENTIMETERS_TO_METERS(cm)       (cm / 100.0f)
+#define CENTIMETERS_TO_METERS(cm)       (cm * 0.01f)
 
 #define METERS_TO_CENTIMETERS(m)        (m * 100)
 #define METERS_TO_KILOMETERS(m)         (m / 1000.0f)

src/main/drivers/display.c

@@ -178,13 +178,13 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
 
 #ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-		//some FCs do not power max7456 from USB power 
+        //some FCs do not power max7456 from USB power 
-		//driver can not read font metadata 
+        //driver can not read font metadata 
-		//chip assumed to not support second bank of font
+        //chip assumed to not support second bank of font
-		//artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar())
+        //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar())
-		//return dummy metadata to let all OSD elements to work in simulator mode
+        //return dummy metadata to let all OSD elements to work in simulator mode
-		instance->maxChar = 512;
+        instance->maxChar = 512;
-	}
+    }
 #endif
 
     if (c > instance->maxChar) {

src/main/drivers/osd_symbols.h

@@ -180,6 +180,8 @@
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
 
+#define SYM_ADSB                    0xFD // 253 ADSB
+
 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo
 #define SYM_LOGO_WIDTH              10
 #define SYM_LOGO_HEIGHT             4

src/main/drivers/sound_beeper.c

@@ -48,10 +48,10 @@ void systemBeep(bool onoff)
 
 #ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-		if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) {
+        if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) {
-			onoff = false;
+            onoff = false;
-		}
+        }
-	}
+    }
 #endif
 
     if (beeperConfig()->pwmMode) {

src/main/fc/cli.c

@@ -143,8 +143,9 @@ static void cliAssert(char *cmdline);
 #endif
 
 #ifdef USE_CLI_BATCH
-static bool commandBatchActive = false;
+static bool     commandBatchActive = false;
-static bool commandBatchError = false;
+static bool     commandBatchError = false;
+static uint8_t  commandBatchErrorCount = 0;
 #endif
 
 // sync this with features_e
@@ -257,6 +258,7 @@ static void cliPrintError(const char *str)
 #ifdef USE_CLI_BATCH
     if (commandBatchActive) {
         commandBatchError = true;
+        commandBatchErrorCount++;
     }
 #endif
 }
@@ -268,6 +270,7 @@ static void cliPrintErrorLine(const char *str)
 #ifdef USE_CLI_BATCH
     if (commandBatchActive) {
         commandBatchError = true;
+        commandBatchErrorCount++;
     }
 #endif
 }
@@ -370,6 +373,7 @@ static void cliPrintErrorVa(const char *format, va_list va)
 #ifdef USE_CLI_BATCH
     if (commandBatchActive) {
         commandBatchError = true;
+        commandBatchErrorCount++;
     }
 #endif
 }
@@ -661,6 +665,7 @@ static void cliAssert(char *cmdline)
 #ifdef USE_CLI_BATCH
         if (commandBatchActive) {
             commandBatchError = true;
+            commandBatchErrorCount++;
         }
 #endif
     }
@@ -3419,7 +3424,10 @@ static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask)
 #ifdef USE_CLI_BATCH
 static void cliPrintCommandBatchWarning(const char *warning)
 {
-    cliPrintErrorLinef("ERRORS WERE DETECTED - PLEASE REVIEW BEFORE CONTINUING");
+    char errorBuf[59];
+    tfp_sprintf(errorBuf, "%d ERRORS WERE DETECTED - Please review and fix before continuing!", commandBatchErrorCount);
+
+    cliPrintErrorLinef(errorBuf);
     if (warning) {
         cliPrintErrorLinef(warning);
     }
@@ -3429,6 +3437,7 @@ static void resetCommandBatch(void)
 {
     commandBatchActive = false;
     commandBatchError = false;
+    commandBatchErrorCount = 0;
 }
 
 static void cliBatch(char *cmdline)
@@ -3437,6 +3446,7 @@ static void cliBatch(char *cmdline)
         if (!commandBatchActive) {
             commandBatchActive = true;
             commandBatchError = false;
+            commandBatchErrorCount = 0;
         }
         cliPrintLine("Command batch started");
     } else if (strncasecmp(cmdline, "end", 3) == 0) {

src/main/fc/fc_msp.c

@@ -83,6 +83,7 @@
 #include "config/config_eeprom.h"
 #include "config/feature.h"
 
+#include "io/adsb.h"
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
@@ -950,6 +951,33 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, gpsSol.epv);
         break;
 #endif
+    case MSP2_ADSB_VEHICLE_LIST:
+#ifdef USE_ADSB
+        sbufWriteU8(dst, MAX_ADSB_VEHICLES);
+        sbufWriteU8(dst, ADSB_CALL_SIGN_MAX_LENGTH);
+
+        for(uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++){
+
+            adsbVehicle_t *adsbVehicle = findVehicle(i);
+
+            for(uint8_t ii = 0; ii < ADSB_CALL_SIGN_MAX_LENGTH; ii++){
+                sbufWriteU8(dst, adsbVehicle->vehicleValues.callsign[ii]);
+            }
+
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.icao);
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.lat);
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.lon);
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.alt);
+            sbufWriteU16(dst, (uint16_t)CENTIDEGREES_TO_DEGREES(adsbVehicle->vehicleValues.heading));
+            sbufWriteU8(dst,  adsbVehicle->vehicleValues.tslc);
+            sbufWriteU8(dst,  adsbVehicle->vehicleValues.emitterType);
+            sbufWriteU8(dst,  adsbVehicle->ttl);
+        }
+#else
+        sbufWriteU8(dst, 0);
+        sbufWriteU8(dst, 0);
+#endif
+            break;
     case MSP_DEBUG:
         // output some useful QA statistics
         // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
@@ -1085,7 +1113,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             legacyLedConfig |= ledConfig->led_function << shiftCount;
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
-            shiftCount += 6;
+            shiftCount += 6; 
             legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
@@ -1312,9 +1340,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP_NAV_POSHOLD:
         sbufWriteU8(dst, navConfig()->general.flags.user_control_mode);
         sbufWriteU16(dst, navConfig()->general.max_auto_speed);
-        sbufWriteU16(dst, navConfig()->general.max_auto_climb_rate);
+        sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate);
         sbufWriteU16(dst, navConfig()->general.max_manual_speed);
-        sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
+        sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate);
         sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
         sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
         sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
@@ -1520,6 +1548,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #else
         sbufWriteU16(dst, 0);
         sbufWriteU16(dst, 0);
+#endif
+#ifdef USE_ADSB
+        sbufWriteU16(dst, osdConfig()->adsb_distance_warning);
+        sbufWriteU16(dst, osdConfig()->adsb_distance_alert);
+#else
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
 #endif
         break;
 
@@ -2359,9 +2394,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         if (dataSize == 13) {
             navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
             navConfigMutable()->general.max_auto_speed = sbufReadU16(src);
-            navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src);
+            navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
             navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
-            navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
+            if (mixerConfig()->platformType != PLATFORM_AIRPLANE) {
+                navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
+            }else{
+                navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src);
+            }
             navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
             navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
             currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
@@ -2904,7 +2943,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             ledConfig->led_position = legacyConfig & 0xFF;
             ledConfig->led_function = (legacyConfig >> 8) & 0xF;
             ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
-            ledConfig->led_color = (legacyConfig >> 18) & 0xF;
+            ledConfig->led_color = (legacyConfig >> 18) & 0xF; 
             ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
             ledConfig->led_params = (legacyConfig >> 28) & 0xF;
 
@@ -3521,7 +3560,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
 bool isOSDTypeSupportedBySimulator(void)
 {
 #ifdef USE_OSD
-	displayPort_t *osdDisplayPort = osdGetDisplayPort();
+    displayPort_t *osdDisplayPort = osdGetDisplayPort();
 	return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar);
 #else
     return false;
@@ -3748,132 +3787,130 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 #endif
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
-		tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
+        tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
-
+        
         // Check the MSP version of simulator
-		if (tmp_u8 != SIMULATOR_MSP_VERSION) {
+        if (tmp_u8 != SIMULATOR_MSP_VERSION) {
             break;
         }
 
-		simulatorData.flags = sbufReadU8(src);
+        simulatorData.flags = sbufReadU8(src);
 
         if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) {
 
-			if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
+            if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
-				DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
+                DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
 
 #ifdef USE_BARO
             if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
-				baroStartCalibration();
+                baroStartCalibration();
             }
 #endif
 #ifdef USE_MAG
-				DISABLE_STATE(COMPASS_CALIBRATED);
+                DISABLE_STATE(COMPASS_CALIBRATED);
-				compassInit();
+                compassInit();
 #endif
-				simulatorData.flags = HITL_RESET_FLAGS;
+                simulatorData.flags = HITL_RESET_FLAGS;
                 // Review: Many states were affected. Reboot?
 
-				disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
+                disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
 			}
         } else {
-			if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
+            if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
 #ifdef USE_BARO
                 if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
                     sensorsSet(SENSOR_BARO);
                     setTaskEnabled(TASK_BARO, true);
                     DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
-				    baroStartCalibration();
+                    baroStartCalibration();
                 }
 #endif
 
 #ifdef USE_MAG
-				if (compassConfig()->mag_hardware != MAG_NONE) {
+                if (compassConfig()->mag_hardware != MAG_NONE) {
-					sensorsSet(SENSOR_MAG);
+                    sensorsSet(SENSOR_MAG);
-					ENABLE_STATE(COMPASS_CALIBRATED);
+                    ENABLE_STATE(COMPASS_CALIBRATED);
-					DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
+                    DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
-					mag.magADC[X] = 800;
+                    mag.magADC[X] = 800;
-					mag.magADC[Y] = 0;
+                    mag.magADC[Y] = 0;
-					mag.magADC[Z] = 0;
+                    mag.magADC[Z] = 0;
-				}
+                }
 #endif
-				ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
+                ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
                 ENABLE_STATE(ACCELEROMETER_CALIBRATED);
-				LOG_DEBUG(SYSTEM, "Simulator enabled");
+                LOG_DEBUG(SYSTEM, "Simulator enabled");
-			}
+            }
 
-			if (dataSize >= 14) {
+            if (dataSize >= 14) {
 
-				if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
+                if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
-					gpsSol.fixType = sbufReadU8(src);
+                    gpsSolDRV.fixType = sbufReadU8(src);
-					gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
+                    gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
-					gpsSol.flags.hasNewData = true;
+                    gpsSolDRV.numSat = sbufReadU8(src);
-					gpsSol.numSat = sbufReadU8(src);
 
-					if (gpsSol.fixType != GPS_NO_FIX) {
+                    if (gpsSolDRV.fixType != GPS_NO_FIX) {
-						gpsSol.flags.validVelNE = true;
+                        gpsSolDRV.flags.validVelNE = true;
-						gpsSol.flags.validVelD = true;
+                        gpsSolDRV.flags.validVelD = true;
-						gpsSol.flags.validEPE = true;
+                        gpsSolDRV.flags.validEPE = true;
-						gpsSol.flags.validTime = false;
+                        gpsSolDRV.flags.validTime = false;
 
-						gpsSol.llh.lat = sbufReadU32(src);
+                        gpsSolDRV.llh.lat = sbufReadU32(src);
-						gpsSol.llh.lon = sbufReadU32(src);
+                        gpsSolDRV.llh.lon = sbufReadU32(src);
-						gpsSol.llh.alt = sbufReadU32(src);
+                        gpsSolDRV.llh.alt = sbufReadU32(src);
-						gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src);
-						gpsSol.groundCourse = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src);
 
-						gpsSol.velNED[X] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src);
-						gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src);
-						gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src);
 
-						gpsSol.eph = 100;
+                        gpsSolDRV.eph = 100;
-						gpsSol.epv = 100;
+                        gpsSolDRV.epv = 100;
-
+                    } else {
-						ENABLE_STATE(GPS_FIX);
+                        sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
-					} else {
+                    }
-						sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
-					}
                     // Feed data to navigation
-					gpsProcessNewSolutionData();
+                    gpsProcessNewDriverData();
-				} else {
+                    gpsProcessNewSolutionData(false);
-					sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
+                } else {
-				}
+                    sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
-
+                }
-				if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
-					attitude.values.roll = (int16_t)sbufReadU16(src);
-					attitude.values.pitch = (int16_t)sbufReadU16(src);
-					attitude.values.yaw = (int16_t)sbufReadU16(src);
-				} else {
-					sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
-				}
 
+                if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
+                    attitude.values.roll = (int16_t)sbufReadU16(src);
+                    attitude.values.pitch = (int16_t)sbufReadU16(src);
+                    attitude.values.yaw = (int16_t)sbufReadU16(src);
+                } else {
+                    sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
+                }
+                
                 // Get the acceleration in 1G units
-				acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
+                acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
-				acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
+                acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
-				acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
+                acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
-				acc.accVibeSq[X] = 0.0f;
+                acc.accVibeSq[X] = 0.0f;
-				acc.accVibeSq[Y] = 0.0f;
+                acc.accVibeSq[Y] = 0.0f;
-				acc.accVibeSq[Z] = 0.0f;
+                acc.accVibeSq[Z] = 0.0f;
-
+                
                 // Get the angular velocity in DPS
-				gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
+                gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
-				gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
+                gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
-				gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
+                gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
 
-				if (sensors(SENSOR_BARO)) {
+                if (sensors(SENSOR_BARO)) {
-					baro.baroPressure = (int32_t)sbufReadU32(src);
+                    baro.baroPressure = (int32_t)sbufReadU32(src);
-					baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
+                    baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
-				} else {
+                } else {
-					sbufAdvance(src, sizeof(uint32_t));
+                    sbufAdvance(src, sizeof(uint32_t));
-				}
+                }
 
-				if (sensors(SENSOR_MAG)) {
+                if (sensors(SENSOR_MAG)) {
-					mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  // 16000 / 20 = 800uT
+                    mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  // 16000 / 20 = 800uT
-					mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;   //note that mag failure is simulated by setting all readings to zero
+                    mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;   //note that mag failure is simulated by setting all readings to zero
-					mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
+                    mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
-				} else {
+                } else {
-					sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
+                    sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
-				}
+                }
 
                 if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) {
                     simulatorData.vbat = sbufReadU8(src);
@@ -3883,44 +3920,44 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
                 if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
                     simulatorData.airSpeed = sbufReadU16(src);
-			    } else {
+                } else {
                     if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
-                        sbufReadU16(src);
+                        sbufReadU16(src); 
                     }
                 }
 
                 if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
                     simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
                 }
-			} else {
+            } else {
-				DISABLE_STATE(GPS_FIX);
+                DISABLE_STATE(GPS_FIX);
-			}
+            }
-		}
+        }
 
-		sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
+        sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
-		sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
+        sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
-		sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
+        sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
-		sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
+        sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
 
-		simulatorData.debugIndex++;
+        simulatorData.debugIndex++;
-		if (simulatorData.debugIndex == 8) {
+        if (simulatorData.debugIndex == 8) {
-			simulatorData.debugIndex = 0;
+            simulatorData.debugIndex = 0;
-		}
+        }
 
-		tmp_u8 = simulatorData.debugIndex |
+        tmp_u8 = simulatorData.debugIndex |
-			((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
+            ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
-			(ARMING_FLAG(ARMED) ? 64 : 0) |
+            (ARMING_FLAG(ARMED) ? 64 : 0) |
-			(!feature(FEATURE_OSD) ? 32: 0) |
+            (!feature(FEATURE_OSD) ? 32: 0) |
-			(!isOSDTypeSupportedBySimulator() ? 16 : 0);
+            (!isOSDTypeSupportedBySimulator() ? 16 : 0);
 
-		sbufWriteU8(dst, tmp_u8);
+        sbufWriteU8(dst, tmp_u8);
-		sbufWriteU32(dst, debug[simulatorData.debugIndex]);
+        sbufWriteU32(dst, debug[simulatorData.debugIndex]);
 
-		sbufWriteU16(dst, attitude.values.roll);
+        sbufWriteU16(dst, attitude.values.roll);
-		sbufWriteU16(dst, attitude.values.pitch);
+        sbufWriteU16(dst, attitude.values.pitch);
-		sbufWriteU16(dst, attitude.values.yaw);
+        sbufWriteU16(dst, attitude.values.yaw);
 
-		mspWriteSimulatorOSD(dst);
+        mspWriteSimulatorOSD(dst);
 
         *ret = MSP_RESULT_ACK;
         break;

src/main/fc/fc_tasks.c

@@ -69,6 +69,7 @@
 #include "io/osd_dji_hd.h"
 #include "io/displayport_msp_osd.h"
 #include "io/servo_sbus.h"
+#include "io/adsb.h"
 
 #include "msp/msp_serial.h"
 
@@ -181,6 +182,14 @@ void taskUpdateCompass(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef USE_ADSB
+void taskAdsb(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    adsbTtlClean(currentTimeUs);
+}
+#endif
+
 #ifdef USE_BARO
 void taskUpdateBaro(timeUs_t currentTimeUs)
 {
@@ -360,6 +369,9 @@ void fcTasksInit(void)
 #ifdef USE_PITOT
     setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT));
 #endif
+#ifdef USE_ADSB
+    setTaskEnabled(TASK_ADSB, true);
+#endif
 #ifdef USE_RANGEFINDER
     setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
 #endif
@@ -495,6 +507,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_ADSB
+        [TASK_ADSB] = {
+        .taskName = "ADSB",
+        .taskFunc = taskAdsb,
+        .desiredPeriod = TASK_PERIOD_HZ(1),      // ADSB is updated at 1 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
 #ifdef USE_BARO
     [TASK_BARO] = {
         .taskName = "BARO",

src/main/fc/multifunction.h

@@ -24,6 +24,8 @@
 
 #pragma once
 
+#include <stdbool.h>
+
 #ifdef USE_MULTI_FUNCTIONS
 
 extern uint8_t multiFunctionFlags;

src/main/fc/runtime_config.h

@@ -124,6 +124,9 @@ typedef enum {
     NAV_MOTOR_STOP_OR_IDLE              = (1 << 7),     // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
     COMPASS_CALIBRATED                  = (1 << 8),
     ACCELEROMETER_CALIBRATED            = (1 << 9),
+#ifdef USE_GPS_FIX_ESTIMATION
+    GPS_ESTIMATED_FIX                   = (1 << 10),
+#endif
     NAV_CRUISE_BRAKING                  = (1 << 11),
     NAV_CRUISE_BRAKING_BOOST            = (1 << 12),
     NAV_CRUISE_BRAKING_LOCKED           = (1 << 13),

src/main/fc/settings.yaml

@@ -838,6 +838,12 @@ groups:
         default_value: 0
         min: -1
         max: 600
+      - name: failsafe_gps_fix_estimation_delay
+        description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation."
+        condition: USE_GPS_FIX_ESTIMATION
+        default_value: 7
+        min: -1
+        max: 600
 
   - name: PG_LIGHTS_CONFIG
     type: lightsConfig_t
@@ -1030,7 +1036,7 @@ groups:
         max: 1
       - name: throttle_idle
         description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle."
-        default_value: 15
+        default_value: 8
         field: motor.throttleIdle
         min: 0
         max: 30
@@ -2296,6 +2302,12 @@ groups:
         default_value: OFF
         field: allow_dead_reckoning
         type: bool
+      - name: inav_allow_gps_fix_estimation
+        description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay."
+        condition: USE_GPS_FIX_ESTIMATION
+        default_value: OFF
+        field: allow_gps_fix_estimation
+        type: bool
       - name: inav_reset_altitude
         description: "Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming)"
         default_value: "FIRST_ARM"
@@ -2504,24 +2516,12 @@ groups:
         field: general.max_auto_speed
         min: 10
         max: 2000
-      - name: nav_auto_climb_rate
-        description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]"
-        default_value: 500
-        field: general.max_auto_climb_rate
-        min: 10
-        max: 2000
       - name: nav_manual_speed
         description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
         default_value: 500
         field: general.max_manual_speed
         min: 10
         max: 2000
-      - name: nav_manual_climb_rate
-        description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
-        default_value: 200
-        field: general.max_manual_climb_rate
-        min: 10
-        max: 2000
       - name: nav_land_minalt_vspd
         description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]"
         default_value: 50
@@ -2688,7 +2688,7 @@ groups:
         type: bool
       - name: nav_cruise_yaw_rate
         description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]"
-        default_value: 20
+        default_value: 60
         field: general.cruise_yaw_rate
         min: 0
         max: 120
@@ -2698,6 +2698,18 @@ groups:
         field: mc.max_bank_angle
         min: 15
         max: 45
+      - name: nav_mc_auto_climb_rate
+        description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]"
+        default_value: 500
+        field: mc.max_auto_climb_rate
+        min: 10
+        max: 2000
+      - name: nav_mc_manual_climb_rate
+        description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
+        default_value: 200
+        field: mc.max_manual_climb_rate
+        min: 10
+        max: 2000
       - name: nav_auto_disarm_delay
         description: "Delay before craft disarms when `nav_disarm_on_landing` is set (ms)"
         default_value: 1000
@@ -2783,6 +2795,12 @@ groups:
         field: fw.max_bank_angle
         min: 5
         max: 80
+      - name: nav_fw_manual_climb_rate
+        description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
+        default_value: 300
+        field: fw.max_manual_climb_rate
+        min: 10
+        max: 2500
       - name: nav_fw_climb_angle
         description: "Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit"
         default_value: 20
@@ -3441,6 +3459,31 @@ groups:
         max: 11
         default_value: 9
 
+      - name: osd_adsb_distance_warning
+        description: "Distance in meters of ADSB aircraft that is displayed"
+        default_value: 20000
+        condition: USE_ADSB
+        field: adsb_distance_warning
+        min: 1
+        max: 64000
+        type: uint16_t
+      - name: osd_adsb_distance_alert
+        description: "Distance inside which ADSB data flashes for proximity warning"
+        default_value: 3000
+        condition: USE_ADSB
+        field: adsb_distance_alert
+        min: 1
+        max: 64000
+        type: uint16_t
+      - name: osd_adsb_ignore_plane_above_me_limit
+        description: "Ignore adsb planes above, limit, 0 disabled (meters)"
+        default_value: 0
+        condition: USE_ADSB
+        field: adsb_ignore_plane_above_me_limit
+        min: 0
+        max: 64000
+        type: uint16_t
+
       - name: osd_estimations_wind_compensation
         description: "Use wind estimation for remaining flight time/distance estimation"
         default_value: ON

src/main/flight/failsafe.c

@@ -82,6 +82,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT,                     // No minimum distance for failsafe by default
     .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
     .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe activated during WP mission (s)
+#ifdef USE_GPS_FIX_ESTIMATION
+    .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
+#endif    
 );
 
 typedef enum {
@@ -350,7 +353,13 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
 
     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
     // GPS must also be working, and home position set
-    if (failsafeConfig()->failsafe_min_distance > 0 && sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
+    if (failsafeConfig()->failsafe_min_distance > 0 &&
+            ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) 
+#ifdef USE_GPS_FIX_ESTIMATION    
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+                ) && STATE(GPS_FIX_HOME)) {
+
         // get the distance to the original arming point
         uint32_t distance = calculateDistanceToDestination(&posControl.rthState.originalHomePosition);
         if (distance < failsafeConfig()->failsafe_min_distance) {
@@ -362,6 +371,28 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
     return failsafeConfig()->failsafe_procedure;
 }
 
+#ifdef USE_GPS_FIX_ESTIMATION
+bool checkGPSFixFailsafe(void)
+{
+    if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) {
+        if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) {
+            failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis();
+        } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) {
+            if ( !posControl.flags.forcedRTHActivated ) {
+                failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH);
+                failsafeActivate(FAILSAFE_RETURN_TO_HOME);
+                activateForcedRTH();
+                return true;
+            }
+        }
+    } else {
+        failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0;
+    }
+    return false;
+}
+#endif
+
+
 void failsafeUpdateState(void)
 {
     if (!failsafeIsMonitoring() || failsafeIsSuspended()) {
@@ -390,6 +421,12 @@ void failsafeUpdateState(void)
                     if (!throttleStickIsLow()) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
+
+#ifdef USE_GPS_FIX_ESTIMATION
+                    if ( checkGPSFixFailsafe() ) {
+                        reprocessState = true;
+                    } else 
+#endif                    
                     if (!receivingRxDataAndNotFailsafeMode) {
                         if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
                             // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
@@ -458,7 +495,15 @@ void failsafeUpdateState(void)
                 } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) {  // trigger new failsafe procedure if changed
                     failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
                     reprocessState = true;
+                } 
+#ifdef USE_GPS_FIX_ESTIMATION
+                else {
+                    if ( checkGPSFixFailsafe() ) {
+                        reprocessState = true;
+                    }
                 }
+#endif
+
                 break;
 
             case FAILSAFE_RETURN_TO_HOME:

src/main/flight/failsafe.h

@@ -43,6 +43,9 @@ typedef struct failsafeConfig_s {
     uint16_t failsafe_min_distance;             // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
     uint8_t failsafe_min_distance_procedure;    // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
     int16_t failsafe_mission_delay;             // Time delay before Failsafe triggered when WP mission in progress (s)
+#ifdef USE_GPS_FIX_ESTIMATION
+    int16_t failsafe_gps_fix_estimation_delay;  // Time delay before Failsafe triggered when GPX Fix estimation is applied (s)
+#endif
 } failsafeConfig_t;
 
 PG_DECLARE(failsafeConfig_t, failsafeConfig);
@@ -149,6 +152,9 @@ typedef struct failsafeState_s {
     timeMs_t receivingRxDataPeriod;         // period for the required period of valid rxData
     timeMs_t receivingRxDataPeriodPreset;   // preset for the required period of valid rxData
     timeMs_t wpModeDelayedFailsafeStart;    // waypoint mission delayed failsafe timer start time
+#ifdef USE_GPS_FIX_ESTIMATION
+    timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart;    // waypoint mission delayed failsafe timer start time on GPS fix estimation
+#endif
     failsafeProcedure_e activeProcedure;
     failsafePhase_e phase;
     failsafeRxLinkState_e rxLinkState;

src/main/flight/imu.c

@@ -330,7 +330,11 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c
 
 bool isGPSTrustworthy(void)
 {
-    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6;
+    return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6)
+#ifdef USE_GPS_FIX_ESTIMATION
+        || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ;
 }
 
 static float imuCalculateMcCogWeight(void)

src/main/flight/wind_estimator.c

@@ -54,7 +54,11 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT];
 
 bool isEstimatedWindSpeedValid(void)
 {
-    return hasValidWindEstimate;
+    return hasValidWindEstimate 
+#ifdef USE_GPS_FIX_ESTIMATION
+        || STATE(GPS_ESTIMATED_FIX)  //use any wind estimate with GPS fix estimation.
+#endif
+        ;
 }
 
 float getEstimatedWindSpeed(int axis)
@@ -85,15 +89,18 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     static float lastValidEstimateAltitude = 0.0f;
     float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m
 
-    if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT)
+    if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) {
-    {
         hasValidWindEstimate = false;
     }
 
     if (!STATE(FIXED_WING_LEGACY) ||
         !isGPSHeadingValid() ||
         !gpsSol.flags.validVelNE ||
-        !gpsSol.flags.validVelD) {
+        !gpsSol.flags.validVelD 
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) {
         return;
     }
 

src/main/io/adsb.c

@@ -0,0 +1,223 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <string.h>
+
+
+#include "adsb.h"
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+#include "common/maths.h"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-function"
+#include "common/mavlink.h"
+#pragma GCC diagnostic pop
+
+
+#include "math.h"
+
+
+#ifdef USE_ADSB
+
+adsbVehicle_t adsbVehiclesList[MAX_ADSB_VEHICLES];
+adsbVehicleStatus_t adsbVehiclesStatus;
+
+adsbVehicleValues_t vehicleValues;
+
+
+adsbVehicleValues_t* getVehicleForFill(void){
+    return &vehicleValues;
+}
+
+// use bsearch function
+adsbVehicle_t *findVehicleByIcao(uint32_t avicao) {
+    for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
+        if (avicao == adsbVehiclesList[i].vehicleValues.icao) {
+            return &adsbVehiclesList[i];
+        }
+    }
+    return NULL;
+}
+
+adsbVehicle_t *findVehicleFarthest(void) {
+    adsbVehicle_t *adsbLocal = NULL;
+    for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
+        if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist < adsbVehiclesList[i].calculatedVehicleValues.dist)) {
+            adsbLocal = &adsbVehiclesList[i];
+        }
+    }
+    return adsbLocal;
+}
+
+uint8_t getActiveVehiclesCount(void) {
+    uint8_t total = 0;
+    for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
+        if (adsbVehiclesList[i].ttl > 0) {
+            total++;
+        }
+    }
+    return total;
+}
+
+adsbVehicle_t *findVehicleClosest(void) {
+    adsbVehicle_t *adsbLocal = NULL;
+    for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
+        if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > adsbVehiclesList[i].calculatedVehicleValues.dist)) {
+            adsbLocal = &adsbVehiclesList[i];
+        }
+    }
+    return adsbLocal;
+}
+
+adsbVehicle_t *findFreeSpaceInList(void) {
+    //find expired first
+    for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
+        if (adsbVehiclesList[i].ttl == 0) {
+            return &adsbVehiclesList[i];
+        }
+    }
+
+    return NULL;
+}
+
+adsbVehicle_t *findVehicleNotCalculated(void) {
+    //find expired first
+    for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
+        if (adsbVehiclesList[i].calculatedVehicleValues.valid == false) {
+            return &adsbVehiclesList[i];
+        }
+    }
+
+    return NULL;
+}
+
+adsbVehicle_t* findVehicle(uint8_t index)
+{
+    if (index < MAX_ADSB_VEHICLES){
+        return &adsbVehiclesList[index];
+    }
+
+    return NULL;
+}
+
+adsbVehicleStatus_t* getAdsbStatus(void){
+    return &adsbVehiclesStatus;
+}
+
+void gpsDistanceCmBearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing) {
+    float GPS_scaleLonDown = cos_approx((fabsf((float) gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f);
+    const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
+    const float dLon = (float) (destinationLon2 - currentLon1) * GPS_scaleLonDown;
+
+    *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
+    *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon));      // Convert the output radians to 100xdeg
+    *bearing = wrap_36000(*bearing);
+};
+
+void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) {
+
+    // no valid lat lon or altitude
+    if((vehicleValuesLocal->flags & (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)) != (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)){
+        return;
+    }
+
+    adsbVehiclesStatus.vehiclesMessagesTotal++;
+    adsbVehicle_t *vehicle = NULL;
+
+    vehicle = findVehicleByIcao(vehicleValuesLocal->icao);
+    if(vehicle != NULL && vehicleValuesLocal->tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST){
+        vehicle->ttl = 0;
+        return;
+    }
+
+    // non GPS mode, GPS is not fix, just find free space in list or by icao and save vehicle without calculated values
+    if (!enviromentOkForCalculatingDistaceBearing()) {
+
+        if(vehicle == NULL){
+            vehicle = findFreeSpaceInList();
+        }
+
+        if (vehicle != NULL) {
+            memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues));
+            vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST;
+            vehicle->calculatedVehicleValues.valid = false;
+            return;
+        }
+    } else {
+        // GPS mode, GPS is fixed and has enough sats
+
+
+        if(vehicle == NULL){
+            vehicle = findFreeSpaceInList();
+        }
+
+        if(vehicle == NULL){
+            vehicle = findVehicleNotCalculated();
+        }
+
+        if(vehicle == NULL){
+            vehicle = findVehicleFarthest();
+        }
+
+        if (vehicle != NULL) {
+            memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues));
+            recalculateVehicle(vehicle);
+            vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST;
+            return;
+        }
+    }
+};
+
+void recalculateVehicle(adsbVehicle_t* vehicle){
+    gpsDistanceCmBearing(gpsSol.llh.lat, gpsSol.llh.lon, vehicle->vehicleValues.lat, vehicle->vehicleValues.lon, &(vehicle->calculatedVehicleValues.dist), &(vehicle->calculatedVehicleValues.dir));
+
+    if (vehicle != NULL && vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) {
+        vehicle->ttl = 0;
+        return;
+    }
+
+    vehicle->calculatedVehicleValues.verticalDistance = vehicle->vehicleValues.alt - (int32_t)getEstimatedActualPosition(Z) - GPS_home.alt;
+    vehicle->calculatedVehicleValues.valid = true;
+}
+
+void adsbTtlClean(timeUs_t currentTimeUs) {
+
+    static timeUs_t adsbTtlLastCleanServiced = 0;
+    timeDelta_t adsbTtlSinceLastCleanServiced = cmpTimeUs(currentTimeUs, adsbTtlLastCleanServiced);
+
+
+    if (adsbTtlSinceLastCleanServiced > 1000000) // 1s
+    {
+        for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
+            if (adsbVehiclesList[i].ttl > 0) {
+                adsbVehiclesList[i].ttl--;
+            }
+        }
+
+        adsbTtlLastCleanServiced = currentTimeUs;
+    }
+};
+
+bool enviromentOkForCalculatingDistaceBearing(void){
+    return (STATE(GPS_FIX) && gpsSol.numSat > 4);
+}
+
+#endif
+

src/main/io/adsb.h

@@ -0,0 +1,67 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "common/time.h"
+#include "fc/runtime_config.h"
+
+#define ADSB_CALL_SIGN_MAX_LENGTH 9
+#define ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST 10
+
+typedef struct {
+    bool valid;
+    int32_t dir;   // centidegrees direction to plane, pivot is inav FC
+    uint32_t dist;  // CM distance to plane, pivot is inav FC
+    int32_t verticalDistance; // CM, vertical distance to plane, pivot is inav FC
+} adsbVehicleCalculatedValues_t;
+
+typedef struct {
+    uint32_t icao; // ICAO address
+    int32_t lat; // Latitude, expressed as degrees * 1E7
+    int32_t lon; // Longitude, expressed as degrees * 1E7
+    int32_t alt;  // Barometric/Geometric Altitude (ASL), in cm
+    uint16_t heading; // Course over ground in centidegrees
+    uint16_t flags; // Flags to indicate various statuses including valid data fields
+    uint8_t altitudeType; // Type from ADSB_ALTITUDE_TYPE enum
+    char callsign[ADSB_CALL_SIGN_MAX_LENGTH]; // The callsign, 8 chars + NULL
+    uint8_t emitterType; // Type from ADSB_EMITTER_TYPE enum
+    uint8_t tslc; // Time since last communication in seconds
+} adsbVehicleValues_t;
+
+typedef struct {
+    adsbVehicleValues_t vehicleValues;
+    adsbVehicleCalculatedValues_t calculatedVehicleValues;
+    uint8_t ttl;
+} adsbVehicle_t;
+
+
+
+typedef struct {
+   uint32_t vehiclesMessagesTotal;
+} adsbVehicleStatus_t;
+
+void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal);
+adsbVehicle_t * findVehicleClosest(void);
+adsbVehicle_t * findVehicle(uint8_t index);
+uint8_t getActiveVehiclesCount(void);
+void adsbTtlClean(timeUs_t currentTimeUs);
+adsbVehicleStatus_t* getAdsbStatus(void);
+adsbVehicleValues_t* getVehicleForFill(void);
+bool enviromentOkForCalculatingDistaceBearing(void);
+void recalculateVehicle(adsbVehicle_t* vehicle);

src/main/io/gps.c

@@ -45,8 +45,12 @@
 #include "fc/runtime_config.h"
 #endif
 
+#include "fc/rc_modes.h"
+
 #include "sensors/sensors.h"
 #include "sensors/compass.h"
+#include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 
 #include "io/serial.h"
 #include "io/gps.h"
@@ -54,6 +58,7 @@
 #include "io/gps_ublox.h"
 
 #include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
 
 #include "config/feature.h"
 
@@ -61,6 +66,13 @@
 #include "fc/runtime_config.h"
 #include "fc/settings.h"
 
+#include "flight/imu.h"
+#include "flight/wind_estimator.h"
+#include "flight/pid.h"
+#include "flight/mixer.h"
+
+#include "programming/logic_condition.h"
+
 typedef struct {
     bool                isDriverBased;
     portMode_t          portMode;           // Port mode RX/TX (only for serial based)
@@ -71,7 +83,13 @@ typedef struct {
 // GPS public data
 gpsReceiverData_t gpsState;
 gpsStatistics_t   gpsStats;
-gpsSolutionData_t gpsSol;
+
+//it is not safe to access gpsSolDRV which is filled in gps thread by driver.
+//gpsSolDRV can be accessed only after driver signalled that data is ready
+//we copy gpsSolDRV to gpsSol, process by "Disable GPS logic condition" and "GPS Fix estimation" features
+//and use it in the rest of code.
+gpsSolutionData_t gpsSolDRV;  //filled by driver
+gpsSolutionData_t gpsSol;     //used in the rest of the code
 
 // Map gpsBaudRate_e index to baudRate_e
 baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 };
@@ -203,22 +221,177 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs)
     gpsState.timeoutMs = timeoutMs;
 }
 
-void gpsProcessNewSolutionData(void)
+#ifdef USE_GPS_FIX_ESTIMATION
+bool canEstimateGPSFix(void)
 {
-    // Set GPS fix flag only if we have 3D fix
+#if defined(USE_GPS) && defined(USE_BARO)
-    if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
+
-        ENABLE_STATE(GPS_FIX);
+    //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because:
-    }
+    //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once
-    else {
+    //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix
-        /* When no fix available - reset flags as well */
+    return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && 
+        sensors(SENSOR_BARO) && baroIsHealthy() &&
+        ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME);
+        
+#else
+    return false;
+#endif
+}
+#endif
+
+#ifdef USE_GPS_FIX_ESTIMATION
+void processDisableGPSFix(void) 
+{
+    static int32_t last_lat = 0;
+    static int32_t last_lon = 0;
+    static int32_t last_alt = 0;
+
+    if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) {
+        gpsSol.fixType = GPS_NO_FIX;
+        gpsSol.hdop = 9999;
+        gpsSol.numSat = 0;
+
         gpsSol.flags.validVelNE = false;
-        gpsSol.flags.validVelD = false;
+        gpsSol.flags.validVelD = false;  
         gpsSol.flags.validEPE = false;
-        DISABLE_STATE(GPS_FIX);
+        gpsSol.flags.validTime = false;
+
+        //freeze coordinates
+        gpsSol.llh.lat = last_lat;
+        gpsSol.llh.lon = last_lon;
+        gpsSol.llh.alt = last_alt;
+    } else {
+        last_lat = gpsSol.llh.lat;
+        last_lon = gpsSol.llh.lon;
+        last_alt = gpsSol.llh.alt;
+    }
+}
+#endif
+
+#ifdef USE_GPS_FIX_ESTIMATION
+//called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition"
+void updateEstimatedGPSFix(void) 
+{
+    static uint32_t lastUpdateMs = 0;
+    static int32_t estimated_lat = 0;
+    static int32_t estimated_lon = 0;
+    static int32_t estimated_alt = 0;
+
+    uint32_t t = millis();
+    int32_t dt = t - lastUpdateMs;
+    lastUpdateMs = t;
+
+    bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats;
+
+    if (sensorHasFix || !canEstimateGPSFix()) {
+        estimated_lat = gpsSol.llh.lat;
+        estimated_lon = gpsSol.llh.lon;
+        estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
+        return;
     }
 
-    // Set sensor as ready and available
+    gpsSol.fixType = GPS_FIX_3D;
-    sensorsSet(SENSOR_GPS);
+    gpsSol.hdop = 99;
+    gpsSol.numSat = 99;
+
+    gpsSol.eph = 100;
+    gpsSol.epv = 100;
+
+    gpsSol.flags.validVelNE = true;
+    gpsSol.flags.validVelD = false;  //do not provide velocity.z
+    gpsSol.flags.validEPE = true;
+    gpsSol.flags.validTime = false;
+
+    float speed = pidProfile()->fixedWingReferenceAirspeed;
+
+#ifdef USE_PITOT
+    if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
+        speed = getAirspeedEstimate();
+    }
+#endif
+
+    float velX = rMat[0][0] * speed;
+    float velY = -rMat[1][0] * speed;
+    // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame
+
+    if (isEstimatedWindSpeedValid()) {
+        velX += getEstimatedWindSpeed(X);
+        velY += getEstimatedWindSpeed(Y);
+    }
+    // here (velX, velY) is estimated horizontal speed with wind influence = ground speed
+
+    if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent(false) == 0))) {
+        velX = 0;
+        velY = 0;
+    }
+
+    estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) );
+    estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) );
+    estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
+
+    gpsSol.llh.lat = estimated_lat;
+    gpsSol.llh.lon = estimated_lon;
+    gpsSol.llh.alt = estimated_alt;
+
+    gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY);
+
+    float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction
+    if (groundCourse < 0) {
+        groundCourse += 2 * M_PIf;
+    }
+    gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse);
+
+    gpsSol.velNED[X] = (int16_t)(velX);
+    gpsSol.velNED[Y] = (int16_t)(velY);
+    gpsSol.velNED[Z] = 0;
+}
+#endif
+
+
+void gpsProcessNewDriverData(void)
+{
+    gpsSol = gpsSolDRV;
+
+#ifdef USE_GPS_FIX_ESTIMATION
+    processDisableGPSFix();
+    updateEstimatedGPSFix();
+#endif
+}
+
+//called after: 
+//1)driver copies gpsSolDRV to gpsSol
+//2)gpsSol is processed by "Disable GPS logical switch"
+//3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix()
+//On GPS sensor timeout - called after updateEstimatedGPSFix()
+void gpsProcessNewSolutionData(bool timeout)
+{
+#ifdef USE_GPS_FIX_ESTIMATION
+    if ( gpsSol.numSat == 99 ) {
+        ENABLE_STATE(GPS_ESTIMATED_FIX);
+        DISABLE_STATE(GPS_FIX);
+    } else {
+        DISABLE_STATE(GPS_ESTIMATED_FIX);
+#endif
+
+        // Set GPS fix flag only if we have 3D fix
+        if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
+            ENABLE_STATE(GPS_FIX);
+        }
+        else {
+            /* When no fix available - reset flags as well */
+            gpsSol.flags.validVelNE = false;
+            gpsSol.flags.validVelD = false;
+            gpsSol.flags.validEPE = false;
+            DISABLE_STATE(GPS_FIX);
+        }
+#ifdef USE_GPS_FIX_ESTIMATION
+    }
+#endif
+
+    if (!timeout) {
+        // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix)
+        sensorsSet(SENSOR_GPS);
+    }
 
     // Pass on GPS update to NAV and IMU
     onNewGPSData();
@@ -237,20 +410,35 @@ void gpsProcessNewSolutionData(void)
     gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 }
 
-static void gpsResetSolution(void)
+static void gpsResetSolution(gpsSolutionData_t* gpsSol)
 {
-    gpsSol.eph = 9999;
+    gpsSol->eph = 9999;
-    gpsSol.epv = 9999;
+    gpsSol->epv = 9999;
-    gpsSol.numSat = 0;
+    gpsSol->numSat = 0;
-    gpsSol.hdop = 9999;
+    gpsSol->hdop = 9999;
 
-    gpsSol.fixType = GPS_NO_FIX;
+    gpsSol->fixType = GPS_NO_FIX;
 
-    gpsSol.flags.validVelNE = false;
+    gpsSol->flags.validVelNE = false;
-    gpsSol.flags.validVelD = false;
+    gpsSol->flags.validVelD = false;
-    gpsSol.flags.validMag = false;
+    gpsSol->flags.validEPE = false;
-    gpsSol.flags.validEPE = false;
+    gpsSol->flags.validTime = false;
-    gpsSol.flags.validTime = false;
+}
+
+void gpsTryEstimateOnTimeout(void)
+{
+    gpsResetSolution(&gpsSol);
+    DISABLE_STATE(GPS_FIX);
+
+#ifdef USE_GPS_FIX_ESTIMATION
+    if ( canEstimateGPSFix() ) {
+        updateEstimatedGPSFix();
+
+        if (gpsSol.fixType == GPS_FIX_3D) {  //estimation kicked in
+            gpsProcessNewSolutionData(true);
+        }
+    }
+#endif
 }
 
 void gpsPreInit(void)
@@ -269,7 +457,8 @@ void gpsInit(void)
     gpsStats.timeouts = 0;
 
     // Reset solution, timeout and prepare to start
-    gpsResetSolution();
+    gpsResetSolution(&gpsSolDRV);
+    gpsResetSolution(&gpsSol);
     gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
     gpsSetState(GPS_UNKNOWN);
 
@@ -345,27 +534,21 @@ bool gpsUpdate(void)
 
 #ifdef USE_SIMULATOR
     if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-        if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT))
+        if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) {
-        {
             gpsSetState(GPS_LOST_COMMUNICATION);
             sensorsClear(SENSOR_GPS);
             gpsStats.timeouts = 5;
-            return false;
+            gpsTryEstimateOnTimeout();
-        }
+        } else {
-        else
-        {
             gpsSetState(GPS_RUNNING);
             sensorsSet(SENSOR_GPS);
-            bool res = gpsSol.flags.hasNewData;
-            gpsSol.flags.hasNewData = false;
-            return res;
         }
+        bool res = gpsSol.flags.hasNewData;
+        gpsSol.flags.hasNewData = false;
+        return res;
     }
 #endif
 
-    // Assume that we don't have new data this run
-    gpsSol.flags.hasNewData = false;
-
     switch (gpsState.state) {
     default:
     case GPS_INITIALIZING:
@@ -373,10 +556,9 @@ bool gpsUpdate(void)
         if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
             // Reset internals
             DISABLE_STATE(GPS_FIX);
-            gpsSol.fixType = GPS_NO_FIX;
 
             // Reset solution
-            gpsResetSolution();
+            gpsResetSolution(&gpsSolDRV);
 
             // Call GPS protocol reset handler
             gpsProviders[gpsState.gpsConfig->provider].restart();
@@ -406,7 +588,13 @@ bool gpsUpdate(void)
         break;
     }
 
-    return gpsSol.flags.hasNewData;
+    if ( !sensors(SENSOR_GPS) ) {
+        gpsTryEstimateOnTimeout();
+    }
+
+    bool res = gpsSol.flags.hasNewData;
+    gpsSol.flags.hasNewData = false;
+    return res;
 }
 
 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
@@ -453,7 +641,11 @@ bool isGPSHealthy(void)
 
 bool isGPSHeadingValid(void)
 {
-    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 400;
+    return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) 
+#ifdef USE_GPS_FIX_ESTIMATION
+        || STATE(GPS_ESTIMATED_FIX)
+#endif        
+        ) && gpsSol.groundSpeed >= 300;
 }
 
 #endif

src/main/io/gps.h

@@ -126,7 +126,6 @@ typedef struct gpsSolutionData_s {
         bool gpsHeartbeat;  // Toggle each update
         bool validVelNE;
         bool validVelD;
-        bool validMag;
         bool validEPE;      // EPH/EPV values are valid - actual accuracy
         bool validTime;
     } flags;
@@ -135,7 +134,6 @@ typedef struct gpsSolutionData_s {
     uint8_t numSat;
 
     gpsLocation_t llh;
-    int16_t       magData[3];
     int16_t       velNED[3];
 
     int16_t groundSpeed;

src/main/io/gps_fake.c

@@ -47,7 +47,7 @@ void gpsFakeRestart(void)
 
 void gpsFakeHandle(void)
 {
-    gpsProcessNewSolutionData();
+    gpsProcessNewSolutionData(false);
 }
 
 void gpsFakeSet(
@@ -63,37 +63,38 @@ void gpsFakeSet(
     int16_t velNED_Z,
     time_t time)
 {
-    gpsSol.fixType = fixType;
+    gpsSolDRV.fixType = fixType;
-    gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
+    gpsSolDRV.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
-    gpsSol.numSat = numSat;
+    gpsSolDRV.numSat = numSat;
     
-    gpsSol.llh.lat = lat;
+    gpsSolDRV.llh.lat = lat;
-    gpsSol.llh.lon = lon;
+    gpsSolDRV.llh.lon = lon;
-    gpsSol.llh.alt = alt;
+    gpsSolDRV.llh.alt = alt;
-    gpsSol.groundSpeed = groundSpeed;
+    gpsSolDRV.groundSpeed = groundSpeed;
-    gpsSol.groundCourse = groundCourse;
+    gpsSolDRV.groundCourse = groundCourse;
-    gpsSol.velNED[X] = velNED_X;
+    gpsSolDRV.velNED[X] = velNED_X;
-    gpsSol.velNED[Y] = velNED_Y;
+    gpsSolDRV.velNED[Y] = velNED_Y;
-    gpsSol.velNED[Z] = velNED_Z;
+    gpsSolDRV.velNED[Z] = velNED_Z;
-    gpsSol.eph = 100;
+    gpsSolDRV.eph = 100;
-    gpsSol.epv = 100;
+    gpsSolDRV.epv = 100;
-    gpsSol.flags.validVelNE = true;
+    gpsSolDRV.flags.validVelNE = true;
-    gpsSol.flags.validVelD = true;
+    gpsSolDRV.flags.validVelD = true;
-    gpsSol.flags.validEPE = true;
+    gpsSolDRV.flags.validEPE = true;
-    gpsSol.flags.hasNewData = true;
     
     if (time) {
         struct tm* gTime = gmtime(&time);
 
-        gpsSol.time.year   = (uint16_t)(gTime->tm_year + 1900);
+        gpsSolDRV.time.year   = (uint16_t)(gTime->tm_year + 1900);
-        gpsSol.time.month  = (uint16_t)(gTime->tm_mon + 1);
+        gpsSolDRV.time.month  = (uint16_t)(gTime->tm_mon + 1);
-        gpsSol.time.day    = (uint8_t)gTime->tm_mday;
+        gpsSolDRV.time.day    = (uint8_t)gTime->tm_mday;
-        gpsSol.time.hours  = (uint8_t)gTime->tm_hour;
+        gpsSolDRV.time.hours  = (uint8_t)gTime->tm_hour;
-        gpsSol.time.minutes = (uint8_t)gTime->tm_min;
+        gpsSolDRV.time.minutes = (uint8_t)gTime->tm_min;
-        gpsSol.time.seconds = (uint8_t)gTime->tm_sec;
+        gpsSolDRV.time.seconds = (uint8_t)gTime->tm_sec;
-        gpsSol.time.millis  = 0;
+        gpsSolDRV.time.millis  = 0;
-        gpsSol.flags.validTime = gpsSol.fixType >= 3;
+        gpsSolDRV.flags.validTime = gpsSol.fixType >= 3;
     }
+    
+    gpsProcessNewDriverData();
 }
    
 #endif

src/main/io/gps_msp.c

@@ -63,7 +63,7 @@ void gpsRestartMSP(void)
 void gpsHandleMSP(void)
 {
     if (newDataReady) {
-        gpsProcessNewSolutionData();
+        gpsProcessNewSolutionData(false);
         newDataReady = false;
     }
 }
@@ -81,33 +81,34 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr)
 {
     const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
 
-    gpsSol.fixType   = gpsMapFixType(pkt->fixType);
+    gpsSolDRV.fixType   = gpsMapFixType(pkt->fixType);
-    gpsSol.numSat    = pkt->satellitesInView;
+    gpsSolDRV.numSat    = pkt->satellitesInView;
-    gpsSol.llh.lon   = pkt->longitude;
+    gpsSolDRV.llh.lon   = pkt->longitude;
-    gpsSol.llh.lat   = pkt->latitude;
+    gpsSolDRV.llh.lat   = pkt->latitude;
-    gpsSol.llh.alt   = pkt->mslAltitude;
+    gpsSolDRV.llh.alt   = pkt->mslAltitude;
-    gpsSol.velNED[X] = pkt->nedVelNorth;
+    gpsSolDRV.velNED[X] = pkt->nedVelNorth;
-    gpsSol.velNED[Y] = pkt->nedVelEast;
+    gpsSolDRV.velNED[Y] = pkt->nedVelEast;
-    gpsSol.velNED[Z] = pkt->nedVelDown;
+    gpsSolDRV.velNED[Z] = pkt->nedVelDown;
-    gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
+    gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
-    gpsSol.groundCourse = pkt->groundCourse / 10;   // in deg * 10
+    gpsSolDRV.groundCourse = pkt->groundCourse / 10;   // in deg * 10
-    gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
+    gpsSolDRV.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
-    gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
+    gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
-    gpsSol.hdop = gpsConstrainHDOP(pkt->hdop);
+    gpsSolDRV.hdop = gpsConstrainHDOP(pkt->hdop);
-    gpsSol.flags.validVelNE = true;
+    gpsSolDRV.flags.validVelNE = true;
-    gpsSol.flags.validVelD = true;
+    gpsSolDRV.flags.validVelD = true;
-    gpsSol.flags.validEPE = true;
+    gpsSolDRV.flags.validEPE = true;
 
-    gpsSol.time.year   = pkt->year;
+    gpsSolDRV.time.year   = pkt->year;
-    gpsSol.time.month  = pkt->month;
+    gpsSolDRV.time.month  = pkt->month;
-    gpsSol.time.day    = pkt->day;
+    gpsSolDRV.time.day    = pkt->day;
-    gpsSol.time.hours  = pkt->hour;
+    gpsSolDRV.time.hours  = pkt->hour;
-    gpsSol.time.minutes = pkt->min;
+    gpsSolDRV.time.minutes = pkt->min;
-    gpsSol.time.seconds = pkt->sec;
+    gpsSolDRV.time.seconds = pkt->sec;
-    gpsSol.time.millis  = 0;
+    gpsSolDRV.time.millis  = 0;
 
-    gpsSol.flags.validTime = (pkt->fixType >= 3);
+    gpsSolDRV.flags.validTime = (pkt->fixType >= 3);
 
+    gpsProcessNewDriverData();
     newDataReady = true;
 }
 #endif

src/main/io/gps_private.h

@@ -61,6 +61,7 @@ typedef struct {
 } gpsReceiverData_t;
 
 extern gpsReceiverData_t gpsState;
+extern gpsSolutionData_t gpsSolDRV;
 
 extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT];
 
@@ -70,7 +71,8 @@ extern void gpsFinalizeChangeBaud(void);
 extern uint16_t gpsConstrainEPE(uint32_t epe);
 extern uint16_t gpsConstrainHDOP(uint32_t hdop);
 
-void gpsProcessNewSolutionData(void);
+void gpsProcessNewDriverData(void);
+void gpsProcessNewSolutionData(bool);
 void gpsSetProtocolTimeout(timeMs_t timeoutMs);
 
 extern void gpsRestartUBLOX(void);

src/main/io/gps_ublox.c

@@ -251,8 +251,8 @@ static const uint8_t default_payload[] = {
     0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 };
 
-#define GNSSID_SBAS     1
+#define GNSSID_SBAS 1
-#define GNSSID_GALILEO  2
+#define GNSSID_GALILEO 2
 #define GNSSID_BEIDOU   3
 #define GNSSID_GZSS     5
 #define GNSSID_GLONASS  6
@@ -411,18 +411,18 @@ static void configureGNSS10(void)
 
 static void configureGNSS(void)
 {
-        int blocksUsed = 0;
+    int blocksUsed = 0;
-        send_buffer.message.header.msg_class = CLASS_CFG;
+    send_buffer.message.header.msg_class = CLASS_CFG;
         send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET
-        send_buffer.message.payload.gnss.msgVer = 0;
+    send_buffer.message.payload.gnss.msgVer = 0;
-        send_buffer.message.payload.gnss.numTrkChHw = 0;     // read only, so unset
+    send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
         send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max
 
-        /* SBAS, always generated */
+    /* SBAS, always generated */
-        blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
+    blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
 
-        /* Galileo */
+    /* Galileo */
-        blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
+    blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
 
         /* BeiDou */
         blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]);
@@ -430,9 +430,9 @@ static void configureGNSS(void)
         /* GLONASS */
         blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]);
 
-        send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
+    send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
-        send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed);
+    send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
-        sendConfigMessageUBLOX();
+    sendConfigMessageUBLOX();
 }
 
 static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
@@ -539,84 +539,84 @@ static bool gpsParseFrameUBLOX(void)
 {
     switch (_msg_id) {
     case MSG_POSLLH:
-        gpsSol.llh.lon = _buffer.posllh.longitude;
+        gpsSolDRV.llh.lon = _buffer.posllh.longitude;
-        gpsSol.llh.lat = _buffer.posllh.latitude;
+        gpsSolDRV.llh.lat = _buffer.posllh.latitude;
-        gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
+        gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
-        gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
+        gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
-        gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
+        gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
-        gpsSol.flags.validEPE = true;
+        gpsSolDRV.flags.validEPE = true;
         if (next_fix_type != GPS_NO_FIX)
-            gpsSol.fixType = next_fix_type;
+            gpsSolDRV.fixType = next_fix_type;
         _new_position = true;
         break;
     case MSG_STATUS:
         next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type);
         if (next_fix_type == GPS_NO_FIX)
-            gpsSol.fixType = GPS_NO_FIX;
+            gpsSolDRV.fixType = GPS_NO_FIX;
         break;
     case MSG_SOL:
         next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type);
         if (next_fix_type == GPS_NO_FIX)
-            gpsSol.fixType = GPS_NO_FIX;
+            gpsSolDRV.fixType = GPS_NO_FIX;
-        gpsSol.numSat = _buffer.solution.satellites;
+        gpsSolDRV.numSat = _buffer.solution.satellites;
-        gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
+        gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
         break;
     case MSG_VELNED:
-        gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
+        gpsSolDRV.groundSpeed = _buffer.velned.speed_2d;    // cm/s
-        gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
+        gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
-        gpsSol.velNED[X] = _buffer.velned.ned_north;
+        gpsSolDRV.velNED[X] = _buffer.velned.ned_north;
-        gpsSol.velNED[Y] = _buffer.velned.ned_east;
+        gpsSolDRV.velNED[Y] = _buffer.velned.ned_east;
-        gpsSol.velNED[Z] = _buffer.velned.ned_down;
+        gpsSolDRV.velNED[Z] = _buffer.velned.ned_down;
-        gpsSol.flags.validVelNE = true;
+        gpsSolDRV.flags.validVelNE = true;
-        gpsSol.flags.validVelD = true;
+        gpsSolDRV.flags.validVelD = true;
         _new_speed = true;
         break;
     case MSG_TIMEUTC:
         if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) {
-            gpsSol.time.year = _buffer.timeutc.year;
+            gpsSolDRV.time.year = _buffer.timeutc.year;
-            gpsSol.time.month = _buffer.timeutc.month;
+            gpsSolDRV.time.month = _buffer.timeutc.month;
-            gpsSol.time.day = _buffer.timeutc.day;
+            gpsSolDRV.time.day = _buffer.timeutc.day;
-            gpsSol.time.hours = _buffer.timeutc.hour;
+            gpsSolDRV.time.hours = _buffer.timeutc.hour;
-            gpsSol.time.minutes = _buffer.timeutc.min;
+            gpsSolDRV.time.minutes = _buffer.timeutc.min;
-            gpsSol.time.seconds = _buffer.timeutc.sec;
+            gpsSolDRV.time.seconds = _buffer.timeutc.sec;
-            gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000);
+            gpsSolDRV.time.millis = _buffer.timeutc.nano / (1000*1000);
 
-            gpsSol.flags.validTime = true;
+            gpsSolDRV.flags.validTime = true;
         } else {
-            gpsSol.flags.validTime = false;
+            gpsSolDRV.flags.validTime = false;
         }
         break;
     case MSG_PVT:
         next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type);
-        gpsSol.fixType = next_fix_type;
+        gpsSolDRV.fixType = next_fix_type;
-        gpsSol.llh.lon = _buffer.pvt.longitude;
+        gpsSolDRV.llh.lon = _buffer.pvt.longitude;
-        gpsSol.llh.lat = _buffer.pvt.latitude;
+        gpsSolDRV.llh.lat = _buffer.pvt.latitude;
-        gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10;  //alt in cm
+        gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10;  //alt in cm
-        gpsSol.velNED[X]=_buffer.pvt.ned_north / 10;  // to cm/s
+        gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10;  // to cm/s
-        gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10;   // to cm/s
+        gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10;   // to cm/s
-        gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10;   // to cm/s
+        gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10;   // to cm/s
-        gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10;    // to cm/s
+        gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10;    // to cm/s
-        gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
+        gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
-        gpsSol.numSat = _buffer.pvt.satellites;
+        gpsSolDRV.numSat = _buffer.pvt.satellites;
-        gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
+        gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
-        gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
+        gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
-        gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
+        gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
-        gpsSol.flags.validVelNE = true;
+        gpsSolDRV.flags.validVelNE = true;
-        gpsSol.flags.validVelD = true;
+        gpsSolDRV.flags.validVelD = true;
-        gpsSol.flags.validEPE = true;
+        gpsSolDRV.flags.validEPE = true;
 
         if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
-            gpsSol.time.year = _buffer.pvt.year;
+            gpsSolDRV.time.year = _buffer.pvt.year;
-            gpsSol.time.month = _buffer.pvt.month;
+            gpsSolDRV.time.month = _buffer.pvt.month;
-            gpsSol.time.day = _buffer.pvt.day;
+            gpsSolDRV.time.day = _buffer.pvt.day;
-            gpsSol.time.hours = _buffer.pvt.hour;
+            gpsSolDRV.time.hours = _buffer.pvt.hour;
-            gpsSol.time.minutes = _buffer.pvt.min;
+            gpsSolDRV.time.minutes = _buffer.pvt.min;
-            gpsSol.time.seconds = _buffer.pvt.sec;
+            gpsSolDRV.time.seconds = _buffer.pvt.sec;
-            gpsSol.time.millis = _buffer.pvt.nano / (1000*1000);
+            gpsSolDRV.time.millis = _buffer.pvt.nano / (1000*1000);
 
-            gpsSol.flags.validTime = true;
+            gpsSolDRV.flags.validTime = true;
         } else {
-            gpsSol.flags.validTime = false;
+            gpsSolDRV.flags.validTime = false;
         }
 
         _new_position = true;
@@ -650,7 +650,7 @@ static bool gpsParseFrameUBLOX(void)
                         }
                     }
                 }
-                for (int j = 40; j < _payload_length; j += 30) {
+                for(int j = 40; j < _payload_length; j += 30) {
                     if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) {
                         gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30);
                         break;
@@ -969,11 +969,11 @@ STATIC_PROTOTHREAD(gpsConfigure)
 
          ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
 
-         if(_ack_state == UBX_ACK_GOT_NAK) {
+        if(_ack_state == UBX_ACK_GOT_NAK) {
             gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT;
             gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT;
             gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT;
-         }
+        }
     }
 
     ptEnd(0);
@@ -993,6 +993,7 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
         while (serialRxBytesWaiting(gpsState.gpsPort)) {
             uint8_t newChar = serialRead(gpsState.gpsPort);
             if (gpsNewFrameUBLOX(newChar)) {
+                gpsProcessNewDriverData();
                 ptSemaphoreSignal(semNewDataReady);
                 break;
             }
@@ -1051,7 +1052,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
         pollVersion();
         gpsState.autoConfigStep++;
         ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
-    } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
+    } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
 
     gpsState.autoConfigStep = 0;
     ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
@@ -1076,7 +1077,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
     // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
     while (1) {
         ptSemaphoreWait(semNewDataReady);
-        gpsProcessNewSolutionData();
+        gpsProcessNewSolutionData(false);
 
         if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
             if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {

src/main/io/osd.c

@@ -62,6 +62,7 @@
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"
 
+#include "io/adsb.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
 #include "io/osd.h"
@@ -549,7 +550,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
         buff[0] = ' ';
     }
 
-#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values
+#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values  
     if (isBfCompatibleVideoSystem(osdConfig())) {
         totalDigits++;
         digits++;
@@ -644,7 +645,7 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
 }
 
 /**
- * Trim whitespace from string.
+ * Trim whitespace from string. 
  * Used in Stats screen on lines with multiple values.
 */
 char *osdFormatTrimWhiteSpace(char *buff)
@@ -655,7 +656,7 @@ char *osdFormatTrimWhiteSpace(char *buff)
     while(isspace((unsigned char)*buff)) buff++;
 
     // All spaces?
-    if(*buff == 0)
+    if(*buff == 0)  
     return buff;
 
     // Trim trailing spaces
@@ -1124,7 +1125,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
  * Check if this OSD layout is using scaled or unscaled throttle.
  * If both are used, it will default to scaled.
  */
-bool osdUsingScaledThrottle(void)
+bool osdUsingScaledThrottle(void) 
 {
     bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]);
     bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]);
@@ -1348,7 +1349,11 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen
         }
     }
 
-    if (STATE(GPS_FIX)) {
+    if (STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
 
         int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
         float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
@@ -1462,7 +1467,11 @@ static void osdDisplayTelemetry(void)
     static uint16_t   trk_bearing   = 0;
 
     if (ARMING_FLAG(ARMED)) {
-      if (STATE(GPS_FIX)){
+      if (STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ){
         if (GPS_distanceToHome > 5) {
           trk_bearing = GPS_directionToHome;
           trk_bearing += 360 + 180;
@@ -1825,8 +1834,8 @@ static bool osdDrawSingleElement(uint8_t item)
             osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false);
 
         if (!unitsDrawn) {
-            buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
+        buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
-            buff[5] = '\0';
+        buff[5] = '\0';
         }
 
         if (batteryUsesCapacityThresholds()) {
@@ -1855,6 +1864,12 @@ static bool osdDrawSingleElement(uint8_t item)
         buff[0] = SYM_SAT_L;
         buff[1] = SYM_SAT_R;
         tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
+#ifdef USE_GPS_FIX_ESTIMATION
+        if (STATE(GPS_ESTIMATED_FIX)) {
+            strcpy(buff + 2, "ES");
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        } else 
+#endif
         if (!STATE(GPS_FIX)) {
             hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
             if (sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY) {
@@ -1912,7 +1927,11 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_HOME_DIR:
         {
-            if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
+            if ((STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION
+                    || STATE(GPS_ESTIMATED_FIX)
+#endif                
+                    ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
                 if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
                     displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
                 }
@@ -2089,7 +2108,73 @@ static bool osdDrawSingleElement(uint8_t item)
             osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits, false);
             break;
         }
+#ifdef USE_ADSB
+        case OSD_ADSB_WARNING:
+        {
+            static uint8_t adsblen = 1;
+            uint8_t arrowPositionX = 0;
 
+            for (int i = 0; i <= adsblen; i++) {
+                buff[i] = SYM_BLANK;
+            }
+
+            buff[adsblen]='\0';
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); // clear any previous chars because variable element size
+            adsblen=1;
+            adsbVehicle_t *vehicle = findVehicleClosest();
+
+            if(vehicle != NULL){
+                recalculateVehicle(vehicle);
+            }
+
+            if (
+                    vehicle != NULL &&
+                    (vehicle->calculatedVehicleValues.dist > 0) &&
+                    vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning) &&
+                    (osdConfig()->adsb_ignore_plane_above_me_limit == 0 || METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit) > vehicle->calculatedVehicleValues.verticalDistance)
+            ){
+                buff[0] = SYM_ADSB;
+                osdFormatDistanceStr(&buff[1], (int32_t)vehicle->calculatedVehicleValues.dist);
+                adsblen = strlen(buff);
+
+                buff[adsblen-1] = SYM_BLANK;
+
+                arrowPositionX = adsblen-1;
+                osdFormatDistanceStr(&buff[adsblen], vehicle->calculatedVehicleValues.verticalDistance);
+                adsblen = strlen(buff)-1;
+
+                if (vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_alert)) {
+                    TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+                }
+            }
+
+            buff[adsblen]='\0';
+            displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
+
+            if (arrowPositionX > 0){
+                int16_t panHomeDirOffset = 0;
+                if (osdConfig()->pan_servo_pwm2centideg != 0){
+                    panHomeDirOffset = osdGetPanServoOffset();
+                }
+                int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
+                osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + arrowPositionX, elemPosY), CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir) - flightDirection + panHomeDirOffset);
+            }
+
+            return true;
+        }
+        case OSD_ADSB_INFO:
+        {
+            buff[0] = SYM_ADSB;
+            if(getAdsbStatus()->vehiclesMessagesTotal > 0){
+                tfp_sprintf(buff + 1, "%2d", getActiveVehiclesCount());
+            }else{
+                buff[1] = '-';
+            }
+
+            break;
+        }
+
+#endif
     case OSD_MAP_NORTH:
         {
             static uint16_t drawn = 0;
@@ -2133,8 +2218,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 int8_t blankPos;
                 for (blankPos = 2; blankPos >= 0; blankPos--) {
                     if (buff[blankPos] == SYM_BLANK) {
-                        break;
+            break;
-                    }
+        }
                 }
                 if (blankPos >= 0 || OSD_ALTERNATING_CHOICES(600, 2) == 0) {
                     blankPos = blankPos < 0 ? 0 : blankPos;
@@ -2461,11 +2546,19 @@ static bool osdDrawSingleElement(uint8_t item)
         osdCrosshairPosition(&elemPosX, &elemPosY);
         osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY);
 
-        if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
+        if (osdConfig()->hud_homing && (STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION        
+            || STATE(GPS_ESTIMATED_FIX)
+#endif            
+            ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
             osdHudDrawHoming(elemPosX, elemPosY);
         }
 
-        if (STATE(GPS_FIX) && isImuHeadingValid()) {
+        if ((STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION        
+            || STATE(GPS_ESTIMATED_FIX)
+#endif            
+            ) && isImuHeadingValid()) {
 
             if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
                     osdHudClear();
@@ -3089,7 +3182,11 @@ static bool osdDrawSingleElement(uint8_t item)
                 digits = 4U;
             }
 #endif
-            if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
+            if ((STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif                
+                ) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                     value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
                         1, US2S(efficiencyTimeDelta));
@@ -3111,7 +3208,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
                         buff[digits] = SYM_MAH_MI_0;        // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode
                         buff[digits + 1] = SYM_MAH_MI_1;
                         buff[digits + 2] = '\0';
@@ -3125,7 +3222,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
                         buff[digits] = SYM_MAH_NM_0;
                         buff[digits + 1] = SYM_MAH_NM_1;
                         buff[digits + 2] = '\0';
@@ -3141,7 +3238,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
                         buff[digits] = SYM_MAH_KM_0;
                         buff[digits + 1] = SYM_MAH_KM_1;
                         buff[digits + 2] = '\0';
@@ -3160,7 +3257,11 @@ static bool osdDrawSingleElement(uint8_t item)
             int32_t value = 0;
             timeUs_t currentTimeUs = micros();
             timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
-            if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
+            if ((STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+                    || STATE(GPS_ESTIMATED_FIX)
+#endif
+                ) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                     value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
                         1, US2S(efficiencyTimeDelta));
@@ -3312,7 +3413,11 @@ static bool osdDrawSingleElement(uint8_t item)
             STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier);
             int digits = osdConfig()->plus_code_digits;
             int digitsRemoved = osdConfig()->plus_code_short * 2;
-            if (STATE(GPS_FIX)) {
+            if ((STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION
+                    || STATE(GPS_ESTIMATED_FIX)
+#endif
+            )) {
                 olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff));
             } else {
                 // +codes with > 8 digits have a + at the 9th digit
@@ -3730,7 +3835,7 @@ void osdDrawNextElement(void)
         elementIndex = osdIncElementIndex(elementIndex);
     } while (!osdDrawSingleElement(elementIndex) && index != elementIndex);
 
-    // Draw artificial horizon + tracking telemtry last
+    // Draw artificial horizon + tracking telemetry last
     osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
     if (osdConfig()->telemetry>0){
         osdDisplayTelemetry();
@@ -3755,6 +3860,11 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT,
     .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT,
 #endif
+#ifdef USE_ADSB
+    .adsb_distance_warning = SETTING_OSD_ADSB_DISTANCE_WARNING_DEFAULT,
+    .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT,
+    .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT,
+#endif
 #ifdef USE_SERIALRX_CRSF
     .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT,
     .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT,
@@ -3994,6 +4104,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9);
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10);
 
+    osdLayoutsConfig->item_pos[0][OSD_ADSB_WARNING] = OSD_POS(2, 7);
+    osdLayoutsConfig->item_pos[0][OSD_ADSB_INFO] = OSD_POS(2, 8);
 #if defined(USE_ESC_SENSOR)
     osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2);
     osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3);
@@ -4040,7 +4152,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
 
     if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) {
         logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing
-    }
+}
 
     // Draw Logo(s)
     if (usePilotLogo && !singular) {
@@ -4068,9 +4180,9 @@ uint8_t drawLogos(bool singular, uint8_t row) {
         logoRow = row;
         if (singular) {
             logo_x = logoColOffset;
-        } else {
+    } else {
             logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing;
-        }
+    }
 
         for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
             for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) {
@@ -4081,7 +4193,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
     }
 
     return logoRow;
-}
+    }
 
 uint8_t drawStats(uint8_t row) {
 #ifdef USE_STATS
@@ -4312,7 +4424,7 @@ static void osdUpdateStats(void)
 static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 {
     const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
-    uint8_t top = 1;  // Start one line down leaving space at the top of the screen.
+    uint8_t top = 1;  // Start one line down leaving space at the top of the screen. 
     size_t multiValueLengthOffset = 0;
 
     const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
@@ -4341,7 +4453,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                 osdLeftAlignString(buff);
                 strcat(osdFormatTrimWhiteSpace(buff),"/");
                 multiValueLengthOffset = strlen(buff);
-                displayWrite(osdDisplayPort, statValuesX, top, buff);
+                displayWrite(osdDisplayPort, statValuesX, top, buff);            
                 osdGenerateAverageVelocityStr(buff);
                 osdLeftAlignString(buff);
                 displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
@@ -4378,7 +4490,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                     osdLeftAlignString(buff);
                     strcat(osdFormatTrimWhiteSpace(buff), "%/");
                     multiValueLengthOffset = strlen(buff);
-                    displayWrite(osdDisplayPort, statValuesX, top, buff);
+                    displayWrite(osdDisplayPort, statValuesX, top, buff);                
                     itoa(stats.min_rssi_dbm, buff, 10);
                     tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
                     osdLeftAlignString(buff);
@@ -4393,7 +4505,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                     itoa(stats.min_rssi_dbm, buff, 10);
                     tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
                     displayWrite(osdDisplayPort, statValuesX, top++, buff);
-                }
+                } 
 
                 displayWrite(osdDisplayPort, statNameX, top, "MIN LQ           :");
                 itoa(stats.min_lq, buff, 10);
@@ -4405,7 +4517,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                 itoa(stats.min_rssi, buff, 10);
                 strcat(buff, "%");
                 displayWrite(osdDisplayPort, statValuesX, top++, buff);
-            }
+        }
 
         displayWrite(osdDisplayPort, statNameX, top, "FLY TIME         :");
         uint16_t flySeconds = getFlightTime();
@@ -4457,13 +4569,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
             bool efficiencyValid = totalDistance >= 10000;
             if (feature(FEATURE_GPS)) {
                 displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY   :");
-                uint8_t digits = 3U;    // Total number of digits (including decimal point)
+                    uint8_t digits = 3U;    // Total number of digits (including decimal point)
-                #ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values
+                    #ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values
-                    if (isBfCompatibleVideoSystem(osdConfig())) {
+                        if (isBfCompatibleVideoSystem(osdConfig())) {
-                        // Add one digit so no switch to scaled decimal occurs above 99
+                            // Add one digit so no switch to scaled decimal occurs above 99
-                        digits = 4U;
+                            digits = 4U;
-                    }
+                        }
-                #endif
+                    #endif
                 switch (osdConfig()->units) {
                     case OSD_UNIT_UK:
                         FALLTHROUGH;
@@ -4552,9 +4664,9 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
         displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
         osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4, false);
         osdLeftAlignString(buff);
-        strcat(osdFormatTrimWhiteSpace(buff),"/");
+        strcat(osdFormatTrimWhiteSpace(buff),"/"); 
-        multiValueLengthOffset = strlen(buff);
+        multiValueLengthOffset = strlen(buff);        
-        displayWrite(osdDisplayPort, statValuesX, top, buff);
+        displayWrite(osdDisplayPort, statValuesX, top, buff);        
         osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3, false);
         osdLeftAlignString(buff);
         displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
@@ -4576,10 +4688,10 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 // HD arming screen. based on the minimum HD OSD grid size of 50 x 18
 static void osdShowHDArmScreen(void)
 {
-    dateTime_t  dt;
+    dateTime_t dt;
     char        buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
     char        buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
-    char        craftNameBuf[MAX_NAME_LENGTH];
+    char craftNameBuf[MAX_NAME_LENGTH];
     char        versionBuf[osdDisplayPort->cols];
     uint8_t     safehomeRow     = 0;
     uint8_t     armScreenRow    = 2; // Start at row 2
@@ -4619,7 +4731,7 @@ static void osdShowHDArmScreen(void)
 #if defined(USE_GPS)
     if (feature(FEATURE_GPS)) {
         if (STATE(GPS_FIX_HOME)) {
-            if (osdConfig()->osd_home_position_arm_screen) {
+            if (osdConfig()->osd_home_position_arm_screen){
                 osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
                 osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
                 uint8_t gap = 1;
@@ -4675,7 +4787,7 @@ static void osdShowHDArmScreen(void)
     if (armScreenRow < (osdDisplayPort->rows - 4))
         armScreenRow = drawStats(armScreenRow);
 #endif // USE_STATS
-}
+    }
 
 static void osdShowSDArmScreen(void)
 {
@@ -4902,27 +5014,27 @@ static void osdRefresh(timeUs_t currentTimeUs)
 
              // Manual paging stick commands are only applicable to multi-page stats.
              // ******************************
-             // For single-page stats, this effectively disables the ability to cancel the
+             // For single-page stats, this effectively disables the ability to cancel the 
              // automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time
-             // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then
+             // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then 
-             // "Saved Settings" should display if it is active within the refresh interval.
+             // "Saved Settings" should display if it is active within the refresh interval. 
              // ******************************
              // With multi-page stats, "Saved Settings" could also be missed if the user
-             // has canceled automatic paging using the stick commands, because that is only
+             // has canceled automatic paging using the stick commands, because that is only 
-             // updated when osdShowStats() is called. So, in that case, they would only see
+             // updated when osdShowStats() is called. So, in that case, they would only see 
-             // the "Saved Settings" message if they happen to manually change pages using the
+             // the "Saved Settings" message if they happen to manually change pages using the 
-             // stick commands within the interval the message is displayed.
+             // stick commands within the interval the message is displayed. 
             bool manualPageUpRequested = false;
-            bool manualPageDownRequested = false;
+            bool manualPageDownRequested = false;       
             if (!statsSinglePageCompatible) {
                 // These methods ensure the paging stick commands are held for a brief period
-                // Otherwise it can result in a race condition where the stats are
+                // Otherwise it can result in a race condition where the stats are 
-                // updated too quickly and can result in partial blanks, etc.
+                // updated too quickly and can result in partial blanks, etc. 
-                if (osdIsPageUpStickCommandHeld()) {
+                if (osdIsPageUpStickCommandHeld()) {               
                     manualPageUpRequested = true;
                     statsAutoPagingEnabled = false;
                 } else if (osdIsPageDownStickCommandHeld()) {
-                    manualPageDownRequested = true;
+                    manualPageDownRequested = true;                
                     statsAutoPagingEnabled = false;
                 }
             }
@@ -4945,16 +5057,16 @@ static void osdRefresh(timeUs_t currentTimeUs)
                 // Process manual page change events for multi-page stats.
                 if (manualPageUpRequested) {
                     osdShowStats(statsSinglePageCompatible, 1);
-                    statsCurrentPage = 1;
+                    statsCurrentPage = 1;                   
                 } else if (manualPageDownRequested) {
                     osdShowStats(statsSinglePageCompatible, 0);
                     statsCurrentPage = 0;
-                }
             }
         }
+        }
 
         // Handle events when either "Splash", "Armed" or "Stats" screens are displayed.
-        if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) {
+        if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { 
             // Time elapsed or canceled by stick commands.
             // Exit to normal OSD operation.
             displayClearScreen(osdDisplayPort);
@@ -4964,7 +5076,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             // Continue "Splash", "Armed" or "Stats" screens.
             displayHeartbeat(osdDisplayPort);
         }
-
+        
         return;
     }
 
@@ -5247,7 +5359,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         }
                     }
                     if (isFixedWingLevelTrimActive()) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
+                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
                     }
                     if (FLIGHT_MODE(HEADFREE_MODE)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);

src/main/io/osd.h

@@ -284,6 +284,8 @@ typedef enum {
     OSD_CUSTOM_ELEMENT_1,
     OSD_CUSTOM_ELEMENT_2,
     OSD_CUSTOM_ELEMENT_3,
+    OSD_ADSB_WARNING,
+    OSD_ADSB_INFO,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -459,6 +461,11 @@ typedef struct osdConfig_s {
     bool            use_pilot_logo;                     // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
     uint8_t         inav_to_pilot_logo_spacing;         // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
     uint16_t        arm_screen_display_time;            // Length of time the arm screen is displayed
+    #ifdef USE_ADSB
+    uint16_t adsb_distance_warning;                     // in metres
+    uint16_t adsb_distance_alert;                       // in metres
+    uint16_t adsb_ignore_plane_above_me_limit;          // in metres
+#endif
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

src/main/io/osd_dji_hd.c

@@ -787,7 +787,11 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
     timeUs_t currentTimeUs = micros();
     timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
 
-    if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
+    if ((STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) && gpsSol.groundSpeed > 0) {
         if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
             value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
                 1, US2S(efficiencyTimeDelta));

src/main/msp/msp_protocol_v2_inav.h

@@ -103,6 +103,8 @@
 
 #define MSP2_INAV_SELECT_MIXER_PROFILE          0x2080
 
+#define MSP2_ADSB_VEHICLE_LIST                  0x2090
+
 #define MSP2_INAV_CUSTOM_OSD_ELEMENTS           0x2100
 #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS       0x2101
 

src/main/navigation/navigation.c

@@ -56,6 +56,7 @@
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
+#include "navigation/rth_trackback.h"
 
 #include "rx/rx.h"
 
@@ -154,9 +155,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT,                                           // speed in autonomous modes (3 m/s = 10.8 km/h)
         .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT,                               // Minimum ground speed (m/s)
         .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT,                                   // max allowed speed autonomous modes
-        .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT,                             // 5 m/s
         .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
-        .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
         .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT,                       // altitude in centimeters
         .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT,                       // altitude in meters
         .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT,                               // centimeters/s
@@ -182,6 +181,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     // MC-specific
     .mc = {
         .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT,                          // degrees
+        .max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT,                             // 5 m/s
+        .max_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT,
 
 #ifdef USE_MR_BRAKING_MODE
         .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT,               // Braking can become active above 1m/s
@@ -203,6 +204,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     // Fixed wing
     .fw = {
         .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT,                    // degrees
+        .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT,      // 3 m/s
         .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT,                  // degrees
         .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT,                    // degrees
         .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT,                    // cm/s
@@ -283,11 +285,9 @@ static void resetJumpCounter(void);
 static void clearJumpCounters(void);
 
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
-static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
 void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance);
 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
-static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
 bool isWaypointAltitudeReached(void);
 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
 static navigationFSMEvent_t nextForNonGeoStates(void);
@@ -1426,16 +1426,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
             setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 
             return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;   // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
-        }
+        } else {
-        else {
+            if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
-            // Switch to RTH trackback
+                rthTrackBackUpdate(true);       // save final trackpoint for altitude and max trackback distance reference
-            bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||
-                                   (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
-
-            if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
-                updateRthTrackback(true);       // save final trackpoint for altitude and max trackback distance reference
                 posControl.flags.rthTrackbackActive = true;
-                calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
+                calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
                 return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK;
             }
 
@@ -1550,36 +1545,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
-    if (posControl.flags.estPosStatus >= EST_USABLE) {
+    if (rthTrackBackSetNewPosition()) {
-        const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
+        return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
-#ifdef USE_MULTI_FUNCTIONS
-        const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
-#else
-        const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
-#endif
-        const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
-                                     (overrideTrackback && !posControl.flags.forcedRTHActivated);
-
-        if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
-            posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
-            posControl.flags.rthTrackbackActive = false;
-            return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;    // procede to home after final trackback point
-        }
-
-        if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
-            posControl.activeRthTBPointIndex--;
-
-            if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
-                posControl.activeRthTBPointIndex = NAV_RTH_TRACKBACK_POINTS - 1;
-            }
-            calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
-
-            if (posControl.activeRthTBPointIndex - posControl.rthTBWrapAroundCounter == 0) {
-                posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
-            }
-        } else {
-            setDesiredPosition(rthGetTrackbackPos(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
-        }
     }
 
     return NAV_FSM_EVENT_NONE;
@@ -2832,7 +2799,7 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
  * Check if waypoint is/was reached.
  * waypointBearing stores initial bearing to waypoint
  *-----------------------------------------------------------*/
-static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
+bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing)
 {
     posControl.wpDistance = calculateDistanceToDestination(waypointPos);
 
@@ -2956,8 +2923,19 @@ bool validateRTHSanityChecker(void)
         return true;
     }
 
+#ifdef USE_GPS_FIX_ESTIMATION
+    if (STATE(GPS_ESTIMATED_FIX)) {
+        //disable sanity checks in GPS estimation mode
+        //when estimated GPS fix is replaced with real fix, coordinates may jump
+        posControl.rthSanityChecker.minimalDistanceToHome = 1e10f;
+        //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump
+        posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
+        return true;
+    }
+#endif
+
     // Check at 10Hz rate
-    if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
+    if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
         const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
         posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
 
@@ -3181,12 +3159,13 @@ void updateHomePosition(void)
  * Climb First override limited to Fixed Wing only
  * Roll also cancels RTH trackback on Fixed Wing and Multirotor
  *-----------------------------------------------------------*/
-static bool rthAltControlStickOverrideCheck(unsigned axis)
+bool rthAltControlStickOverrideCheck(uint8_t axis)
 {
     if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated ||
         (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) {
         return false;
     }
+    
     static timeMs_t rthOverrideStickHoldStartTime[2];
 
     if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
@@ -3225,110 +3204,6 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
     return false;
  }
 
-/* --------------------------------------------------------------------------------
- * == RTH Trackback ==
- * Saves track during flight which is used during RTH to back track
- * along arrival route rather than immediately heading directly toward home.
- * Max desired trackback distance set by user or limited by number of available points.
- * Reverts to normal RTH heading direct to home when end of track reached.
- * Trackpoints logged with precedence for course/altitude changes. Distance based changes
- * only logged if no course/altitude changes logged over an extended distance.
- * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
- * --------------------------------------------------------------------------------- */
- static void updateRthTrackback(bool forceSaveTrackPoint)
-{
-    static bool suspendTracking = false;
-    bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
-    if (!fwLoiterIsActive && suspendTracking) {
-        suspendTracking = false;
-    }
-
-    if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) {
-        return;
-    }
-
-    // Record trackback points based on significant change in course/altitude until
-    // points limit reached. Overwrite older points from then on.
-    if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) {
-        static int32_t previousTBTripDist;      // cm
-        static int16_t previousTBCourse;        // degrees
-        static int16_t previousTBAltitude;      // meters
-        static uint8_t distanceCounter = 0;
-        bool saveTrackpoint = forceSaveTrackPoint;
-        bool GPSCourseIsValid = isGPSHeadingValid();
-
-        // start recording when some distance from home, 50m seems reasonable.
-        if (posControl.activeRthTBPointIndex < 0) {
-            saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
-
-            previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
-            previousTBTripDist = posControl.totalTripDistance;
-        } else {
-            // Minimum distance increment between course change track points when GPS course valid - set to 10m
-            const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(10);
-
-            // Altitude change
-            if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > 10) {   // meters
-                saveTrackpoint = true;
-            } else if (distanceIncrement && GPSCourseIsValid) {
-                // Course change - set to 45 degrees
-                if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) {
-                    saveTrackpoint = true;
-                } else if (distanceCounter >= 9) {
-                    // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change
-                    // and deviation from projected course path > 20m
-                    float distToPrevPoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]);
-
-                    fpVector3_t virtualCoursePoint;
-                    virtualCoursePoint.x = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse));
-                    virtualCoursePoint.y = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse));
-
-                    saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(20);
-                }
-                distanceCounter++;
-                previousTBTripDist = posControl.totalTripDistance;
-            } else if (!GPSCourseIsValid) {
-                // if no reliable course revert to basic distance logging based on direct distance from last point - set to 20m
-                saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20);
-                previousTBTripDist = posControl.totalTripDistance;
-            }
-
-            // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
-            if (distanceCounter && fwLoiterIsActive) {
-                saveTrackpoint = suspendTracking = true;
-            }
-        }
-
-        // when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position
-        if (saveTrackpoint) {
-            if (posControl.activeRthTBPointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) {   // wraparound to start
-                posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = 0;
-            } else {
-                posControl.activeRthTBPointIndex++;
-                if (posControl.rthTBWrapAroundCounter > -1) {   // track wraparound overwrite position after store first filled
-                    posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex;
-                }
-            }
-            posControl.rthTBPointsList[posControl.activeRthTBPointIndex] = posControl.actualState.abs.pos;
-
-            posControl.rthTBLastSavedIndex = posControl.activeRthTBPointIndex;
-            previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z);
-            previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse;
-            distanceCounter = 0;
-        }
-    }
-}
-
-static fpVector3_t * rthGetTrackbackPos(void)
-{
-    // ensure trackback altitude never lower than altitude of start point
-    if (posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z < posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z) {
-        posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z = posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z;
-    }
-
-    return &posControl.rthTBPointsList[posControl.activeRthTBPointIndex];
-}
-
 /*-----------------------------------------------------------
  * Update flight statistics
  *-----------------------------------------------------------*/
@@ -4007,7 +3882,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
     geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
 }
 
-static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
+void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos)
 {
     // Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints)
     if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
@@ -4100,7 +3975,7 @@ bool isWaypointNavTrackingActive(void)
     // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
     // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
 
-    return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
+    return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex);
 }
 
 /*-----------------------------------------------------------
@@ -4140,9 +4015,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
     if (posControl.flags.estAglStatus == EST_TRUSTED)       navFlags |= (1 << 1);
     if (posControl.flags.estPosStatus == EST_TRUSTED)       navFlags |= (1 << 2);
     if (posControl.flags.isTerrainFollowEnabled)            navFlags |= (1 << 3);
-#if defined(NAV_GPS_GLITCH_DETECTION)
+    // naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag
-    if (isGPSGlitchDetected())                              navFlags |= (1 << 4);
-#endif
     if (posControl.flags.estHeadingStatus == EST_TRUSTED)   navFlags |= (1 << 5);
 
     // Reset all navigation requests - NAV controllers will set them if necessary
@@ -4157,9 +4030,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
         //  ensure WP missions always restart from first waypoint after disarm
         posControl.activeWaypointIndex = posControl.startWpIndex;
         // Reset RTH trackback
-        posControl.activeRthTBPointIndex = -1;
+        resetRthTrackBack();
-        posControl.flags.rthTrackbackActive = false;
-        posControl.rthTBWrapAroundCounter = -1;
 
         return;
     }
@@ -4605,7 +4476,11 @@ void updateWpMissionPlanner(void)
 {
     static timeMs_t resetTimerStart = 0;
     if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
-        const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
+        const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            );
 
         posControl.flags.wpMissionPlannerActive = true;
         if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
@@ -4693,7 +4568,7 @@ void updateWaypointsAndNavigationMode(void)
     updateWpMissionPlanner();
 
     // Update RTH trackback
-    updateRthTrackback(false);
+    rthTrackBackUpdate(false);
 
     //Update Blackbox data
     navCurrentState = (int16_t)posControl.navPersistentId;

src/main/navigation/navigation.h

@@ -262,6 +262,10 @@ typedef struct positionEstimationConfig_s {
     float baro_epv;     // Baro position error
 
     uint8_t use_gps_no_baro;
+
+#ifdef USE_GPS_FIX_ESTIMATION
+    uint8_t allow_gps_fix_estimation;
+#endif    
 } positionEstimationConfig_t;
 
 PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);
@@ -300,9 +304,7 @@ typedef struct navConfig_s {
         uint16_t auto_speed;                        // autonomous navigation speed cm/sec
         uint8_t  min_ground_speed;                  // Minimum navigation ground speed [m/s]
         uint16_t max_auto_speed;                    // maximum allowed autonomous navigation speed cm/sec
-        uint16_t max_auto_climb_rate;               // max vertical speed limitation cm/sec
         uint16_t max_manual_speed;                  // manual velocity control max horizontal speed
-        uint16_t max_manual_climb_rate;             // manual velocity control max vertical speed
         uint16_t land_minalt_vspd;                  // Final RTH landing descent rate under minalt
         uint16_t land_maxalt_vspd;                  // RTH landing descent rate target at maxalt
         uint16_t land_slowdown_minalt;              // Altitude to stop lowering descent rate during RTH descend
@@ -327,6 +329,8 @@ typedef struct navConfig_s {
 
     struct {
         uint8_t  max_bank_angle;                // multicopter max banking angle (deg)
+        uint16_t max_auto_climb_rate;           // max vertical speed limitation cm/sec
+        uint16_t max_manual_climb_rate;         // manual velocity control max vertical speed
 
 #ifdef USE_MR_BRAKING_MODE
         uint16_t braking_speed_threshold;       // above this speed braking routine might kick in
@@ -347,6 +351,7 @@ typedef struct navConfig_s {
 
     struct {
         uint8_t  max_bank_angle;             // Fixed wing max banking angle (deg)
+        uint16_t max_manual_climb_rate;      // manual velocity control max vertical speed
         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
         uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
@@ -630,6 +635,9 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
 // Select absolute or relative altitude based on WP mission flag setting
 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
 
+void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos);
+bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
+
 /* Distance/bearing calculation */
 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);   // NOT USED
 uint32_t distanceToFirstWP(void);
@@ -682,6 +690,7 @@ bool isEstimatedAglTrusted(void);
 
 void checkManualEmergencyLandingControl(bool forcedActivation);
 float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
+bool rthAltControlStickOverrideCheck(uint8_t axis);
 
 int8_t navCheckActiveAngleHoldAxis(void);
 uint8_t getActiveWpNumber(void);

src/main/navigation/navigation_fixedwing.c

@@ -115,7 +115,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
 
     if (rcAdjustment) {
         // set velocity proportional to stick movement
-        float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
+        float rcClimbRate = -rcAdjustment * navConfig()->fw.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
         updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
         return true;
     }
@@ -743,7 +743,7 @@ bool isFixedWingLandingDetected(void)
 
     // Check horizontal and vertical velocities are low (cm/s)
     bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
-                        posControl.actualState.velXY < (100.0f * sensitivity);
+                        ( posControl.actualState.velXY < (100.0f * sensitivity));
     // Check angular rates are low (degs/s)
     bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity);
     DEBUG_SET(DEBUG_LANDING, 2, velCondition);

src/main/navigation/navigation_multicopter.c

@@ -77,9 +77,9 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
     float vel_max_z = 0.0f;
 
     if (posControl.flags.isAdjustingAltitude) {
-        vel_max_z = navConfig()->general.max_manual_climb_rate;
+        vel_max_z = navConfig()->mc.max_manual_climb_rate;
     } else {
-        vel_max_z = navConfig()->general.max_auto_climb_rate;
+        vel_max_z = navConfig()->mc.max_auto_climb_rate;
     }
 
     targetVel = constrainf(targetVel, -vel_max_z, vel_max_z);
@@ -151,11 +151,11 @@ bool adjustMulticopterAltitudeFromRCInput(void)
             // Make sure we can satisfy max_manual_climb_rate in both up and down directions
             if (rcThrottleAdjustment > 0) {
                 // Scaling from altHoldThrottleRCZero to maxthrottle
-                rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband);
+                rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband);
             }
             else {
                 // Scaling from minthrottle to altHoldThrottleRCZero
-                rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
+                rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
             }
 
             updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
@@ -251,8 +251,8 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
             if (prepareForTakeoffOnReset) {
                 const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
 
-                posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
+                posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate;
-                posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
+                posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->mc.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
                 posControl.pids.vel[Z].integrator = -500.0f;
                 pt1FilterReset(&altholdThrottleFilterState, -500.0f);
                 prepareForTakeoffOnReset = false;

src/main/navigation/navigation_pos_estimator.c

@@ -90,7 +90,10 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT,
 
         .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
-        .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT
+        .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,
+#ifdef USE_GPS_FIX_ESTIMATION
+        .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
+#endif
 );
 
 #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
@@ -159,47 +162,6 @@ static timeUs_t getGPSDeltaTimeFilter(timeUs_t dTus)
     return dTus;                                                 // Filter failed. Set GPS Hz by measurement
 }
 
-#if defined(NAV_GPS_GLITCH_DETECTION)
-static bool detectGPSGlitch(timeUs_t currentTimeUs)
-{
-    static timeUs_t previousTime = 0;
-    static fpVector3_t lastKnownGoodPosition;
-    static fpVector3_t lastKnownGoodVelocity;
-
-    bool isGlitching = false;
-
-    if (previousTime == 0) {
-        isGlitching = false;
-    }
-    else {
-        fpVector3_t predictedGpsPosition;
-        float gpsDistance;
-        float dT = US2S(currentTimeUs - previousTime);
-
-        /* We predict new position based on previous GPS velocity and position */
-        predictedGpsPosition.x = lastKnownGoodPosition.x + lastKnownGoodVelocity.x * dT;
-        predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
-
-        /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
-        gpsDistance = calc_length_pythagorean_2D(predictedGpsPosition.x - lastKnownGoodPosition.x, predictedGpsPosition.y - lastKnownGoodPosition.y);
-        if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
-            isGlitching = false;
-        }
-        else {
-            isGlitching = true;
-        }
-    }
-
-    if (!isGlitching) {
-        previousTime = currentTimeUs;
-        lastKnownGoodPosition = posEstimator.gps.pos;
-        lastKnownGoodVelocity = posEstimator.gps.vel;
-    }
-
-    return isGlitching;
-}
-#endif
-
 /**
  * Update GPS topic
  *  Function is called on each GPS update
@@ -219,8 +181,16 @@ void onNewGPSData(void)
     newLLH.lon = gpsSol.llh.lon;
     newLLH.alt = gpsSol.llh.alt;
 
-    if (sensors(SENSOR_GPS)) {
+    if (sensors(SENSOR_GPS) 
-        if (!STATE(GPS_FIX)) {
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
+        if (!(STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            )) {
             isFirstGPSUpdate = true;
             return;
         }
@@ -275,19 +245,6 @@ void onNewGPSData(void)
                     posEstimator.gps.vel.z = (posEstimator.gps.vel.z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f;
                 }
 
-#if defined(NAV_GPS_GLITCH_DETECTION)
-                /* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */
-                if (detectGPSGlitch(currentTimeUs)) {
-                    posEstimator.gps.glitchRecovery = false;
-                    posEstimator.gps.glitchDetected = true;
-                }
-                else {
-                    /* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */
-                    posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected;
-                    posEstimator.gps.glitchDetected = false;
-                }
-#endif
-
                 /* FIXME: use HDOP/VDOP */
                 if (gpsSol.flags.validEPE) {
                     posEstimator.gps.eph = gpsSol.eph;
@@ -502,7 +459,11 @@ static bool navIsAccelerationUsable(void)
 
 static bool navIsHeadingUsable(void)
 {
-    if (sensors(SENSOR_GPS)) {
+    if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+        || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
         // If we have GPS - we need true IMU north (valid heading)
         return isImuHeadingValid();
     }
@@ -517,7 +478,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
     /* Figure out if we have valid position data from our data sources */
     uint32_t newFlags = 0;
 
-    if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid &&
+    if ((sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) && posControl.gpsOrigin.valid &&
         ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
         (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
         if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
@@ -713,7 +678,11 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
 static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
-    if (STATE(GPS_FIX) && navIsHeadingUsable()) {
+    if ((STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+    ) && navIsHeadingUsable()) {
         uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y, posEstimator.est.vel.x)));
         posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
     }
@@ -863,13 +832,6 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
     }
 }
 
-#if defined(NAV_GPS_GLITCH_DETECTION)
-bool isGPSGlitchDetected(void)
-{
-    return posEstimator.gps.glitchDetected;
-}
-#endif
-
 float getEstimatedAglPosition(void) {
     return posEstimator.est.aglAlt;
 }

src/main/navigation/navigation_pos_estimator_private.h

@@ -65,10 +65,6 @@ typedef struct {
 
 typedef struct {
     timeUs_t    lastUpdateTime; // Last update time (us)
-#if defined(NAV_GPS_GLITCH_DETECTION)
-    bool        glitchDetected;
-    bool        glitchRecovery;
-#endif
     fpVector3_t pos;            // GPS position in NEU coordinate system (cm)
     fpVector3_t vel;            // GPS velocity (cms)
     float       eph;

src/main/navigation/navigation_private.h

@@ -45,8 +45,6 @@
 #define MC_LAND_DESCEND_THROTTLE            40      // RC pwm units (us)
 #define MC_LAND_SAFE_SURFACE                5.0f    // cm
 
-#define NAV_RTH_TRACKBACK_POINTS            50      // max number RTH trackback points
-
 #define MAX_POSITION_UPDATE_INTERVAL_US     HZ2US(MIN_POSITION_UPDATE_RATE_HZ)        // convenience macro
 _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");
 
@@ -467,12 +465,6 @@ typedef struct {
     timeMs_t                    wpReachedTime;              // Time the waypoint was reached
     bool                        wpAltitudeReached;          // WP altitude achieved
 
-    /* RTH Trackback */
-    fpVector3_t                 rthTBPointsList[NAV_RTH_TRACKBACK_POINTS];
-    int8_t                      rthTBLastSavedIndex;        // last trackback point index saved
-    int8_t                      activeRthTBPointIndex;
-    int8_t                      rthTBWrapAroundCounter;     // stores trackpoint array overwrite index position
-
 #ifdef USE_FW_AUTOLAND
     /* Fixedwing autoland */
     fwLandState_t fwLandState;

src/main/navigation/rth_trackback.c

@@ -0,0 +1,182 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* --------------------------------------------------------------------------------
+ * == RTH Trackback ==
+ * Saves track during flight which is used during RTH to back track
+ * along arrival route rather than immediately heading directly toward home.
+ * Max desired trackback distance set by user or limited by number of available points.
+ * Reverts to normal RTH heading direct to home when end of track reached.
+ * Trackpoints logged with precedence for course/altitude changes. Distance based changes
+ * only logged if no course/altitude changes logged over an extended distance.
+ * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
+ * --------------------------------------------------------------------------------- */
+
+#include "platform.h"
+
+#include "fc/multifunction.h"
+#include "fc/rc_controls.h"
+
+#include "navigation/rth_trackback.h"
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+rth_trackback_t rth_trackback;
+
+bool rthTrackBackIsActive(void) 
+{
+    return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||  (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
+}
+
+void rthTrackBackUpdate(bool forceSaveTrackPoint)
+{
+    static bool suspendTracking = false;
+    bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
+
+    if (!fwLoiterIsActive && suspendTracking) {
+        suspendTracking = false;
+    }
+
+    if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) {
+        return;
+    }
+
+    // Record trackback points based on significant change in course/altitude until points limit reached. Overwrite older points from then on.
+    if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) {
+        static int32_t previousTBTripDist;      // cm
+        static int16_t previousTBCourse;        // degrees
+        static int16_t previousTBAltitude;      // meters
+        static uint8_t distanceCounter = 0;
+        bool saveTrackpoint = forceSaveTrackPoint;
+        bool GPSCourseIsValid = isGPSHeadingValid();
+
+        // Start recording when some distance from home
+        if (rth_trackback.activePointIndex < 0) {
+            saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_DIST_TO_START);
+            previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
+            previousTBTripDist = posControl.totalTripDistance;
+        } else {
+            // Minimum distance increment between course change track points when GPS course valid
+            const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_TRIP_DIST_TO_SAVE);
+
+            // Altitude change
+            if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > NAV_RTH_TRACKBACK_MIN_Z_DIST_TO_SAVE) {
+                saveTrackpoint = true;
+            } else if (distanceIncrement && GPSCourseIsValid) {
+                // Course change - set to 45 degrees
+                if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) {
+                    saveTrackpoint = true;
+                } else if (distanceCounter >= 9) {
+                    // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change and deviation from projected course path > 20m
+                    float distToPrevPoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]);
+
+                    fpVector3_t virtualCoursePoint;
+                    virtualCoursePoint.x = rth_trackback.pointsList[rth_trackback.activePointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse));
+                    virtualCoursePoint.y = rth_trackback.pointsList[rth_trackback.activePointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse));
+
+                    saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE);
+                }
+                distanceCounter++;
+                previousTBTripDist = posControl.totalTripDistance;
+            } else if (!GPSCourseIsValid) {
+                // If no reliable course revert to basic distance logging based on direct distance from last point
+                saveTrackpoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE);
+                previousTBTripDist = posControl.totalTripDistance;
+            }
+
+            // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
+            if (distanceCounter && fwLoiterIsActive) {
+                saveTrackpoint = suspendTracking = true;
+            }
+        }
+
+        // When trackpoint store full, overwrite from start of store using 'WrapAroundCounter' to track overwrite position
+        if (saveTrackpoint) {
+            if (rth_trackback.activePointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) {   // Wraparound to start
+                rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = 0;
+            } else {
+                rth_trackback.activePointIndex++;
+                if (rth_trackback.WrapAroundCounter > -1) {   // Track wraparound overwrite position after store first filled
+                    rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex;
+                }
+            }
+
+            rth_trackback.pointsList[rth_trackback.activePointIndex] = posControl.actualState.abs.pos;
+            rth_trackback.lastSavedIndex = rth_trackback.activePointIndex;
+            previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z);
+            previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse;
+            distanceCounter = 0;
+        }
+    }
+}
+
+bool rthTrackBackSetNewPosition(void)
+{
+    if (posControl.flags.estPosStatus == EST_NONE) {
+        return false;
+    }
+
+    const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex]));
+
+#ifdef USE_MULTI_FUNCTIONS
+    const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
+#else
+    const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
+#endif
+    const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance || (overrideTrackback && !posControl.flags.forcedRTHActivated);
+
+    if (rth_trackback.activePointIndex < 0 || cancelTrackback) {
+        rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
+        posControl.flags.rthTrackbackActive = false;
+        return true;    // Procede to home after final trackback point
+    }
+
+    if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
+        rth_trackback.activePointIndex--;
+
+        if (rth_trackback.WrapAroundCounter > -1 && rth_trackback.activePointIndex < 0) {
+            rth_trackback.activePointIndex = NAV_RTH_TRACKBACK_POINTS - 1;
+        }
+
+        calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
+
+        if (rth_trackback.activePointIndex - rth_trackback.WrapAroundCounter == 0) {
+            rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
+        }
+    } else {
+        setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+    }
+
+    return false;
+}
+
+fpVector3_t *getRthTrackBackPosition(void)
+{
+    // Ensure trackback altitude never lower than altitude of start point
+    if (rth_trackback.pointsList[rth_trackback.activePointIndex].z < rth_trackback.pointsList[rth_trackback.lastSavedIndex].z) {
+        rth_trackback.pointsList[rth_trackback.activePointIndex].z = rth_trackback.pointsList[rth_trackback.lastSavedIndex].z;
+    }
+
+    return &rth_trackback.pointsList[rth_trackback.activePointIndex];
+}
+
+void resetRthTrackBack(void) 
+{
+    rth_trackback.activePointIndex = -1;
+    posControl.flags.rthTrackbackActive = false;
+    rth_trackback.WrapAroundCounter = -1;   
+}

src/main/navigation/rth_trackback.h

@@ -0,0 +1,42 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "common/vector.h"
+
+#define NAV_RTH_TRACKBACK_POINTS                50 // max number RTH trackback points
+#define NAV_RTH_TRACKBACK_MIN_DIST_TO_START     50 // start recording when some distance from home (meters)
+#define NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE   20 // minimum XY distance between two points to store in the buffer (meters)
+#define NAV_RTH_TRACKBACK_MIN_Z_DIST_TO_SAVE    10 // minimum Z distance between two points to store in the buffer (meters)
+#define NAV_RTH_TRACKBACK_MIN_TRIP_DIST_TO_SAVE 10 // minimum trip distance between two points to store in the buffer (meters)
+
+typedef struct
+{
+    fpVector3_t pointsList[NAV_RTH_TRACKBACK_POINTS]; // buffer of points stored
+    int16_t lastSavedIndex;                           // last trackback point index saved
+    int16_t activePointIndex;                         // trackback points counter
+    int16_t WrapAroundCounter;                        // stores trackpoint array overwrite index position
+} rth_trackback_t;
+
+extern rth_trackback_t rth_trackback;
+
+bool rthTrackBackIsActive(void);
+bool rthTrackBackSetNewPosition(void);
+void rthTrackBackUpdate(bool forceSaveTrackPoint);
+fpVector3_t *getRthTrackBackPosition(void);
+void resetRthTrackBack(void);

src/main/programming/logic_condition.c

@@ -97,7 +97,7 @@ static int logicConditionCompute(
     int temporaryValue;
 #if defined(USE_VTX_CONTROL)
     vtxDeviceCapability_t vtxDeviceCapability;
-#endif
+#endif    
 
     switch (operation) {
 
@@ -154,7 +154,7 @@ static int logicConditionCompute(
 
         case LOGIC_CONDITION_NOR:
             return !(operandA || operandB);
-            break;
+            break; 
 
         case LOGIC_CONDITION_NOT:
             return !operandA;
@@ -170,7 +170,7 @@ static int logicConditionCompute(
                 return false;
             }
 
-            //When both operands are not met, keep current value
+            //When both operands are not met, keep current value 
             return currentValue;
             break;
 
@@ -238,7 +238,7 @@ static int logicConditionCompute(
             gvSet(operandA, operandB);
             return operandB;
             break;
-
+        
         case LOGIC_CONDITION_GVAR_INC:
             temporaryValue = gvGet(operandA) + operandB;
             gvSet(operandA, temporaryValue);
@@ -270,7 +270,7 @@ static int logicConditionCompute(
                 return operandA;
             }
             break;
-
+        
         case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY:
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
             return true;
@@ -287,11 +287,18 @@ static int logicConditionCompute(
             return true;
             break;
 
+#ifdef USE_MAG
+        case LOGIC_CONDITION_RESET_MAG_CALIBRATION:
+
+            ENABLE_STATE(CALIBRATE_MAG);
+            return true;
+            break;
+#endif  
         case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
-#if defined(USE_VTX_CONTROL)
+#if defined(USE_VTX_CONTROL) 
 #if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
             if (
-                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
+                logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && 
                 vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
             ) {
                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
@@ -339,18 +346,18 @@ static int logicConditionCompute(
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
             return true;
             break;
-
+        
         case LOGIC_CONDITION_INVERT_YAW:
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
             return true;
             break;
-
+        
         case LOGIC_CONDITION_OVERRIDE_THROTTLE:
             logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA;
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
             return operandA;
             break;
-
+        
         case LOGIC_CONDITION_SET_OSD_LAYOUT:
             logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA;
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
@@ -366,18 +373,18 @@ static int logicConditionCompute(
 
         case LOGIC_CONDITION_SIN:
             temporaryValue = (operandB == 0) ? 500 : operandB;
-            return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
+            return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; 
             break;
-
+    
         case LOGIC_CONDITION_COS:
             temporaryValue = (operandB == 0) ? 500 : operandB;
-            return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
+            return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; 
             break;
         break;
-
+    
         case LOGIC_CONDITION_TAN:
             temporaryValue = (operandB == 0) ? 500 : operandB;
-            return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
+            return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; 
         break;
 
         case LOGIC_CONDITION_MIN:
@@ -387,11 +394,11 @@ static int logicConditionCompute(
         case LOGIC_CONDITION_MAX:
             return (operandA > operandB) ? operandA : operandB;
         break;
-
+    
         case LOGIC_CONDITION_MAP_INPUT:
             return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
         break;
-
+    
         case LOGIC_CONDITION_MAP_OUTPUT:
             return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
         break;
@@ -486,9 +493,20 @@ static int logicConditionCompute(
             return operandA;
             break;
 #endif
+#ifdef USE_GPS_FIX_ESTIMATION
+        case LOGIC_CONDITION_DISABLE_GPS_FIX:
+            if (operandA > 0) {
+                LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
+            } else {
+                LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
+            }
+                return true;
+            break;
+#endif
+
         default:
             return false;
-            break;
+            break; 
     }
 }
 
@@ -497,7 +515,7 @@ void logicConditionProcess(uint8_t i) {
     const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
 
     if (logicConditions(i)->enabled && activatorValue && !cliMode) {
-
+        
         /*
          * Process condition only when latch flag is not set
          * Latched LCs can only go from OFF to ON, not the other way
@@ -506,13 +524,13 @@ void logicConditionProcess(uint8_t i) {
             const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value);
             const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
             const int newValue = logicConditionCompute(
-                logicConditionStates[i].value,
+                logicConditionStates[i].value, 
-                logicConditions(i)->operation,
+                logicConditions(i)->operation, 
-                operandAValue,
+                operandAValue, 
                 operandBValue,
                 i
             );
-
+        
             logicConditionStates[i].value = newValue;
 
             /*
@@ -587,7 +605,7 @@ static int logicConditionGetWaypointOperandValue(int operand) {
                 return distance;
             }
             break;
-
+        
         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION:
             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
             break;
@@ -633,11 +651,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
             return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
             break;
-
+        
         case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
             return constrain(GPS_distanceToHome, 0, INT16_MAX);
             break;
-
+        
         case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
             return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
             break;
@@ -645,7 +663,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
             return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
             break;
-
+        
         case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
             return getBatteryVoltage();
             break;
@@ -661,19 +679,24 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
             return getAmperage();
             break;
-
+        
         case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
             return getMAhDrawn();
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
+#ifdef USE_GPS_FIX_ESTIMATION
+            if ( STATE(GPS_ESTIMATED_FIX) ){
+                return gpsSol.numSat; //99
+            } else
+#endif
             if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
                 return 0;
             } else {
                 return gpsSol.numSat;
             }
             break;
-
+            
         case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
             return STATE(GPS_FIX) ? 1 : 0;
             break;
@@ -725,15 +748,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
             return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
-            break;
+            break; 
-
+        
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
             return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
-            break;
+            break; 
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
             return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
-            break;
+            break; 
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
             return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
@@ -741,7 +764,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
             return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
-            break;
+            break; 
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
 #ifdef USE_FW_AUTOLAND
@@ -755,16 +778,16 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
             return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0;
             break;
-
+        
-        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // 
             return axisPID[YAW];
             break;
-
+        
-        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // 
             return axisPID[ROLL];
             break;
-
+        
-        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // 
             return axisPID[PITCH];
             break;
 
@@ -791,7 +814,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
             return getConfigProfile() + 1;
             break;
-
+        
         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
             return currentMixerProfileIndex + 1;
             break;
@@ -806,11 +829,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
             return isEstimatedAglTrusted();
             break;
-
+    
         case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
             return getEstimatedAglPosition();
-            break;
+            break;    
-
+        
         case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
             return rangefinderGetLatestRawAltitude();
             break;
@@ -918,7 +941,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
             //Extract RC channel raw value
             if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
                 retVal = rxGetChannelValue(operand - 1);
-            }
+            } 
             break;
 
         case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
@@ -946,7 +969,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
                 retVal = programmingPidGetOutput(operand);
             }
             break;
-
+        
         case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
             retVal = logicConditionGetWaypointOperandValue(operand);
             break;
@@ -960,7 +983,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
 
 /*
  * conditionId == -1 is always evaluated as true
- */
+ */ 
 int logicConditionGetValue(int8_t conditionId) {
     if (conditionId >= 0) {
         return logicConditionStates[conditionId].value;
@@ -1042,7 +1065,7 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
 
 uint32_t getLoiterRadius(uint32_t loiterRadius) {
 #ifdef USE_PROGRAMMING_FRAMEWORK
-    if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
+    if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && 
         !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
         return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
     } else {

src/main/programming/logic_condition.h

@@ -82,7 +82,9 @@ typedef enum {
     LOGIC_CONDITION_DELTA                       = 50,
     LOGIC_CONDITION_APPROX_EQUAL                = 51,
     LOGIC_CONDITION_LED_PIN_PWM                 = 52,
-    LOGIC_CONDITION_LAST                        = 53,
+    LOGIC_CONDITION_DISABLE_GPS_FIX             = 53,
+    LOGIC_CONDITION_RESET_MAG_CALIBRATION       = 54,
+    LOGIC_CONDITION_LAST                        = 55,
 } logicOperation_e;
 
 typedef enum logicOperandType_s {
@@ -191,6 +193,9 @@ typedef enum {
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10),
+#ifdef USE_GPS_FIX_ESTIMATION
+    LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11),
+#endif
 } logicConditionsGlobalFlags_t;
 
 typedef enum {

src/main/scheduler/scheduler.h

@@ -70,6 +70,9 @@ typedef enum {
 #ifdef USE_BARO
     TASK_BARO,
 #endif
+#ifdef USE_ADSB
+    TASK_ADSB,
+#endif
 #ifdef USE_PITOT
     TASK_PITOT,
 #endif

src/main/sensors/diagnostics.c

@@ -43,12 +43,10 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
     if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
         if (accIsHealthy()) {
             return HW_SENSOR_OK;
-        }
+        } else {
-        else {
             return HW_SENSOR_UNHEALTHY;
         }
-    }
+    } else {
-    else {
         if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
             // Selected but not detected
             return HW_SENSOR_UNAVAILABLE;
@@ -64,29 +62,25 @@ hardwareSensorStatus_e getHwCompassStatus(void)
 {
 #if defined(USE_MAG)
 #ifdef USE_SIMULATOR
-	if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) {
+    if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) {
         if (compassIsHealthy()) {
             return HW_SENSOR_OK;
-        }
+        } else {
-        else {
             return HW_SENSOR_UNHEALTHY;
         }
-	}
+    }
 #endif
     if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
         if (compassIsHealthy()) {
             return HW_SENSOR_OK;
-        }
+        } else {
-        else {
             return HW_SENSOR_UNHEALTHY;
         }
-    }
+    } else {
-    else {
         if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
             // Selected but not detected
             return HW_SENSOR_UNAVAILABLE;
-        }
+        } else {
-        else {
             // Not selected and not detected
             return HW_SENSOR_NONE;
         }
@@ -100,7 +94,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void)
 {
 #if defined(USE_BARO)
 #ifdef USE_SIMULATOR
-	if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
+    if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
         if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) {
             return HW_SENSOR_NONE;
         } else if (baroIsHealthy()) {
@@ -108,7 +102,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void)
         } else {
             return HW_SENSOR_UNHEALTHY;
         }
-	}
+    }
 #endif
     if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) {
         if (baroIsHealthy()) {
@@ -139,8 +133,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
     if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
         if (rangefinderIsHealthy()) {
             return HW_SENSOR_OK;
-        }
+        } else {
-        else {
             return HW_SENSOR_UNHEALTHY;
         }
     }
@@ -148,8 +141,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
         if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
             // Selected but not detected
             return HW_SENSOR_UNAVAILABLE;
-        }
+        } else {
-        else {
             // Not selected and not detected
             return HW_SENSOR_NONE;
         }
@@ -165,8 +157,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
     if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
         if (pitotIsHealthy()) {
             return HW_SENSOR_OK;
-        }
+        } else {
-        else {
             return HW_SENSOR_UNHEALTHY;
         }
     }
@@ -174,8 +165,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
         if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
             // Selected but not detected
             return HW_SENSOR_UNAVAILABLE;
-        }
+        } else {
-        else {
             // Not selected and not detected
             return HW_SENSOR_NONE;
         }
@@ -191,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
     if (sensors(SENSOR_GPS)) {
         if (isGPSHealthy()) {
             return HW_SENSOR_OK;
-        }
+        } else {
-        else {
             return HW_SENSOR_UNHEALTHY;
         }
     }
@@ -200,8 +189,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
         if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) {
             // Selected but not detected
             return HW_SENSOR_UNAVAILABLE;
-        }
+        } else {
-        else {
             // Not selected and not detected
             return HW_SENSOR_NONE;
         }
@@ -217,8 +205,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
     if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
         if (opflowIsHealthy()) {
             return HW_SENSOR_OK;
-        }
+        } else {
-        else {
             return HW_SENSOR_UNHEALTHY;
         }
     }
@@ -226,8 +213,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
         if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
             // Selected but not detected
             return HW_SENSOR_UNAVAILABLE;
-        }
+        } else {
-        else {
             // Not selected and not detected
             return HW_SENSOR_NONE;
         }

src/main/sensors/pitotmeter.c

@@ -238,14 +238,14 @@ STATIC_PROTOTHREAD(pitotThread)
         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
 
 #ifdef USE_SIMULATOR
-    	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
+        if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
-        	pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
+            pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
-    	}
+        }
 #endif
 #if defined(USE_PITOT_FAKE)
         if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
             pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
-    	} 
+        } 
 #endif
         ptYield();
 
@@ -275,15 +275,15 @@ STATIC_PROTOTHREAD(pitotThread)
             pitot.airSpeed = 0.0f;
         }
 
-#ifdef USE_SIMULATOR
-    	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
-            pitot.airSpeed = simulatorData.airSpeed;
-    	}
-#endif
 #if defined(USE_PITOT_FAKE)
         if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
             pitot.airSpeed = fakePitotGetAirspeed();
-    }
+        }
+#endif
+#ifdef USE_SIMULATOR
+        if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
+            pitot.airSpeed = simulatorData.airSpeed;
+        }
 #endif
     }
 

src/main/target/AOCODARCF405AIO/target.h

@@ -142,7 +142,7 @@
 #define SERIALRX_UART        SERIAL_PORT_USART2
 #define DEFAULT_RX_TYPE      RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER    SERIALRX_CRSF
-#define CURRENT_METER_SCALE  250
+#define CURRENT_METER_SCALE  500
 
 /*** Optical Flow & Lidar ***/
 #define USE_RANGEFINDER

src/main/target/FLYWOOF411/CMakeLists.txt

@@ -1,2 +1,2 @@
-target_stm32f411xe(FLYWOOF411)
+target_stm32f411xe(FLYWOOF411 SKIP_RELEASES)
-target_stm32f411xe(FLYWOOF411_V2)
+target_stm32f411xe(FLYWOOF411_V2 SKIP_RELEASES)

src/main/target/HAKRCF411D/CMakeLists.txt

@@ -1 +1 @@
-target_stm32f411xe(HAKRCF411D)
+target_stm32f411xe(HAKRCF411D SKIP_RELEASES)

src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt

@@ -1 +1 @@
-target_stm32f411xe(IFLIGHTF4_SUCCEXD)
+target_stm32f411xe(IFLIGHTF4_SUCCEXD SKIP_RELEASES)

src/main/target/KAKUTEH7WING/target.h

@@ -109,11 +109,11 @@
 #define MAG_I2C_BUS BUS_I2C1
 #define USE_MAG_ALL
 
-#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define TEMPERATURE_I2C_BUS     BUS_I2C2
-#define PITOT_I2C_BUS           BUS_I2C1
+#define PITOT_I2C_BUS           BUS_I2C2
 
 #define USE_RANGEFINDER
-#define RANGEFINDER_I2C_BUS BUS_I2C1
+#define RANGEFINDER_I2C_BUS BUS_I2C2
 
 // *************** UART *****************************
 #define USE_VCP

src/main/target/MATEKF411/CMakeLists.txt

@@ -1,4 +1,4 @@
-target_stm32f411xe(MATEKF411)
+target_stm32f411xe(MATEKF411 SKIP_RELEASES)
-target_stm32f411xe(MATEKF411_FD_SFTSRL)
+target_stm32f411xe(MATEKF411_FD_SFTSRL SKIP_RELEASES)
-target_stm32f411xe(MATEKF411_RSSI)
+target_stm32f411xe(MATEKF411_RSSI SKIP_RELEASES)
-target_stm32f411xe(MATEKF411_SFTSRL2)
+target_stm32f411xe(MATEKF411_SFTSRL2 SKIP_RELEASES)

src/main/target/MATEKF411SE/CMakeLists.txt

@@ -1,4 +1,4 @@
-target_stm32f411xe(MATEKF411SE)
+target_stm32f411xe(MATEKF411SE SKIP_RELEASES)
-target_stm32f411xe(MATEKF411SE_PINIO)
+target_stm32f411xe(MATEKF411SE_PINIO SKIP_RELEASES)
-target_stm32f411xe(MATEKF411SE_FD_SFTSRL1)
+target_stm32f411xe(MATEKF411SE_FD_SFTSRL1 SKIP_RELEASES)
-target_stm32f411xe(MATEKF411SE_SS2_CH6)
+target_stm32f411xe(MATEKF411SE_SS2_CH6 SKIP_RELEASES)

src/main/target/MATEKF411TE/CMakeLists.txt

@@ -1 +1 @@
-target_stm32f411xe(MATEKF411TE)
+target_stm32f411xe(MATEKF411TE SKIP_RELEASES)

src/main/target/NOX/CMakeLists.txt

@@ -1 +1 @@
-target_stm32f411xe(NOX)
+target_stm32f411xe(NOX SKIP_RELEASES)

src/main/target/common.h

@@ -56,6 +56,7 @@
 #define USE_GPS_PROTO_MSP
 #define USE_TELEMETRY
 #define USE_TELEMETRY_LTM
+#define USE_GPS_FIX_ESTIMATION
 
 // This is the shortest period in microseconds that the scheduler will allow
 #define SCHEDULER_DELAY_LIMIT           10
@@ -182,6 +183,14 @@
 
 #define USE_CMS_FONT_PREVIEW
 
+//ADSB RECEIVER
+#ifdef USE_GPS
+#define USE_ADSB
+#define MAX_ADSB_VEHICLES               5
+#define ADSB_LIMIT_CM                   6400000
+#endif
+
+
 //Designed to free space of F722 and F411 MCUs
 #if (MCU_FLASH_SIZE > 512)
 #define USE_VTX_FFPV

src/main/telemetry/ghst.c

@@ -147,7 +147,11 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
     sbufWriteU16(dst, GPS_directionToHome);
 
     uint8_t gpsFlags = 0;
-    if (STATE(GPS_FIX)) {
+    if (STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) {
         gpsFlags |= GPS_FLAGS_FIX;
     }
     if (STATE(GPS_FIX_HOME)) {

src/main/telemetry/hott.c

@@ -234,7 +234,12 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
     const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120);
     hottGPSMessage->climbrate3s = climbrate3s & 0xFF;
 
-    if (!STATE(GPS_FIX)) {
+#ifdef USE_GPS_FIX_ESTIMATION
+    if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)))
+#else            
+    if (!(STATE(GPS_FIX)))
+#endif
+         {
         hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
         return;
     }
@@ -476,7 +481,11 @@ static bool processBinaryModeRequest(uint8_t address)
     switch (address) {
 #ifdef USE_GPS
     case 0x8A:
-        if (sensors(SENSOR_GPS)) {
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) {
             hottPrepareGPSResponse(&hottGPSMessage);
             hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
             return true;

src/main/telemetry/ibus_shared.c

@@ -136,7 +136,11 @@ static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8
 static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
 #if defined(USE_GPS)
     uint8_t fix = 0;
-    if (sensors(SENSOR_GPS)) {
+    if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+        || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
         if (gpsSol.fixType == GPS_NO_FIX) fix = 1;
         else if (gpsSol.fixType == GPS_FIX_2D) fix = 2;
         else if (gpsSol.fixType == GPS_FIX_3D) fix = 3;
@@ -196,7 +200,11 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0
         uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()];
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) {
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) {
             status += gpsSol.numSat * 1000;
             status += fix * 100;
             if (STATE(GPS_FIX_HOME)) status += 500;
@@ -206,72 +214,128 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
         return sendIbusMeasurement2(address, status);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north)
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t
 #endif
         return sendIbusMeasurement4(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t
 #endif
         return sendIbusMeasurement4(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10));
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10));
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else
 #endif
         return sendIbusMeasurement2(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t
 #endif
         return sendIbusMeasurement4(address, 0);
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m
 #if defined(USE_GPS)
-        if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t
+        if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t
 #endif
         return sendIbusMeasurement2(address, 0);
     }

src/main/telemetry/jetiexbus.c

@@ -575,7 +575,11 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
         createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
 
         if (!allSensorsActive) {
-            if (sensors(SENSOR_GPS)) {
+            if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            ) {
                 enableGpsTelemetry(true);
                 allSensorsActive = true;
             }

src/main/telemetry/ltm.c

@@ -117,7 +117,11 @@ void ltm_gframe(sbuf_t *dst)
     uint8_t gps_fix_type = 0;
     int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0;
 
-    if (sensors(SENSOR_GPS)) {
+    if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
         if (gpsSol.fixType == GPS_NO_FIX)
             gps_fix_type = 1;
         else if (gpsSol.fixType == GPS_FIX_2D)

src/main/telemetry/mavlink.c

@@ -58,6 +58,7 @@
 #include "flight/pid.h"
 #include "flight/servos.h"
 
+#include "io/adsb.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/serial.h"
@@ -525,7 +526,11 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
 {
     uint8_t gpsFixType = 0;
 
-    if (!sensors(SENSOR_GPS))
+    if (!(sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ))
         return;
 
     if (gpsSol.fixType == GPS_NO_FIX)
@@ -640,7 +645,11 @@ void mavlinkSendHUDAndHeartbeat(void)
 
 #if defined(USE_GPS)
     // use ground speed if source available
-    if (sensors(SENSOR_GPS)) {
+    if (sensors(SENSOR_GPS)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
         mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
     }
 #endif
@@ -1054,6 +1063,50 @@ static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) {
     return true;
 }
 
+#ifdef USE_ADSB
+static bool handleIncoming_ADSB_VEHICLE(void) {
+    mavlink_adsb_vehicle_t msg;
+    mavlink_msg_adsb_vehicle_decode(&mavRecvMsg, &msg);
+
+    adsbVehicleValues_t* vehicle = getVehicleForFill();
+    if(vehicle != NULL){
+        vehicle->icao = msg.ICAO_address;
+        vehicle->lat = msg.lat;
+        vehicle->lon = msg.lon;
+        vehicle->alt = (int32_t)(msg.altitude / 10);
+        vehicle->heading = msg.heading;
+        vehicle->flags = msg.flags;
+        vehicle->altitudeType = msg.altitude_type;
+        memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign));
+        vehicle->emitterType = msg.emitter_type;
+        vehicle->tslc = msg.tslc;
+
+        adsbNewVehicle(vehicle);
+    }
+
+    //debug vehicle
+   /* if(vehicle != NULL){
+
+        char name[9] = "DUMMY    ";
+
+        vehicle->icao = 666;
+        vehicle->lat = 492383514;
+        vehicle->lon = 165148681;
+        vehicle->alt = 100000;
+        vehicle->heading = 180;
+        vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS;
+        vehicle->altitudeType = 0;
+        memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign));
+        vehicle->emitterType = 6;
+        vehicle->tslc = 0;
+
+        adsbNewVehicle(vehicle);
+    }*/
+
+    return true;
+}
+#endif
+
 static bool processMAVLinkIncomingTelemetry(void)
 {
     while (serialRxBytesWaiting(mavlinkPort) > 0) {
@@ -1076,6 +1129,10 @@ static bool processMAVLinkIncomingTelemetry(void)
                     return handleIncoming_MISSION_REQUEST();
                 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
                     return handleIncoming_RC_CHANNELS_OVERRIDE();
+#ifdef USE_ADSB
+                case MAVLINK_MSG_ID_ADSB_VEHICLE:
+                    return handleIncoming_ADSB_VEHICLE();
+#endif
                 default:
                     return false;
             }

src/main/telemetry/sim.c

@@ -348,7 +348,11 @@ static void sendSMS(void)
 
     ZERO_FARRAY(pluscode_url);
 
-    if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
+    if ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
         groundSpeed = gpsSol.groundSpeed / 100;
 
         char buf[20];

src/main/telemetry/smartport.c

@@ -414,7 +414,11 @@ static bool smartPortShouldSendGPSData(void)
     // or the craft has never been armed yet. This way if GPS stops working
     // while in flight, the user will easily notice because the sensor will stop
     // updating.
-    return feature(FEATURE_GPS) && (STATE(GPS_FIX) || !ARMING_FLAG(WAS_EVER_ARMED));
+    return feature(FEATURE_GPS) && (STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        || !ARMING_FLAG(WAS_EVER_ARMED));
 }
 
 void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)

src/main/telemetry/srxl.c

@@ -305,7 +305,11 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
     uint16_t altitudeLoBcd, groundCourseBcd, hdop;
     uint8_t hdopBcd, gpsFlags;
 
-    if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
+    if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) || gpsSol.numSat < 6) {
         return false;
     }
 
@@ -373,7 +377,11 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
     uint8_t numSatBcd, altitudeHighBcd;
     bool timeProvided = false;
 
-    if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
+    if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        )|| gpsSol.numSat < 6) {
         return false;
     }