Merge remote-tracking branch 'upstream/master' into abo_fw_alt_vel_control

.gitignore

@@ -22,6 +22,7 @@ cov-int*
 /downloads/
 /debug/
 /release/
+/*_SITL/
 
 # script-generated files
 docs/Manual.pdf
@@ -35,3 +36,4 @@ make/local.mk
 launch.json
 .vscode/tasks.json
 .vscode/c_cpp_properties.json
+/cmake-build-debug/

.vscode/settings.json

@@ -2,11 +2,17 @@
     "files.associations": {
         "chrono": "c",
         "cmath": "c",
-        "ranges": "c"
+        "ranges": "c",
+        "navigation.h": "c",
+        "rth_trackback.h": "c"
+        "platform.h": "c",
+        "timer.h": "c",
+        "bus.h": "c"
     },
     "editor.tabSize": 4,
     "editor.insertSpaces": true,
     "editor.detectIndentation": false,
     "editor.expandTabs": true,
     "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4,  BreakBeforeBraces: Mozilla }"
+
 }

CMakeLists.txt

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

Dockerfile

@@ -4,15 +4,18 @@ ARG USER_ID
 ARG GROUP_ID
 ENV DEBIAN_FRONTEND noninteractive
 
-RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build gdb
+RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build
+
+RUN if [ "$GDB" = "yes" ]; then apt-get install -y gdb; fi
 
 RUN pip install pyyaml
 
 # if either of these are already set the same as the user's machine, leave them be and ignore the error
-RUN if [ -n "$USER_ID" ]; then RUN addgroup --gid $GROUP_ID inav; exit 0; fi
+RUN addgroup --gid $GROUP_ID inav; exit 0;
-RUN if [ -n "$USER_ID" ]; then  RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; fi
+RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0;
+
+USER inav
 
-RUN if [ -n "$USER_ID" ]; then USER inav; fi
 RUN git config --global --add safe.directory /src
 
 VOLUME /src

cmake/arm-none-eabi-checks.cmake

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

cmake/at32.cmake

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

docs/ADSB.md

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

docs/Boards.md

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

docs/ESC and servo outputs.md

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

docs/Fixed Wing Landing.md

@@ -0,0 +1,100 @@
+# Fixed Wing Landing
+
+## Introducion
+
+INAV supports advanced automatic landings for fixed wing aircraft from version 7.1.
+The procedure is based on landings for man-carrying aircraft, so that safe landings at a specific location are possible.
+Supported are landings at safehome after "Return to Home" or at a defined LAND waypoint for missions. 
+
+## General procedure:
+
+1. After reaching Safehome/LAND Waypoint the altitude is corrected to "Approach Altitude".
+2. The aircraft circles for at least 30 seconds to determine the wind direction and strength.
+3. The landing direction and the approach waypoints are calculated on the basis of the measured wind parameters. If no headwind landing is possible or the wind strength is greater than "Max. tailwind" (see Global Parameters), return to point 2.
+4. The landing is initiated. The aircraft flies the downwind course, "Approach Altitude" is held.
+5. Base Leg: the altitude is reduced from 2/3 of "Approach Altitude".
+6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is reduced to "Land Altitude".
+7. Glide: When "Glide Altitude" is reached, the motor is switched off and the pitch angle of "Glide Pitch" is held.
+7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held
+8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting `nav_disarm_on_landing`.
+
+To activate the automatic landing, the parameter `nav_rth_allow_landing` must be set to `ALWAYS` or `FAILSAFE`. 
+
+> [!WARNING]
+> If landing is activated and no parameters are set for the landing site (Safehome and/or landing waypoint), the old landing procedure (circling until close to the ground, then hovering out) is performed. 
+> This is probably not what you want. 
+ 
+The following graphics illustrate the process:
+
+![Approach Drawing Up](/docs/assets/images/Approach-Drawing-Up.png  "Approach Drawing Up")
+
+![Approach Drawing Side](/docs/assets/images/Approach-Drawing-Side.png  "Approach Drawing Side")
+
+## Landing site parameters
+
+### The following parameters are set for each landing site (Safefome/LAND waypoint):
+
+All settings can also be conveniently made in the Configurator via Missionplanner.
+
+CLI command `fwapproach`:
+`fwapproach <index> <Approach altitude> <Land altitude> <Approach direction> <approach heading 1> <approach heading 2> <sea level>`
+
+`fwapproach` has 17 slots in which landing parameters can be stored. In slot 0-7 the landing parameters for Safehome are stored, in 8 - 16 the parameters for waypoint missions. Only one landing point per mission can be saved. 
+
+* index: 0 - 17, 0 - 7 Safehome, 8 - 16 Mission
+* Approach direction: 0 - Left, 1 - Right. Always seen from the primary landing direction (positive value), i.e. whether the aircraft flies left or right turns on approach.
+* Approach Altitude: Initial altitude of the approach, the altitude at which the wind direction is determined and the downwind approach, in cm
+* Land Altitude: Altitude of the landing site, in cm
+* Approach heading 1 and 2: Two landing directions can be set, values: 0 - +/-360. 0 = landing direction is deactivated. 
+A positive value means that you can approach in both directions, a negative value means that this direction is exclusive.
+Example: 90 degrees: It is possible to land in 90 degrees as well as in 270 degrees. -90 means that you can only land in a 90 degree direction.
+This means that practically 4 landing directions can be saved.
+* Sea Level: 0 - Deactivated, 1 - Activated. If activated, approach and land altitude refer to normal zero (sea level), otherwise relative altitude to the altitude during first GPS fix.
+
+> [!CAUTION]
+> The Configuator automatically determines the ground altitude based on databases on the Internet, which may be inaccurate. Please always compare with the measured GPS altitude at the landing site to avoid crashes.
+
+### Global paramters
+
+All settings are available via “Advanced Tuning” in the Configurator.
+
+* `nav_fw_land_approach_length`: Length of the final approach, measured from the land site (Safehome/Waypoint) to the last turning point.
+In cm. Max: 100000, Min: 100, Default: 35000
+
+* `nav_fw_land_final_approach_pitch2throttle_mod`: Modifier for pitch to throttle ratio at final approach. This parameter can be used to reduce speed during the final approach. 
+Example: If the parameter is set to 200% and Pitch To Throttle Ratio is set to 15, Pitch To Throttle Ratio is set to 30 on the final approach. This causes a reduction in engine power on approach when the nose is pointing downwards.
+In Percent. Min: 100, Max: 400, Default: 100
+
+* `nav_fw_land_glide_alt`: Initial altitude of the glide phase. The altitude refers to "Landing Altitude", see above under "Landing site parameters"
+In cm. Min: 100, Max: 5000, Default: 200
+
+* `nav_fw_land_flare_alt`: Initial altitude of the flare phase. The altitude refers to "Landing Altitude", see above under "Landing site parameters"
+In cm. Min: 0, Max: 5000, Default: 200
+
+* `nav_fw_land_glide_pitch`: Pitch value for glide phase. 
+In degrees. Min: 0, Max: 45, Default: 0
+
+* `nav_fw_land_flare_pitch`: Pitch value for flare phase. 
+  In degrees. Min: 0, Max: 45, Default: 8
+
+* `nav_fw_land_max_tailwind`: Max. tailwind if no landing direction with downwind is available. Wind strengths below this value are ignored (error tolerance in the wind measurement). Landing then takes place in the main direction. If, for example, 90 degrees is configured, landing takes place in this direction, NOT in 270 degrees (see above).
+In cm/s. Min: 0; Max: 3000, Default: 140
+
+## Waypoint missions
+
+Only one landing waypoint per mission can be active and saved and the landing waypoint must be the last waypoint of the mission. 
+If the altitude of the waypoint and the "Approach Altitude" are different, the altitude of the waypoint is approached first and then the altitude is corrected to "Approach Altitude".
+
+## Logic Conditions
+
+The current landing state can be retrieved via ID 41 in "Flight" (FW Land State). This allows additional actions to be executed according to the landing phases, e.g. deplyoment of the landing flaps.
+
+| Returned value | State |
+| --- | --- |
+| 0 | Idle/Inactive |
+| 1 | Loiter |
+| 2 | Downwind |
+| 3 | Base Leg |
+| 4 | Final Approach |
+| 5 | Glide |
+| 6 | Flare |

docs/GPS_fix_estimation.md

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

docs/LedStrip.md

@@ -7,7 +7,7 @@ require that all the LEDs in the strip show the same color.
 Addressable LED strips can be used to show information from the flight controller system, the current implementation
 supports the following:
 
-* Up to 32 LEDs.
+* Up to 128 LEDs. _If using more than 20 LEDs, you should look to use a separate power supply._
 * Indicators showing pitch/roll stick positions.
 * Heading/Orientation lights.
 * Flight mode specific color schemes.
@@ -17,12 +17,12 @@ supports the following:
 * RSSI level.
 * Battery level.
 
-Support for more than 32 LEDs is possible, it just requires additional development.
+Support for more than 128 LEDs is possible, it just requires additional development.
 
 ## Supported hardware
 
-Only strips of 32 WS2811/WS2812 LEDs are supported currently.  If the strip is longer than 32 LEDs it does not matter,
+Only strips of 128 WS2811/WS2812 LEDs are supported currently.  If the strip is longer than 128 LEDs it does not matter,
-but only the first 32 are used.
+but only the first 128 are used.
 
 WS2812 LEDs require an 800khz signal and precise timings and thus requires the use of a dedicated hardware timer.
 
@@ -42,11 +42,11 @@ It could be possible to be able to specify the timings required via CLI if users
 
 WS2812 LED strips generally require a single data line, 5V and GND.
 
-WS2812 LEDs on full brightness can consume quite a bit of current.  It is recommended to verify the current draw and ensure your
+WS2812 LEDs on full brightness can consume quite a bit of current.  **It is recommended to verify the current draw of you LEDs and ensure your supply can cope with the load. Remember, your flight controller will likely be using the same BEC to operate.** Check the specs of the LED chips. Some are more power hungry than others. Remember that if using the flight controller's 5v supply. This is also powering other components on your flight controller. Make sure there is enough overhead so that they don't brownout.
-supply can cope with the load.  On a multirotor that uses multiple BEC ESC's you can try use a different BEC to the one the FC
-uses.  e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip.   It's also possible to power one half of the strip from one BEC and the other half
-from another BEC.  Just ensure that the GROUND is the same for all BEC outputs and LEDs.
 
+On a multirotor that uses multiple BEC ESC's you can try use a different BEC to the one the FC uses. e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip. It's also possible to power one half of the strip from one BEC and the other half from another BEC. Just ensure that the GROUND is the same for all BEC outputs and LEDs.
+
+If using a large number of LEDs. It would be more efficient to use 12v LEDs and power them with a separate regulated supply. Especially if using long strips. You would use the data line (LED pad) from the flight controller. Make sure there is continuity between the ground on the LEDS and the ground on the flight controller.
 
 | Target                | Pin  | LED Strip | Signal |
 | --------------------- | ---- | --------- | -------|
@@ -605,4 +605,4 @@ This also means that you can make sure that each R,G and B LED in each LED modul
 
 After a short delay the LEDs will show the unarmed color sequence and or low-battery warning sequence.
 
-Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above.
+Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above.

docs/MixerProfile.md

@@ -104,6 +104,9 @@ TailSitter is supported by add a 90deg offset to the board alignment. Set the bo
   - Rate Settings
   - ·······
 
+### TailSitter support
+TailSitter is supported by add a 90deg offset to the board alignment. Set the board aliment normally in the mixer_profile for FW mode(`set platform_type = AIRPLANE`), The motor trust axis should be same direction as the airplane nose. Then, in the mixer_profile for takeoff and landing `set platform_type = TAILSITTER`. The `TAILSITTER` platform type is same as `MULTIROTOR` platform type, expect for a 90 deg board alignment offset. In `TAILSITTER` mixer_profile, when motor trust/airplane nose is pointing to the sky, 'airplane bottom'/'multi rotor front' should facing forward in model preview. Set the motor/servo mixer according to multirotor orientation, Model should roll around geography's longitudinal axis, the roll axis of `TAILSITTER` will be yaw axis of `AIRPLANE`. In addition, When `MIXER TRANSITION` input is activated, a 45deg offset will be add to the target angle for angle mode.
+
 ## Happy flying
 
 Remember that this is currently an emerging capability:

docs/Programming Framework.md

@@ -156,6 +156,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 38            | ACTIVE_MIXER_PROFILE          | Which mixers are currently active (for vtol etc) |
 | 39            | MIXER_TRANSITION_ACTIVE       | Currently switching between mixers (quad to plane etc) |
 | 40            | ATTITUDE_YAW                  | current heading (yaw) in `degrees` |
+| 41            | FW Land Sate                  | integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
 
 #### FLIGHT_MODE
 

docs/SITL/RealFlight.md

@@ -3,8 +3,15 @@
 Supported are RealFlight 9.5S and RealFlight Evolution, NOT RealFlight-X.
 
 RealFlight is very well suited to simulate the model flight specific aspects. Autolaunch and the mixers can be used.
-However, since the sceneries do not correspond to a real environment, the GPS data must be "faked". The position is always shown somewhere in southern Nevada ;).
+
-GPS data and flight modes work fine though, only for missions with waypoints it is of course not ideal. 
+The RealFlight 3D sceneries are based on real topographic data of the Sierra Nevada in Southern Spain.
+INAV uses as reference the scenery "RealFlight Ranch" which is located at the coordinates Lat: 37.118949°, Lon: -2.772960.
+Use these scenery to use the mission planner and other GPS features.
+
+> [!CAUTION]:
+> The immediate surroundings of the airfield have been levelled in the scenery.  If, for example, Autoland is to be tested here, do not use "Sea level ref" and the automatically determined heights of the Configurator. 
+> Either use relarive elevations or correct the elevation manually.
+> The altitude of the airfield is exactly 1300 metres. 
 
 ## Joystick 
 In the settings, calibrate the joystick, set it up and assign the axes in the same order as in INAV.

docs/SITL/SITL.md

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

docs/Settings.md

@@ -242,6 +242,16 @@ Inertial Measurement Unit KP Gain for compass measurements
 
 ---
 
+### ahrs_gps_yaw_weight
+
+Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 500 |
+
+---
+
 ### ahrs_gps_yaw_windcomp
 
 Wind compensation in heading estimation from gps groundcourse(fixed wing only)
@@ -1002,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].
@@ -1474,11 +1494,11 @@ Enable automatic configuration of UBlox GPS receivers.
 
 ### gps_dyn_model
 
-GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying.
+GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| AIR_1G |  |  |
+| AIR_2G |  |  |
 
 ---
 
@@ -1762,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.
@@ -1912,19 +1942,9 @@ Decay coefficient for estimated velocity when GPS reference for position is lost
 
 ---
 
-### inav_w_xyz_acc_p
-
-_// TODO_
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 1.0 | 0 | 1 |
-
----
-
 ### inav_w_z_baro_p
 
-Weight of barometer measurements in estimated altitude and climb rate
+Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -1934,7 +1954,7 @@ Weight of barometer measurements in estimated altitude and climb rate
 
 ### inav_w_z_gps_p
 
-Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes
+Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -2444,7 +2464,7 @@ This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK
 
 ### mc_cd_lpf_hz
 
-Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky
+Cutoff frequency for Control Derivative. This controls the cutoff for the LPF that is applied to the CD (Feed Forward) signal to the PID controller. Lower value will produce a smoother CD gain to the controller, but it will be more delayed. Higher values will produce CD gain that may have more noise in the signal depending on your RC link but wil be less delayed.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -2454,7 +2474,7 @@ Cutoff frequency for Control Derivative. Lower value smoother reaction on fast s
 
 ### mc_cd_pitch
 
-Multicopter Control Derivative gain for PITCH
+Multicopter Control Derivative gain for PITCH (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -2464,7 +2484,7 @@ Multicopter Control Derivative gain for PITCH
 
 ### mc_cd_roll
 
-Multicopter Control Derivative gain for ROLL
+Multicopter Control Derivative gain for ROLL (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -2474,7 +2494,7 @@ Multicopter Control Derivative gain for ROLL
 
 ### mc_cd_yaw
 
-Multicopter Control Derivative gain for YAW
+Multicopter Control Derivative gain for YAW (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -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 |
 
 ---
 
@@ -2922,6 +2932,16 @@ P gain of Heading Hold controller (Fixedwing)
 
 ---
 
+### nav_fw_land_approach_length
+
+Length of the final approach
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 35000 | 100 | 100000 |
+
+---
+
 ### nav_fw_land_dive_angle
 
 Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees
@@ -2932,13 +2952,63 @@ Dive angle that airplane will use during final landing phase. During dive phase,
 
 ---
 
-### nav_fw_launch_abort_deadband
+### nav_fw_land_final_approach_pitch2throttle_mod
 
-Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch.
+Modifier for pitch to throttle ratio at final approach. In Percent.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 100 | 2 | 250 |
+| 100 | 100 | 400 |
+
+---
+
+### nav_fw_land_flare_alt
+
+Initial altitude of the flare phase
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 150 | 0 | 10000 |
+
+---
+
+### nav_fw_land_flare_pitch
+
+Pitch value for flare phase. In degrees
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 8 | -15 | 45 |
+
+---
+
+### nav_fw_land_glide_alt
+
+Initial altitude of the glide phase
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 200 | 100 | 5000 |
+
+---
+
+### nav_fw_land_glide_pitch
+
+Pitch value for glide phase. In degrees.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -15 | 45 |
+
+---
+
+### nav_fw_land_max_tailwind
+
+Max. tailwind (in cm/s) if no landing direction with downwind is available
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 140 | 0 | 3000 |
 
 ---
 
@@ -3002,6 +3072,16 @@ Launch idle throttle - throttle to be set before launch sequence is initiated. I
 
 ---
 
+### nav_fw_launch_land_abort_deadband
+
+Launch and landing abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch or landing.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 2 | 250 |
+
+---
+
 ### nav_fw_launch_manual_throttle
 
 Allows launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised).
@@ -3102,6 +3182,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
@@ -3372,16 +3462,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]
@@ -3432,6 +3512,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
@@ -3542,6 +3632,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
@@ -3702,6 +3802,16 @@ When ON, NAV engine will slow down when switching to the next waypoint. This pri
 
 ---
 
+### nav_min_ground_speed
+
+Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 7 | 6 | 50 |
+
+---
+
 ### nav_min_rth_distance
 
 Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase.
@@ -3832,6 +3942,16 @@ If set to ON, aircraft will execute initial climb regardless of position sensor
 
 ---
 
+### nav_rth_fs_landing_delay
+
+If landing is active on Failsafe and this is above 0. The aircraft will hover or loiter for X seconds before performing the landing. If the battery enters the warning or critical levels, the land will proceed. Default = 0 [seconds]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 1800 |
+
+---
+
 ### nav_rth_home_altitude
 
 Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm]
@@ -3992,6 +4112,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)
@@ -5068,7 +5218,7 @@ Selection of pitot hardware.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| VIRTUAL |  |  |
+| NONE |  |  |
 
 ---
 
@@ -5772,6 +5922,16 @@ Delay before disarming when requested by switch (ms) [0-1000]
 
 ---
 
+### tailsitter_orientation_offset
+
+Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### telemetry_halfduplex
 
 S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details
@@ -5838,7 +5998,7 @@ The percentage of the throttle range (`max_throttle` - `min_command`) above `min
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 15 | 0 | 30 |
+| 8 | 0 | 30 |
 
 ---
 
@@ -6004,11 +6164,11 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units,
 
 ### vtx_band
 
-Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race.
+Configure the VTX band. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND |
+| 1 | VTX_SETTINGS_MIN_BAND | VTX_SETTINGS_MAX_BAND |
 
 ---
 

docs/boards/KAKUTEH7WING.md

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

docs/boards/MAMBAH743_2022B.md

@@ -0,0 +1,5 @@
+# VTX Power SWITCH
+
+Contrary to what the documentation suggests, VTX power is actually on USER2.
+
+

docs/boards/SPEEDYBEEF405V3.md

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

docs/development/Building in Docker.md

@@ -33,23 +33,23 @@ If you are getting error "standard_init_linux.go:219: exec user process caused:
 
 You'll have to manually execute the same steps that the build script does:
 
-1. `docker build -t inav-build .`
+1. `docker build --build-arg USER_ID=1000 --build-arg GROUP_ID=1000 -t inav-build .`
    + This step is only needed the first time.
-2. `docker run --rm -it -u root -v <PATH_TO_REPO>:/src inav-build <TARGET>`
+   + If GDB should be installed in the image, add argument '--build-arg GDB=yes'
+2. `docker run --rm -it -v <PATH_TO_REPO>:/src inav-build <TARGET>`
    + Where `<PATH_TO_REPO>` must be replaced with the absolute path of where you cloned this repo (see above), and `<TARGET>` with the name of the target that you want to build.
-   + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above.   
 
 3. If you need to update `Settings.md`, run:
 
-`docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v <PATH_TO_REPO>:/src inav-build`
+`docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -v <PATH_TO_REPO>:/src inav-build`
 
 4. Building SITL: 
 
-`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -u root -v <PATH_TO_REPO>:/src inav-build`
+`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -v <PATH_TO_REPO>:/src inav-build`
 
 5. Running SITL: 
 
-`docker run -p 5760:5760 -p 5761:5761 -p 5762:5762 -p 5763:5763 -p 5764:5764 -p 5765:5765 -p 5766:5766 -p 5767:5767 --entrypoint /src/cmake/docker_run_sitl.sh --rm -it -u root -v <PATH_TO_REPO>:/src inav-build`.
+`docker run -p 5760:5760 -p 5761:5761 -p 5762:5762 -p 5763:5763 -p 5764:5764 -p 5765:5765 -p 5766:5766 -p 5767:5767 --entrypoint /src/cmake/docker_run_sitl.sh --rm -it -v <PATH_TO_REPO>:/src inav-build`.
    + SITL command line parameters can be adjusted in `cmake/docker_run_sitl.sh`.
 
 Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details.

docs/development/IDE - Visual Studio Code with Windows 10.md

@@ -39,8 +39,7 @@ Edit file `./.vscode/c_cpp_properties.json` to setup enabled `defines`
             "intelliSenseMode": "msvc-x64",
             "cStandard": "c11",
             "cppStandard": "c++17",
-            "defines": [
+            "defines": [,
-                "NAV_FIXED_WING_LANDING",
                 "USE_OSD",
                 "USE_GYRO_NOTCH_1",
                 "USE_GYRO_NOTCH_2",

docs/development/Windows 11 - VS Code - WSL2 - Hardware Debugging.md

@@ -235,7 +235,6 @@ sudo udevadm control --reload-rules
             "cStandard": "c11",
             "cppStandard": "c++17",
             "defines": [
-                "NAV_FIXED_WING_LANDING",
                 "USE_OSD",
                 "USE_GYRO_NOTCH_1",
                 "USE_GYRO_NOTCH_2",

docs/development/serial_printf_debugging.md

@@ -98,10 +98,13 @@ LOG_BUF_DEBUG(TEMPERATURE, &tstruct, sizeof(tstruct));
 
 Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messages (`MSP_DEBUGMSG`). It is possible to use any serial terminal to display these messages, however it is advisable to use an application that understands `MSP_DEBUGMSG` in order to maintain readability (in a raw serial terminal the MSP message envelope may result in the display of strange characters). `MSP_DEBUGMSG` aware applications include:
 
-* [msp-tool](https://github.com/fiam/msp-tool)
+* [dbg-tool](https://codeberg.org/stronnag/dbg-tool)
-* [mwp](https://github.com/stronnag/mwptools)
-* [dbg-tool](https://github.com/stronnag/mwptools)
 * [INAV Configurator](https://github.com/iNavFlight/inav-configurator)
+* [mwp](https://github.com/stronnag/mwptools)
+
+In addtion:
+
+* [msp-tool](https://github.com/fiam/msp-tool) is obsolete and has limited OS support.
 
 For example, with the final lines of `src/main/fc/fc_init.c` set to:
 
@@ -121,10 +124,16 @@ set log_topics = 4294967295
 The output will be formatted as follows:
 
 ```
-# msp-tool
+# dbg-tool
-[DEBUG] [     3.967] Init is complete
+[dbg-tool] 12:46:49.909079 DBG: [     3.967] Init is complete
+
 # mwp (stderr log file)
 2020-02-02T19:09:02+0000 DEBUG:[     3.968] Init is complete
+
+# msp-tool
+[DEBUG] [     3.967] Init is complete
 ```
 
-The numeric value in square brackets is the FC uptime in seconds.
+For the Configurator, debug messages are shown in the developer console log.
+
+Note: The numeric value in square brackets is the FC uptime in seconds.

docs/development/wp_mission_schema/README.md

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

docs/development/wp_mission_schema/mw-mission.xsd

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

lib/main/MAVLink/common/common.h

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

lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c

@@ -343,6 +343,12 @@ static int8_t SCSI_ModeSense6 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense6_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7);
+  }
+
   return 0;
 }
 
@@ -365,6 +371,12 @@ static int8_t SCSI_ModeSense10 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense10_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7);
+  }
+
   return 0;
 }
 

lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c

@@ -347,6 +347,12 @@ static int8_t SCSI_ModeSense6 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense6_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7);
+  }
+
   return 0;
 }
 
@@ -370,6 +376,12 @@ static int8_t SCSI_ModeSense10 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense10_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7);
+  }
+
   return 0;
 }
 

lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c

@@ -346,6 +346,12 @@ static int8_t SCSI_ModeSense6(USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *p
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense6_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7);
+  }
+
   return 0;
 }
 
@@ -371,6 +377,11 @@ static int8_t SCSI_ModeSense10(USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *
     hmsc->bot_data[len] = MSC_Mode_Sense10_data[len];
   }
 
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7);
+  }
+
   return 0;
 }
 

lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c

@@ -184,8 +184,8 @@ __ALIGN_BEGIN uint8_t APP_Rx_Buffer   [APP_RX_DATA_SIZE] __ALIGN_END ;
 #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
 __ALIGN_BEGIN uint8_t CmdBuff[CDC_CMD_PACKET_SZE] __ALIGN_END ;
 
-uint32_t APP_Rx_ptr_in  = 0;
+volatile uint32_t APP_Rx_ptr_in  = 0;
-uint32_t APP_Rx_ptr_out = 0;
+volatile uint32_t APP_Rx_ptr_out = 0;
 uint32_t APP_Rx_length  = 0;
 
 uint8_t  USB_Tx_State = USB_CDC_IDLE;
@@ -482,8 +482,8 @@ uint8_t  usbd_cdc_Init (void  *pdev,
 uint8_t  usbd_cdc_DeInit (void  *pdev, 
                                  uint8_t cfgidx)
 {
-	  (void)pdev;
+      (void)pdev;
-	  (void)cfgidx;
+      (void)cfgidx;
   /* Open EP IN */
   DCD_EP_Close(pdev,
               CDC_IN_EP);
@@ -594,7 +594,7 @@ uint8_t  usbd_cdc_Setup (void  *pdev,
   */
 uint8_t  usbd_cdc_EP0_RxReady (void  *pdev)
 { 
-	(void)pdev;
+    (void)pdev;
   if (cdcCmd != NO_CMD)
   {
     /* Process the data */
@@ -617,60 +617,45 @@ uint8_t  usbd_cdc_EP0_RxReady (void  *pdev)
   */
 uint8_t  usbd_cdc_DataIn (void *pdev, uint8_t epnum)
 {
-	(void)pdev;
+    (void)pdev;
-	(void)epnum;
+    (void)epnum;
-  uint16_t USB_Tx_ptr;
+    uint16_t USB_Tx_length;
-  uint16_t USB_Tx_length;
+
-  
+    if (USB_Tx_State == USB_CDC_BUSY)
-  if (USB_Tx_State == USB_CDC_BUSY)
-  {
-    if (APP_Rx_length == 0) 
     {
-      USB_Tx_State = USB_CDC_IDLE;
+        if (APP_Rx_length == 0)
-    }
-    else 
-    {
-      if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE){
-        USB_Tx_ptr = APP_Rx_ptr_out;
-        USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
-        
-        APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE;
-        APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE;    
-      }
-      else 
-      {
-        USB_Tx_ptr = APP_Rx_ptr_out;
-        USB_Tx_length = APP_Rx_length;
-        
-        APP_Rx_ptr_out += APP_Rx_length;
-        APP_Rx_length = 0;
-        if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE)
         {
-          USB_Tx_State = USB_CDC_ZLP;
+            USB_Tx_State = USB_CDC_IDLE;
+        } else {
+            if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) {
+                USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
+            } else {
+                USB_Tx_length = APP_Rx_length;
+
+                if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) {
+                    USB_Tx_State = USB_CDC_ZLP;
+                }
+            }
+
+            /* Prepare the available data buffer to be sent on IN endpoint */
+            DCD_EP_Tx(pdev, CDC_IN_EP, (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out], USB_Tx_length);
+
+            // Advance the out pointer
+            APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE;
+            APP_Rx_length -= USB_Tx_length;
+
+            return USBD_OK;
         }
-      }
-      
-      /* Prepare the available data buffer to be sent on IN endpoint */
-      DCD_EP_Tx (pdev,
-                 CDC_IN_EP,
-                 (uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr],
-                 USB_Tx_length);
-      return USBD_OK;
     }
-  }  
+
-  
+    /* Avoid any asynchronous transfer during ZLP */
-  /* Avoid any asynchronous transfer during ZLP */
+    if (USB_Tx_State == USB_CDC_ZLP) {
-  if (USB_Tx_State == USB_CDC_ZLP)
+        /*Send ZLP to indicate the end of the current transfer */
-  {
+        DCD_EP_Tx(pdev, CDC_IN_EP, NULL, 0);
-    /*Send ZLP to indicate the end of the current transfer */
+
-    DCD_EP_Tx (pdev,
+        USB_Tx_State = USB_CDC_IDLE;
-               CDC_IN_EP,
+    }
-               NULL,
+    return USBD_OK;
-               0);
-    
-    USB_Tx_State = USB_CDC_IDLE;
-  }
-  return USBD_OK;
 }
 
 /**
@@ -731,67 +716,49 @@ uint8_t  usbd_cdc_SOF (void *pdev)
   */
 static void Handle_USBAsynchXfer (void *pdev)
 {
-  uint16_t USB_Tx_ptr;
+    uint16_t USB_Tx_length;
-  uint16_t USB_Tx_length;
+
-  
+    if (USB_Tx_State == USB_CDC_IDLE) {
-  if(USB_Tx_State == USB_CDC_IDLE)
+        if (APP_Rx_ptr_out == APP_Rx_ptr_in) {
-  {
+            // Ring buffer is empty
-    if (APP_Rx_ptr_out == APP_RX_DATA_SIZE)
+            return;
-    {
+        }
-      APP_Rx_ptr_out = 0;
+
-    }
+        if (APP_Rx_ptr_out > APP_Rx_ptr_in) {
-    
+            // Transfer bytes up to the end of the ring buffer
-    if(APP_Rx_ptr_out == APP_Rx_ptr_in) 
+            APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out;
-    {
+        } else {
-      USB_Tx_State = USB_CDC_IDLE; 
+            // Transfer all bytes in ring buffer
-      return;
+            APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out;
-    }
+        }
-    
+
-    if(APP_Rx_ptr_out > APP_Rx_ptr_in) /* rollback */
-    { 
-      APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out;
-      
-    }
-    else 
-    {
-      APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out;
-      
-    }
 #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED
-    APP_Rx_length &= ~0x03;
+        // Only transfer whole 32 bit words of data
+        APP_Rx_length &= ~0x03;
 #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
+
+        if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) {
+            USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
+
+            USB_Tx_State = USB_CDC_BUSY;
+        } else {
+            USB_Tx_length = APP_Rx_length;
+
+            if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) {
+                USB_Tx_State = USB_CDC_ZLP;
+            } else {
+                USB_Tx_State = USB_CDC_BUSY;
+            }
+        }
     
-    if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE)
+        DCD_EP_Tx (pdev,
-    {
+                   CDC_IN_EP,
-      USB_Tx_ptr = APP_Rx_ptr_out;
+                   (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out],
-      USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
+                   USB_Tx_length);
-      
+
-      APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE;	
+        APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE;
-      APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE;
+        APP_Rx_length -= USB_Tx_length;
-      USB_Tx_State = USB_CDC_BUSY;
     }
-    else
-    {
-      USB_Tx_ptr = APP_Rx_ptr_out;
-      USB_Tx_length = APP_Rx_length;
-      
-      APP_Rx_ptr_out += APP_Rx_length;
-      APP_Rx_length = 0;
-      if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE)
-      {
-        USB_Tx_State = USB_CDC_ZLP;
-      }
-      else
-      {
-        USB_Tx_State = USB_CDC_BUSY;
-      }
-    }
-    
-    DCD_EP_Tx (pdev,
-               CDC_IN_EP,
-               (uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr],
-               USB_Tx_length);
-  }  
 }
 
 /**
@@ -803,7 +770,7 @@ static void Handle_USBAsynchXfer (void *pdev)
   */
 static uint8_t  *USBD_cdc_GetCfgDesc (uint8_t speed, uint16_t *length)
 {
-	(void)speed;
+    (void)speed;
   *length = sizeof (usbd_cdc_CfgDesc);
   return usbd_cdc_CfgDesc;
 }

readme.md

@@ -1,5 +1,11 @@
 # INAV - navigation capable flight controller
 
+# PSA
+
+> INAV no longer accepts targets based on STM32 F411 MCU.
+
+> INAV 7 is the last INAV official release available for F411 based flight controllers. The next milestone, INAV 8 will not be available for F411 boards.
+
 ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
 
 # INAV Community
@@ -10,7 +16,7 @@
 
 ## Features
 
-* Runs on the most popular F4, F7 and H7 flight controllers
+* Runs on the most popular F4, AT32, F7 and H7 flight controllers
 * On Screen Display (OSD) - both character and pixel style
 * DJI OSD integration: all elements, system messages and warnings
 * Outstanding performance out of the box

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
@@ -371,6 +372,8 @@ main_sources(COMMON_SRC
     io/rcdevice_cam.c
     io/rcdevice_cam.h
 
+    io/osd/custom_elements.c
+
     msp/msp_serial.c
     msp/msp_serial.h
 
@@ -558,6 +561,8 @@ main_sources(COMMON_SRC
     navigation/navigation_rover_boat.c
     navigation/sqrt_controller.c
     navigation/sqrt_controller.h
+    navigation/rth_trackback.c
+    navigation/rth_trackback.h
 
     sensors/barometer.c
     sensors/barometer.h

src/main/blackbox/blackbox.c

@@ -310,6 +310,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"accSmooth",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"accSmooth",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"accSmooth",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
+    {"accVib",     -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"attitude",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
     {"attitude",    1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
     {"attitude",    2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
@@ -436,6 +437,7 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
     {"escTemperature",        -1, SIGNED,   PREDICT(PREVIOUS),      ENCODING(SIGNED_VB)},
 #endif
     {"rxUpdateRate",          -1, UNSIGNED, PREDICT(PREVIOUS),      ENCODING(UNSIGNED_VB)},
+    {"activeWpNumber",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 };
 
 typedef enum BlackboxState {
@@ -487,6 +489,7 @@ typedef struct blackboxMainState_s {
     int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
 
     int16_t accADC[XYZ_AXIS_COUNT];
+    int16_t accVib;
     int16_t attitude[XYZ_AXIS_COUNT];
     int32_t debug[DEBUG32_VALUE_COUNT];
     int16_t motor[MAX_SUPPORTED_MOTORS];
@@ -554,6 +557,7 @@ typedef struct blackboxSlowState_s {
     int8_t escTemperature;
 #endif
     uint16_t rxUpdateRate;
+    uint8_t activeWpNumber;
 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 
 //From rc_controls.c
@@ -917,6 +921,7 @@ static void writeIntraframe(void)
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
         blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
+        blackboxWriteUnsignedVB(blackboxCurrent->accVib);
     }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@@ -1182,6 +1187,7 @@ static void writeInterframe(void)
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
         blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
+        blackboxWriteSignedVB(blackboxCurrent->accVib - blackboxLast->accVib);
     }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@@ -1291,6 +1297,7 @@ static void writeSlowFrame(void)
     blackboxWriteSignedVB(slowHistory.escTemperature);
 #endif
     blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
+    blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
 
     blackboxSlowFrameIterationTimer = 0;
 }
@@ -1368,6 +1375,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
 #endif
 
     slow->rxUpdateRate = getRcUpdateFrequency();
+    slow->activeWpNumber = getActiveWpNumber();
 }
 
 /**
@@ -1601,6 +1609,7 @@ static void loadMainState(timeUs_t currentTimeUs)
             blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
         }
     }
+    blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
 
     if (STATE(FIXED_WING_LEGACY)) {
 

src/main/cms/cms_menu_navigation.c

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

src/main/common/fp_pid.c

@@ -92,6 +92,11 @@ float navPidApply3(
     /* Pre-calculate output and limit it if actuator is saturating */
     const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
     const float outValConstrained = constrainf(outVal, outMin, outMax);
+    float backCalc = outValConstrained - outVal;//back-calculation anti-windup
+    if (SIGN(backCalc) == SIGN(pid->integrator)) {
+        //back calculation anti-windup can only shrink integrator, will not push it to the opposite direction
+        backCalc = 0.0f;
+    }
 
     pid->proportional = newProportional;
     pid->integral = pid->integrator;
@@ -104,7 +109,7 @@ float navPidApply3(
         !(pidFlags & PID_ZERO_INTEGRATOR) &&
         !(pidFlags & PID_FREEZE_INTEGRATOR) 
     ) {
-        const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
+        const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + (backCalc * pid->param.kT * dt);
 
         if (pidFlags & PID_SHRINK_INTEGRATOR) {
             // Only allow integrator to shrink

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,13 +54,16 @@
 #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)
+#define METERS_TO_MILES(m)              (m / 1609.344f)
+#define METERS_TO_NAUTICALMILES(m)      (m / 1852.00f)
 
 #define CMSEC_TO_CENTIMPH(cms)          (cms * 2.2369363f)
 #define CMSEC_TO_CENTIKPH(cms)          (cms * 3.6f)
@@ -91,6 +94,7 @@
         )
 #define MIN(a, b) _CHOOSE(<, a, b)
 #define MAX(a, b) _CHOOSE(>, a, b)
+#define SIGN(a) ((a >= 0) ? 1 : -1)
 
 #define _ABS_II(x, var)             \
     ( __extension__ ({              \

src/main/config/parameter_group_ids.h

@@ -123,8 +123,10 @@
 #define PG_EZ_TUNE 1033
 #define PG_LEDPIN_CONFIG 1034
 #define PG_OSD_JOYSTICK_CONFIG 1035
-#define PG_INAV_END PG_OSD_JOYSTICK_CONFIG
+#define PG_FW_AUTOLAND_CONFIG 1036
-
+#define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
+#define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
+#define PG_INAV_END PG_OSD_CUSTOM_ELEMENTS_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/accgyro/accgyro_bmi160.c

@@ -195,9 +195,9 @@ bool bmi160AccReadScratchpad(accDev_t *acc)
     bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 
     if (ctx->lastReadStatus) {
-        acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
+        acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->accRaw, 0);
-        acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
+        acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->accRaw, 1);
-        acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);
+        acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->accRaw, 2);
         return true;
     }
 

src/main/drivers/accgyro/accgyro_icm42605.c

@@ -26,6 +26,7 @@
 #include "common/axis.h"
 #include "common/maths.h"
 #include "common/utils.h"
+#include "common/log.h"
 
 #include "drivers/system.h"
 #include "drivers/time.h"
@@ -44,13 +45,20 @@
 #define ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF         (0 << 5)
 #define ICM42605_PWR_MGMT0_TEMP_DISABLE_ON          (1 << 5)
 
+#define ICM426XX_RA_REG_BANK_SEL                    0x76
+#define ICM426XX_BANK_SELECT0                       0x00
+#define ICM426XX_BANK_SELECT1                       0x01
+#define ICM426XX_BANK_SELECT2                       0x02
+#define ICM426XX_BANK_SELECT3                       0x03
+#define ICM426XX_BANK_SELECT4                       0x04
+
 #define ICM42605_RA_GYRO_CONFIG0                    0x4F
 #define ICM42605_RA_ACCEL_CONFIG0                   0x50
 
 #define ICM42605_RA_GYRO_ACCEL_CONFIG0              0x52
 
-#define ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY       (14 << 4)
+#define ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY       (15 << 4)
-#define ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY        (14 << 0)
+#define ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY        (15 << 0)
 
 #define ICM42605_RA_GYRO_DATA_X1                    0x25
 #define ICM42605_RA_ACCEL_DATA_X1                   0x1F
@@ -86,6 +94,63 @@
 #define ICM42605_INTF_CONFIG1_AFSR_MASK             0xC0
 #define ICM42605_INTF_CONFIG1_AFSR_DISABLE          0x40
 
+// --- Registers for gyro and acc Anti-Alias Filter ---------
+#define ICM426XX_RA_GYRO_CONFIG_STATIC3             0x0C  // User Bank 1
+#define ICM426XX_RA_GYRO_CONFIG_STATIC4             0x0D  // User Bank 1
+#define ICM426XX_RA_GYRO_CONFIG_STATIC5             0x0E  // User Bank 1
+#define ICM426XX_RA_ACCEL_CONFIG_STATIC2            0x03  // User Bank 2
+#define ICM426XX_RA_ACCEL_CONFIG_STATIC3            0x04  // User Bank 2
+#define ICM426XX_RA_ACCEL_CONFIG_STATIC4            0x05  // User Bank 2
+
+static bool is42688P = false;
+
+typedef struct aafConfig_s {
+    uint16_t freq;
+    uint8_t delt;
+    uint16_t deltSqr;
+    uint8_t bitshift;
+} aafConfig_t;
+
+// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
+static aafConfig_t aafLUT42688[] = {  // see table in section 5.3 https://invensense.tdk.com/wp-content/uploads/2020/04/ds-000347_icm-42688-p-datasheet.pdf
+    // freq, delt, deltSqr, bitshift
+    { 42,  1,    1, 15 },
+    { 84,  2,    4, 13 },
+    {126,  3,    9, 12 },
+    {170,  4,   16, 11 },
+    {213,  5,   25, 10 },
+    {258,  6,   36, 10 },
+    {303,  7,   49,  9 },
+    {536, 12,  144,  8 },
+    {997, 21,  440,  6 },
+    {1962, 37, 1376,  4 },
+    { 0, 0, 0, 0}, // 42HZ
+};
+
+// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
+static aafConfig_t aafLUT42605[] = {  // see table in section 5.3 https://invensense.tdk.com/wp-content/uploads/2022/09/DS-000292-ICM-42605-v1.7.pdf
+    // freq, delt, deltSqr, bitshift
+    {  10,  1,    1, 15 },
+    {  21,  2,    4, 13 },
+    {  32,  3,    9, 12 },
+    {  42,  4,   16, 11 },
+    {  99,  9,   81,  9 },
+    { 171, 15,  224,  7 },
+    { 184, 16,  256,  7 },
+    { 196, 17,  288,  7 },
+    { 249, 21,  440,  6 },
+    { 524, 39, 1536,  4 },
+    { 995, 63, 3968,  3 },
+    {   0,  0,    0,  0 }
+};
+
+static const aafConfig_t *getGyroAafConfig(bool is42688, const uint16_t desiredLpf);
+
+static void setUserBank(const busDevice_t *dev, const uint8_t user_bank)
+{
+    busWrite(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7);
+}
+
 static void icm42605AccInit(accDev_t *acc)
 {
     acc->acc_1G = 512 * 4;
@@ -159,6 +224,7 @@ static void icm42605AccAndGyroInit(gyroDev_t *gyro)
 
     busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
 
+    setUserBank(dev, ICM426XX_BANK_SELECT0);
     busWrite(dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN);
     delay(15);
 
@@ -170,9 +236,27 @@ static void icm42605AccAndGyroInit(gyroDev_t *gyro)
     delay(15);
 
     /* LPF bandwidth */
-    busWrite(dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, (config->gyroConfigValues[1]) | (config->gyroConfigValues[1] << 4));
+    // low latency, same as BF
+    busWrite(dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
     delay(15);
 
+    if (gyro->lpf != GYRO_LPF_NONE) {
+        // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
+        const aafConfig_t *aafConfig = getGyroAafConfig(is42688P, gyro->lpf);
+    
+        setUserBank(dev, ICM426XX_BANK_SELECT1);
+        busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig->delt);
+        busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig->deltSqr & 0xFF);
+        busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig->deltSqr >> 8) | (aafConfig->bitshift << 4));
+
+        aafConfig = getGyroAafConfig(is42688P, 256);  // This was hard coded on BF
+        setUserBank(dev, ICM426XX_BANK_SELECT2);
+        busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig->delt << 1);
+        busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig->deltSqr & 0xFF);
+        busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig->deltSqr >> 8) | (aafConfig->bitshift << 4));
+    }
+
+    setUserBank(dev, ICM426XX_BANK_SELECT0);
     busWrite(dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
     delay(15);
 
@@ -221,7 +305,10 @@ static bool icm42605DeviceDetect(busDevice_t * dev)
         switch (tmp) {
             /* ICM42605 and ICM42688P share the register structure*/
             case ICM42605_WHO_AM_I_CONST:
+                is42688P = false;
+                return true;
             case ICM42688P_WHO_AM_I_CONST:
+                is42688P = true;
                 return true;
 
             default:
@@ -275,4 +362,57 @@ bool icm42605GyroDetect(gyroDev_t *gyro)
     return true;
 }
 
+static uint16_t getAafFreq(const uint8_t gyroLpf)
+{
+    switch (gyroLpf) {
+        default:
+        case GYRO_LPF_256HZ:
+            return 256;
+        case GYRO_LPF_188HZ:
+            return 188;
+        case GYRO_LPF_98HZ:
+            return 98;
+        case GYRO_LPF_42HZ:
+            return 42;
+        case GYRO_LPF_20HZ:
+            return 20;
+        case GYRO_LPF_10HZ:
+            return 10;
+        case GYRO_LPF_5HZ:
+            return 5;
+        case GYRO_LPF_NONE:
+            return 0;
+    }
+}
+
+static const aafConfig_t *getGyroAafConfig(bool is42688, const uint16_t desiredLpf)
+{
+    uint16_t desiredFreq = getAafFreq(desiredLpf);
+    const aafConfig_t *aafConfigs = NULL;
+    if (is42688) {
+        aafConfigs = aafLUT42688;
+    } else {
+        aafConfigs = aafLUT42605;
+    }
+    int i;
+    int8_t selectedFreq = aafConfigs[0].freq;
+    const aafConfig_t * candidate = &aafConfigs[0];
+
+    // Choose closest supported LPF value
+    for (i = 1; aafConfigs[i].freq != 0; i++) {
+        if (ABS(desiredFreq - aafConfigs[i].freq) < ABS(desiredFreq - selectedFreq)) {
+            selectedFreq = aafConfigs[i].freq;
+            candidate = &aafConfigs[i];
+        }
+    }
+
+    LOG_VERBOSE(GYRO, "ICM426%s AAF CONFIG { %d, %d } -> { %d }; delt: %d deltSqr: %d, shift: %d",
+		(is42688P ? "88" : "05"),
+                desiredLpf, desiredFreq,
+                candidate->freq,
+                candidate->delt, candidate->deltSqr, candidate->bitshift);
+
+    return candidate;
+}
+
 #endif

src/main/drivers/adc_at32f43x.c

@@ -179,11 +179,11 @@ void adcHardwareInit(drv_adc_config_t *init)
         adcDevice_t * adc = &adcHardware[adcConfig[i].adcDevice];
         // init adc gpio port
         IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_CH1 + (i - ADC_CHN_1), 0);
-        IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_OUTPUT_OPEN_DRAIN, GPIO_PULL_NONE));
+        IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, 0, 0));
 
         adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
         adcConfig[i].dmaIndex = adc->usedChannelCount++; 
-        adcConfig[i].sampleTime = ADC_SAMPLETIME_6_5;
+        adcConfig[i].sampleTime = ADC_SAMPLETIME_640_5;
         adcConfig[i].enabled = true;
 
         adc->enabled = true;

src/main/drivers/bus_spi.h

@@ -78,7 +78,7 @@ typedef struct SPIDevice_s {
     ioTag_t mosi;
     ioTag_t miso;
     rccPeriphTag_t rcc;
-#if defined(STM32F7) || defined(STM32H7)
+#if defined(STM32F7) || defined(STM32H7) || defined(AT32F43x)
     uint8_t sckAF;
     uint8_t misoAF;
     uint8_t mosiAF;

src/main/drivers/bus_spi_at32f43x.c

@@ -80,25 +80,61 @@
     };
     #endif
 
-    static spiDevice_t spiHardwareMap[] = {
+//MAP spi pin config and af 
-    #ifdef USE_SPI_DEVICE_1
+static spiDevice_t spiHardwareMap[] = {
-        { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_MUX_5, .divisorMap = spiDivisorMapFast },
+#ifdef USE_SPI_DEVICE_1
-    #else
+#if defined(SPI1_SCK_AF) || defined(SPI1_MISO_AF) || defined(SPI1_MOSI_AF)
-        { .dev = NULL },    // No SPI1
+#if !defined(SPI1_SCK_AF) || !defined(SPI1_MISO_AF) || !defined(SPI1_MOSI_AF)
-    #endif
+#error SPI1: SCK, MISO and MOSI AFs should be defined together in target.h!
-    #ifdef USE_SPI_DEVICE_2
+#endif
-        { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_MUX_5, .divisorMap = spiDivisorMapSlow },
+    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = SPI1_SCK_AF, .misoAF = SPI1_MISO_AF, .mosiAF = SPI1_MOSI_AF, .divisorMap = spiDivisorMapFast },
-    #else
-        { .dev = NULL },    // No SPI2
-    #endif
-    #ifdef USE_SPI_DEVICE_3
-        { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_MUX_6, .divisorMap = spiDivisorMapSlow },
-    #else
-        { .dev = NULL },    // No SPI3
-    #endif
-        { .dev = NULL },    // No SPI4
-    };
 #else
+    { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .sckAF = GPIO_MUX_5, .misoAF = GPIO_MUX_5, .mosiAF = GPIO_MUX_5, .divisorMap = spiDivisorMapFast },
+#endif
+#else
+    { .dev = NULL },    // No SPI1
+#endif
+
+#ifdef USE_SPI_DEVICE_2
+#if defined(SPI2_SCK_AF) || defined(SPI2_MISO_AF) || defined(SPI2_MOSI_AF)
+#if !defined(SPI2_SCK_AF) || !defined(SPI2_MISO_AF) || !defined(SPI2_MOSI_AF)
+#error SPI2: SCK, MISO and MOSI AFs should be defined together in target.h!
+#endif
+    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .sckAF = SPI2_SCK_AF, .misoAF = SPI2_MISO_AF, .mosiAF = SPI2_MOSI_AF, .divisorMap = spiDivisorMapSlow },
+#else
+    { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .sckAF = GPIO_MUX_5, .misoAF = GPIO_MUX_5, .mosiAF = GPIO_MUX_5, .divisorMap = spiDivisorMapSlow },
+#endif
+#else
+    { .dev = NULL },    // No SPI2
+#endif
+
+#ifdef USE_SPI_DEVICE_3
+#if defined(SPI3_SCK_AF) || defined(SPI3_MISO_AF) || defined(SPI3_MOSI_AF)
+#if !defined(SPI3_SCK_AF) || !defined(SPI3_MISO_AF) || !defined(SPI3_MOSI_AF)
+#error SPI3: SCK, MISO and MOSI AFs should be defined together in target.h!
+#endif
+    { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .sckAF = SPI3_SCK_AF, .misoAF = SPI3_MISO_AF, .mosiAF = SPI3_MOSI_AF, .divisorMap = spiDivisorMapSlow },
+#else
+    { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .sckAF = GPIO_MUX_6, .misoAF = GPIO_MUX_6, .mosiAF = GPIO_MUX_6, .divisorMap = spiDivisorMapSlow },
+#endif
+#else
+    { .dev = NULL },    // No SPI3
+#endif
+
+#ifdef USE_SPI_DEVICE_4
+#if defined(SPI4_SCK_AF) || defined(SPI4_MISO_AF) || defined(SPI4_MOSI_AF)
+#if !defined(SPI4_SCK_AF) || !defined(SPI4_MISO_AF) || !defined(SPI4_MOSI_AF)
+#error SPI4: SCK, MISO and MOSI AFs should be defined together in target.h!
+#endif
+    { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = SPI4_SCK_AF, .misoAF = SPI4_MISO_AF, .mosiAF = SPI4_MOSI_AF, .divisorMap = spiDivisorMapSlow }
+#else
+    { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .sckAF = GPIO_MUX_6, .misoAF = GPIO_MUX_6, .mosiAF = GPIO_MUX_6, .divisorMap = spiDivisorMapSlow }
+#endif
+#else
+    { .dev = NULL }     // No SPI4
+#endif
+};
+ #else
 #error "Invalid CPU"
 #endif
 
@@ -135,12 +171,13 @@ bool spiInitDevice(SPIDevice device, bool leadingEdge)
     IOInit(IOGetByTag(spi->miso), OWNER_SPI, RESOURCE_SPI_MISO, device + 1);
     IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1);
 
-    IOConfigGPIOAF(IOGetByTag(spi->sck),  SPI_IO_AF_SCK_CFG, spi->af);
+    IOConfigGPIOAF(IOGetByTag(spi->sck),  SPI_IO_AF_SCK_CFG, spi->sckAF);
-    IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af);
+    IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF);
-    IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
+    IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF);
 
     if (spi->nss) {
-        IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af);
+        IOInit(IOGetByTag(spi->nss),  OWNER_SPI, RESOURCE_SPI_CS,  device + 1);
+        IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
     }
 
     spi_i2s_reset(spi->dev);

src/main/drivers/compass/compass_fake.c

@@ -37,7 +37,7 @@ static bool fakeMagInit(magDev_t *magDev)
 {
     UNUSED(magDev);
     // initially point north
-    fakeMagData[X] = 4096;
+    fakeMagData[X] = 1024;
     fakeMagData[Y] = 0;
     fakeMagData[Z] = 0;
     return true;

src/main/drivers/display.c

@@ -33,8 +33,6 @@
 #include "fc/settings.h"
 #include "fc/runtime_config.h"
 
-#define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off
-
 // XXX: This is the number of characters in a MAX7456 line.
 // Increment this number appropiately or enable support for
 // multiple iterations in displayWriteWithAttr() if bigger
@@ -66,7 +64,7 @@ static bool displayEmulateTextAttributes(displayPort_t *instance,
     // We only emulate blink for now, so there's no need to test
     // for it again.
     TEXT_ATTRIBUTES_REMOVE_BLINK(*attr);
-    if ((millis() / SW_BLINK_CYCLE_MS) % 2) {
+    if (getBlinkOnOff()) {
         memset(buf, ' ', length);
         buf[length] = '\0';
         // Tell the caller to use buf
@@ -180,13 +178,13 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
 
 #ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-		//some FCs do not power max7456 from USB power 
+        //some FCs do not power max7456 from USB power 
-		//driver can not read font metadata 
+        //driver can not read font metadata 
-		//chip assumed to not support second bank of font
+        //chip assumed to not support second bank of font
-		//artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar())
+        //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar())
-		//return dummy metadata to let all OSD elements to work in simulator mode
+        //return dummy metadata to let all OSD elements to work in simulator mode
-		instance->maxChar = 512;
+        instance->maxChar = 512;
-	}
+    }
 #endif
 
     if (c > instance->maxChar) {

src/main/drivers/display.h

@@ -20,8 +20,14 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "drivers/time.h"
+
 #include "config/parameter_group.h"
 
+#define SW_BLINK_CYCLE_MS 500 // Xms on / Xms off
+
+#define getBlinkOnOff()  ( (millis() / SW_BLINK_CYCLE_MS) & 1 )
+
 typedef struct osdCharacter_s osdCharacter_t;
 
 typedef struct displayConfig_s {

src/main/drivers/light_ws2811strip.h

@@ -19,7 +19,7 @@
 #include "common/color.h"
 #include "config/parameter_group.h"
 
-#define WS2811_LED_STRIP_LENGTH 32
+#define WS2811_LED_STRIP_LENGTH 128
 #define WS2811_BITS_PER_LED 24
 #define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay 
 

src/main/drivers/osd_symbols.h

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

src/main/drivers/serial_tcp.c

@@ -265,13 +265,8 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
 
 uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
 {
-    tcpPort_t *port = (tcpPort_t*)instance;
+    UNUSED(instance);
-
+    return TCP_MAX_PACKET_SIZE;
-    if (port->isClientConnected) {
-        return TCP_MAX_PACKET_SIZE;
-    } else {
-        return 0;
-    }
 }
 
 bool isTcpTransmitBufferEmpty(const serialPort_t *instance)

src/main/drivers/serial_usb_vcp_at32f43x.c

@@ -226,19 +226,12 @@ void OTG_WKUP_HANDLER(void)
 
 uint32_t CDC_Send_FreeBytes(void)
 {
-    /*
+  cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata;
-        return the bytes free in the circular buffer
+  if(pcdc->g_tx_completed){
-
+    return APP_TX_BLOCK_SIZE;
-        functionally equivalent to:
+  }else{
-        (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in)
+    return 0;
-        but without the impact of the condition check.
+  }
-    */
-    uint32_t freeBytes;
-    ATOMIC_BLOCK(NVIC_PRIO_VCP) {
-        freeBytes = ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1;
-    }
-
-    return freeBytes;
 }
 
 /**
@@ -251,92 +244,29 @@ uint32_t CDC_Send_FreeBytes(void)
  */
 uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
 {
-    for (uint32_t i = 0; i < sendLength; i++) {
+  cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata;
-        while (CDC_Send_FreeBytes() == 0) {
+  uint32_t start = millis();
-            // block until there is free space in the ring buffer
+   
-            delay(1);
+  uint32_t pos=0;
-        }
+  while(pos < sendLength || (pos==sendLength && sendLength%64 == 0) ){//`==` is intended for sending 0 length packet
-
+    int tosend=sendLength-pos;
-        ATOMIC_BLOCK(NVIC_PRIO_VCP) {
+    if(tosend>APP_TX_BLOCK_SIZE){
-            UserTxBuffer[UserTxBufPtrIn] = ptrBuffer[i];
+      tosend=APP_TX_BLOCK_SIZE;
-            UserTxBufPtrIn = (UserTxBufPtrIn + 1) % APP_TX_DATA_SIZE;
-        }
     }
-    return sendLength;
+    while(pcdc->g_tx_completed != 1) {
-}
+      if (millis() - start > USB_TIMEOUT) {
-
+        return pos;
-void TxTimerConfig(void){
+      }
-	  /* Initialize TIMx peripheral as follow:
-	       + Period = CDC_POLLING_INTERVAL*1000 - 1  every 5ms
-	       + Prescaler = ((SystemCoreClock/2)/10000) - 1
-	       + ClockDivision = 0
-	       + Counter direction = Up
-	  */
-  crm_periph_clock_enable(CRM_TMR20_PERIPH_CLOCK, TRUE);
-	//timer, period, perscaler
-	tmr_base_init(usbTxTmr,(CDC_POLLING_INTERVAL - 1),((system_core_clock)/10000 - 1));
-	//TMR_CLOCK_DIV1 = 0X00 NO DIV
-	tmr_clock_source_div_set(usbTxTmr,TMR_CLOCK_DIV1);
-	//COUNT UP
-	tmr_cnt_dir_set(usbTxTmr,TMR_COUNT_UP);
-
-	tmr_period_buffer_enable(usbTxTmr,TRUE);
-
-	tmr_interrupt_enable(usbTxTmr, TMR_OVF_INT, TRUE);
-
-	nvic_irq_enable(TMR20_OVF_IRQn,NVIC_PRIO_USB,0);
-
-	tmr_counter_enable(usbTxTmr,TRUE);
-
-}
-
-/**
-  * @brief  TIM period elapsed callback
-  * @param  htim: TIM handle
-  * @retval None
-  */
-void TMR20_OVF_IRQHandler(void)
-{
-
-    uint32_t buffsize;
-    static uint32_t lastBuffsize = 0;
-
-    cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata;
-
-    if (pcdc->g_tx_completed == 1) {
-        // endpoint has finished transmitting previous block
-        if (lastBuffsize) {
-            bool needZeroLengthPacket = lastBuffsize % 64 == 0;
-
-            // move the ring buffer tail based on the previous succesful transmission
-            UserTxBufPtrOut += lastBuffsize;
-            if (UserTxBufPtrOut == APP_TX_DATA_SIZE) {
-                UserTxBufPtrOut = 0;
-            }
-            lastBuffsize = 0;
-
-            if (needZeroLengthPacket) {
-            	usb_vcp_send_data(&otg_core_struct.dev, (uint8_t*)&UserTxBuffer[UserTxBufPtrOut], 0);
-                return;
-            }
-        }
-        if (UserTxBufPtrOut != UserTxBufPtrIn) {
-            if (UserTxBufPtrOut > UserTxBufPtrIn) { /* Roll-back */
-                buffsize = APP_TX_DATA_SIZE - UserTxBufPtrOut;
-            } else {
-                buffsize = UserTxBufPtrIn - UserTxBufPtrOut;
-            }
-            if (buffsize > APP_TX_BLOCK_SIZE) {
-                buffsize = APP_TX_BLOCK_SIZE;
-            }
-
-            uint32_t txed=usb_vcp_send_data(&otg_core_struct.dev,(uint8_t*)&UserTxBuffer[UserTxBufPtrOut], buffsize);
-            if (txed==SUCCESS) {
-                lastBuffsize = buffsize;
-            }
-        }
     }
-    tmr_flag_clear(usbTxTmr,TMR_OVF_FLAG);
+    uint32_t txed=usb_vcp_send_data(&otg_core_struct.dev,(uint8_t *)(ptrBuffer+pos), tosend);
+    if(pos==sendLength){
+      break;
+    }
+    if (txed==SUCCESS) {
+      pos+=tosend;
+    }
+  }
+  return pos;
 }
 
 /************************************************************/
@@ -542,8 +472,6 @@ void usbVcpInitHardware(void)
                &cdc_class_handler,
                &cdc_desc_handler); 
 
-    //config TX timer
-    TxTimerConfig();
 }
 
 serialPort_t *usbVcpOpen(void)

src/main/drivers/sound_beeper.c

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

src/main/drivers/vtx_common.h

@@ -19,7 +19,6 @@
 
 #include "common/time.h"
 
-#define VTX_SETTINGS_NO_BAND        0 // used for custom frequency selection mode
 #define VTX_SETTINGS_MIN_BAND       1
 #define VTX_SETTINGS_MAX_BAND       5
 #define VTX_SETTINGS_MIN_CHANNEL    1
@@ -33,9 +32,6 @@
 #define VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL    1
 #define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM   0
 
-#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0             //min freq (in MHz) for 'vtx_freq' setting
-#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999          //max freq (in MHz) for 'vtx_freq' setting
-
 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
 
 #define VTX_SETTINGS_POWER_COUNT        5

src/main/fc/cli.c

@@ -93,6 +93,7 @@ bool cliMode = false;
 #include "io/gps_ublox.h"
 #include "io/ledstrip.h"
 #include "io/osd.h"
+#include "io/osd/custom_elements.h"
 #include "io/serial.h"
 
 #include "fc/fc_msp_box.h"
@@ -142,8 +143,9 @@ static void cliAssert(char *cmdline);
 #endif
 
 #ifdef USE_CLI_BATCH
-static bool commandBatchActive = false;
+static bool     commandBatchActive = false;
-static bool commandBatchError = false;
+static bool     commandBatchError = false;
+static uint8_t  commandBatchErrorCount = 0;
 #endif
 
 // sync this with features_e
@@ -256,6 +258,7 @@ static void cliPrintError(const char *str)
 #ifdef USE_CLI_BATCH
     if (commandBatchActive) {
         commandBatchError = true;
+        commandBatchErrorCount++;
     }
 #endif
 }
@@ -267,6 +270,7 @@ static void cliPrintErrorLine(const char *str)
 #ifdef USE_CLI_BATCH
     if (commandBatchActive) {
         commandBatchError = true;
+        commandBatchErrorCount++;
     }
 #endif
 }
@@ -369,6 +373,7 @@ static void cliPrintErrorVa(const char *format, va_list va)
 #ifdef USE_CLI_BATCH
     if (commandBatchActive) {
         commandBatchError = true;
+        commandBatchErrorCount++;
     }
 #endif
 }
@@ -660,6 +665,7 @@ static void cliAssert(char *cmdline)
 #ifdef USE_CLI_BATCH
         if (commandBatchActive) {
             commandBatchError = true;
+            commandBatchErrorCount++;
         }
 #endif
     }
@@ -1309,6 +1315,110 @@ static void cliTempSensor(char *cmdline)
 }
 #endif
 
+#ifdef USE_FW_AUTOLAND
+static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach) 
+{
+    const char *format = "fwapproach %u %d %d %u %d %d %u";
+    for (uint8_t i = 0; i < MAX_FW_LAND_APPOACH_SETTINGS; i++) {
+        bool equalsDefault = false;
+        if (defaultFwAutolandApproach) {
+               equalsDefault = navFwAutolandApproach[i].approachDirection == defaultFwAutolandApproach[i].approachDirection
+               && navFwAutolandApproach[i].approachAlt == defaultFwAutolandApproach[i].approachAlt
+               && navFwAutolandApproach[i].landAlt == defaultFwAutolandApproach[i].landAlt
+               && navFwAutolandApproach[i].landApproachHeading1 == defaultFwAutolandApproach[i].landApproachHeading1
+               && navFwAutolandApproach[i].landApproachHeading2 == defaultFwAutolandApproach[i].landApproachHeading2
+               && navFwAutolandApproach[i].isSeaLevelRef == defaultFwAutolandApproach[i].isSeaLevelRef;
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,
+               defaultFwAutolandApproach[i].approachAlt, defaultFwAutolandApproach[i].landAlt, defaultFwAutolandApproach[i].approachDirection, defaultFwAutolandApproach[i].landApproachHeading1, defaultFwAutolandApproach[i].landApproachHeading2, defaultFwAutolandApproach[i].isSeaLevelRef);
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format, i,
+            navFwAutolandApproach[i].approachAlt, navFwAutolandApproach[i].landAlt, navFwAutolandApproach[i].approachDirection, navFwAutolandApproach[i].landApproachHeading1, navFwAutolandApproach[i].landApproachHeading2, navFwAutolandApproach[i].isSeaLevelRef);
+    }
+}
+
+static void cliFwAutolandApproach(char * cmdline)
+{
+     if (isEmpty(cmdline)) {
+        printFwAutolandApproach(DUMP_MASTER, fwAutolandApproachConfig(0), NULL);
+    } else if (sl_strcasecmp(cmdline, "reset") == 0) {
+        resetFwAutolandApproach(-1);
+    } else {
+        int32_t approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0;
+        bool isSeaLevelRef = false;
+        uint8_t validArgumentCount = 0;
+        const char *ptr = cmdline;
+        int8_t i = fastA2I(ptr);
+        if (i < 0 || i >= MAX_FW_LAND_APPOACH_SETTINGS) {
+             cliShowArgumentRangeError("fwapproach index", 0, MAX_FW_LAND_APPOACH_SETTINGS - 1);
+        } else {
+            if ((ptr = nextArg(ptr))) {
+                approachAlt = fastA2I(ptr);
+                validArgumentCount++;
+            }
+
+            if ((ptr = nextArg(ptr))) {
+                landAlt = fastA2I(ptr);
+                validArgumentCount++;
+            }
+
+            if ((ptr = nextArg(ptr))) {
+                landDirection = fastA2I(ptr);
+                
+                if (landDirection != 0 && landDirection != 1) {
+                    cliShowParseError();
+                    return;
+                }
+
+                validArgumentCount++;
+            }
+
+            if ((ptr = nextArg(ptr))) {
+                heading1 = fastA2I(ptr);
+
+                if (heading1 < -360 || heading1 > 360) {
+                    cliShowParseError();
+                    return;
+                }
+
+                validArgumentCount++;
+            }
+
+            if ((ptr = nextArg(ptr))) {
+                heading2 = fastA2I(ptr);
+
+                if (heading2 < -360 || heading2 > 360) {
+                    cliShowParseError();
+                    return;
+                }
+
+                validArgumentCount++;
+            }
+            
+            if ((ptr = nextArg(ptr))) {
+                isSeaLevelRef = fastA2I(ptr);
+                validArgumentCount++;
+            }
+
+            if ((ptr = nextArg(ptr))) {
+                // check for too many arguments
+                validArgumentCount++;
+            }
+
+            if (validArgumentCount != 6) {
+                cliShowParseError();
+            } else {
+                fwAutolandApproachConfigMutable(i)->approachAlt = approachAlt;
+                fwAutolandApproachConfigMutable(i)->landAlt = landAlt;
+                fwAutolandApproachConfigMutable(i)->approachDirection = (fwAutolandApproachDirection_e)landDirection;
+                fwAutolandApproachConfigMutable(i)->landApproachHeading1 = (int16_t)heading1;
+                fwAutolandApproachConfigMutable(i)->landApproachHeading2 = (int16_t)heading2;
+                fwAutolandApproachConfigMutable(i)->isSeaLevelRef = isSeaLevelRef;
+            }
+        }
+    }
+}
+#endif
+
 #if defined(USE_SAFE_HOME)
 static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
 {
@@ -2229,6 +2339,137 @@ static void cliPid(char *cmdline) {
     }
 }
 
+static void printOsdCustomElements(uint8_t dumpMask, const osdCustomElement_t *osdCustomElements, const osdCustomElement_t *defaultosdCustomElements)
+{
+    const char *format = "osd_custom_elements %d %d %d %d %d %d %d %d %d \"%s\"";
+
+    if(CUSTOM_ELEMENTS_PARTS != 3)
+    {
+        cliPrintHashLine("Incompatible count of elements for custom OSD elements");
+    }
+
+    for (uint8_t i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
+        bool equalsDefault = false;
+
+        const osdCustomElement_t osdCustomElement = osdCustomElements[i];
+        if(defaultosdCustomElements){
+            const osdCustomElement_t defaultValue = defaultosdCustomElements[i];
+            equalsDefault =
+                    osdCustomElement.part[0].type == defaultValue.part[0].type &&
+                    osdCustomElement.part[0].value == defaultValue.part[0].value &&
+                    osdCustomElement.part[1].type == defaultValue.part[1].type &&
+                    osdCustomElement.part[1].value == defaultValue.part[1].value &&
+                    osdCustomElement.part[2].type == defaultValue.part[2].type &&
+                    osdCustomElement.part[2].value == defaultValue.part[2].value &&
+                    osdCustomElement.visibility.type == defaultValue.visibility.type &&
+                    osdCustomElement.visibility.value == defaultValue.visibility.value &&
+                    strcmp(osdCustomElement.osdCustomElementText, defaultValue.osdCustomElementText) == 0;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                osdCustomElement.part[0].type,
+                osdCustomElement.part[0].value,
+                osdCustomElement.part[1].type,
+                osdCustomElement.part[1].value,
+                osdCustomElement.part[2].type,
+                osdCustomElement.part[2].value,
+                osdCustomElement.visibility.type,
+                osdCustomElement.visibility.value,
+                osdCustomElement.osdCustomElementText
+            );
+        }
+
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            osdCustomElement.part[0].type,
+            osdCustomElement.part[0].value,
+            osdCustomElement.part[1].type,
+            osdCustomElement.part[1].value,
+            osdCustomElement.part[2].type,
+            osdCustomElement.part[2].value,
+            osdCustomElement.visibility.type,
+            osdCustomElement.visibility.value,
+            osdCustomElement.osdCustomElementText
+        );
+    }
+}
+
+static void osdCustom(char *cmdline){
+    char * saveptrMain;
+    char * saveptrParams;
+    int args[10], check = 0;
+    char text[OSD_CUSTOM_ELEMENT_TEXT_SIZE];
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printOsdCustomElements(DUMP_MASTER, osdCustomElements(0), NULL);
+    } else {
+        //split by ", first are params second is text
+        char *ptrMain = strtok_r(cmdline, "\"", &saveptrMain);
+        enum {
+            INDEX = 0,
+            PART0_TYPE,
+            PART0_VALUE,
+            PART1_TYPE,
+            PART1_VALUE,
+            PART2_TYPE,
+            PART2_VALUE,
+            VISIBILITY_TYPE,
+            VISIBILITY_VALUE,
+            ARGS_COUNT
+        };
+        char *ptrParams = strtok_r(ptrMain, " ", &saveptrParams);
+        while (ptrParams != NULL && check < ARGS_COUNT) {
+            args[check++] = fastA2I(ptrParams);
+            ptrParams = strtok_r(NULL, " ", &saveptrParams);
+        }
+
+        if (check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        //text
+        char *ptrText = strtok_r(NULL, "\"", &saveptrMain);
+        size_t copySize = 0;
+        if(ptrText != NULL){
+            copySize = MIN(strlen(ptrText), (size_t)(sizeof(text) - 1));
+            if(copySize > 0){
+                memcpy(text, ptrText, copySize);
+            }
+        }
+        text[copySize] = '\0';
+
+        int32_t i = args[INDEX];
+        if (
+                i >= 0 && i < MAX_CUSTOM_ELEMENTS &&
+                args[PART0_TYPE] >= 0 && args[PART0_TYPE] <= 7 &&
+                args[PART0_VALUE] >= 0 && args[PART0_VALUE] <= UINT8_MAX &&
+                args[PART1_TYPE] >= 0 && args[PART1_TYPE] <= 7 &&
+                args[PART1_VALUE] >= 0 && args[PART1_VALUE] <= UINT8_MAX &&
+                args[PART2_TYPE] >= 0 && args[PART2_TYPE] <= 7 &&
+                args[PART2_VALUE] >= 0 && args[PART2_VALUE] <= UINT8_MAX &&
+                args[VISIBILITY_TYPE] >= 0 && args[VISIBILITY_TYPE] <= 2 &&
+                args[VISIBILITY_VALUE] >= 0 && args[VISIBILITY_VALUE] <= UINT8_MAX
+                ) {
+            osdCustomElementsMutable(i)->part[0].type = args[PART0_TYPE];
+            osdCustomElementsMutable(i)->part[0].value = args[PART0_VALUE];
+            osdCustomElementsMutable(i)->part[1].type = args[PART1_TYPE];
+            osdCustomElementsMutable(i)->part[1].value = args[PART1_VALUE];
+            osdCustomElementsMutable(i)->part[2].type = args[PART2_TYPE];
+            osdCustomElementsMutable(i)->part[2].value = args[PART2_VALUE];
+            osdCustomElementsMutable(i)->visibility.type = args[VISIBILITY_TYPE];
+            osdCustomElementsMutable(i)->visibility.value = args[VISIBILITY_VALUE];
+            memcpy(osdCustomElementsMutable(i)->osdCustomElementText, text, OSD_CUSTOM_ELEMENT_TEXT_SIZE);
+
+            osdCustom("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+
+
 #endif
 
 #ifdef USE_SDCARD
@@ -3183,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);
     }
@@ -3193,6 +3437,7 @@ static void resetCommandBatch(void)
 {
     commandBatchActive = false;
     commandBatchError = false;
+    commandBatchErrorCount = 0;
 }
 
 static void cliBatch(char *cmdline)
@@ -3201,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) {
@@ -3799,6 +4045,11 @@ static void printConfig(const char *cmdline, bool doDiff)
         printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0));
 #endif
 
+#ifdef USE_FW_AUTOLAND
+        cliPrintHashLine("Fixed Wing Approach");
+        printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0));
+#endif
+
         cliPrintHashLine("features");
         printFeature(dumpMask, &featureConfig_Copy, featureConfig());
 
@@ -3863,6 +4114,10 @@ static void printConfig(const char *cmdline, bool doDiff)
         cliPrintHashLine("Programming: PID controllers");
         printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif
+#ifdef USE_PROGRAMMING_FRAMEWORK
+        cliPrintHashLine("OSD: custom elements");
+        printOsdCustomElements(dumpMask, osdCustomElements_CopyArray, osdCustomElements(0));
+#endif
 
         cliPrintHashLine("master");
         dumpAllValues(MASTER_VALUE, dumpMask);
@@ -4042,6 +4297,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
     CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
 #endif
+#endif
+#ifdef USE_FW_AUTOLAND
+    CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach),
 #endif
     CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
 #ifdef USE_GPS
@@ -4089,6 +4347,10 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("pid", "configurable PID controllers",
         "<#> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>\r\n"
         "\treset\r\n", cliPid),
+
+    CLI_COMMAND_DEF("osd_custom_elements", "configurable OSD custom elements",
+                    "<#> <part0 type> <part0 value> <part1 type> <part1 value> <part2 type> <part2 value> <visibility type> <visibility value> <text>\r\n"
+                    , osdCustom),
 #endif
     CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
     CLI_COMMAND_DEF("smix", "servo mixer",

src/main/fc/fc_core.c

@@ -456,7 +456,6 @@ void disarm(disarmReason_t disarmReason)
 #ifdef USE_PROGRAMMING_FRAMEWORK
         programmingPidReset();
 #endif
-
         beeper(BEEPER_DISARMING);      // emit disarm tone
 
         prearmWasReset = false;

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"
@@ -96,6 +97,8 @@
 #include "io/vtx_string.h"
 #include "io/gps_private.h"  //for MSP_SIMULATOR
 
+#include "io/osd/custom_elements.h"
+
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
@@ -462,9 +465,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU16(dst, packSensorStatus());
             sbufWriteU16(dst, averageSystemLoadPercent);
             sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile());
-            sbufWriteU8(dst, getConfigMixerProfile());
             sbufWriteU32(dst, armingFlags);
             sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
+            sbufWriteU8(dst, getConfigMixerProfile());
         }
         break;
 
@@ -478,7 +481,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
             for (int i = 0; i < 3; i++) {
 #ifdef USE_MAG
-                sbufWriteU16(dst, mag.magADC[i]);
+                sbufWriteU16(dst, lrintf(mag.magADC[i]));
 #else
                 sbufWriteU16(dst, 0);
 #endif
@@ -948,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]
@@ -1083,7 +1113,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             legacyLedConfig |= ledConfig->led_function << shiftCount;
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
-            shiftCount += 6;
+            shiftCount += 6; 
             legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
@@ -1310,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);
@@ -1518,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;
 
@@ -1637,12 +1674,30 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
 #endif
+#ifdef USE_PROGRAMMING_FRAMEWORK
+    case MSP2_INAV_CUSTOM_OSD_ELEMENTS:
+        sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS);
+        sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1);
 
+        for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
+            const osdCustomElement_t *customElement = osdCustomElements(i);
+            for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
+                sbufWriteU8(dst, customElement->part[ii].type);
+                sbufWriteU16(dst, customElement->part[ii].value);
+            }
+            sbufWriteU8(dst, customElement->visibility.type);
+            sbufWriteU16(dst, customElement->visibility.value);
+            for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
+                sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
+            }
+        }
+        break;
     default:
         return false;
     }
     return true;
 }
+#endif
 
 #ifdef USE_SAFE_HOME
 static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
@@ -1660,6 +1715,24 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
 }
 #endif
 
+#ifdef USE_FW_AUTOLAND
+static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
+{
+    const uint8_t idx = sbufReadU8(src);
+    if(idx < MAX_FW_LAND_APPOACH_SETTINGS) {
+        sbufWriteU8(dst, idx);
+        sbufWriteU32(dst, fwAutolandApproachConfig(idx)->approachAlt);
+        sbufWriteU32(dst, fwAutolandApproachConfig(idx)->landAlt);
+        sbufWriteU8(dst, fwAutolandApproachConfig(idx)->approachDirection);
+        sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading1);
+        sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading2);
+        sbufWriteU8(dst, fwAutolandApproachConfig(idx)->isSeaLevelRef);
+        return MSP_RESULT_ACK;
+    } else {
+         return MSP_RESULT_ERROR;
+    }
+}
+#endif
 
 static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
     const uint8_t idx = sbufReadU8(src);
@@ -2321,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);
@@ -2581,6 +2658,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                         if (sbufBytesRemaining(src) > 0) {
                             vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
                         }
+
+                        // //////////////////////////////////////////////////////////
+                        // this code is taken from BF, it's hack for HDZERO VTX MSP frame
+                        // API version 1.42 - this parameter kept separate since clients may already be supplying
+                        if (sbufBytesRemaining(src) >= 2) {
+                            sbufReadU16(src); //skip pitModeFreq
+                        }
+
+                        // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
+                        if (sbufBytesRemaining(src) >= 4) {
+                            uint8_t newBand = sbufReadU8(src);
+                            const uint8_t newChannel = sbufReadU8(src);
+                            vtxSettingsConfigMutable()->band = newBand;
+                            vtxSettingsConfigMutable()->channel = newChannel;
+                        }
+
+                       /* if (sbufBytesRemaining(src) >= 4) {
+                            sbufRead8(src); // freq_l
+                            sbufRead8(src); // freq_h
+                            sbufRead8(src); // band count
+                            sbufRead8(src); // channel count
+                        }*/
+                        // //////////////////////////////////////////////////////////
                     }
                 }
             }
@@ -2629,6 +2729,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_WP:
         if (dataSize == 21) {
+            
             const uint8_t msp_wp_no = sbufReadU8(src);     // get the waypoint number
             navWaypoint_t msp_wp;
             msp_wp.action = sbufReadU8(src);    // action
@@ -2640,8 +2741,23 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             msp_wp.p3 = sbufReadU16(src);       // P3
             msp_wp.flag = sbufReadU8(src);      // future: to set nav flag
             setWaypoint(msp_wp_no, &msp_wp);
-        } else
+
+#ifdef USE_FW_AUTOLAND
+            static uint8_t mmIdx = 0, fwAppraochStartIdx = 8;
+#ifdef USE_SAFE_HOME
+            fwAppraochStartIdx = MAX_SAFE_HOMES;
+#endif
+            if (msp_wp_no == 0) {
+                mmIdx = 0;
+            } else if (msp_wp.flag == NAV_WP_FLAG_LAST) {
+                mmIdx++;
+            }
+            resetFwAutolandApproach(fwAppraochStartIdx + mmIdx);
+#endif
+        } else {
             return MSP_RESULT_ERROR;
+        }
+
         break;
     case MSP2_COMMON_SET_RADAR_POS:
         if (dataSize == 19) {
@@ -2827,7 +2943,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             ledConfig->led_position = legacyConfig & 0xFF;
             ledConfig->led_function = (legacyConfig >> 8) & 0xF;
             ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
-            ledConfig->led_color = (legacyConfig >> 18) & 0xF;
+            ledConfig->led_color = (legacyConfig >> 18) & 0xF; 
             ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
             ledConfig->led_params = (legacyConfig >> 28) & 0xF;
 
@@ -3091,13 +3207,41 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_SAFE_HOME
     case MSP2_INAV_SET_SAFEHOME:
         if (dataSize == 10) {
-             uint8_t i;
+            uint8_t i;
-             if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
+            if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
-                 return MSP_RESULT_ERROR;
+                return MSP_RESULT_ERROR;
-             }
+            }
-             safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
+            safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
-             safeHomeConfigMutable(i)->lat = sbufReadU32(src);
+            safeHomeConfigMutable(i)->lat = sbufReadU32(src);
-             safeHomeConfigMutable(i)->lon = sbufReadU32(src);
+            safeHomeConfigMutable(i)->lon = sbufReadU32(src);
+#ifdef USE_FW_AUTOLAND
+            resetFwAutolandApproach(i);
+#endif
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+#endif
+
+#ifdef USE_FW_AUTOLAND
+    case MSP2_INAV_SET_FW_APPROACH:
+        if (dataSize == 15) {
+            uint8_t i;
+            if (!sbufReadU8Safe(&i, src) || i >= MAX_FW_LAND_APPOACH_SETTINGS) {
+                return MSP_RESULT_ERROR;
+            }
+            fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src);
+            fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src);
+            fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src);
+             
+            int16_t head1 = 0, head2 = 0;
+            if (sbufReadI16Safe(&head1, src)) {
+                fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
+            }
+            if (sbufReadI16Safe(&head2, src)) {
+                fwAutolandApproachConfigMutable(i)->landApproachHeading2 = head2;
+            }
+            fwAutolandApproachConfigMutable(i)->isSeaLevelRef = sbufReadU8(src);
         } else {
             return MSP_RESULT_ERROR;
         }
@@ -3147,6 +3291,25 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;    
 
 #endif
+#ifdef USE_PROGRAMMING_FRAMEWORK
+    case MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS:
+        sbufReadU8Safe(&tmp_u8, src);
+        if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (MAX_CUSTOM_ELEMENTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) {
+            for (int i = 0; i < CUSTOM_ELEMENTS_PARTS; i++) {
+                osdCustomElementsMutable(tmp_u8)->part[i].type = sbufReadU8(src);
+                osdCustomElementsMutable(tmp_u8)->part[i].value = sbufReadU16(src);
+            }
+            osdCustomElementsMutable(tmp_u8)->visibility.type = sbufReadU8(src);
+            osdCustomElementsMutable(tmp_u8)->visibility.value = sbufReadU16(src);
+            for (int i = 0; i < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; i++) {
+                osdCustomElementsMutable(tmp_u8)->osdCustomElementText[i] = sbufReadU8(src);
+            }
+            osdCustomElementsMutable(tmp_u8)->osdCustomElementText[OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1] = '\0';
+        } else{
+            return MSP_RESULT_ERROR;
+        }
+
+        break;
 
 
     default:
@@ -3154,6 +3317,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
     }
     return MSP_RESULT_ACK;
 }
+#endif
 
 static const setting_t *mspReadSetting(sbuf_t *src)
 {
@@ -3396,7 +3560,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
 bool isOSDTypeSupportedBySimulator(void)
 {
 #ifdef USE_OSD
-	displayPort_t *osdDisplayPort = osdGetDisplayPort();
+    displayPort_t *osdDisplayPort = osdGetDisplayPort();
 	return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar);
 #else
     return false;
@@ -3616,135 +3780,137 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         *ret = mspFcSafeHomeOutCommand(dst, src);
         break;
 #endif
-
+#ifdef USE_FW_AUTOLAND
+    case MSP2_INAV_FW_APPROACH:
+        *ret = mspFwApproachOutCommand(dst, src);
+        break;
+#endif
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
-		tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
+        tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
-
+        
         // Check the MSP version of simulator
-		if (tmp_u8 != SIMULATOR_MSP_VERSION) {
+        if (tmp_u8 != SIMULATOR_MSP_VERSION) {
             break;
         }
 
-		simulatorData.flags = sbufReadU8(src);
+        simulatorData.flags = sbufReadU8(src);
 
         if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) {
 
-			if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
+            if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
-				DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
+                DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
 
 #ifdef USE_BARO
             if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
-				baroStartCalibration();
+                baroStartCalibration();
             }
 #endif
 #ifdef USE_MAG
-				DISABLE_STATE(COMPASS_CALIBRATED);
+                DISABLE_STATE(COMPASS_CALIBRATED);
-				compassInit();
+                compassInit();
 #endif
-				simulatorData.flags = HITL_RESET_FLAGS;
+                simulatorData.flags = HITL_RESET_FLAGS;
                 // Review: Many states were affected. Reboot?
 
-				disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
+                disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
 			}
         } else {
-			if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
+            if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
 #ifdef USE_BARO
                 if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
                     sensorsSet(SENSOR_BARO);
                     setTaskEnabled(TASK_BARO, true);
                     DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
-				    baroStartCalibration();
+                    baroStartCalibration();
                 }
 #endif
 
 #ifdef USE_MAG
-				if (compassConfig()->mag_hardware != MAG_NONE) {
+                if (compassConfig()->mag_hardware != MAG_NONE) {
-					sensorsSet(SENSOR_MAG);
+                    sensorsSet(SENSOR_MAG);
-					ENABLE_STATE(COMPASS_CALIBRATED);
+                    ENABLE_STATE(COMPASS_CALIBRATED);
-					DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
+                    DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
-					mag.magADC[X] = 800;
+                    mag.magADC[X] = 800;
-					mag.magADC[Y] = 0;
+                    mag.magADC[Y] = 0;
-					mag.magADC[Z] = 0;
+                    mag.magADC[Z] = 0;
-				}
+                }
 #endif
-				ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
+                ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
                 ENABLE_STATE(ACCELEROMETER_CALIBRATED);
-				LOG_DEBUG(SYSTEM, "Simulator enabled");
+                LOG_DEBUG(SYSTEM, "Simulator enabled");
-			}
+            }
 
-			if (dataSize >= 14) {
+            if (dataSize >= 14) {
 
-				if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
+                if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
-					gpsSol.fixType = sbufReadU8(src);
+                    gpsSolDRV.fixType = sbufReadU8(src);
-					gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
+                    gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
-					gpsSol.flags.hasNewData = true;
+                    gpsSolDRV.numSat = sbufReadU8(src);
-					gpsSol.numSat = sbufReadU8(src);
 
-					if (gpsSol.fixType != GPS_NO_FIX) {
+                    if (gpsSolDRV.fixType != GPS_NO_FIX) {
-						gpsSol.flags.validVelNE = true;
+                        gpsSolDRV.flags.validVelNE = true;
-						gpsSol.flags.validVelD = true;
+                        gpsSolDRV.flags.validVelD = true;
-						gpsSol.flags.validEPE = true;
+                        gpsSolDRV.flags.validEPE = true;
-						gpsSol.flags.validTime = false;
+                        gpsSolDRV.flags.validTime = false;
 
-						gpsSol.llh.lat = sbufReadU32(src);
+                        gpsSolDRV.llh.lat = sbufReadU32(src);
-						gpsSol.llh.lon = sbufReadU32(src);
+                        gpsSolDRV.llh.lon = sbufReadU32(src);
-						gpsSol.llh.alt = sbufReadU32(src);
+                        gpsSolDRV.llh.alt = sbufReadU32(src);
-						gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src);
-						gpsSol.groundCourse = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src);
 
-						gpsSol.velNED[X] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src);
-						gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src);
-						gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
+                        gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src);
 
-						gpsSol.eph = 100;
+                        gpsSolDRV.eph = 100;
-						gpsSol.epv = 100;
+                        gpsSolDRV.epv = 100;
-
+                    } else {
-						ENABLE_STATE(GPS_FIX);
+                        sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
-					} else {
+                    }
-						sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
-					}
                     // Feed data to navigation
-					gpsProcessNewSolutionData();
+                    gpsProcessNewDriverData();
-				} else {
+                    gpsProcessNewSolutionData(false);
-					sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
+                } else {
-				}
+                    sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
-
+                }
-				if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
-					attitude.values.roll = (int16_t)sbufReadU16(src);
-					attitude.values.pitch = (int16_t)sbufReadU16(src);
-					attitude.values.yaw = (int16_t)sbufReadU16(src);
-				} else {
-					sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
-				}
 
+                if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) {
+                    attitude.values.roll = (int16_t)sbufReadU16(src);
+                    attitude.values.pitch = (int16_t)sbufReadU16(src);
+                    attitude.values.yaw = (int16_t)sbufReadU16(src);
+                } else {
+                    sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
+                }
+                
                 // Get the acceleration in 1G units
-				acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
+                acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
-				acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
+                acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
-				acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
+                acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
-				acc.accVibeSq[X] = 0.0f;
+                acc.accVibeSq[X] = 0.0f;
-				acc.accVibeSq[Y] = 0.0f;
+                acc.accVibeSq[Y] = 0.0f;
-				acc.accVibeSq[Z] = 0.0f;
+                acc.accVibeSq[Z] = 0.0f;
-
+                
                 // Get the angular velocity in DPS
-				gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
+                gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
-				gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
+                gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
-				gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
+                gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
 
-				if (sensors(SENSOR_BARO)) {
+                if (sensors(SENSOR_BARO)) {
-					baro.baroPressure = (int32_t)sbufReadU32(src);
+                    baro.baroPressure = (int32_t)sbufReadU32(src);
-					baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
+                    baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP);
-				} else {
+                } else {
-					sbufAdvance(src, sizeof(uint32_t));
+                    sbufAdvance(src, sizeof(uint32_t));
-				}
+                }
 
-				if (sensors(SENSOR_MAG)) {
+                if (sensors(SENSOR_MAG)) {
-					mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  // 16000 / 20 = 800uT
+                    mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  // 16000 / 20 = 800uT
-					mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;   //note that mag failure is simulated by setting all readings to zero
+                    mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;   //note that mag failure is simulated by setting all readings to zero
-					mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
+                    mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
-				} else {
+                } else {
-					sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
+                    sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
-				}
+                }
 
                 if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) {
                     simulatorData.vbat = sbufReadU8(src);
@@ -3754,44 +3920,44 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
                 if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
                     simulatorData.airSpeed = sbufReadU16(src);
-			    } else {
+                } else {
                     if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
-                        sbufReadU16(src);
+                        sbufReadU16(src); 
                     }
                 }
 
                 if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
                     simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
                 }
-			} else {
+            } else {
-				DISABLE_STATE(GPS_FIX);
+                DISABLE_STATE(GPS_FIX);
-			}
+            }
-		}
+        }
 
-		sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
+        sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]);
-		sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
+        sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]);
-		sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
+        sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]);
-		sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
+        sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500));
 
-		simulatorData.debugIndex++;
+        simulatorData.debugIndex++;
-		if (simulatorData.debugIndex == 8) {
+        if (simulatorData.debugIndex == 8) {
-			simulatorData.debugIndex = 0;
+            simulatorData.debugIndex = 0;
-		}
+        }
 
-		tmp_u8 = simulatorData.debugIndex |
+        tmp_u8 = simulatorData.debugIndex |
-			((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
+            ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
-			(ARMING_FLAG(ARMED) ? 64 : 0) |
+            (ARMING_FLAG(ARMED) ? 64 : 0) |
-			(!feature(FEATURE_OSD) ? 32: 0) |
+            (!feature(FEATURE_OSD) ? 32: 0) |
-			(!isOSDTypeSupportedBySimulator() ? 16 : 0);
+            (!isOSDTypeSupportedBySimulator() ? 16 : 0);
 
-		sbufWriteU8(dst, tmp_u8);
+        sbufWriteU8(dst, tmp_u8);
-		sbufWriteU32(dst, debug[simulatorData.debugIndex]);
+        sbufWriteU32(dst, debug[simulatorData.debugIndex]);
 
-		sbufWriteU16(dst, attitude.values.roll);
+        sbufWriteU16(dst, attitude.values.roll);
-		sbufWriteU16(dst, attitude.values.pitch);
+        sbufWriteU16(dst, attitude.values.pitch);
-		sbufWriteU16(dst, attitude.values.yaw);
+        sbufWriteU16(dst, attitude.values.yaw);
 
-		mspWriteSimulatorOSD(dst);
+        mspWriteSimulatorOSD(dst);
 
         *ret = MSP_RESULT_ACK;
         break;

src/main/fc/fc_msp_box.c

@@ -323,7 +323,7 @@ void initActiveBoxIds(void)
     ADD_ACTIVE_BOX(BOXKILLSWITCH);
     ADD_ACTIVE_BOX(BOXFAILSAFE);
 
-#ifdef USE_RCDEVICE
+#if defined(USE_RCDEVICE) || defined(USE_MSP_DISPLAYPORT)
     ADD_ACTIVE_BOX(BOXCAMERA1);
     ADD_ACTIVE_BOX(BOXCAMERA2);
     ADD_ACTIVE_BOX(BOXCAMERA3);

src/main/fc/fc_tasks.c

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

src/main/fc/multifunction.h

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

src/main/fc/rc_controls.c

@@ -119,8 +119,7 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e
         value = rcCommand[THROTTLE];
     }
 
-    const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
+    bool midThrottle = value > (reversibleMotorsConfig()->deadband_low) && value < (reversibleMotorsConfig()->deadband_high);
-    bool midThrottle = value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband);
     if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) {
         return THROTTLE_LOW;
     }

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),
@@ -141,6 +144,7 @@ typedef enum {
     ANTI_WINDUP_DEACTIVATED             = (1 << 25),
     LANDING_DETECTED                    = (1 << 26),
     IN_FLIGHT_EMERG_REARM               = (1 << 27),
+    TAILSITTER                          = (1 << 28), //offset the pitch angle by 90 degrees
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/fc/settings.yaml

@@ -50,7 +50,7 @@ tables:
     values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"]
     enum: sbasMode_e
   - name: gps_dyn_model
-    values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"]
+    values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G"]
     enum: gpsDynModel_e
   - name: reset_type
     values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
@@ -588,7 +588,7 @@ groups:
     members:
       - name: pitot_hardware
         description: "Selection of pitot hardware."
-        default_value: "VIRTUAL"
+        default_value: "NONE"
         table: pitot_hardware
       - name: pitot_lpf_milli_hz
         min: 0
@@ -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
@@ -1185,6 +1191,11 @@ groups:
         field: mixer_config.switchTransitionTimer
         min: 0
         max: 200
+      - name: tailsitter_orientation_offset
+        description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode"
+        default_value: OFF
+        field: mixer_config.tailsitterOrientationOffset
+        type: bool
 
 
 
@@ -1463,6 +1474,12 @@ groups:
         default_value: ADAPTIVE
         field: inertia_comp_method
         table: imu_inertia_comp_method
+      - name: ahrs_gps_yaw_weight
+        description: "Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass"
+        default_value: 100
+        field: gps_yaw_weight
+        min: 0
+        max: 500
 
   - name: PG_ARMING_CONFIG
     type: armingConfig_t
@@ -1608,8 +1625,8 @@ groups:
         table: gps_sbas_mode
         type: uint8_t
       - name: gps_dyn_model
-        description: "GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying."
+        description: "GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying."
-        default_value: "AIR_1G"
+        default_value: "AIR_2G"
         field: dynModel
         table: gps_dyn_model
         type: uint8_t
@@ -1730,7 +1747,7 @@ groups:
         min: RPYL_PID_MIN
         max: RPYL_PID_MAX
       - name: mc_cd_pitch
-        description: "Multicopter Control Derivative gain for PITCH"
+        description: "Multicopter Control Derivative gain for PITCH (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough."
         default_value: 60
         field: bank_mc.pid[PID_PITCH].FF
         min: RPYL_PID_MIN
@@ -1754,7 +1771,7 @@ groups:
         min: RPYL_PID_MIN
         max: RPYL_PID_MAX
       - name: mc_cd_roll
-        description: "Multicopter Control Derivative gain for ROLL"
+        description: "Multicopter Control Derivative gain for ROLL (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough."
         default_value: 60
         field: bank_mc.pid[PID_ROLL].FF
         min: RPYL_PID_MIN
@@ -1778,7 +1795,7 @@ groups:
         min: RPYL_PID_MIN
         max: RPYL_PID_MAX
       - name: mc_cd_yaw
-        description: "Multicopter Control Derivative gain for YAW"
+        description: "Multicopter Control Derivative gain for YAW (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough."
         default_value: 60
         field: bank_mc.pid[PID_YAW].FF
         min: RPYL_PID_MIN
@@ -2201,7 +2218,7 @@ groups:
         table: pidTypeTable
         default_value: AUTO
       - name: mc_cd_lpf_hz
-        description: "Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky"
+        description: "Cutoff frequency for Control Derivative. This controls the cutoff for the LPF that is applied to the CD (Feed Forward) signal to the PID controller. Lower value will produce a smoother CD gain to the controller, but it will be more delayed. Higher values will produce CD gain that may have more noise in the signal depending on your RC link but wil be less delayed."
         default_value: 30
         field: controlDerivativeLpfHz
         min: 0
@@ -2291,6 +2308,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"
@@ -2311,7 +2334,7 @@ groups:
         field: w_z_surface_p
         min: 0
         max: 100
-        default_value: 3.5
+        default_value: 3.5 
       - name: inav_w_z_surface_v
         field: w_z_surface_v
         min: 0
@@ -2328,13 +2351,13 @@ groups:
         max: 100
         default_value: 2.0
       - name: inav_w_z_baro_p
-        description: "Weight of barometer measurements in estimated altitude and climb rate"
+        description: "Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors."
         field: w_z_baro_p
         min: 0
         max: 10
         default_value: 0.35
       - name: inav_w_z_gps_p
-        description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes"
+        description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
         field: w_z_gps_p
         min: 0
         max: 10
@@ -2369,11 +2392,6 @@ groups:
         field: w_xy_res_v
         min: 0
         max: 10
-      - name: inav_w_xyz_acc_p
-        field: w_xyz_acc_p
-        min: 0
-        max: 1
-        default_value: 1.0
       - name: inav_w_acc_bias
         description: "Weight for accelerometer drift estimation"
         default_value: 0.01
@@ -2492,30 +2510,24 @@ groups:
         field: general.auto_speed
         min: 10
         max: 2000
+      - name: nav_min_ground_speed
+        description: "Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s."
+        default_value: 7
+        field: general.min_ground_speed
+        min: 6
+        max: 50
       - name: nav_max_auto_speed
         description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]"
         default_value: 1000
         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
@@ -2598,6 +2610,12 @@ groups:
         default_value: "ALWAYS"
         field: general.flags.rth_allow_landing
         table: nav_rth_allow_landing
+      - name: nav_rth_fs_landing_delay
+        description: "If landing is active on Failsafe and this is above 0. The aircraft will hover or loiter for X seconds before performing the landing. If the battery enters the warning or critical levels, the land will proceed. Default = 0 [seconds]"
+        default_value: 0
+        min: 0
+        max: 1800
+        field: general.rth_fs_landing_delay
       - name: nav_rth_alt_mode
         description: "Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details"
         default_value: "AT_LEAST"
@@ -2676,7 +2694,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
@@ -2686,6 +2704,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
@@ -2771,6 +2801,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
@@ -2824,15 +2860,12 @@ groups:
         field: fw.control_smoothness
         min: 0
         max: 9
-
       - name: nav_fw_land_dive_angle
         description: "Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees"
         default_value: 2
         field: fw.land_dive_angle
-        condition: NAV_FIXED_WING_LANDING
         min: -20
         max: 20
-
       - name: nav_fw_launch_velocity
         description: "Forward velocity threshold for swing-launch detection [cm/s]"
         default_value: 300
@@ -2909,10 +2942,10 @@ groups:
         default_value: OFF
         field: fw.launch_manual_throttle
         type: bool
-      - name: nav_fw_launch_abort_deadband
+      - name: nav_fw_launch_land_abort_deadband
-        description: "Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch."
+        description: "Launch and landing abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch or landing."
         default_value: 100
-        field: fw.launch_abort_deadband
+        field: fw.launch_land_abort_deadband
         min: 2
         max: 250
       - name: nav_fw_allow_manual_thr_increase
@@ -3432,6 +3465,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
@@ -3816,10 +3874,10 @@ groups:
     condition: USE_VTX_SMARTAUDIO || USE_VTX_TRAMP
     members:
       - name: vtx_band
-        description: "Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race."
+        description: "Configure the VTX band. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race."
         default_value: 1
         field: band
-        min: VTX_SETTINGS_NO_BAND
+        min: VTX_SETTINGS_MIN_BAND
         max: VTX_SETTINGS_MAX_BAND
       - name: vtx_channel
         description: "Channel to use within the configured `vtx_band`. Valid values are [1, 8]."
@@ -4080,3 +4138,51 @@ groups:
         field: osd_joystick_enter
         min: 0
         max: 100
+
+  - name: PG_FW_AUTOLAND_CONFIG
+    type: navFwAutolandConfig_t
+    headers: ["navigation/navigation.h"]
+    condition: USE_FW_AUTOLAND
+    members: 
+      - name: nav_fw_land_approach_length
+        description: "Length of the final approach"
+        default_value: 35000
+        field: approachLength
+        min: 100
+        max: 100000
+      - name: nav_fw_land_final_approach_pitch2throttle_mod
+        description: "Modifier for pitch to throttle ratio at final approach. In Percent."
+        default_value: 100
+        field: finalApproachPitchToThrottleMod
+        min: 100
+        max: 400
+      - name: nav_fw_land_glide_alt
+        description: "Initial altitude of the glide phase"
+        default_value: 200
+        field: glideAltitude
+        min: 100
+        max: 5000
+      - name: nav_fw_land_flare_alt
+        description: "Initial altitude of the flare phase"
+        default_value: 150
+        field: flareAltitude
+        min: 0
+        max: 10000
+      - name: nav_fw_land_glide_pitch
+        description: "Pitch value for glide phase. In degrees."
+        default_value: 0
+        field: glidePitch
+        min: -15
+        max: 45
+      - name: nav_fw_land_flare_pitch
+        description: "Pitch value for flare phase. In degrees"
+        default_value: 8
+        field: flarePitch
+        min: -15
+        max: 45
+      - name: nav_fw_land_max_tailwind
+        description: "Max. tailwind (in cm/s) if no landing direction with downwind is available"
+        default_value: 140
+        field: maxTailwind
+        min: 0
+        max: 3000

src/main/flight/failsafe.c

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

src/main/flight/failsafe.h

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

src/main/flight/imu.c

@@ -49,6 +49,7 @@
 
 #include "flight/imu.h"
 #include "flight/mixer.h"
+#include "flight/mixer_profile.h"
 #include "flight/pid.h"
 #if defined(USE_WIND_ESTIMATOR)
 #include "flight/wind_estimator.h"
@@ -77,6 +78,9 @@
 
 #define SPIN_RATE_LIMIT             20
 #define MAX_ACC_NEARNESS            0.2    // 20% or G error soft-accepted (0.8-1.2G)
+#define MAX_MAG_NEARNESS            0.25    // 25% or magnetic field error soft-accepted (0.75-1.25)
+#define COS10DEG 0.985f
+#define COS20DEG 0.940f
 #define IMU_ROTATION_LPF         3       // Hz
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
@@ -111,6 +115,8 @@ FASTRAM bool gpsHeadingInitialized;
 
 FASTRAM bool imuUpdated = false;
 
+static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF);
+
 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
 
 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
@@ -122,7 +128,8 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT,
     .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
     .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
-    .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT
+    .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT,
+    .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT
 );
 
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
@@ -323,7 +330,20 @@ 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)
+{
+    float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
+    float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
+    const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f;
+    wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
+    return wCoG;
 }
 
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
@@ -339,11 +359,15 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
     /* Step 1: Yaw correction */
     // Use measured magnetic field vector
     if (magBF || useCOG) {
-        static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } };
+        float wMag = 1.0f;
+        float wCoG = 1.0f;
+        if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;}
 
-        fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
 
         if (magBF && vectorNormSquared(magBF) > 0.01f) {
+            wMag *= bellCurve((fast_fsqrtf(vectorNormSquared(magBF)) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS);
             fpVector3_t vMag;
 
             // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
@@ -369,13 +393,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
                 // Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
                 // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
-                vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);
+                vectorCrossProduct(&vMagErr, &vMag, &vCorrectedMagNorth);
 
                 // Rotate error back into body frame
-                quaternionRotateVector(&vErr, &vErr, &orientation);
+                quaternionRotateVector(&vMagErr, &vMagErr, &orientation);
             }
         }
-        else if (useCOG) {
+        if (useCOG) {
+            fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
+            //vForward as trust vector
+            if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){
+                vForward.z = 1.0f;
+            }else{
+                vForward.x = 1.0f;
+            }
             fpVector3_t vHeadingEF;
 
             // Use raw heading error (from GPS or whatever else)
@@ -386,6 +417,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
             // (Rxx; Ryx) - measured (estimated) heading vector (EF)
             // (-cos(COG), sin(COG)) - reference heading vector (EF)
 
+            float airSpeed = gpsSol.groundSpeed;
             // Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards.
             fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
 #if defined(USE_WIND_ESTIMATOR)
@@ -395,12 +427,21 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed);
                 vCoG.x += getEstimatedWindSpeed(X);
                 vCoG.y -= getEstimatedWindSpeed(Y);
+                airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG));
                 vectorNormalize(&vCoG, &vCoG);
             }
 #endif
-
+            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f);
             // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
             quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
+
+            if (STATE(MULTIROTOR)){
+                //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
+                wCoG *= imuCalculateMcCogWeight();
+                //scale accroading to multirotor`s tilt angle
+                wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f);
+                //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed
+            }
             vHeadingEF.z = 0.0f;
 
             // We zeroed out vHeadingEF.z -  make sure the whole vector didn't go to zero
@@ -409,13 +450,16 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorNormalize(&vHeadingEF, &vHeadingEF);
 
                 // error is cross product between reference heading and estimated heading (calculated in EF)
-                vectorCrossProduct(&vErr, &vCoG, &vHeadingEF);
+                vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF);
 
                 // Rotate error back into body frame
-                quaternionRotateVector(&vErr, &vErr, &orientation);
+                quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation);
             }
         }
-
+        fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        vectorScale(&vMagErr, &vMagErr, wMag);
+        vectorScale(&vCoGErr, &vCoGErr, wCoG);
+        vectorAdd(&vErr, &vMagErr, &vCoGErr);
         // Compute and apply integral feedback if enabled
         if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
             // Stop integrating if spinning beyond the certain limit
@@ -535,10 +579,10 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
     }
 }
 
-static float imuCalculateAccelerometerWeightNearness(void)
+static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF)
 {
     fpVector3_t accBFNorm;
-    vectorScale(&accBFNorm, &compansatedGravityBF, 1.0f / GRAVITY_CMSS);
+    vectorScale(&accBFNorm, accBF, 1.0f / GRAVITY_CMSS);
     const float accMagnitudeSq = vectorNormSquared(&accBFNorm);
     const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
     return accWeight_Nearness;
@@ -660,6 +704,33 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
     lastspeed = currentspeed;
 }
 
+fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){
+    static bool firstRun = true;
+    static fpQuaternion_t qNormal2Tail;
+    static fpQuaternion_t qTail2Normal;
+    if(firstRun){
+        fpAxisAngle_t axisAngle;
+        axisAngle.axis.x = 0;
+        axisAngle.axis.y = 1;
+        axisAngle.axis.z = 0;
+        axisAngle.angle = DEGREES_TO_RADIANS(-90);
+        axisAngleToQuaternion(&qNormal2Tail, &axisAngle);
+        quaternionConjugate(&qTail2Normal, &qNormal2Tail);
+        firstRun = false;
+    }
+    return normal2tail ? &qNormal2Tail : &qTail2Normal;
+}
+
+void imuUpdateTailSitter(void)
+{
+    static bool lastTailSitter=false;
+    if (((bool)STATE(TAILSITTER)) != lastTailSitter){
+        fpQuaternion_t* rotation_for_tailsitter= getTailSitterQuaternion(STATE(TAILSITTER));
+        quaternionMultiply(&orientation, &orientation, rotation_for_tailsitter);
+    }
+    lastTailSitter = STATE(TAILSITTER);
+}
+
 static void imuCalculateEstimatedAttitude(float dT)
 {
 #if defined(USE_MAG)
@@ -672,36 +743,29 @@ static void imuCalculateEstimatedAttitude(float dT)
     bool useMag = false;
     bool useCOG = false;
 #if defined(USE_GPS)
-    if (STATE(FIXED_WING_LEGACY)) {
+    bool canUseCOG = isGPSHeadingValid();
-        bool canUseCOG = isGPSHeadingValid();
 
-        // Prefer compass (if available)
+    // Use compass (if available)
-        if (canUseMAG) {
+    if (canUseMAG) {
-            useMag = true;
+        useMag = true;
-            gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
+        gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
-        }
-        else if (canUseCOG) {
-            if (gpsHeadingInitialized) {
-                courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-                useCOG = true;
-            }
-            else {
-                // Re-initialize quaternion from known Roll, Pitch and GPS heading
-                imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
-                gpsHeadingInitialized = true;
-
-                // Force reset of heading hold target
-                resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
-            }
-        } else if (!ARMING_FLAG(ARMED)) {
-            gpsHeadingInitialized = false;
-        }
     }
-    else {
+    // Use GPS (if available)
-        // Multicopters don't use GPS heading
+    if (canUseCOG) {
-        if (canUseMAG) {
+        if (gpsHeadingInitialized) {
-            useMag = true;
+            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+            useCOG = true;
         }
+        else if (!canUseMAG) {
+            // Re-initialize quaternion from known Roll, Pitch and GPS heading
+            imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
+            gpsHeadingInitialized = true;
+
+            // Force reset of heading hold target
+            resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+        }
+    } else if (!ARMING_FLAG(ARMED)) {
+        gpsHeadingInitialized = false;
     }
 
     imuCalculateFilters(dT);
@@ -751,7 +815,7 @@ static void imuCalculateEstimatedAttitude(float dT)
     }
     compansatedGravityBF = imuMeasuredAccelBF
 #endif
-    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness();
+    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF);
     accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler);
     const bool useAcc = (accWeight > 0.001f);
 
@@ -763,6 +827,7 @@ static void imuCalculateEstimatedAttitude(float dT)
                             useCOG, courseOverGround,
                             accWeight,
                             magWeight);
+    imuUpdateTailSitter();
     imuUpdateEulerAngles();
 }
 
@@ -806,6 +871,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         acc.accADCf[Z] = 0.0f;
     }
 }
+ 
 
 bool isImuReady(void)
 {
@@ -814,7 +880,7 @@ bool isImuReady(void)
 
 bool isImuHeadingValid(void)
 {
-    return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized);
+    return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || gpsHeadingInitialized;
 }
 
 float calculateCosTiltAngle(void)

src/main/flight/imu.h

@@ -54,6 +54,7 @@ typedef struct imuConfig_s {
     uint8_t acc_ignore_slope;
     uint8_t gps_yaw_windcomp;
     uint8_t inertia_comp_method;
+    uint16_t gps_yaw_weight;
 } imuConfig_t;
 
 PG_DECLARE(imuConfig_t, imuConfig);

src/main/flight/mixer.c

@@ -161,6 +161,7 @@ void mixerUpdateStateFlags(void)
     DISABLE_STATE(BOAT);
     DISABLE_STATE(AIRPLANE);
     DISABLE_STATE(MOVE_FORWARD_ONLY);
+    DISABLE_STATE(TAILSITTER);
 
     if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
         ENABLE_STATE(FIXED_WING_LEGACY);
@@ -186,6 +187,12 @@ void mixerUpdateStateFlags(void)
         ENABLE_STATE(ALTITUDE_CONTROL);
     }
 
+    if (currentMixerConfig.tailsitterOrientationOffset) {
+        ENABLE_STATE(TAILSITTER);
+    } else {
+        DISABLE_STATE(TAILSITTER);
+    }
+
     if (currentMixerConfig.hasFlaps) {
         ENABLE_STATE(FLAPERON_AVAILABLE);
     } else {

src/main/flight/mixer_profile.c

@@ -53,6 +53,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
                          .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT,
                          .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT,
                          .switchTransitionTimer =  SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT,
+                         .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT,
                      });
         for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++)
         {

src/main/flight/mixer_profile.h

@@ -18,6 +18,7 @@ typedef struct mixerConfig_s {
     bool PIDProfileLinking;
     bool automated_switch;
     int16_t switchTransitionTimer;
+    bool tailsitterOrientationOffset;
 } mixerConfig_t;
 typedef struct mixerProfile_s {
     mixerConfig_t mixer_config;

src/main/flight/pid.c

@@ -587,10 +587,18 @@ int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis)
 
 static float computePidLevelTarget(flight_dynamics_index_t axis) {
     // This is ROLL/PITCH, run ANGLE/HORIZON controllers
+#ifdef USE_PROGRAMMING_FRAMEWORK
+    float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), pidProfile()->max_angle_inclination[axis]);
+#else
     float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
+#endif
 
     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
+#ifdef USE_FW_AUTOLAND
+    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !isFwLandInProgess()) {
+#else
     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
+#endif
         angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
     }
 
@@ -1143,7 +1151,6 @@ void FAST_CODE pidController(float dT)
     }
 
     for (int axis = 0; axis < 3; axis++) {
-        // Step 1: Calculate gyro rates
         pidState[axis].gyroRate = gyro.gyroADCf[axis];
 
         // Step 2: Read target
@@ -1181,6 +1188,11 @@ void FAST_CODE pidController(float dT)
             // If axis angle override, get the correct angle from Logic Conditions
             float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
 
+            //apply 45 deg offset for tailsitter when isMixerTransitionMixing is activated
+            if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH){
+                angleTarget += DEGREES_TO_DECIDEGREES(45);
+            }
+
             if (STATE(AIRPLANE)) {  // update anglehold mode
                 updateAngleHold(&angleTarget, axis);
             }
@@ -1316,7 +1328,7 @@ void pidInit(void)
     navPidInit(
         &fixedWingLevelTrimController,
         0.0f,
-        (float)pidProfile()->fixedWingLevelTrimGain / 100.0f,
+        (float)pidProfile()->fixedWingLevelTrimGain / 200.0f,
         0.0f,
         0.0f,
         2.0f,
@@ -1372,8 +1384,8 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
      */
     pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
 
-    // Iterm should freeze when conditions for setting level trim aren't met
+    // Iterm should freeze when conditions for setting level trim aren't met or time since last expected update too long ago
-    if (!isFixedWingLevelTrimActive()) {
+    if (!isFixedWingLevelTrimActive() || (dT > 5.0f * US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)))) {
         flags |= PID_FREEZE_INTEGRATOR;
     }
 

src/main/flight/wind_estimator.c

@@ -38,6 +38,8 @@
 
 #include "flight/imu.h"
 
+#include "navigation/navigation_pos_estimator_private.h"
+
 #include "io/gps.h"
 
 
@@ -52,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)
@@ -83,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;
     }
 
@@ -105,9 +114,9 @@ void updateWindEstimator(timeUs_t currentTimeUs)
 
     // Get current 3D velocity from GPS in cm/s
     // relative to earth frame
-    groundVelocity[X] = gpsSol.velNED[X];
+    groundVelocity[X] = posEstimator.gps.vel.x;
-    groundVelocity[Y] = gpsSol.velNED[Y];
+    groundVelocity[Y] = posEstimator.gps.vel.y;
-    groundVelocity[Z] = gpsSol.velNED[Z];
+    groundVelocity[Z] = posEstimator.gps.vel.z;
 
     // Fuselage direction in earth frame
     fuselageDirection[X] = HeadVecEFFiltered.x;

src/main/io/adsb.c

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

src/main/io/adsb.h

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

src/main/io/displayport_msp_bf_compat.c

@@ -578,6 +578,48 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
     case (SYM_AH_V_START+4):
     case (SYM_AH_V_START+5):
         return BF_SYM_AH_BAR9_4;
+
+    // BF for ESP_RADAR Symbols
+    case SYM_HUD_CARDINAL:
+        return BF_SYM_ARROW_SOUTH;
+    case SYM_HUD_CARDINAL + 1:
+        return BF_SYM_ARROW_16;
+    case SYM_HUD_CARDINAL + 2:
+        return BF_SYM_ARROW_15;
+    case SYM_HUD_CARDINAL + 3:
+        return BF_SYM_ARROW_WEST;
+    case SYM_HUD_CARDINAL + 4:
+        return BF_SYM_ARROW_12;
+    case SYM_HUD_CARDINAL + 5:
+        return BF_SYM_ARROW_11;
+    case SYM_HUD_CARDINAL + 6:
+        return BF_SYM_ARROW_NORTH;
+    case SYM_HUD_CARDINAL + 7:
+        return BF_SYM_ARROW_7;
+    case SYM_HUD_CARDINAL + 8:
+        return BF_SYM_ARROW_6;
+    case SYM_HUD_CARDINAL + 9:
+        return BF_SYM_ARROW_EAST;
+    case SYM_HUD_CARDINAL + 10:
+        return BF_SYM_ARROW_3;
+    case SYM_HUD_CARDINAL + 11:
+        return BF_SYM_ARROW_2;
+
+    case SYM_HUD_ARROWS_R3:
+        return BF_SYM_AH_RIGHT;
+    case SYM_HUD_ARROWS_L3:
+        return BF_SYM_AH_LEFT;
+
+    case SYM_HUD_SIGNAL_0:
+        return BF_SYM_AH_BAR9_1;
+    case SYM_HUD_SIGNAL_1:
+        return BF_SYM_AH_BAR9_3;
+    case SYM_HUD_SIGNAL_2:
+        return BF_SYM_AH_BAR9_4;
+    case SYM_HUD_SIGNAL_3:
+        return BF_SYM_AH_BAR9_5;
+    case SYM_HUD_SIGNAL_4:
+        return BF_SYM_AH_BAR9_7;
 /*
     case SYM_VARIO_UP_2A:
         return BF_SYM_VARIO_UP_2A;

src/main/io/displayport_msp_osd.c

@@ -293,7 +293,8 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
         uint8_t len = 4;
         do {
             bitArrayClr(dirty, pos);
-            subcmd[len++] = isBfCompatibleVideoSystem(osdConfig()) ? getBfCharacter(screen[pos++], page): screen[pos++];
+            subcmd[len] = isBfCompatibleVideoSystem(osdConfig()) ? getBfCharacter(screen[pos++], page): screen[pos++];
+            len++;
 
             if (bitArrayGet(dirty, pos)) {
                 next = pos;
@@ -330,7 +331,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
         vtxReset = false;
     }
 
-return 0;
+    return 0;
 }
 
 static void resync(displayPort_t *displayPort)
@@ -361,6 +362,10 @@ static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t
 static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort)
 {
     UNUSED(displayPort);
+
+    //textAttributes_t attr = TEXT_ATTRIBUTES_NONE;
+    //TEXT_ATTRIBUTES_ADD_BLINK(attr);
+    //return attr;
     return TEXT_ATTRIBUTES_NONE;
 }
 

src/main/io/gps.c

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

src/main/io/gps.h

@@ -76,7 +76,9 @@ typedef enum {
 
 typedef enum {
     GPS_DYNMODEL_PEDESTRIAN = 0,
+    GPS_DYNMODEL_AUTOMOTIVE,
     GPS_DYNMODEL_AIR_1G,
+    GPS_DYNMODEL_AIR_2G,
     GPS_DYNMODEL_AIR_4G,
 } gpsDynModel_e;
 
@@ -124,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;
@@ -133,7 +134,6 @@ typedef struct gpsSolutionData_s {
     uint8_t numSat;
 
     gpsLocation_t llh;
-    int16_t       magData[3];
     int16_t       velNED[3];
 
     int16_t groundSpeed;

src/main/io/gps_fake.c

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

src/main/io/gps_msp.c

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

src/main/io/gps_private.h

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

src/main/io/gps_ublox.c

@@ -251,8 +251,8 @@ static const uint8_t default_payload[] = {
     0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 };
 
-#define GNSSID_SBAS     1
+#define GNSSID_SBAS 1
-#define GNSSID_GALILEO  2
+#define GNSSID_GALILEO 2
 #define GNSSID_BEIDOU   3
 #define GNSSID_GZSS     5
 #define GNSSID_GLONASS  6
@@ -391,9 +391,10 @@ static void configureGNSS10(void)
             {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo},
 
             // Beidou
+            // M10 can't use BDS_B1I and Glonass together. Instead, use BDS_B1C
             {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou},
-            {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou},
+            {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou && ! gpsState.gpsConfig->ubloxUseGlonass},
-            {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0},
+            {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, gpsState.gpsConfig->ubloxUseBeidou && gpsState.gpsConfig->ubloxUseGlonass},
 
             // Should be enabled with GPS
             {UBLOX_CFG_QZSS_ENA, 1},
@@ -410,18 +411,18 @@ static void configureGNSS10(void)
 
 static void configureGNSS(void)
 {
-        int blocksUsed = 0;
+    int blocksUsed = 0;
-        send_buffer.message.header.msg_class = CLASS_CFG;
+    send_buffer.message.header.msg_class = CLASS_CFG;
         send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET
-        send_buffer.message.payload.gnss.msgVer = 0;
+    send_buffer.message.payload.gnss.msgVer = 0;
-        send_buffer.message.payload.gnss.numTrkChHw = 0;     // read only, so unset
+    send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
         send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max
 
-        /* SBAS, always generated */
+    /* SBAS, always generated */
-        blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
+    blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
 
-        /* Galileo */
+    /* Galileo */
-        blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
+    blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
 
         /* BeiDou */
         blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]);
@@ -429,9 +430,9 @@ static void configureGNSS(void)
         /* GLONASS */
         blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]);
 
-        send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
+    send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
-        send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed);
+    send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
-        sendConfigMessageUBLOX();
+    sendConfigMessageUBLOX();
 }
 
 static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
@@ -538,84 +539,84 @@ static bool gpsParseFrameUBLOX(void)
 {
     switch (_msg_id) {
     case MSG_POSLLH:
-        gpsSol.llh.lon = _buffer.posllh.longitude;
+        gpsSolDRV.llh.lon = _buffer.posllh.longitude;
-        gpsSol.llh.lat = _buffer.posllh.latitude;
+        gpsSolDRV.llh.lat = _buffer.posllh.latitude;
-        gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
+        gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
-        gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
+        gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
-        gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
+        gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
-        gpsSol.flags.validEPE = true;
+        gpsSolDRV.flags.validEPE = true;
         if (next_fix_type != GPS_NO_FIX)
-            gpsSol.fixType = next_fix_type;
+            gpsSolDRV.fixType = next_fix_type;
         _new_position = true;
         break;
     case MSG_STATUS:
         next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type);
         if (next_fix_type == GPS_NO_FIX)
-            gpsSol.fixType = GPS_NO_FIX;
+            gpsSolDRV.fixType = GPS_NO_FIX;
         break;
     case MSG_SOL:
         next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type);
         if (next_fix_type == GPS_NO_FIX)
-            gpsSol.fixType = GPS_NO_FIX;
+            gpsSolDRV.fixType = GPS_NO_FIX;
-        gpsSol.numSat = _buffer.solution.satellites;
+        gpsSolDRV.numSat = _buffer.solution.satellites;
-        gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
+        gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
         break;
     case MSG_VELNED:
-        gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
+        gpsSolDRV.groundSpeed = _buffer.velned.speed_2d;    // cm/s
-        gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
+        gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
-        gpsSol.velNED[X] = _buffer.velned.ned_north;
+        gpsSolDRV.velNED[X] = _buffer.velned.ned_north;
-        gpsSol.velNED[Y] = _buffer.velned.ned_east;
+        gpsSolDRV.velNED[Y] = _buffer.velned.ned_east;
-        gpsSol.velNED[Z] = _buffer.velned.ned_down;
+        gpsSolDRV.velNED[Z] = _buffer.velned.ned_down;
-        gpsSol.flags.validVelNE = true;
+        gpsSolDRV.flags.validVelNE = true;
-        gpsSol.flags.validVelD = true;
+        gpsSolDRV.flags.validVelD = true;
         _new_speed = true;
         break;
     case MSG_TIMEUTC:
         if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) {
-            gpsSol.time.year = _buffer.timeutc.year;
+            gpsSolDRV.time.year = _buffer.timeutc.year;
-            gpsSol.time.month = _buffer.timeutc.month;
+            gpsSolDRV.time.month = _buffer.timeutc.month;
-            gpsSol.time.day = _buffer.timeutc.day;
+            gpsSolDRV.time.day = _buffer.timeutc.day;
-            gpsSol.time.hours = _buffer.timeutc.hour;
+            gpsSolDRV.time.hours = _buffer.timeutc.hour;
-            gpsSol.time.minutes = _buffer.timeutc.min;
+            gpsSolDRV.time.minutes = _buffer.timeutc.min;
-            gpsSol.time.seconds = _buffer.timeutc.sec;
+            gpsSolDRV.time.seconds = _buffer.timeutc.sec;
-            gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000);
+            gpsSolDRV.time.millis = _buffer.timeutc.nano / (1000*1000);
 
-            gpsSol.flags.validTime = true;
+            gpsSolDRV.flags.validTime = true;
         } else {
-            gpsSol.flags.validTime = false;
+            gpsSolDRV.flags.validTime = false;
         }
         break;
     case MSG_PVT:
         next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type);
-        gpsSol.fixType = next_fix_type;
+        gpsSolDRV.fixType = next_fix_type;
-        gpsSol.llh.lon = _buffer.pvt.longitude;
+        gpsSolDRV.llh.lon = _buffer.pvt.longitude;
-        gpsSol.llh.lat = _buffer.pvt.latitude;
+        gpsSolDRV.llh.lat = _buffer.pvt.latitude;
-        gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10;  //alt in cm
+        gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10;  //alt in cm
-        gpsSol.velNED[X]=_buffer.pvt.ned_north / 10;  // to cm/s
+        gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10;  // to cm/s
-        gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10;   // to cm/s
+        gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10;   // to cm/s
-        gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10;   // to cm/s
+        gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10;   // to cm/s
-        gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10;    // to cm/s
+        gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10;    // to cm/s
-        gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
+        gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
-        gpsSol.numSat = _buffer.pvt.satellites;
+        gpsSolDRV.numSat = _buffer.pvt.satellites;
-        gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
+        gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
-        gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
+        gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
-        gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
+        gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
-        gpsSol.flags.validVelNE = true;
+        gpsSolDRV.flags.validVelNE = true;
-        gpsSol.flags.validVelD = true;
+        gpsSolDRV.flags.validVelD = true;
-        gpsSol.flags.validEPE = true;
+        gpsSolDRV.flags.validEPE = true;
 
         if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
-            gpsSol.time.year = _buffer.pvt.year;
+            gpsSolDRV.time.year = _buffer.pvt.year;
-            gpsSol.time.month = _buffer.pvt.month;
+            gpsSolDRV.time.month = _buffer.pvt.month;
-            gpsSol.time.day = _buffer.pvt.day;
+            gpsSolDRV.time.day = _buffer.pvt.day;
-            gpsSol.time.hours = _buffer.pvt.hour;
+            gpsSolDRV.time.hours = _buffer.pvt.hour;
-            gpsSol.time.minutes = _buffer.pvt.min;
+            gpsSolDRV.time.minutes = _buffer.pvt.min;
-            gpsSol.time.seconds = _buffer.pvt.sec;
+            gpsSolDRV.time.seconds = _buffer.pvt.sec;
-            gpsSol.time.millis = _buffer.pvt.nano / (1000*1000);
+            gpsSolDRV.time.millis = _buffer.pvt.nano / (1000*1000);
 
-            gpsSol.flags.validTime = true;
+            gpsSolDRV.flags.validTime = true;
         } else {
-            gpsSol.flags.validTime = false;
+            gpsSolDRV.flags.validTime = false;
         }
 
         _new_position = true;
@@ -649,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;
@@ -800,10 +801,16 @@ STATIC_PROTOTHREAD(gpsConfigure)
         case GPS_DYNMODEL_PEDESTRIAN:
             configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
             break;
-        case GPS_DYNMODEL_AIR_1G:   // Default to this
+        case GPS_DYNMODEL_AUTOMOTIVE:
-        default:
+            configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO);
+            break;
+        case GPS_DYNMODEL_AIR_1G:
             configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
             break;
+        case GPS_DYNMODEL_AIR_2G:   // Default to this
+        default:
+            configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO);
+            break;
         case GPS_DYNMODEL_AIR_4G:
             configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
             break;
@@ -948,19 +955,25 @@ STATIC_PROTOTHREAD(gpsConfigure)
 
     // Configure GNSS for M8N and later
     if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
-         gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
+        gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
-         if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) {
+        bool use_VALSET = 0;
+        if ( (gpsState.swVersionMajor > 23) || (gpsState.swVersionMajor == 23 && gpsState.swVersionMinor > 1) ) {
+            use_VALSET = 1;
+        }
+
+        if ( use_VALSET && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10) ) {
             configureGNSS10();
-         } else {
+        } else {
             configureGNSS();
-         }
+        }
+
          ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
 
-         if(_ack_state == UBX_ACK_GOT_NAK) {
+        if(_ack_state == UBX_ACK_GOT_NAK) {
             gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT;
             gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT;
             gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT;
-         }
+        }
     }
 
     ptEnd(0);
@@ -980,6 +993,7 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
         while (serialRxBytesWaiting(gpsState.gpsPort)) {
             uint8_t newChar = serialRead(gpsState.gpsPort);
             if (gpsNewFrameUBLOX(newChar)) {
+                gpsProcessNewDriverData();
                 ptSemaphoreSignal(semNewDataReady);
                 break;
             }
@@ -1038,16 +1052,19 @@ 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;
-    do {
+    // M7 and earlier will never get pass this step, so skip it (#9440).
-        pollGnssCapabilities();
+    // UBLOX documents that this is M8N and later
-        gpsState.autoConfigStep++;
+    if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) {
-        ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
+	do {
-    } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
+	    pollGnssCapabilities();
-
+	    gpsState.autoConfigStep++;
+	    ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
+	} while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
+    }
     // Configure GPS module if enabled
     if (gpsState.gpsConfig->autoConfig) {
         // Configure GPS
@@ -1060,7 +1077,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
     // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
     while (1) {
         ptSemaphoreWait(semNewDataReady);
-        gpsProcessNewSolutionData();
+        gpsProcessNewSolutionData(false);
 
         if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
             if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {

src/main/io/gps_ublox.h

@@ -34,7 +34,9 @@ extern "C" {
 #define GPS_CAPA_INTERVAL                   5000
 
 #define UBX_DYNMODEL_PEDESTRIAN 3
+#define UBX_DYNMODEL_AUTOMOVITE 4
 #define UBX_DYNMODEL_AIR_1G     6
+#define UBX_DYNMODEL_AIR_2G     7
 #define UBX_DYNMODEL_AIR_4G     8
 
 #define UBX_FIXMODE_2D_ONLY 1

src/main/io/ledstrip.h

@@ -21,7 +21,7 @@
 #include "common/color.h"
 #include "rx/rx.h"
 
-#define LED_MAX_STRIP_LENGTH           32
+#define LED_MAX_STRIP_LENGTH           128
 #define LED_CONFIGURABLE_COLOR_COUNT   16
 #define LED_MODE_COUNT                  6
 #define LED_DIRECTION_COUNT             6

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"
@@ -72,6 +73,8 @@
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
+#include "io/osd/custom_elements.h"
+
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
@@ -154,8 +157,9 @@
 
 #define OSD_MIN_FONT_VERSION 3
 
-static timeMs_t notify_settings_saved = 0;
+static timeMs_t linearDescentMessageMs  = 0;
-static bool     savingSettings = false;
+static timeMs_t notify_settings_saved   = 0;
+static bool     savingSettings          = false;
 
 static unsigned currentLayout = 0;
 static int layoutOverride = -1;
@@ -546,7 +550,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
         buff[0] = ' ';
     }
 
-#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values
+#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values  
     if (isBfCompatibleVideoSystem(osdConfig())) {
         totalDigits++;
         digits++;
@@ -641,7 +645,7 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
 }
 
 /**
- * Trim whitespace from string.
+ * Trim whitespace from string. 
  * Used in Stats screen on lines with multiple values.
 */
 char *osdFormatTrimWhiteSpace(char *buff)
@@ -652,7 +656,7 @@ char *osdFormatTrimWhiteSpace(char *buff)
     while(isspace((unsigned char)*buff)) buff++;
 
     // All spaces?
-    if(*buff == 0)
+    if(*buff == 0)  
     return buff;
 
     // Trim trailing spaces
@@ -988,18 +992,27 @@ static const char * osdFailsafeInfoMessage(void)
     }
     return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
 }
+
 #if defined(USE_SAFE_HOME)
 static const char * divertingToSafehomeMessage(void)
 {
+#ifdef USE_FW_AUTOLAND
+	if (!posControl.fwLandState.landWp && (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied)) {
+#else
     if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
-        return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
-    }
-    return NULL;
-}
 #endif
+	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
+	}
+#endif
+	return NULL;   
+}
+
 
 static const char * navigationStateMessage(void)
 {
+    if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0)
+        linearDescentMessageMs = 0;
+
     switch (NAV_Status.state) {
         case MW_NAV_STATE_NONE:
             break;
@@ -1011,7 +1024,13 @@ static const char * navigationStateMessage(void)
             if (posControl.flags.rthTrackbackActive) {
                 return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK);
             } else {
-                return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
+                if (posControl.rthState.rthLinearDescentActive && (linearDescentMessageMs == 0 || linearDescentMessageMs > millis())) {
+                    if (linearDescentMessageMs == 0)
+                        linearDescentMessageMs = millis() + 5000; // Show message for 5 seconds.
+
+                    return OSD_MESSAGE_STR(OSD_MSG_RTH_LINEAR_DESCENT);
+                } else 
+                    return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
             }
         case MW_NAV_STATE_HOLD_INFINIT:
             // Used by HOLD flight modes. No information to add.
@@ -1047,11 +1066,18 @@ static const char * navigationStateMessage(void)
         case MW_NAV_STATE_LANDED:
             return OSD_MESSAGE_STR(OSD_MSG_LANDED);
         case MW_NAV_STATE_LAND_SETTLE:
+        {
+            // If there is a FS landing delay occurring. That is handled by the calling function.
+            if (posControl.landingDelay > 0)
+                break;
+            
             return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND);
+        }
         case MW_NAV_STATE_LAND_START_DESCENT:
             // Not used
             break;
     }
+
     return NULL;
 }
 
@@ -1099,7 +1125,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
  * Check if this OSD layout is using scaled or unscaled throttle.
  * If both are used, it will default to scaled.
  */
-bool osdUsingScaledThrottle(void)
+bool osdUsingScaledThrottle(void) 
 {
     bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]);
     bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]);
@@ -1133,7 +1159,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes
 #endif
     int8_t throttlePercent = getThrottlePercent(useScaled);
     if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) {
-        const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM";
+        const char* message = ARMING_FLAG(ARMED) ? (throttlePercent == 0 && !ifMotorstopFeatureEnabled()) ? "IDLE" : "STOP" : "DARM";
         buff[0] = SYM_THR;
         strcpy(buff + 1, message);
         return;
@@ -1323,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);
@@ -1437,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;
@@ -1672,6 +1706,21 @@ static bool osdDrawSingleElement(uint8_t item)
     char buff[32] = {0};
 
     switch (item) {
+    case OSD_CUSTOM_ELEMENT_1:
+    {
+        customElementDrawElement(buff, 0);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_2:
+    {
+        customElementDrawElement(buff, 1);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_3:
+    {
+        customElementDrawElement(buff, 2);
+        break;
+    }
     case OSD_RSSI_VALUE:
         {
             uint16_t osdRssi = osdConvertRSSI();
@@ -1785,8 +1834,8 @@ static bool osdDrawSingleElement(uint8_t item)
             osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false);
 
         if (!unitsDrawn) {
-            buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
+        buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
-            buff[5] = '\0';
+        buff[5] = '\0';
         }
 
         if (batteryUsesCapacityThresholds()) {
@@ -1815,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) {
@@ -1872,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);
                 }
@@ -1935,28 +1994,27 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_ODOMETER:
         {
             displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_ODOMETER);
-            uint32_t odometerDist = (uint32_t)(getTotalTravelDistance() / 100);
+            float_t odometerDist = CENTIMETERS_TO_METERS(getTotalTravelDistance());
 #ifdef USE_STATS
             odometerDist+= statsConfig()->stats_total_dist;
 #endif
-            odometerDist = odometerDist / 10;
 
             switch (osdConfig()->units) {
                 case OSD_UNIT_UK:
                     FALLTHROUGH;
                 case OSD_UNIT_IMPERIAL:
-                    osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(odometerDist), FEET_PER_MILE, 1, 0, 6, true);
+                    osdFormatCentiNumber(buff, METERS_TO_MILES(odometerDist) * 100, 1, 1, 1, 6, true);
                     buff[6] = SYM_MI;
                     break;
                 default:
                 case OSD_UNIT_GA:
-                    osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(odometerDist), (uint32_t)FEET_PER_NAUTICALMILE, 1, 0, 6, true);
+                    osdFormatCentiNumber(buff, METERS_TO_NAUTICALMILES(odometerDist) * 100, 1, 1, 1, 6, true);
                     buff[6] = SYM_NM;
                     break;
                 case OSD_UNIT_METRIC_MPH:
                     FALLTHROUGH;
                 case OSD_UNIT_METRIC:
-                    osdFormatCentiNumber(buff, odometerDist, METERS_PER_KILOMETER, 1, 0, 6, true);
+                    osdFormatCentiNumber(buff, METERS_TO_KILOMETERS(odometerDist) * 100, 1, 1, 1, 6, true);
                     buff[6] = SYM_KM;
                     break;
             }
@@ -2050,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;
@@ -2094,8 +2218,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 int8_t blankPos;
                 for (blankPos = 2; blankPos >= 0; blankPos--) {
                     if (buff[blankPos] == SYM_BLANK) {
-                        break;
+            break;
-                    }
+        }
                 }
                 if (blankPos >= 0 || OSD_ALTERNATING_CHOICES(600, 2) == 0) {
                     blankPos = blankPos < 0 ? 0 : blankPos;
@@ -2237,7 +2361,11 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_FLYMODE:
         {
             char *p = "ACRO";
-
+#ifdef USE_FW_AUTOLAND
+            if (isFwLandInProgess()) 
+                p = "LAND";
+            else
+#endif
             if (FLIGHT_MODE(FAILSAFE_MODE))
                 p = "!FS!";
             else if (FLIGHT_MODE(MANUAL_MODE))
@@ -2418,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();
@@ -3046,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));
@@ -3068,7 +3208,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
                         buff[digits] = SYM_MAH_MI_0;        // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode
                         buff[digits + 1] = SYM_MAH_MI_1;
                         buff[digits + 2] = '\0';
@@ -3082,7 +3222,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
                         buff[digits] = SYM_MAH_NM_0;
                         buff[digits + 1] = SYM_MAH_NM_1;
                         buff[digits + 2] = '\0';
@@ -3098,7 +3238,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
                         buff[digits] = SYM_MAH_KM_0;
                         buff[digits + 1] = SYM_MAH_KM_1;
                         buff[digits + 2] = '\0';
@@ -3117,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));
@@ -3269,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
@@ -3687,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();
@@ -3712,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,
@@ -3951,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);
@@ -3997,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) {
@@ -4025,9 +4180,9 @@ uint8_t drawLogos(bool singular, uint8_t row) {
         logoRow = row;
         if (singular) {
             logo_x = logoColOffset;
-        } else {
+    } else {
             logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing;
-        }
+    }
 
         for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
             for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) {
@@ -4038,7 +4193,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
     }
 
     return logoRow;
-}
+    }
 
 uint8_t drawStats(uint8_t row) {
 #ifdef USE_STATS
@@ -4269,7 +4424,7 @@ static void osdUpdateStats(void)
 static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 {
     const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
-    uint8_t top = 1;  // Start one line down leaving space at the top of the screen.
+    uint8_t top = 1;  // Start one line down leaving space at the top of the screen. 
     size_t multiValueLengthOffset = 0;
 
     const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
@@ -4298,7 +4453,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                 osdLeftAlignString(buff);
                 strcat(osdFormatTrimWhiteSpace(buff),"/");
                 multiValueLengthOffset = strlen(buff);
-                displayWrite(osdDisplayPort, statValuesX, top, buff);
+                displayWrite(osdDisplayPort, statValuesX, top, buff);            
                 osdGenerateAverageVelocityStr(buff);
                 osdLeftAlignString(buff);
                 displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
@@ -4335,7 +4490,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                     osdLeftAlignString(buff);
                     strcat(osdFormatTrimWhiteSpace(buff), "%/");
                     multiValueLengthOffset = strlen(buff);
-                    displayWrite(osdDisplayPort, statValuesX, top, buff);
+                    displayWrite(osdDisplayPort, statValuesX, top, buff);                
                     itoa(stats.min_rssi_dbm, buff, 10);
                     tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
                     osdLeftAlignString(buff);
@@ -4350,7 +4505,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                     itoa(stats.min_rssi_dbm, buff, 10);
                     tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
                     displayWrite(osdDisplayPort, statValuesX, top++, buff);
-                }
+                } 
 
                 displayWrite(osdDisplayPort, statNameX, top, "MIN LQ           :");
                 itoa(stats.min_lq, buff, 10);
@@ -4362,7 +4517,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                 itoa(stats.min_rssi, buff, 10);
                 strcat(buff, "%");
                 displayWrite(osdDisplayPort, statValuesX, top++, buff);
-            }
+        }
 
         displayWrite(osdDisplayPort, statNameX, top, "FLY TIME         :");
         uint16_t flySeconds = getFlightTime();
@@ -4414,13 +4569,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
             bool efficiencyValid = totalDistance >= 10000;
             if (feature(FEATURE_GPS)) {
                 displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY   :");
-                uint8_t digits = 3U;    // Total number of digits (including decimal point)
+                    uint8_t digits = 3U;    // Total number of digits (including decimal point)
-                #ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values
+                    #ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values
-                    if (isBfCompatibleVideoSystem(osdConfig())) {
+                        if (isBfCompatibleVideoSystem(osdConfig())) {
-                        // Add one digit so no switch to scaled decimal occurs above 99
+                            // Add one digit so no switch to scaled decimal occurs above 99
-                        digits = 4U;
+                            digits = 4U;
-                    }
+                        }
-                #endif
+                    #endif
                 switch (osdConfig()->units) {
                     case OSD_UNIT_UK:
                         FALLTHROUGH;
@@ -4509,9 +4664,9 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
         displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
         osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4, false);
         osdLeftAlignString(buff);
-        strcat(osdFormatTrimWhiteSpace(buff),"/");
+        strcat(osdFormatTrimWhiteSpace(buff),"/"); 
-        multiValueLengthOffset = strlen(buff);
+        multiValueLengthOffset = strlen(buff);        
-        displayWrite(osdDisplayPort, statValuesX, top, buff);
+        displayWrite(osdDisplayPort, statValuesX, top, buff);        
         osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3, false);
         osdLeftAlignString(buff);
         displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
@@ -4533,10 +4688,10 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 // HD arming screen. based on the minimum HD OSD grid size of 50 x 18
 static void osdShowHDArmScreen(void)
 {
-    dateTime_t  dt;
+    dateTime_t dt;
     char        buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
     char        buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
-    char        craftNameBuf[MAX_NAME_LENGTH];
+    char craftNameBuf[MAX_NAME_LENGTH];
     char        versionBuf[osdDisplayPort->cols];
     uint8_t     safehomeRow     = 0;
     uint8_t     armScreenRow    = 2; // Start at row 2
@@ -4576,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;
@@ -4632,7 +4787,7 @@ static void osdShowHDArmScreen(void)
     if (armScreenRow < (osdDisplayPort->rows - 4))
         armScreenRow = drawStats(armScreenRow);
 #endif // USE_STATS
-}
+    }
 
 static void osdShowSDArmScreen(void)
 {
@@ -4859,27 +5014,27 @@ static void osdRefresh(timeUs_t currentTimeUs)
 
              // Manual paging stick commands are only applicable to multi-page stats.
              // ******************************
-             // For single-page stats, this effectively disables the ability to cancel the
+             // For single-page stats, this effectively disables the ability to cancel the 
              // automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time
-             // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then
+             // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then 
-             // "Saved Settings" should display if it is active within the refresh interval.
+             // "Saved Settings" should display if it is active within the refresh interval. 
              // ******************************
              // With multi-page stats, "Saved Settings" could also be missed if the user
-             // has canceled automatic paging using the stick commands, because that is only
+             // has canceled automatic paging using the stick commands, because that is only 
-             // updated when osdShowStats() is called. So, in that case, they would only see
+             // updated when osdShowStats() is called. So, in that case, they would only see 
-             // the "Saved Settings" message if they happen to manually change pages using the
+             // the "Saved Settings" message if they happen to manually change pages using the 
-             // stick commands within the interval the message is displayed.
+             // stick commands within the interval the message is displayed. 
             bool manualPageUpRequested = false;
-            bool manualPageDownRequested = false;
+            bool manualPageDownRequested = false;       
             if (!statsSinglePageCompatible) {
                 // These methods ensure the paging stick commands are held for a brief period
-                // Otherwise it can result in a race condition where the stats are
+                // Otherwise it can result in a race condition where the stats are 
-                // updated too quickly and can result in partial blanks, etc.
+                // updated too quickly and can result in partial blanks, etc. 
-                if (osdIsPageUpStickCommandHeld()) {
+                if (osdIsPageUpStickCommandHeld()) {               
                     manualPageUpRequested = true;
                     statsAutoPagingEnabled = false;
                 } else if (osdIsPageDownStickCommandHeld()) {
-                    manualPageDownRequested = true;
+                    manualPageDownRequested = true;                
                     statsAutoPagingEnabled = false;
                 }
             }
@@ -4902,16 +5057,16 @@ static void osdRefresh(timeUs_t currentTimeUs)
                 // Process manual page change events for multi-page stats.
                 if (manualPageUpRequested) {
                     osdShowStats(statsSinglePageCompatible, 1);
-                    statsCurrentPage = 1;
+                    statsCurrentPage = 1;                   
                 } else if (manualPageDownRequested) {
                     osdShowStats(statsSinglePageCompatible, 0);
                     statsCurrentPage = 0;
-                }
             }
         }
+        }
 
         // Handle events when either "Splash", "Armed" or "Stats" screens are displayed.
-        if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) {
+        if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { 
             // Time elapsed or canceled by stick commands.
             // Exit to normal OSD operation.
             displayClearScreen(osdDisplayPort);
@@ -4921,7 +5076,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             // Continue "Splash", "Armed" or "Stats" screens.
             displayHeartbeat(osdDisplayPort);
         }
-
+        
         return;
     }
 
@@ -5098,8 +5253,13 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
         const char *invertedInfoMessage = NULL;
 
         if (ARMING_FLAG(ARMED)) {
+#ifdef USE_FW_AUTOLAND
+            if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) {
+                if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) {
+#else
             if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
                 if (isWaypointMissionRTHActive()) {
+#endif
                     // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
                 }
@@ -5124,12 +5284,28 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
                         messages[messageCount++] = messageBuf;
                     }
-                } else {
+                } else if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) {
-                    const char *navStateMessage = navigationStateMessage();
+                    uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis());
-                    if (navStateMessage) {
+                    tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
-                        messages[messageCount++] = navStateMessage;
+
+                    messages[messageCount++] = messageBuf;
+                } 
+
+                else {
+#ifdef USE_FW_AUTOLAND
+                    if (canFwLandCanceld()) {
+                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
+                    } else if (!isFwLandInProgess()) {
+#endif
+                        const char *navStateMessage = navigationStateMessage();
+                        if (navStateMessage) {
+                            messages[messageCount++] = navStateMessage;
+                        }
+#ifdef USE_FW_AUTOLAND
                     }
+#endif
                 }
+
 #if defined(USE_SAFE_HOME)
                 const char *safehomeMessage = divertingToSafehomeMessage();
                 if (safehomeMessage) {
@@ -5183,7 +5359,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         }
                     }
                     if (isFixedWingLevelTrimActive()) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
+                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
                     }
                     if (FLIGHT_MODE(HEADFREE_MODE)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);

src/main/io/osd.h

@@ -93,6 +93,7 @@
 #define OSD_MSG_RTH_CLIMB           "ADJUSTING RTH ALTITUDE"
 #define OSD_MSG_RTH_TRACKBACK       "RTH BACK TRACKING"
 #define OSD_MSG_HEADING_HOME        "EN ROUTE TO HOME"
+#define OSD_MSG_RTH_LINEAR_DESCENT  "BEGIN LINEAR DESCENT"
 #define OSD_MSG_WP_FINISHED         "WP END>HOLDING POSITION"
 #define OSD_MSG_WP_LANDED           "WP END>LANDED"
 #define OSD_MSG_PREPARE_NEXT_WP     "PREPARING FOR NEXT WAYPOINT"
@@ -121,6 +122,7 @@
 #define OSD_MSG_ANGLEHOLD_ROLL      "(ANGLEHOLD ROLL)"
 #define OSD_MSG_ANGLEHOLD_PITCH     "(ANGLEHOLD PITCH)"
 #define OSD_MSG_ANGLEHOLD_LEVEL     "(ANGLEHOLD LEVEL)"
+#define OSD_MSG_MOVE_STICKS         "MOVE STICKS TO ABORT"
 
 #ifdef USE_DEV_TOOLS
 #define OSD_MSG_GRD_TEST_MODE       "GRD TEST > MOTORS DISABLED"
@@ -279,6 +281,11 @@ typedef enum {
     OSD_MULTI_FUNCTION,
     OSD_ODOMETER,
     OSD_PILOT_LOGO,
+    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;
 
@@ -454,6 +461,11 @@ typedef struct osdConfig_s {
     bool            use_pilot_logo;                     // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
     uint8_t         inav_to_pilot_logo_spacing;         // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
     uint16_t        arm_screen_display_time;            // Length of time the arm screen is displayed
+    #ifdef USE_ADSB
+    uint16_t adsb_distance_warning;                     // in metres
+    uint16_t adsb_distance_alert;                       // in metres
+    uint16_t adsb_ignore_plane_above_me_limit;          // in metres
+#endif
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

src/main/io/osd/custom_elements.c

@@ -0,0 +1,141 @@
+/*
+ * 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 "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "common/string_light.h"
+#include "common/maths.h"
+
+#include "programming/logic_condition.h"
+#include "programming/global_variables.h"
+
+#include "io/osd.h"
+#include "io/osd/custom_elements.h"
+
+#include "drivers/osd_symbols.h"
+
+PG_REGISTER_ARRAY_WITH_RESET_FN(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements, PG_OSD_CUSTOM_ELEMENTS_CONFIG, 1);
+
+void pgResetFn_osdCustomElements(osdCustomElement_t *instance)
+{
+    for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
+        RESET_CONFIG(osdCustomElement_t, &instance[i],
+            .part[0]              = {.type = CUSTOM_ELEMENT_TYPE_NONE, .value = 0},
+            .part[1]              = {.type = CUSTOM_ELEMENT_TYPE_NONE, .value = 0},
+            .part[2]              = {.type = CUSTOM_ELEMENT_TYPE_NONE, .value = 0},
+            .visibility           = {.type = CUSTOM_ELEMENT_VISIBILITY_ALWAYS, .value = 0},
+            .osdCustomElementText   = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+        );
+    }
+}
+
+bool isCustomelementVisible(const osdCustomElement_t* customElement){
+    if(customElement->visibility.type == CUSTOM_ELEMENT_VISIBILITY_ALWAYS){
+        return true;
+    }
+
+    if(customElement->visibility.type == CUSTOM_ELEMENT_VISIBILITY_GV && gvGet(customElement->visibility.value)){
+        return true;
+    }
+
+    if(customElement->visibility.type == CUSTOM_ELEMENT_VISIBILITY_LOGIC_CON && logicConditionGetValue(customElement->visibility.value)){
+        return true;
+    }
+
+    return false;
+}
+
+uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t customElementItemIndex){
+    const osdCustomElement_t* customElement = osdCustomElements(customElementIndex);
+    const int customPartType = osdCustomElements(customElementIndex)->part[customElementItemIndex].type;
+    const int customPartValue = osdCustomElements(customElementIndex)->part[customElementItemIndex].value;
+
+    switch (customPartType) {
+        case CUSTOM_ELEMENT_TYPE_GV:
+        {
+            osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue) * (int32_t) 100, 1, 0, 0, 6, false);
+            return 6;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT:
+        {
+            osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue), 1, 2, 0, 6, false);
+            return 6;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_SMALL:
+        {
+            osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 1000 ) * (int32_t) 100), 1, 0, 0, 3, false);
+            return 3;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT:
+        {
+            osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 100)  * (int32_t) 10), 1, 1, 0, 2, false);
+            return 2;
+        }
+        case CUSTOM_ELEMENT_TYPE_ICON_GV:
+        {
+            *buff = (uint8_t)gvGet(customPartValue);
+            return 1;
+        }
+        case CUSTOM_ELEMENT_TYPE_ICON_STATIC:
+        {
+            *buff = (uint8_t)customPartValue;
+            return 1;
+        }
+        case CUSTOM_ELEMENT_TYPE_TEXT:
+        {
+            for (int i = 0; i < OSD_CUSTOM_ELEMENT_TEXT_SIZE; i++) {
+                if (customElement->osdCustomElementText[i] == 0){
+                    return i;
+                }
+                *buff = sl_toupper((unsigned char)customElement->osdCustomElementText[i]);
+                buff++;
+            }
+            return OSD_CUSTOM_ELEMENT_TEXT_SIZE;
+        }
+    }
+
+    return 0;
+}
+
+void customElementDrawElement(char *buff, uint8_t customElementIndex){
+
+    if(customElementIndex >= MAX_CUSTOM_ELEMENTS){
+        return;
+    }
+
+    static uint8_t prevLength[MAX_CUSTOM_ELEMENTS];
+
+    uint8_t buffSeek = 0;
+    const osdCustomElement_t* customElement = osdCustomElements(customElementIndex);
+    if(isCustomelementVisible(customElement))
+    {
+        for (uint8_t i = 0; i < CUSTOM_ELEMENTS_PARTS; ++i) {
+            uint8_t currentSeek = customElementDrawPart(buff, customElementIndex, i);
+            buff += currentSeek;
+            buffSeek += currentSeek;
+        }
+    }
+
+    for (uint8_t i = buffSeek; i < prevLength[customElementIndex]; i++) {
+        *buff++ = SYM_BLANK;
+    }
+    prevLength[customElementIndex] = buffSeek;
+}
+

src/main/io/osd/custom_elements.h

@@ -0,0 +1,61 @@
+/*
+ * 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 "config/parameter_group.h"
+
+#define OSD_CUSTOM_ELEMENT_TEXT_SIZE 16
+#define CUSTOM_ELEMENTS_PARTS 3
+#define MAX_CUSTOM_ELEMENTS 3
+
+typedef enum {
+    CUSTOM_ELEMENT_TYPE_NONE            = 0,
+    CUSTOM_ELEMENT_TYPE_TEXT            = 1,
+    CUSTOM_ELEMENT_TYPE_ICON_STATIC     = 2,
+    CUSTOM_ELEMENT_TYPE_ICON_GV         = 3,
+    CUSTOM_ELEMENT_TYPE_GV              = 4,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT        = 5,
+    CUSTOM_ELEMENT_TYPE_GV_SMALL        = 6,
+    CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT  = 7,
+} osdCustomElementType_e;
+
+typedef enum {
+    CUSTOM_ELEMENT_VISIBILITY_ALWAYS    = 0,
+    CUSTOM_ELEMENT_VISIBILITY_GV        = 1,
+    CUSTOM_ELEMENT_VISIBILITY_LOGIC_CON = 2,
+} osdCustomElementTypeVisibility_e;
+
+typedef struct {
+    osdCustomElementType_e  type;
+    uint16_t                value;
+} osdCustomElementItem_t;
+
+typedef struct {
+    osdCustomElementTypeVisibility_e    type;
+    uint16_t                            value;
+} osdCustomElementVisibility_t;
+
+typedef struct  {
+    osdCustomElementItem_t          part[CUSTOM_ELEMENTS_PARTS];
+    osdCustomElementVisibility_t    visibility;
+    char                            osdCustomElementText[OSD_CUSTOM_ELEMENT_TEXT_SIZE];
+} osdCustomElement_t;
+
+PG_DECLARE_ARRAY(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements);
+
+void customElementDrawElement(char *buff, uint8_t customElementIndex);

src/main/io/osd_canvas.c

@@ -345,7 +345,9 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display
         int ahiWidth = osdConfig()->ahi_width;
         int ahiX = (canvas->width - ahiWidth) / 2;
         int ahiHeight = osdConfig()->ahi_height;
-        int ahiY = ((canvas->height - ahiHeight) / 2) + osdConfig()->ahi_vertical_offset;
+        int ahiY = ((canvas->height - ahiHeight) / 2);
+        ahiY += osdConfig()->ahi_vertical_offset;
+
         if (ahiY < 0) {
             ahiY = 0;
         }

src/main/io/osd_common.c

@@ -44,7 +44,6 @@
 #include "navigation/navigation.h"
 #include "sensors/pitotmeter.h"
 
-
 #if defined(USE_OSD) || defined(USE_DJI_HD_OSD)
 
 PG_REGISTER_WITH_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig, PG_OSD_COMMON_CONFIG, 0);
@@ -148,11 +147,17 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c
 {
     uint8_t gx;
     uint8_t gy;
+        
 #if defined(USE_CANVAS)
     if (canvas) {
         osdCanvasDrawArtificialHorizon(display, canvas, p, pitchAngle, rollAngle);
     } else {
 #endif
+        // Correct pitch when inverted
+        if (rollAngle < -1.570796f || rollAngle > 1.570796f) {
+            pitchAngle = -pitchAngle;
+        }
+
         osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
         osdGridDrawArtificialHorizon(display, gx, gy, pitchAngle, rollAngle);
 #if defined(USE_CANVAS)

src/main/io/osd_dji_hd.c

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

src/main/msp/msp_protocol_v2_inav.h

@@ -92,6 +92,9 @@
 #define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049
 
+#define MSP2_INAV_FW_APPROACH                   0x204A
+#define MSP2_INAV_SET_FW_APPROACH               0x204B
+
 #define MSP2_INAV_RATE_DYNAMICS                 0x2060
 #define MSP2_INAV_SET_RATE_DYNAMICS             0x2061
 
@@ -99,3 +102,9 @@
 #define MSP2_INAV_EZ_TUNE_SET                   0x2071
 
 #define MSP2_INAV_SELECT_MIXER_PROFILE          0x2080
+
+#define MSP2_ADSB_VEHICLE_LIST                  0x2090
+
+#define MSP2_INAV_CUSTOM_OSD_ELEMENTS           0x2100
+#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS       0x2101
+

src/main/navigation/navigation.h

@@ -31,6 +31,7 @@
 
 #include "io/gps.h"
 
+
 /* GPS Home location data */
 extern gpsLocation_t        GPS_home;
 extern uint32_t             GPS_distanceToHome;        // distance to home point in meters
@@ -40,6 +41,7 @@ extern bool autoThrottleManuallyIncreased;
 
 /* Navigation system updates */
 void onNewGPSData(void);
+
 #if defined(USE_SAFE_HOME)
 
 #define MAX_SAFE_HOMES 8
@@ -58,10 +60,62 @@ typedef enum {
 
 PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
 
-void resetSafeHomes(void);           // remove all safehomes
+void resetSafeHomes(void);                       // remove all safehomes
-bool findNearestSafeHome(void);      // Find nearest safehome
+bool findNearestSafeHome(void);                  // Find nearest safehome
+
 #endif // defined(USE_SAFE_HOME)
 
+#ifdef USE_FW_AUTOLAND
+
+#ifndef MAX_SAFE_HOMES
+#define MAX_SAFE_HOMES 0
+#endif
+
+#define MAX_FW_LAND_APPOACH_SETTINGS (MAX_SAFE_HOMES + 9)
+
+typedef enum  {
+    FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
+    FW_AUTOLAND_APPROACH_DIRECTION_RIGHT
+} fwAutolandApproachDirection_e;
+
+typedef enum {
+    FW_AUTOLAND_STATE_IDLE,
+    FW_AUTOLAND_STATE_LOITER,
+    FW_AUTOLAND_STATE_DOWNWIND,
+    FW_AUTOLAND_STATE_BASE_LEG,
+    FW_AUTOLAND_STATE_FINAL_APPROACH,
+    FW_AUTOLAND_STATE_GLIDE,
+    FW_AUTOLAND_STATE_FLARE
+} fwAutolandState_t;
+
+typedef struct {
+    int32_t approachAlt;
+    int32_t landAlt;
+    bool isSeaLevelRef;
+    fwAutolandApproachDirection_e approachDirection;
+    int16_t landApproachHeading1;
+    int16_t landApproachHeading2;
+} navFwAutolandApproach_t;
+
+PG_DECLARE_ARRAY(navFwAutolandApproach_t, MAX_FW_LAND_APPOACH_SETTINGS, fwAutolandApproachConfig);
+
+typedef struct navFwAutolandConfig_s
+{
+    uint32_t approachLength;
+    uint16_t finalApproachPitchToThrottleMod;
+    uint16_t glideAltitude;
+    uint16_t flareAltitude;
+    uint8_t flarePitch;
+    uint16_t maxTailwind;
+    int8_t glidePitch;
+} navFwAutolandConfig_t;
+
+PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig);
+
+void resetFwAutolandApproach(int8_t idx);
+
+#endif
+
 #ifndef NAV_MAX_WAYPOINTS
 #define NAV_MAX_WAYPOINTS 15
 #endif
@@ -203,12 +257,15 @@ typedef struct positionEstimationConfig_s {
     float w_xy_res_v;
 
     float w_acc_bias;   // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
-    float w_xyz_acc_p;
 
     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
     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);
@@ -245,10 +302,9 @@ typedef struct navConfig_s {
 #endif
         bool     waypoint_load_on_boot;             // load waypoints automatically during boot
         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
@@ -268,10 +324,13 @@ typedef struct navConfig_s {
         uint16_t auto_disarm_delay;                 // safety time delay for landing detector
         uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
         uint8_t  cruise_yaw_rate;                   // Max yaw rate (dps) when CRUISE MODE is enabled
+        uint16_t rth_fs_landing_delay;              // Delay upon reaching home before starting landing if in FS (0 = immediate)
     } general;
 
     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
@@ -292,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
@@ -315,7 +375,8 @@ typedef struct navConfig_s {
         uint8_t  launch_climb_angle;         // Target climb angle for launch (deg)
         uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
         bool     launch_manual_throttle;     // Allows launch with manual throttle control
-        uint8_t  launch_abort_deadband;      // roll/pitch stick movement deadband for launch abort
+        uint8_t  launch_land_abort_deadband;      // roll/pitch stick movement deadband for launch abort
+        uint8_t  cruise_yaw_rate;            // Max yaw rate (dps) when CRUISE MODE is enabled
         bool     allow_manual_thr_increase;
         bool     useFwNavYawControl;
         uint8_t  yawControlDeadband;
@@ -574,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);
@@ -595,7 +659,7 @@ bool isFixedWingAutoThrottleManuallyIncreased(void);
 bool navigationIsFlyingAutonomousMode(void);
 bool navigationIsExecutingAnEmergencyLanding(void);
 bool navigationIsControllingAltitude(void);
-/* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
+/* Returns true if navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
  * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
  */
 bool navigationRTHAllowsLanding(void);
@@ -626,8 +690,10 @@ 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);
 
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles
@@ -635,6 +701,11 @@ int8_t navCheckActiveAngleHoldAxis(void);
  */
 int32_t navigationGetHomeHeading(void);
 
+#ifdef USE_FW_AUTOLAND
+bool isFwLandInProgess(void);
+bool canFwLandCanceld(void);
+#endif
+
 /* Compatibility data */
 extern navSystemStatus_t    NAV_Status;
 

src/main/navigation/navigation_fixedwing.c

@@ -60,9 +60,8 @@
 #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f
 #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ     10.0f
 
-// If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
+// If we are going slower than the minimum ground speed (navConfig()->general.min_ground_speed) - boost throttle to fight against the wind
 #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN        1.5f
-#define NAV_FW_MIN_VEL_SPEED_BOOST              700.0f      // 7 m/s
 
 // If this is enabled navigation won't be applied if velocity is below 3 m/s
 //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY
@@ -116,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;
     }
@@ -506,8 +505,8 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
                 // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
                 // FIXME: verify the above
                 calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
-
                 updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
+                needToCalculateCircularLoiter = false;
             }
             else {
                 // Position update has not occurred in time (first iteration or glitch), reset altitude controller
@@ -540,10 +539,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
             previousTimePositionUpdate = currentTimeUs;
 
             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
-                float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
+                float velThrottleBoost = ((navConfig()->general.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
 
                 // If we are in the deadband of 50cm/s - don't update speed boost
-                if (fabsf(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) {
+                if (fabsf(posControl.actualState.velXY - (navConfig()->general.min_ground_speed * 100.0f)) > 50) {
                     throttleSpeedAdjustment += velThrottleBoost;
                 }
 
@@ -577,13 +576,21 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
     // Apply low-pass filter to pitch angle to smooth throttle correction
     int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
 
+    int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle;
+
+#ifdef USE_FW_AUTOLAND
+    if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) {
+        pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f;
+    }
+#endif
+
     if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
         // Unfiltered throttle correction outside of pitch deadband
-        return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
+        return DECIDEGREES_TO_DEGREES(pitch) * pitchToThrottle;
     }
     else {
         // Filtered throttle correction inside of pitch deadband
-        return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
+        return DECIDEGREES_TO_DEGREES(filteredPitch) * pitchToThrottle;
     }
 }
 
@@ -608,16 +615,12 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
         rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
         int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
 
-#ifdef NAV_FIXED_WING_LANDING
         if (navStateFlags & NAV_CTL_LAND) {
-            // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
+        // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
         } else {
-#endif
             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
-#ifdef NAV_FIXED_WING_LANDING
         }
-#endif
 
         // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
         if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
@@ -628,7 +631,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
         uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
 
         // Manual throttle increase
+#ifdef USE_FW_AUTOLAND
+        if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) {
+#else
         if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
+#endif
             if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
                 correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
             } else {
@@ -642,12 +649,24 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
         rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
     }
 
-#ifdef NAV_FIXED_WING_LANDING
+#ifdef USE_FW_AUTOLAND
-    /*
+    // Advanced autoland
-     * Then altitude is below landing slowdown min. altitude, enable final approach procedure
+    if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) {
-     * TODO refactor conditions in this metod if logic is proven to be correct
+        // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
-     */
+        rcCommand[THROTTLE] = getThrottleIdleValue();
-    if (navStateFlags & NAV_CTL_LAND || STATE(LANDING_DETECTED)) {
+        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
+
+        if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) {
+            rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->glidePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
+        }
+
+        if (posControl.navState == NAV_STATE_FW_LANDING_FLARE) {
+            rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
+        }
+    }
+#endif
+    // "Traditional" landing as fallback option
+    if (navStateFlags & NAV_CTL_LAND) {
         int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
 
         if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) ||
@@ -663,7 +682,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
             rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
         }
     }
-#endif
 }
 
 bool isFixedWingAutoThrottleManuallyIncreased(void)
@@ -713,7 +731,7 @@ bool isFixedWingLandingDetected(void)
 
     // Check horizontal and vertical velocities are low (cm/s)
     bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
-                        posControl.actualState.velXY < (100.0f * sensitivity);
+                        ( posControl.actualState.velXY < (100.0f * sensitivity));
     // Check angular rates are low (degs/s)
     bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity);
     DEBUG_SET(DEBUG_LANDING, 2, velCondition);

src/main/navigation/navigation_fw_launch.c

@@ -251,7 +251,7 @@ static inline bool isLaunchMaxAltitudeReached(void)
 static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
 {
     return (initialTime + currentStateElapsedMs(currentTimeUs)) >= navConfig()->fw.launch_min_time &&
-            isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband);
+            isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband);
 }
 
 static inline bool isProbablyNotFlying(void)
@@ -420,7 +420,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
         if (throttleStickIsLow()) {
             fwLaunch.currentStateTimeUs = currentTimeUs;
             fwLaunch.pitchAngle = 0;
-            if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
+            if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
                 return FW_LAUNCH_EVENT_ABORT;
             }
         } else {
@@ -454,7 +454,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
 
-    if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
+    if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
         return FW_LAUNCH_EVENT_SUCCESS;     // End launch go to FW_LAUNCH_STATE_FLYING state
     }
     else {

src/main/navigation/navigation_multicopter.c

@@ -148,11 +148,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);
@@ -215,14 +215,13 @@ void resetMulticopterAltitudeController(void)
     pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f);
 
     if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-        const float maxSpeed = getActiveSpeed();
+        nav_speed_up = navConfig()->mc.max_auto_climb_rate;
-        nav_speed_up = maxSpeed;
+        nav_accel_z = navConfig()->mc.max_auto_climb_rate;
-        nav_accel_z = maxSpeed;
+        nav_speed_down = navConfig()->mc.max_auto_climb_rate;
-        nav_speed_down = navConfig()->general.max_auto_climb_rate;
     } else {
-        nav_speed_up = navConfig()->general.max_manual_speed;
+        nav_speed_up = navConfig()->mc.max_manual_climb_rate;
-        nav_accel_z = navConfig()->general.max_manual_speed;
+        nav_accel_z = navConfig()->mc.max_manual_climb_rate;
-        nav_speed_down = navConfig()->general.max_manual_climb_rate;
+        nav_speed_down = navConfig()->mc.max_manual_climb_rate;
     }
 
     sqrtControllerInit(
@@ -249,8 +248,8 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
             if (prepareForTakeoffOnReset) {
                 const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
 
-                posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
+                posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate;
-                posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
+                posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->mc.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
                 posControl.pids.vel[Z].integrator = -500.0f;
                 pt1FilterReset(&altholdThrottleFilterState, -500.0f);
                 prepareForTakeoffOnReset = false;

src/main/navigation/navigation_pos_estimator.c

@@ -70,8 +70,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
 
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
 
-        .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
-
         .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
 
         .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
@@ -92,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; }
@@ -161,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
@@ -221,8 +181,16 @@ void onNewGPSData(void)
     newLLH.lon = gpsSol.llh.lon;
     newLLH.alt = gpsSol.llh.alt;
 
-    if (sensors(SENSOR_GPS)) {
+    if (sensors(SENSOR_GPS) 
-        if (!STATE(GPS_FIX)) {
+#ifdef USE_GPS_FIX_ESTIMATION
+            || STATE(GPS_ESTIMATED_FIX)
+#endif
+        ) {
+        if (!(STATE(GPS_FIX) 
+#ifdef USE_GPS_FIX_ESTIMATION
+                || STATE(GPS_ESTIMATED_FIX)
+#endif
+            )) {
             isFirstGPSUpdate = true;
             return;
         }
@@ -277,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;
@@ -389,28 +344,30 @@ static bool gravityCalibrationComplete(void)
 
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
-
+#define ACC_VIB_FACTOR_S 1.0f
+#define ACC_VIB_FACTOR_E 3.0f
 static void updateIMUEstimationWeight(const float dt)
 {
-    bool isAccClipped = accIsClipped();
+    static float acc_clip_factor = 1.0f;
-
+    // If accelerometer measurement is clipped - drop the acc weight to 0.3
-    // If accelerometer measurement is clipped - drop the acc weight to zero
     // and gradually restore weight back to 1.0 over time
-    if (isAccClipped) {
+    if (accIsClipped()) {
-        posEstimator.imu.accWeightFactor = 0.0f;
+        acc_clip_factor = 0.5f;
     }
     else {
         const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
-        posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
+        acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha;
     }
-
+    // Update accelerometer weight based on vibration levels and clipping
+    float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s
+    posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor;
     // DEBUG_VIBE[0-3] are used in IMU
     DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
 }
 
 float navGetAccelerometerWeight(void)
 {
-    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    const float accWeightScaled = posEstimator.imu.accWeightFactor;
     DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
 
     return accWeightScaled;
@@ -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) {
@@ -553,13 +518,12 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 
 static void estimationPredict(estimationContext_t * ctx)
 {
-    const float accWeight = navGetAccelerometerWeight();
 
     /* Prediction step: Z-axis */
     if ((ctx->newFlags & EST_Z_VALID)) {
         posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
-        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
+        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
-        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
+        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
     }
 
     /* Prediction step: XY-axis */
@@ -570,10 +534,10 @@ static void estimationPredict(estimationContext_t * ctx)
 
         // If heading is valid, accelNEU is valid as well. Account for acceleration
         if (navIsHeadingUsable() && navIsAccelerationUsable()) {
-            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
+            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
-            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
+            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
-            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
+            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
-            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
+            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
         }
     }
 }
@@ -589,7 +553,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
-    if (ctx->newFlags & EST_BARO_VALID) {
+    bool correctOK = false;
+    
+    //ignore baro if difference is too big, baro is probably wrong
+    const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
+    //fade out the baro to prevent sudden jump
+    const float start_epv = positionEstimationConfig()->max_eph_epv;
+    const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
+    const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
+    //use both baro and gps
+    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -599,44 +572,33 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         }
         else {
             if (posEstimator.est.vel.z > 15) {
-                if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
+                posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true;
-                    posEstimator.state.isBaroGroundValid = false;
-                }
             }
             else {
                 posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
             }
         }
 
-        // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
+        // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
         bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
                                             (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
                                              ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
 
         // Altitude
         const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
-        ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
+        ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
-        ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
+        ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
-
-        // If GPS is available - also use GPS climb rate
-        if (ctx->newFlags & EST_GPS_Z_VALID) {
-            // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
-            const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
-            const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
-            ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
-        }
 
         ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
 
         // Accelerometer bias
         if (!isAirCushionEffectDetected) {
-            ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+            ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
         }
 
-        return true;
+        correctOK = true;
     }
-    else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
+    if (ctx->newFlags & EST_GPS_Z_VALID) {
-        // If baro is not available - use GPS Z for correction on a plane
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@@ -646,20 +608,21 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         else {
             // Altitude
             const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
+            const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
 
             ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
             ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
-            ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
+            ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
             ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
 
             // Accelerometer bias
             ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
         }
 
-        return true;
+        correctOK = true;
     }
 
-    return false;
+    return correctOK;
 }
 
 static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
@@ -714,15 +677,14 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
 
 static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
 {
-    if (STATE(GPS_FIX) && navIsHeadingUsable()) {
+    UNUSED(currentTimeUs);
-        static timeUs_t lastUpdateTimeUs = 0;
+    if ((STATE(GPS_FIX)
-
+#ifdef USE_GPS_FIX_ESTIMATION
-        if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) {   // limit update rate
+            || STATE(GPS_ESTIMATED_FIX)
-            const float dt = US2S(currentTimeUs - lastUpdateTimeUs);
+#endif
-            uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt)));
+    ) && navIsHeadingUsable()) {
-            posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
+        uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y, posEstimator.est.vel.x)));
-            lastUpdateTimeUs = currentTimeUs;
+        posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
-        }
     }
 }
 
@@ -779,7 +741,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
         ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
     }
-
+    // Boost the corrections based on accWeight
+    const float accWeight = navGetAccelerometerWeight();
+    vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
+    vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
     // Apply corrections
     vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
     vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
@@ -817,13 +782,14 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
 {
     static navigationTimer_t posPublishTimer;
 
-    /* IMU operates in decidegrees while INAV operates in deg*100
-     * Use course over ground when GPS heading valid */
-    int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
-    updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
-
     /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
     if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
+        /* Publish heading update */
+        /* IMU operates in decidegrees while INAV operates in deg*100
+        * Use course over ground when GPS heading valid */
+        int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
+        updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
+
         /* Publish position update */
         if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
             // FIXME!!!!!
@@ -866,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;
 }