7.1 mergeback

2025-07-16 04:45:22 +03:00 · 2024-03-02 19:33:27 +01:00 · 2024-03-02 19:33:27 +01:00 · d35a895ad3
commit d35a895ad3
parent 8f74cf3e40 4c7980c75f
80 changed files with 2128 additions and 832 deletions
--- a/.gitignore
+++ b/.gitignore
@ -36,3 +36,4 @@ make/local.mk
 launch.json
 .vscode/tasks.json
 .vscode/c_cpp_properties.json
+/cmake-build-debug/
--- a/.vscode/settings.json
+++ b/.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"
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -51,7 +51,7 @@ else()
    endif()
 endif()

-project(INAV VERSION 7.1.0)
+project(INAV VERSION 8.0.0)

 enable_language(ASM)

--- a/cmake/arm-none-eabi-checks.cmake
+++ b/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_base_url "https://developer.arm.com/-/media/Files/downloads/gnu-rm/10.3-2021.10/gcc-arm-none-eabi-10.3-2021.10")
+set(arm_none_eabi_gcc_version "13.2.1")
+# 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_linux_amd64 "x86_64-linux.tar.bz2" 2383e4eb4ea23f248d33adc70dc3227e)
-set(arm_none_eabi_linux_aarch64 "aarch64-linux.tar.bz2" 3fe3d8bb693bd0a6e4615b6569443d0d)
-set(arm_none_eabi_gcc_macos "mac.tar.bz2" 7f2a7b7b23797302a9d6182c6e482449)
-
-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()
+set(arm_none_eabi_win32 "mingw-w64-i686-arm-none-eabi.zip" 7fd677088038cdf82f33f149e2e943ee)
+set(arm_none_eabi_linux_amd64 "x86_64-arm-none-eabi.tar.xz" 791754852f8c18ea04da7139f153a5b7)
+set(arm_none_eabi_linux_aarch64 "aarch64-arm-none-eabi.tar.xz" 5a08122e6d4caf97c6ccd1d29e62599c)
+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(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}/${dist_name}/bin")
+    set(gcc_path "${TOOLS_DIR}/${base_dir_name}/bin")
    if(CMAKE_HOST_SYSTEM MATCHES ".*Windows.*")
        set(sep "\\;")
    else()
--- a/cmake/at32.cmake
+++ b/cmake/at32.cmake
@ -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)
--- a/docs/ADSB.md
+++ b/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)
+
+
--- a/docs/Boards.md
+++ b/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._
--- a/outputs.md
+++ b/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.
--- a/docs/GPS_fix_estimation.md
+++ b/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
--- a/docs/SITL/SITL.md
+++ b/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.

--- a/docs/Screenshots/programming_disable_gps_sensor_fix.png
+++ b/docs/Screenshots/programming_disable_gps_sensor_fix.png
--- a/docs/Settings.md
+++ b/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 |

 ---

--- a/docs/assets/images/KAKUTEH7WING-wiring-diagram.webp
+++ b/docs/assets/images/KAKUTEH7WING-wiring-diagram.webp
--- a/docs/boards/KAKUTEH7WING.md
+++ b/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)
--- a/docs/boards/SPEEDYBEEF405V3.md
+++ b/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.

--- a/docs/development/serial_printf_debugging.md
+++ b/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:
--- a/docs/development/wp_mission_schema/README.md
+++ b/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

--- a/docs/development/wp_mission_schema/mw-mission.xsd
+++ b/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">
--- a/lib/main/MAVLink/common/common.h
+++ b/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
--- a/src/main/CMakeLists.txt
+++ b/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
--- a/src/main/cms/cms_menu_navigation.c
+++ b/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 AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
+    OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_MC_AUTO_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),
--- a/src/main/common/maths.h
+++ b/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)
--- a/src/main/drivers/osd_symbols.h
+++ b/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
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@ -145,6 +145,7 @@ static void cliAssert(char *cmdline);
 #ifdef USE_CLI_BATCH
 static bool     commandBatchActive = 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) {
--- a/src/main/fc/fc_msp.c
+++ b/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]
@ -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);
@ -3805,36 +3844,34 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
            if (dataSize >= 14) {

                if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
-					gpsSol.fixType = sbufReadU8(src);
-					gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
-					gpsSol.flags.hasNewData = true;
-					gpsSol.numSat = sbufReadU8(src);
+                    gpsSolDRV.fixType = sbufReadU8(src);
+                    gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
+                    gpsSolDRV.numSat = sbufReadU8(src);

-					if (gpsSol.fixType != GPS_NO_FIX) {
-						gpsSol.flags.validVelNE = true;
-						gpsSol.flags.validVelD = true;
-						gpsSol.flags.validEPE = true;
-						gpsSol.flags.validTime = false;
+                    if (gpsSolDRV.fixType != GPS_NO_FIX) {
+                        gpsSolDRV.flags.validVelNE = true;
+                        gpsSolDRV.flags.validVelD = true;
+                        gpsSolDRV.flags.validEPE = true;
+                        gpsSolDRV.flags.validTime = false;

-						gpsSol.llh.lat = sbufReadU32(src);
-						gpsSol.llh.lon = sbufReadU32(src);
-						gpsSol.llh.alt = sbufReadU32(src);
-						gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
-						gpsSol.groundCourse = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.llh.lat = sbufReadU32(src);
+                        gpsSolDRV.llh.lon = sbufReadU32(src);
+                        gpsSolDRV.llh.alt = sbufReadU32(src);
+                        gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src);

-						gpsSol.velNED[X] = (int16_t)sbufReadU16(src);
-						gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
-						gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src);

-						gpsSol.eph = 100;
-						gpsSol.epv = 100;
-
-						ENABLE_STATE(GPS_FIX);
+                        gpsSolDRV.eph = 100;
+                        gpsSolDRV.epv = 100;
                    } 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();
+                    gpsProcessNewSolutionData(false);
                } 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);
                }
--- a/src/main/fc/fc_tasks.c
+++ b/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",
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@ -24,6 +24,8 @@

 #pragma once

+#include <stdbool.h>
+
 #ifdef USE_MULTI_FUNCTIONS

 extern uint8_t multiFunctionFlags;
--- a/src/main/fc/runtime_config.h
+++ b/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),
--- a/src/main/fc/settings.yaml
+++ b/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
--- a/src/main/flight/failsafe.c
+++ b/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
@ -459,6 +496,14 @@ void failsafeUpdateState(void)
                    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:
--- a/src/main/flight/failsafe.h
+++ b/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;
--- a/src/main/flight/imu.c
+++ b/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)
--- a/src/main/flight/wind_estimator.c
+++ b/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;
    }

--- a/src/main/io/adsb.c
+++ b/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
+
--- a/src/main/io/adsb.h
+++ b/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);
--- a/src/main/io/gps.c
+++ b/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,8 +221,158 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs)
    gpsState.timeoutMs = timeoutMs;
 }

-void gpsProcessNewSolutionData(void)
+#ifdef USE_GPS_FIX_ESTIMATION
+bool canEstimateGPSFix(void)
 {
+#if defined(USE_GPS) && defined(USE_BARO)
+
+    //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
+    //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix
+    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.validEPE = false;
+        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;
+    }
+
+    gpsSol.fixType = GPS_FIX_3D;
+    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);
@ -216,9 +384,14 @@ void gpsProcessNewSolutionData(void)
            gpsSol.flags.validEPE = false;
            DISABLE_STATE(GPS_FIX);
        }
+#ifdef USE_GPS_FIX_ESTIMATION
+    }
+#endif

-    // Set sensor as ready and available
+    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.epv = 9999;
-    gpsSol.numSat = 0;
-    gpsSol.hdop = 9999;
+    gpsSol->eph = 9999;
+    gpsSol->epv = 9999;
+    gpsSol->numSat = 0;
+    gpsSol->hdop = 9999;

-    gpsSol.fixType = GPS_NO_FIX;
+    gpsSol->fixType = GPS_NO_FIX;

-    gpsSol.flags.validVelNE = false;
-    gpsSol.flags.validVelD = false;
-    gpsSol.flags.validMag = false;
-    gpsSol.flags.validEPE = false;
-    gpsSol.flags.validTime = false;
+    gpsSol->flags.validVelNE = false;
+    gpsSol->flags.validVelD = false;
+    gpsSol->flags.validEPE = 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;
-        }
-        else
-        {
+            gpsTryEstimateOnTimeout();
+        } else {
            gpsSetState(GPS_RUNNING);
            sensorsSet(SENSOR_GPS);
+        }
        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
--- a/src/main/io/gps.h
+++ b/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;
--- a/src/main/io/gps_fake.c
+++ b/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;
-    gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
-    gpsSol.numSat = numSat;
+    gpsSolDRV.fixType = fixType;
+    gpsSolDRV.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
+    gpsSolDRV.numSat = numSat;
    
-    gpsSol.llh.lat = lat;
-    gpsSol.llh.lon = lon;
-    gpsSol.llh.alt = alt;
-    gpsSol.groundSpeed = groundSpeed;
-    gpsSol.groundCourse = groundCourse;
-    gpsSol.velNED[X] = velNED_X;
-    gpsSol.velNED[Y] = velNED_Y;
-    gpsSol.velNED[Z] = velNED_Z;
-    gpsSol.eph = 100;
-    gpsSol.epv = 100;
-    gpsSol.flags.validVelNE = true;
-    gpsSol.flags.validVelD = true;
-    gpsSol.flags.validEPE = true;
-    gpsSol.flags.hasNewData = true;
+    gpsSolDRV.llh.lat = lat;
+    gpsSolDRV.llh.lon = lon;
+    gpsSolDRV.llh.alt = alt;
+    gpsSolDRV.groundSpeed = groundSpeed;
+    gpsSolDRV.groundCourse = groundCourse;
+    gpsSolDRV.velNED[X] = velNED_X;
+    gpsSolDRV.velNED[Y] = velNED_Y;
+    gpsSolDRV.velNED[Z] = velNED_Z;
+    gpsSolDRV.eph = 100;
+    gpsSolDRV.epv = 100;
+    gpsSolDRV.flags.validVelNE = true;
+    gpsSolDRV.flags.validVelD = true;
+    gpsSolDRV.flags.validEPE = true;
    
    if (time) {
        struct tm* gTime = gmtime(&time);

-        gpsSol.time.year   = (uint16_t)(gTime->tm_year + 1900);
-        gpsSol.time.month  = (uint16_t)(gTime->tm_mon + 1);
-        gpsSol.time.day    = (uint8_t)gTime->tm_mday;
-        gpsSol.time.hours  = (uint8_t)gTime->tm_hour;
-        gpsSol.time.minutes = (uint8_t)gTime->tm_min;
-        gpsSol.time.seconds = (uint8_t)gTime->tm_sec;
-        gpsSol.time.millis  = 0;
-        gpsSol.flags.validTime = gpsSol.fixType >= 3;
+        gpsSolDRV.time.year   = (uint16_t)(gTime->tm_year + 1900);
+        gpsSolDRV.time.month  = (uint16_t)(gTime->tm_mon + 1);
+        gpsSolDRV.time.day    = (uint8_t)gTime->tm_mday;
+        gpsSolDRV.time.hours  = (uint8_t)gTime->tm_hour;
+        gpsSolDRV.time.minutes = (uint8_t)gTime->tm_min;
+        gpsSolDRV.time.seconds = (uint8_t)gTime->tm_sec;
+        gpsSolDRV.time.millis  = 0;
+        gpsSolDRV.flags.validTime = gpsSol.fixType >= 3;
    }
+    
+    gpsProcessNewDriverData();
 }
   
 #endif
--- a/src/main/io/gps_msp.c
+++ b/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);
-    gpsSol.numSat    = pkt->satellitesInView;
-    gpsSol.llh.lon   = pkt->longitude;
-    gpsSol.llh.lat   = pkt->latitude;
-    gpsSol.llh.alt   = pkt->mslAltitude;
-    gpsSol.velNED[X] = pkt->nedVelNorth;
-    gpsSol.velNED[Y] = pkt->nedVelEast;
-    gpsSol.velNED[Z] = pkt->nedVelDown;
-    gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
-    gpsSol.groundCourse = pkt->groundCourse / 10;   // in deg * 10
-    gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
-    gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
-    gpsSol.hdop = gpsConstrainHDOP(pkt->hdop);
-    gpsSol.flags.validVelNE = true;
-    gpsSol.flags.validVelD = true;
-    gpsSol.flags.validEPE = true;
+    gpsSolDRV.fixType   = gpsMapFixType(pkt->fixType);
+    gpsSolDRV.numSat    = pkt->satellitesInView;
+    gpsSolDRV.llh.lon   = pkt->longitude;
+    gpsSolDRV.llh.lat   = pkt->latitude;
+    gpsSolDRV.llh.alt   = pkt->mslAltitude;
+    gpsSolDRV.velNED[X] = pkt->nedVelNorth;
+    gpsSolDRV.velNED[Y] = pkt->nedVelEast;
+    gpsSolDRV.velNED[Z] = pkt->nedVelDown;
+    gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
+    gpsSolDRV.groundCourse = pkt->groundCourse / 10;   // in deg * 10
+    gpsSolDRV.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
+    gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
+    gpsSolDRV.hdop = gpsConstrainHDOP(pkt->hdop);
+    gpsSolDRV.flags.validVelNE = true;
+    gpsSolDRV.flags.validVelD = true;
+    gpsSolDRV.flags.validEPE = true;

-    gpsSol.time.year   = pkt->year;
-    gpsSol.time.month  = pkt->month;
-    gpsSol.time.day    = pkt->day;
-    gpsSol.time.hours  = pkt->hour;
-    gpsSol.time.minutes = pkt->min;
-    gpsSol.time.seconds = pkt->sec;
-    gpsSol.time.millis  = 0;
+    gpsSolDRV.time.year   = pkt->year;
+    gpsSolDRV.time.month  = pkt->month;
+    gpsSolDRV.time.day    = pkt->day;
+    gpsSolDRV.time.hours  = pkt->hour;
+    gpsSolDRV.time.minutes = pkt->min;
+    gpsSolDRV.time.seconds = pkt->sec;
+    gpsSolDRV.time.millis  = 0;

-    gpsSol.flags.validTime = (pkt->fixType >= 3);
+    gpsSolDRV.flags.validTime = (pkt->fixType >= 3);

+    gpsProcessNewDriverData();
    newDataReady = true;
 }
 #endif
--- a/src/main/io/gps_private.h
+++ b/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);
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@ -431,7 +431,7 @@ static void configureGNSS(void)
        blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[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();
 }

@ -539,84 +539,84 @@ static bool gpsParseFrameUBLOX(void)
 {
    switch (_msg_id) {
    case MSG_POSLLH:
-        gpsSol.llh.lon = _buffer.posllh.longitude;
-        gpsSol.llh.lat = _buffer.posllh.latitude;
-        gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
-        gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
-        gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
-        gpsSol.flags.validEPE = true;
+        gpsSolDRV.llh.lon = _buffer.posllh.longitude;
+        gpsSolDRV.llh.lat = _buffer.posllh.latitude;
+        gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
+        gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
+        gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
+        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;
-        gpsSol.numSat = _buffer.solution.satellites;
-        gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
+            gpsSolDRV.fixType = GPS_NO_FIX;
+        gpsSolDRV.numSat = _buffer.solution.satellites;
+        gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
        break;
    case MSG_VELNED:
-        gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
-        gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
-        gpsSol.velNED[X] = _buffer.velned.ned_north;
-        gpsSol.velNED[Y] = _buffer.velned.ned_east;
-        gpsSol.velNED[Z] = _buffer.velned.ned_down;
-        gpsSol.flags.validVelNE = true;
-        gpsSol.flags.validVelD = true;
+        gpsSolDRV.groundSpeed = _buffer.velned.speed_2d;    // cm/s
+        gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
+        gpsSolDRV.velNED[X] = _buffer.velned.ned_north;
+        gpsSolDRV.velNED[Y] = _buffer.velned.ned_east;
+        gpsSolDRV.velNED[Z] = _buffer.velned.ned_down;
+        gpsSolDRV.flags.validVelNE = 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;
-            gpsSol.time.month = _buffer.timeutc.month;
-            gpsSol.time.day = _buffer.timeutc.day;
-            gpsSol.time.hours = _buffer.timeutc.hour;
-            gpsSol.time.minutes = _buffer.timeutc.min;
-            gpsSol.time.seconds = _buffer.timeutc.sec;
-            gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000);
+            gpsSolDRV.time.year = _buffer.timeutc.year;
+            gpsSolDRV.time.month = _buffer.timeutc.month;
+            gpsSolDRV.time.day = _buffer.timeutc.day;
+            gpsSolDRV.time.hours = _buffer.timeutc.hour;
+            gpsSolDRV.time.minutes = _buffer.timeutc.min;
+            gpsSolDRV.time.seconds = _buffer.timeutc.sec;
+            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;
-        gpsSol.llh.lon = _buffer.pvt.longitude;
-        gpsSol.llh.lat = _buffer.pvt.latitude;
-        gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10;  //alt in cm
-        gpsSol.velNED[X]=_buffer.pvt.ned_north / 10;  // to cm/s
-        gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10;   // to cm/s
-        gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10;   // to cm/s
-        gpsSol.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
-        gpsSol.numSat = _buffer.pvt.satellites;
-        gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
-        gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
-        gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
-        gpsSol.flags.validVelNE = true;
-        gpsSol.flags.validVelD = true;
-        gpsSol.flags.validEPE = true;
+        gpsSolDRV.fixType = next_fix_type;
+        gpsSolDRV.llh.lon = _buffer.pvt.longitude;
+        gpsSolDRV.llh.lat = _buffer.pvt.latitude;
+        gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10;  //alt in cm
+        gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10;  // to cm/s
+        gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10;   // to cm/s
+        gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10;   // to cm/s
+        gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10;    // to cm/s
+        gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
+        gpsSolDRV.numSat = _buffer.pvt.satellites;
+        gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
+        gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
+        gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
+        gpsSolDRV.flags.validVelNE = true;
+        gpsSolDRV.flags.validVelD = true;
+        gpsSolDRV.flags.validEPE = true;

        if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
-            gpsSol.time.year = _buffer.pvt.year;
-            gpsSol.time.month = _buffer.pvt.month;
-            gpsSol.time.day = _buffer.pvt.day;
-            gpsSol.time.hours = _buffer.pvt.hour;
-            gpsSol.time.minutes = _buffer.pvt.min;
-            gpsSol.time.seconds = _buffer.pvt.sec;
-            gpsSol.time.millis = _buffer.pvt.nano / (1000*1000);
+            gpsSolDRV.time.year = _buffer.pvt.year;
+            gpsSolDRV.time.month = _buffer.pvt.month;
+            gpsSolDRV.time.day = _buffer.pvt.day;
+            gpsSolDRV.time.hours = _buffer.pvt.hour;
+            gpsSolDRV.time.minutes = _buffer.pvt.min;
+            gpsSolDRV.time.seconds = _buffer.pvt.sec;
+            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;
@ -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) {
--- a/src/main/io/osd.c
+++ b/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"
@ -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;
@ -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;
@ -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));
@ -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) {
@ -4081,7 +4193,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
    }

    return logoRow;
-}
+    }

 uint8_t drawStats(uint8_t row) {
 #ifdef USE_STATS
@ -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)
 {
--- a/src/main/io/osd.h
+++ b/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);
--- a/src/main/io/osd_dji_hd.c
+++ b/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));
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/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

--- a/src/main/navigation/navigation.c
+++ b/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 {
-            // Switch to RTH trackback
-            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
+        } else {
+            if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
+                rthTrackBackUpdate(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) {
-        const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
-#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);
-        }
+    if (rthTrackBackSetNewPosition()) {
+        return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
    }

    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)
-    if (isGPSGlitchDetected())                              navFlags |= (1 << 4);
-#endif
+    // naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag
    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;
-        posControl.flags.rthTrackbackActive = false;
-        posControl.rthTBWrapAroundCounter = -1;
+        resetRthTrackBack();

        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;
--- a/src/main/navigation/navigation.h
+++ b/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);
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/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);
--- a/src/main/navigation/navigation_multicopter.c
+++ b/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.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
+                posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate;
+                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;
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/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 (!STATE(GPS_FIX)) {
+    if (sensors(SENSOR_GPS) 
+#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;
 }
--- a/src/main/navigation/navigation_pos_estimator_private.h
+++ b/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;
--- a/src/main/navigation/navigation_private.h
+++ b/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;
--- a/src/main/navigation/rth_trackback.c
+++ b/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;   
+}
--- a/src/main/navigation/rth_trackback.h
+++ b/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);
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@ -287,6 +287,13 @@ 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_SMARTAUDIO) || defined(USE_VTX_TRAMP))
@ -486,6 +493,17 @@ 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; 
@ -667,6 +685,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
            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 {
--- a/src/main/programming/logic_condition.h
+++ b/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 {
--- a/src/main/scheduler/scheduler.h
+++ b/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
--- a/src/main/sensors/diagnostics.c
+++ b/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;
@ -67,8 +65,7 @@ hardwareSensorStatus_e getHwCompassStatus(void)
    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;
        }
    }
@ -76,17 +73,14 @@ hardwareSensorStatus_e getHwCompassStatus(void)
    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;
        }
@ -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;
        }
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@ -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
    }

--- a/src/main/target/AOCODARCF405AIO/target.h
+++ b/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
--- a/src/main/target/FLYWOOF411/CMakeLists.txt
+++ b/src/main/target/FLYWOOF411/CMakeLists.txt
@ -1,2 +1,2 @@
-target_stm32f411xe(FLYWOOF411)
-target_stm32f411xe(FLYWOOF411_V2)
+target_stm32f411xe(FLYWOOF411 SKIP_RELEASES)
+target_stm32f411xe(FLYWOOF411_V2 SKIP_RELEASES)
--- a/src/main/target/HAKRCF411D/CMakeLists.txt
+++ b/src/main/target/HAKRCF411D/CMakeLists.txt
@ -1 +1 @@
-target_stm32f411xe(HAKRCF411D)
+target_stm32f411xe(HAKRCF411D SKIP_RELEASES)
--- a/src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt
+++ b/src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt
@ -1 +1 @@
-target_stm32f411xe(IFLIGHTF4_SUCCEXD)
+target_stm32f411xe(IFLIGHTF4_SUCCEXD SKIP_RELEASES)
--- a/src/main/target/KAKUTEH7WING/target.h
+++ b/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 PITOT_I2C_BUS           BUS_I2C1
+#define TEMPERATURE_I2C_BUS     BUS_I2C2
+#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
--- a/src/main/target/MATEKF411/CMakeLists.txt
+++ b/src/main/target/MATEKF411/CMakeLists.txt
@ -1,4 +1,4 @@
-target_stm32f411xe(MATEKF411)
-target_stm32f411xe(MATEKF411_FD_SFTSRL)
-target_stm32f411xe(MATEKF411_RSSI)
-target_stm32f411xe(MATEKF411_SFTSRL2)
+target_stm32f411xe(MATEKF411 SKIP_RELEASES)
+target_stm32f411xe(MATEKF411_FD_SFTSRL SKIP_RELEASES)
+target_stm32f411xe(MATEKF411_RSSI SKIP_RELEASES)
+target_stm32f411xe(MATEKF411_SFTSRL2 SKIP_RELEASES)
--- a/src/main/target/MATEKF411SE/CMakeLists.txt
+++ b/src/main/target/MATEKF411SE/CMakeLists.txt
@ -1,4 +1,4 @@
-target_stm32f411xe(MATEKF411SE)
-target_stm32f411xe(MATEKF411SE_PINIO)
-target_stm32f411xe(MATEKF411SE_FD_SFTSRL1)
-target_stm32f411xe(MATEKF411SE_SS2_CH6)
+target_stm32f411xe(MATEKF411SE SKIP_RELEASES)
+target_stm32f411xe(MATEKF411SE_PINIO SKIP_RELEASES)
+target_stm32f411xe(MATEKF411SE_FD_SFTSRL1 SKIP_RELEASES)
+target_stm32f411xe(MATEKF411SE_SS2_CH6 SKIP_RELEASES)
--- a/src/main/target/MATEKF411TE/CMakeLists.txt
+++ b/src/main/target/MATEKF411TE/CMakeLists.txt
@ -1 +1 @@
-target_stm32f411xe(MATEKF411TE)
+target_stm32f411xe(MATEKF411TE SKIP_RELEASES)
--- a/src/main/target/NOX/CMakeLists.txt
+++ b/src/main/target/NOX/CMakeLists.txt
@ -1 +1 @@
-target_stm32f411xe(NOX)
+target_stm32f411xe(NOX SKIP_RELEASES)
--- a/src/main/target/common.h
+++ b/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
--- a/src/main/telemetry/ghst.c
+++ b/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)) {
--- a/src/main/telemetry/hott.c
+++ b/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;
--- a/src/main/telemetry/ibus_shared.c
+++ b/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);
    }
--- a/src/main/telemetry/jetiexbus.c
+++ b/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;
            }
--- a/src/main/telemetry/ltm.c
+++ b/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)
--- a/src/main/telemetry/mavlink.c
+++ b/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;
            }
--- a/src/main/telemetry/sim.c
+++ b/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];
--- a/src/main/telemetry/smartport.c
+++ b/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)
--- a/src/main/telemetry/srxl.c
+++ b/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;
    }