Merge branch 'master' of https://github.com/iNavFlight/inav into submit-gps-fix-estimation

.vscode/c_cpp_properties.json

@@ -0,0 +1,66 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceRoot}/src/main/**",
+                "${workspaceRoot}/lib/main/**",
+                "/usr/include/**"
+            ],
+            "browse": {
+                "limitSymbolsToIncludedHeaders": false,
+                "path": [
+                    "${workspaceRoot}/src/main/**",
+                    "${workspaceRoot}/lib/main/**"
+                ]
+            },
+            "intelliSenseMode": "linux-gcc-arm",
+            "cStandard": "c11",
+            "cppStandard": "c++17",
+            "defines": [
+                "MCU_FLASH_SIZE 512",
+                "USE_NAV",
+                "NAV_FIXED_WING_LANDING",
+                "USE_OSD",
+                "USE_GYRO_NOTCH_1",
+                "USE_GYRO_NOTCH_2",
+                "USE_DTERM_NOTCH",
+                "USE_ACC_NOTCH",
+                "USE_GYRO_BIQUAD_RC_FIR2",
+                "USE_D_BOOST",
+                "USE_SERIALSHOT",
+                "USE_ANTIGRAVITY",
+                "USE_ASYNC_GYRO_PROCESSING",
+                "USE_RPM_FILTER",
+                "USE_GLOBAL_FUNCTIONS",
+                "USE_DYNAMIC_FILTERS",
+                "USE_IMU_BNO055",
+                "USE_SECONDARY_IMU",
+                "USE_DSHOT",
+                "FLASH_SIZE 480",
+                "USE_I2C_IO_EXPANDER",
+                "USE_PCF8574",
+                "USE_ESC_SENSOR",
+                "USE_PROGRAMMING_FRAMEWORK",
+                "USE_SERIALRX_GHST",
+                "USE_TELEMETRY_GHST",
+                "USE_CMS",
+                "USE_DJI_HD_OSD",
+                "USE_GYRO_KALMAN",
+                "USE_RANGEFINDER",
+                "USE_RATE_DYNAMICS",
+                "USE_SMITH_PREDICTOR",
+                "USE_ALPHA_BETA_GAMMA_FILTER",
+                "USE_MAG_VCM5883",
+                "USE_TELEMETRY_JETIEXBUS",
+                "USE_NAV",
+                "USE_SDCARD_SDIO",
+                "USE_SDCARD",
+                "USE_Q_TUNE",
+                "USE_GYRO_FFT_FILTER"
+            ],
+            "configurationProvider": "ms-vscode.cmake-tools"
+        }
+    ],
+    "version": 4
+}

.vscode/tasks.json

@@ -0,0 +1,41 @@
+{
+    // See https://go.microsoft.com/fwlink/?LinkId=733558
+    // for the documentation about the tasks.json format
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "label": "Build Matek F722-SE",
+            "type": "shell",
+            "command": "make MATEKF722SE",
+            "group": "build",
+            "problemMatcher": [],
+            "options": {
+                "cwd": "${workspaceFolder}/build"
+            }
+        },
+        {
+            "label": "Build Matek F722",
+            "type": "shell",
+            "command": "make MATEKF722",
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "problemMatcher": [],
+            "options": {
+                "cwd": "${workspaceFolder}/build"
+            }
+        }
+        ,
+        {
+            "label": "CMAKE Update",
+            "type": "shell",
+            "command": "cmake ..",
+            "group": "build",
+            "problemMatcher": [],
+            "options": {
+                "cwd": "${workspaceFolder}/build"
+            }
+        }
+    ]
+}

AUTHORS

@@ -56,6 +56,7 @@ Krzysztof Rosinski
 Kyle Manna
 Larry Davis
 Marc Egli
+Marcelo Bezerra
 Mark Williams
 Martin Budden
 Matthew Evans

Dockerfile

@@ -4,15 +4,15 @@ 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
+RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build gdb
 
 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 addgroup --gid $GROUP_ID inav; exit 0;
+RUN if [ -n "$USER_ID" ]; then RUN addgroup --gid $GROUP_ID inav; exit 0; fi
-RUN adduser --disabled-password --gecos '' --uid $USER_ID --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
 
-USER inav
+RUN if [ -n "$USER_ID" ]; then USER inav; fi
 RUN git config --global --add safe.directory /src
 
 VOLUME /src

cmake/docker.sh

@@ -6,7 +6,7 @@ CURR_REV="$(git rev-parse HEAD)"
 
 initialize_cmake() {
     echo -e "*** CMake was not initialized yet, doing it now.\n"
-    cmake ..
+    cmake -GNinja ..
     echo "$CURR_REV" > "$LAST_CMAKE_AT_REV_FILE"
 }
 
@@ -26,4 +26,4 @@ else
 fi
 
 # Let Make handle the arguments coming from the build script
-make "$@"
+ninja "$@"

cmake/docker_build_sitl.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+rm -r build_SITL
+mkdir -p build_SITL
+#cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL ..
+cmake -DSITL=ON -DDEBUG=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL ..
+cd build_SITL
+ninja

cmake/docker_run_sitl.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+cd build_SITL
+
+#Lauch SITL - configurator only mode
+./inav_7.0.0_SITL
+
+#Launch SITL - connect to X-Plane. IP address should be host IP address, not 127.0.0.1. Can be found in X-Plane "Network" tab.
+#./inav_7.0.0_SITL --sim=xp --simip=192.168.2.105 --simport=49000

cmake/sitl.cmake

@@ -53,6 +53,11 @@ set(SITL_COMPILE_OPTIONS
     -funsigned-char
 )
 
+if(DEBUG)
+    message(STATUS "Debug mode enabled. Adding -g to SITL_COMPILE_OPTIONS.")
+    list(APPEND SITL_COMPILE_OPTIONS -g)
+endif()
+
 if(NOT MACOSX)
     set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
         -Wno-return-local-addr

docs/Controls.md

@@ -44,9 +44,9 @@ The stick positions are combined to activate different functions:
 | Bypass Nav Arm disable        | LOW      | HIGH    | CENTER | CENTER |
 | Save setting                  | LOW      | LOW     | LOW    | HIGH   |
 | Enter OSD Menu (CMS)          | CENTER   | LOW     | HIGH   | CENTER |
-| Enter Camera OSD(RuncamDevice)| RIGHT    | CENTER  | CENTER | CENTER |
+| Enter Camera OSD(RuncamDevice)| CENTER   | HIGH    | CENTER | CENTER |
-| Exit Camera OSD (RuncamDevice)| LEFT     | CENTER  | CENTER | CENTER |
+| Exit Camera OSD (RuncamDevice)| CENTER   | LOW     | CENTER | CENTER |
-| Confirm - Camera OSD          | RIGHT    | CENTER  | CENTER | CENTER |
+| Confirm - Camera OSD          | CENTER   | HIGH    | CENTER | CENTER |
 | Navigation - Camera OSD       | CENTER   | CENTER  | *      | *      |
 
 For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/).

docs/LED pin PWM.md

@@ -0,0 +1,90 @@
+# LED pin PWM
+
+Normally LED pin is used to drive WS2812 led strip. LED pin is held low, and every 10ms or 20ms a set of pulses is sent to change color of the 32 LEDs:
+
+![alt text](/docs/assets/images/ws2811_packets.png  "ws2811 packets")
+![alt text](/docs/assets/images/ws2811_data.png  "ws2811 data")
+
+As alternative function, it is possible to generate PWM signal with specified duty ratio on the LED pin.
+
+Feature can be used to drive external devices. It is also used to simulate [OSD joystick](OSD%20Joystick.md) to control cameras.
+
+PWM frequency is fixed to 24kHz with duty ratio between 0 and 100%:
+
+![alt text](/docs/assets/images/led_pin_pwm.png  "led pin pwm")
+
+There are four modes of operation:
+- low
+- high
+- shared_low
+- shared_high
+
+Mode is configured using ```led_pin_pwm_mode``` setting: ```LOW```, ```HIGH```, ```SHARED_LOW```, ```SHARED_HIGH```
+
+*Note that in any mode, there will be ~2 seconds LOW pulse on boot.*
+
+## LOW
+LED Pin is initialized to output low level by default and can be used to generate PWM signal.
+
+ws2812 strip can not be controlled.
+
+## HIGH
+LED Pin is initialized to output high level by default and can be used to generate PWM signal.
+
+ws2812 strip can not be controlled.
+
+## SHARED_LOW (default)
+LED Pin is used to drive WS2812 strip. Pauses between pulses are low:
+
+![alt text](/docs/assets/images/ws2811_packets.png  "ws2811 packets")
+
+It is possible to generate PWM signal with duty ratio >0...100%. 
+
+While PWM signal is generated, ws2811 strip is not updated. 
+
+When PWM generation is disabled, LED pin is used to drive ws2812 strip. 
+
+Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio < ~10%.
+
+## SHARED_HIGH
+LED Pin is used to drive WS2812 strip. Pauses between pulses are high. ws2812 pulses are prefixed with 50us low 'reset' pulse:
+
+![alt text](/docs/assets/images/ws2811_packets_high.png  "ws2811 packets_high")
+![alt text](/docs/assets/images/ws2811_data_high.png  "ws2811 data_high")
+
+ It is possible to generate PWM signal with duty ratio 0...<100%. 
+ 
+ While PWM signal is generated, ws2811 strip is not updated. 
+ 
+ When PWM generation is disabled, LED pin is used to drive ws2812 strip. Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio > ~90%.
+ 
+ After sending ws2812 protocol pulses for 32 LEDS, we held line high for 9ms, then send 50us low 'reset' pulse. Datasheet for ws2812 protocol does not describe behavior for long high pulse, but in practice it works the same as 'reset' pulse. To be safe, we also send correct low 'reset' pulse before starting next LEDs update sequence.
+ 
+ This mode is used to simulate OSD joystick. It is Ok that effectively voltage level is held >90% while driving LEDs, because OSD joystick keypress voltages are below 90%.
+ 
+ See [OSD Joystick](OSD%20Joystick.md) for more information.
+
+# Generating PWM signal with programming framework
+
+See "LED Pin PWM" operation in [Programming Framework](Programming%20Framework.md)
+
+
+# Generating PWM signal from CLI
+
+```ledpinpwm <value>``` - value = 0...100 -  enable PWM generation with specified duty cycle
+
+```ledpinpwm``` - disable PWM generation ( disable to allow ws2812 LEDs updates in shared modes )
+
+
+# Example of driving LED
+
+It is possible to drive single color LED with brightness control. Current consumption should not be greater then 1-2ma, thus LED can be used for indication only.
+
+![alt text](/docs/assets/images/ledpinpwmled.png  "led pin pwm led")
+
+# Example of driving powerfull white LED
+
+To drive power LED with brightness control, Mosfet should be used:
+
+![alt text](/docs/assets/images/ledpinpwmpowerled.png  "led pin pwm power_led")
+

docs/OSD Joystick.md

@@ -0,0 +1,94 @@
+# OSD joystick
+
+LED pin can be used to emulate 5key OSD joystick for OSD camera pin, while still driving ws2812 LEDs (shared functionality).
+
+See [LED pin PWM](LED%20pin%20PWM.md) for more details.
+
+Note that for cameras which support RuncamDevice protocol, there is alternative functionality using serial communication: [Runcam device](Runcam%20device.md)
+
+Also special adapters exist to convert RuncamDevice protocol to OSD Joystick: [Runcam control adapter](https://www.runcam.com/download/runcam_control_adapter_manual.pdf)
+
+# OSD Joystick schematics
+
+![alt text](/docs/assets/images/osd_joystick_keys.png  "osd jystick keys")
+
+Camera internal resistance seems to be 47kOhm or 9kOhm depending on camera model.
+
+Each key effectively turns on voltage divider. Voltage is sensed by the camera and is compared to the list of keys voltages with some threshold.
+
+Key voltage has to be held for at least 200ms.
+
+To simulate 5key joystick, it is sufficient to generate correct voltage on camera OSD pin.
+
+# Enabling OSD Joystick emulation
+
+```set led_pin_pwm_mode=shared_high```
+
+```set osd_joystick_enabled=on```
+
+Also enable "Multi-color RGB LED Strip support" in Configuration tab.
+
+# Connection diagram
+
+We use LED pin PWM functionality with RC filter to generate voltage:
+
+![alt text](/docs/assets/images/ledpinpwmfilter.png  "led pin pwm filter")
+
+# Example PCB layout (SMD components)
+
+RC Filter can be soldered on a small piece of PCB:
+
+![alt text](/docs/assets/images/osd_joystick.jpg  "osd joystick")
+
+# Configuring keys voltages
+
+If default voltages does not work with your camera model, then you have to measure voltages and find out corresponding PWM duty ratios.
+
+1. Connect 5keys joystick to camera.
+2. Measure voltages on OSD pin while each key is pressed.
+3. Connect camera to FC throught RC filter as shown on schematix above.
+4. Enable OSD Joystick emulation (see "Enabling OSD Joystick emulation" above)
+4. Use cli command ```led_pin_pwm <value>```, value = 0...100 to find out PWM values for each voltage.
+5. Specify PWM values in configuration and save:
+
+```set osd_joystick_down=0```
+
+```set osd_joystick_up=48```
+
+```set osd_joystick_left=63```
+
+```set osd_joystick_right=28```
+
+```set osd_joystick_enter=75```
+
+```save```
+
+# Entering OSD Joystick emulation mode
+
+Emulation can be enabled in unarmed state only. 
+
+OSD Joystick emulation mode is enabled using the following stick combination:
+
+```Throttle:CENTER Yaw:RIGHT```
+
+
+Than camera OSD can be navigated using right stick. See [Controls](Controls.md) for all stick combinations.
+
+*Note that the same stick combination is used to enable 5keys joystick emulation with RuncamDevice protocol.*
+
+Mode is exited using stick combination:
+
+```Throttle:CENTER Yaw:LEFT```
+
+# RC Box
+
+There are 3 RC Boxes which can be used in armed and unarmed state:
+- Camera 1 - Enter
+- Camera 2 - Up
+- Camera 3 - Down
+
+Other keys can be emulated using Programming framework ( see [LED pin PWM](LED%20pin%20PWM.md) for more details ).
+
+# Behavior on boot
+
+There is ~2 seconds LOW pulse during boot sequence, which corresponds to DOWN key. Fortunately, cameras seem to ignore any key events  few seconds after statup.

docs/Programming Framework.md

@@ -97,6 +97,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 49            | TIMER                         | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
 | 50            | DELTA                         | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
 | 51            | APPROX_EQUAL                  | `true` if `Operand B` is within 1% of `Operand A`. |
+| 52            | LED_PIN_PWM                   | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes)|
 
 ### Operands
 
@@ -152,6 +153,9 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 35            | AGL_STATUS                    | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
 | 36            | AGL                           | integer Above The Groud Altitude in `cm` |
 | 37            | RANGEFINDER_RAW               | integer raw distance provided by the rangefinder in `cm` |
+| 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` |
 
 #### FLIGHT_MODE
 

docs/Runcam device.md

@@ -0,0 +1,32 @@
+# Runcam device 
+
+Cameras which support [Runcam device protocol](https://support.runcam.com/hc/en-us/articles/360014537794-RunCam-Device-Protocol), can be configured using sticks.
+
+Note that for cameras which has OSD pin, there is alternative functionality: [OSD Joystick](OSD%20Joystick.md).
+
+Camera's RX/TX should be connected to FC's UART, which has "Runcam device" option selected.
+
+# Entering Joystick emulation mode
+
+Emulation can be enabled in unarmed state only. 
+
+Joystick emulation mode is enabled using the following stick combination:
+
+```RIGHT CENTER```
+
+
+Than camera OSD can be navigated using right stick. See [Controls](Controls.md) for all stick combinations.
+
+*Note that the same stick combination is used to enable [OSD Joystick](OSD%20Joystick.md).*
+
+Mode is exited using stick combination:
+
+```LEFT CENTER```
+
+# RC Box
+
+There are 3 RC Boxes which can be used in armed and unarmed state:
+- Camera 1 - Simulate Wifi button
+- Camera 2 - Simulate POWER button
+- Camera 3 - Simulate Change Mode button.
+

docs/Settings.md

@@ -258,7 +258,7 @@ Inertia force compensation method when gps is avaliable, VELNED use the acclerat
 
 | Default | Min | Max |
 | --- | --- | --- |
-| VELNED |  |  |
+| ADAPTIVE |  |  |
 
 ---
 
@@ -872,6 +872,96 @@ Enable when BLHeli32 Auto Telemetry function is used. Disable in every other cas
 
 ---
 
+### ez_aggressiveness
+
+EzTune aggressiveness
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
+### ez_axis_ratio
+
+EzTune axis ratio
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 110 | 25 | 175 |
+
+---
+
+### ez_damping
+
+EzTune damping
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
+### ez_enabled
+
+Enables EzTune feature
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
+### ez_expo
+
+EzTune expo
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
+### ez_filter_hz
+
+EzTune filter cutoff frequency
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 110 | 10 | 300 |
+
+---
+
+### ez_rate
+
+EzTune rate
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
+### ez_response
+
+EzTune response
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
+### ez_stability
+
+EzTune stability
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
 ### failsafe_delay
 
 Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay).
@@ -1232,16 +1322,6 @@ Iterm is not allowed to grow when stick position is above threshold. This solves
 
 ---
 
-### fw_iterm_throw_limit
-
-Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX |
-
----
-
 ### fw_level_pitch_gain
 
 I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations
@@ -1952,6 +2032,16 @@ Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened w
 
 ---
 
+### led_pin_pwm_mode
+
+PWM mode of LED pin.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| SHARED_LOW |  |  |
+
+---
+
 ### ledstrip_visual_beeper
 
 _// TODO_
@@ -3458,7 +3548,7 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1500 | 1000 | 2000 |
+| 1300 | 1000 | 2000 |
 
 ---
 
@@ -4032,6 +4122,16 @@ Value above which to make the OSD relative altitude indicator blink (meters)
 
 ---
 
+### osd_arm_screen_display_time
+
+Amount of time to display the arm screen [ms]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1500 | 1000 | 5000 |
+
+---
+
 ### osd_baro_temp_alarm_max
 
 Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade)
@@ -4362,6 +4462,76 @@ Temperature under which the IMU temperature OSD element will start blinking (dec
 
 ---
 
+### osd_inav_to_pilot_logo_spacing
+
+The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 8 | 0 | 20 |
+
+---
+
+### osd_joystick_down
+
+PWM value for DOWN key
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 100 |
+
+---
+
+### osd_joystick_enabled
+
+Enable OSD Joystick emulation
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
+### osd_joystick_enter
+
+PWM value for ENTER key
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 75 | 0 | 100 |
+
+---
+
+### osd_joystick_left
+
+PWM value for LEFT key
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 63 | 0 | 100 |
+
+---
+
+### osd_joystick_right
+
+PWM value for RIGHT key
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 28 | 0 | 100 |
+
+---
+
+### osd_joystick_up
+
+PWM value for UP key
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 48 | 0 | 100 |
+
+---
+
 ### osd_left_sidebar_scroll
 
 _// TODO_
@@ -4392,9 +4562,9 @@ LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50
 
 ---
 
-### osd_mah_used_precision
+### osd_mah_precision
 
-Number of digits used to display mAh used.
+Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -4782,6 +4952,16 @@ IMPERIAL, METRIC, UK
 
 ---
 
+### osd_use_pilot_logo
+
+Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### osd_video_system
 
 Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR' and `BF43COMPAT`
@@ -4792,6 +4972,16 @@ Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF'
 
 ---
 
+### pid_iterm_limit_percent
+
+Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 33 | 0 | 200 |
+
+---
+
 ### pid_type
 
 Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID`
@@ -5692,9 +5882,19 @@ See tpa_rate.
 
 ---
 
+### tpa_on_yaw
+
+Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### tpa_rate
 
-Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate.
+Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate.
 
 | Default | Min | Max |
 | --- | --- | --- |

docs/Temperature sensors.md

@@ -89,23 +89,6 @@ To change for example the configuration of the fourth sensor to label `BATT`, mi
 
 `temp_sensor 3 2 7d01186838f2ff28 5 450 0 BATT`
 
-## Building a custom firmware with temperature sensor support (F3 only)
-
-This needs to be added in the `target.h` file:
-
-```
-#define USE_TEMPERATURE_SENSOR
-#define TEMPERATURE_I2C_BUS BUS_I2Cx // replace x with the index of the I²C bus the temperature sensors will be connected to
-
-// for LM75 sensors support
-#define USE_TEMPERATURE_LM75
-
-// for DS18B20 sensors
-#define USE_1WIRE
-#define USE_1WIRE_DS2482
-#define USE_TEMPERATURE_DS18B20
-```
-
 ## Configuring the way OSD temperature labels are displayed
 
 You can use the `osd_temp_label_align` setting to chose how the labels for the temperature sensor's values are displayed. Possible alignment values are `LEFT` and `RIGHT`.

docs/development/Building in Docker.md

@@ -37,8 +37,19 @@ You'll have to manually execute the same steps that the build script does:
    + This step is only needed the first time.
 2. `docker run --rm -it -u root -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, or by removing "USER inav" line from the .\DockerFile before building image.   
+   + 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`
+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`
+
+4. Building SITL: 
+
+`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -u root -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`.
+   + 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/Hardware In The Loop (HITL) plugin for X-Plane.md

@@ -1,4 +1,4 @@
-# Software In The Loop (HITL) plugin for X-Plane 11
+# Hardware In The Loop (HITL) plugin for X-Plane 11/12
 
 **Hardware-in-the-loop (HITL) simulation**, is a technique that is used in the development and testing of complex real-time embedded systems. 
 
@@ -6,6 +6,6 @@
 
 **INAV-X-Plane-HITL** is plugin for **X-Plane** for testing and developing flight controllers with **INAV flight controller firmware** 
 
-https://github.com/iNavFlight/inav.
+https://github.com/RomanLut/INAV-X-Plane-HITL
 
 HITL technique can be used to test features during development. Please check page above for installation instructions.

docs/development/How to test a pull request.md

@@ -16,7 +16,7 @@ Building the pull request manually or using custom/unofficial targets is not the
 - Make sure the pull request has passed all checks, otherwise you may not have pre-compiled firmware images.
 - Make a diff all backup of your existing INAV configuration.
 - Take notes of what INAV target you are using.
-- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](http://seyrsnys.myzen.co.uk/inav-configurator-next/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI.
+- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](https://seyrsnys-inav-cfg-next.surge.sh/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI.
 
 # Finding the pull request
 This is easy, but you will need to be logged in to your GitHub account.
@@ -31,18 +31,11 @@ Once you find the one you are looking for, go ahead an open it!
 
 Click on the ``Checks`` tab
 
-Click on ``Build firmware``, it should take you to the ``Actions`` tab.
+Click on the down arrow next to the number of artifacts
-![Search results](assets/pr_testing/build_firmware.png)
+![Artifact list](assets/pr_testing/artifacts_download.png)
 
-You should see a summary with a column saying ``Artifacts`` and a number. Click on the number to be taken to the list of artifacts.
+You should see a list of files. The one without SITL in the name, the biggest one, will be a zip file with all official target .hex files. Click on it to download it to your computer.
-
+Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings.
-![Search results](assets/pr_testing/actions_summary.png)
-
-On the ``Artifacts`` list, there should be an artifact without SITL in its name.
-
-![Search results](assets/pr_testing/artifact_listing.png)
-
- Click on it to download the zip file containing pre-compiled firmware images for all INAV official targets. Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings.
 
 # I have flashed the new firmware, what should I do next?
 

src/main/CMakeLists.txt

@@ -341,6 +341,8 @@ main_sources(COMMON_SRC
     flight/secondary_dynamic_gyro_notch.h
     flight/dynamic_lpf.c
     flight/dynamic_lpf.h
+    flight/ez_tune.c
+    flight/ez_tune.h
 
     io/beeper.c
     io/beeper.h
@@ -523,6 +525,8 @@ main_sources(COMMON_SRC
     io/osd_grid.h
     io/osd_hud.c
     io/osd_hud.h
+    io/osd_joystick.c
+    io/osd_joystick.h
     io/smartport_master.c
     io/smartport_master.h
     io/vtx.c

src/main/cms/cms.h

@@ -37,9 +37,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration);
 void cmsUpdate(uint32_t currentTimeUs);
 void cmsSetExternKey(cms_key_e extKey);
 
-#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
+#define CMS_STARTUP_HELP_TEXT1  "MENU: THR MID"
-#define CMS_STARTUP_HELP_TEXT2     "+ YAW LEFT"
+#define CMS_STARTUP_HELP_TEXT2  "+ YAW LEFT"
-#define CMS_STARTUP_HELP_TEXT3     "+ PITCH UP"
+#define CMS_STARTUP_HELP_TEXT3  "+ PITCH UP"
 
 // cmsMenuExit special ptr values
 #define CMS_EXIT                (0)

src/main/common/maths.h

@@ -25,15 +25,15 @@
 #endif
 
 // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
-#define FAST_MATH             // order 9 approximation
+#define FAST_MATH       // order 9 approximation
-//#define VERY_FAST_MATH      // order 7 approximation
+//#define VERY_FAST_MATH  // order 7 approximation
 
 // Use floating point M_PI instead explicitly.
-#define M_PIf       3.14159265358979323846f
+#define M_PIf   3.14159265358979323846f
-#define M_LN2f      0.69314718055994530942f
+#define M_LN2f  0.69314718055994530942f
-#define M_Ef        2.71828182845904523536f
+#define M_Ef    2.71828182845904523536f
 
-#define RAD    (M_PIf / 180.0f)
+#define RAD (M_PIf / 180.0f)
 
 #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
 #define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f)
@@ -56,46 +56,46 @@
 #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
 #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
 
-#define CENTIMETERS_TO_CENTIFEET(cm)            (cm / 0.3048f)
+#define CENTIMETERS_TO_CENTIFEET(cm)    (cm / 0.3048f)
-#define CENTIMETERS_TO_FEET(cm)                 (cm / 30.48f)
+#define CENTIMETERS_TO_FEET(cm)         (cm / 30.48f)
-#define CENTIMETERS_TO_METERS(cm)               (cm / 100.0f)
+#define CENTIMETERS_TO_METERS(cm)       (cm / 100.0f)
 
-#define METERS_TO_CENTIMETERS(m)                (m * 100)
+#define METERS_TO_CENTIMETERS(m)        (m * 100)
 
-#define CMSEC_TO_CENTIMPH(cms)      (cms * 2.2369363f)
+#define CMSEC_TO_CENTIMPH(cms)          (cms * 2.2369363f)
-#define CMSEC_TO_CENTIKPH(cms)      (cms * 3.6f)
+#define CMSEC_TO_CENTIKPH(cms)          (cms * 3.6f)
-#define CMSEC_TO_CENTIKNOTS(cms)    (cms * 1.943845f)
+#define CMSEC_TO_CENTIKNOTS(cms)        (cms * 1.943845f)
 
 #define C_TO_KELVIN(temp) (temp + 273.15f)
 
 // Standard Sea Level values
 // Ref:https://en.wikipedia.org/wiki/Standard_sea_level
-#define SSL_AIR_DENSITY         1.225f // kg/m^3
+#define SSL_AIR_DENSITY		1.225f // kg/m^3
-#define SSL_AIR_PRESSURE 101325.01576f // Pascal
+#define SSL_AIR_PRESSURE	101325.01576f // Pascal
-#define SSL_AIR_TEMPERATURE    288.15f // K
+#define SSL_AIR_TEMPERATURE	288.15f // K
 
 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
-#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar)         \
+#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \
-    ( __extension__ ({                                      \
+    ( __extension__ ({                              \
-            __typeof__(lexpr) lvar = (lexpr);               \
+        __typeof__(lexpr) lvar = (lexpr);           \
-            __typeof__(rexpr) rvar = (rexpr);               \
+        __typeof__(rexpr) rvar = (rexpr);           \
-            lvar binoper rvar ? lvar : rvar;                \
+        lvar binoper rvar ? lvar : rvar;            \
-        }))
+    }))
 #define _CHOOSE_VAR2(prefix, unique) prefix##unique
 #define _CHOOSE_VAR(prefix, unique) _CHOOSE_VAR2(prefix, unique)
-#define _CHOOSE(binoper, lexpr, rexpr)                   \
+#define _CHOOSE(binoper, lexpr, rexpr)          \
-    _CHOOSE2(                                            \
+    _CHOOSE2(                                   \
-        binoper,                                         \
+        binoper,                                \
-        lexpr, _CHOOSE_VAR(_left, __COUNTER__),          \
+        lexpr, _CHOOSE_VAR(_left, __COUNTER__), \
-        rexpr, _CHOOSE_VAR(_right, __COUNTER__)          \
+        rexpr, _CHOOSE_VAR(_right, __COUNTER__) \
         )
 #define MIN(a, b) _CHOOSE(<, a, b)
 #define MAX(a, b) _CHOOSE(>, a, b)
 
-#define _ABS_II(x, var)                           \
+#define _ABS_II(x, var)             \
-    ( __extension__ ({                            \
+    ( __extension__ ({              \
-        __typeof__(x) var = (x);                  \
+        __typeof__(x) var = (x);    \
-        var < 0 ? -var : var;                     \
+        var < 0 ? -var : var;       \
     }))
 #define _ABS_I(x, var) _ABS_II(x, var)
 #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
@@ -114,8 +114,7 @@ typedef union {
     fp_angles_def angles;
 } fp_angles_t;
 
-typedef struct stdev_s
+typedef struct stdev_s {
-{
     float m_oldM, m_newM, m_oldS, m_newS;
     int m_n;
 } stdev_t;

src/main/config/parameter_group_ids.h

@@ -120,7 +120,11 @@
 #define PG_POWER_LIMITS_CONFIG 1030
 #define PG_OSD_COMMON_CONFIG 1031
 #define PG_TIMER_OVERRIDE_CONFIG 1032
-#define PG_INAV_END 1032
+#define PG_EZ_TUNE 1033
+#define PG_LEDPIN_CONFIG 1034
+#define PG_OSD_JOYSTICK_CONFIG 1035
+#define PG_INAV_END PG_OSD_JOYSTICK_CONFIG
+
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/accgyro/accgyro_bmi270.c

@@ -214,7 +214,7 @@ static void bmi270AccAndGyroInit(gyroDev_t *gyro)
     delay(1);
 
     // Configure the accelerometer full-scale range
-    busWrite(busDev, BMI270_REG_ACC_RANGE, BMI270_ACC_RANGE_8G);
+    busWrite(busDev, BMI270_REG_ACC_RANGE, BMI270_ACC_RANGE_16G);
     delay(1);
 
     // Configure the gyro
@@ -301,7 +301,7 @@ static void bmi270GyroInit(gyroDev_t *gyro)
 static void bmi270AccInit(accDev_t *acc)
 {
     // sensor is configured during gyro init
-    acc->acc_1G = 4096;   // 8G sensor scale
+    acc->acc_1G = 2048;   // 16G sensor scale
 }
 
 bool bmi270AccDetect(accDev_t *acc)

src/main/drivers/accgyro/accgyro_icm42605.c

@@ -78,11 +78,13 @@
 #define ICM42605_INT_TPULSE_DURATION_100            (0 << ICM42605_INT_TPULSE_DURATION_BIT)
 #define ICM42605_INT_TPULSE_DURATION_8              (1 << ICM42605_INT_TPULSE_DURATION_BIT)
 
-
 #define ICM42605_RA_INT_SOURCE0                     0x65
 #define ICM42605_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
 #define ICM42605_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
 
+#define ICM42605_INTF_CONFIG1                       0x4D
+#define ICM42605_INTF_CONFIG1_AFSR_MASK             0xC0
+#define ICM42605_INTF_CONFIG1_AFSR_DISABLE          0x40
 
 static void icm42605AccInit(accDev_t *acc)
 {
@@ -190,6 +192,15 @@ static void icm42605AccAndGyroInit(gyroDev_t *gyro)
     busWrite(dev, ICM42605_RA_INT_CONFIG1, intConfig1Value);
     delay(15);
 
+    //Disable AFSR as in BF and Ardupilot
+    uint8_t intfConfig1Value;
+    busRead(dev, ICM42605_INTF_CONFIG1, &intfConfig1Value);
+    intfConfig1Value &= ~ICM42605_INTF_CONFIG1_AFSR_MASK;
+    intfConfig1Value |= ICM42605_INTF_CONFIG1_AFSR_DISABLE;
+    busWrite(dev, ICM42605_INTF_CONFIG1, intfConfig1Value);
+
+    delay(15);
+
     busSetSpeed(dev, BUS_SPEED_FAST);
 }
 

src/main/drivers/bus_i2c_at32f43x.c

@@ -70,7 +70,7 @@ static void i2cUnstick(IO_t scl, IO_t sda);
 
 //Define thi I2C hardware map
 static i2cDevice_t i2cHardwareMap[I2CDEV_COUNT] = {
-    { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C1_EVT_IRQn, .er_irq = I2C1_ERR_IRQn, .af = GPIO_MUX_8 },
+    { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C1_EVT_IRQn, .er_irq = I2C1_ERR_IRQn, .af = GPIO_MUX_4 },
     { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C2_EVT_IRQn, .er_irq = I2C2_ERR_IRQn, .af = GPIO_MUX_4 },
     { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C3_EVT_IRQn, .er_irq = I2C3_ERR_IRQn, .af = GPIO_MUX_4 },
 };

src/main/drivers/light_ws2811strip.c

@@ -43,15 +43,26 @@
 #include "drivers/timer.h"
 #include "drivers/light_ws2811strip.h"
 
+#include "config/parameter_group_ids.h"
+#include "fc/settings.h"
+#include "fc/runtime_config.h"
+
 #define WS2811_PERIOD (WS2811_TIMER_HZ / WS2811_CARRIER_HZ)
 #define WS2811_BIT_COMPARE_1 ((WS2811_PERIOD * 2) / 3)
 #define WS2811_BIT_COMPARE_0 (WS2811_PERIOD / 3)
 
+PG_REGISTER_WITH_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig, PG_LEDPIN_CONFIG, 0);
+
+PG_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig,
+    .led_pin_pwm_mode = SETTING_LED_PIN_PWM_MODE_DEFAULT
+);
+
 static DMA_RAM timerDMASafeType_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
 
 static IO_t ws2811IO = IO_NONE;
 static TCH_t * ws2811TCH = NULL;
 static bool ws2811Initialised = false;
+static bool pwmMode = false;
 
 static hsvColor_t ledColorBuffer[WS2811_LED_STRIP_LENGTH];
 
@@ -91,6 +102,24 @@ void setStripColors(const hsvColor_t *colors)
     }
 }
 
+bool ledConfigureDMA(void) {
+    /* Compute the prescaler value */
+    uint8_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ;
+
+    timerConfigBase(ws2811TCH, period, WS2811_TIMER_HZ);
+    timerPWMConfigChannel(ws2811TCH, 0);
+
+    return timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE);
+}
+
+void ledConfigurePWM(void) {
+        timerConfigBase(ws2811TCH, 100, WS2811_TIMER_HZ );
+        timerPWMConfigChannel(ws2811TCH, 0);
+        timerPWMStart(ws2811TCH);
+        timerEnable(ws2811TCH);
+        pwmMode = true;
+}
+
 void ws2811LedStripInit(void)
 {
     const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
@@ -104,27 +133,32 @@ void ws2811LedStripInit(void)
         return;
     }
 
-    /* Compute the prescaler value */
-    uint8_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ;
-
     ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
     IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0);
     IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction);
 
-    timerConfigBase(ws2811TCH, period, WS2811_TIMER_HZ);
+    if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) {
-    timerPWMConfigChannel(ws2811TCH, 0);
+        ledConfigurePWM();
+        *timerCCR(ws2811TCH) = 0;
+    } else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) {
+        ledConfigurePWM();
+        *timerCCR(ws2811TCH) = 100;
+    } else {
+        if (!ledConfigureDMA()) {
+            // If DMA failed - abort
+            ws2811Initialised = false;
+            return;
+        }
 
-    // If DMA failed - abort
+        // Zero out DMA buffer
-    if (!timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE)) {
+        memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer));
-        ws2811Initialised = false;
+        if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_SHARED_HIGH ) {
-        return;
+           ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE-1] = 255;
+        }
+        ws2811Initialised = true;
+
+        ws2811UpdateStrip();
     }
-
-    // Zero out DMA buffer
-    memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer));
-    ws2811Initialised = true;
-
-    ws2811UpdateStrip();
 }
 
 bool isWS2811LedStripReady(void)
@@ -140,7 +174,7 @@ STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color)
     uint32_t grb = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b);
 
     for (int8_t index = 23; index >= 0; index--) {
-        ledStripDMABuffer[dmaBufferOffset++] = (grb & (1 << index)) ? WS2811_BIT_COMPARE_1 : WS2811_BIT_COMPARE_0;
+        ledStripDMABuffer[WS2811_DELAY_BUFFER_LENGTH + dmaBufferOffset++] = (grb & (1 << index)) ? WS2811_BIT_COMPARE_1 : WS2811_BIT_COMPARE_0;
     }
 }
 
@@ -153,7 +187,7 @@ void ws2811UpdateStrip(void)
     static rgbColor24bpp_t *rgb24;
 
     // don't wait - risk of infinite block, just get an update next time round
-    if (timerPWMDMAInProgress(ws2811TCH)) {
+    if (pwmMode || timerPWMDMAInProgress(ws2811TCH)) {
         return;
     }
 
@@ -178,4 +212,40 @@ void ws2811UpdateStrip(void)
     timerPWMStartDMA(ws2811TCH);
 }
 
+//value
+void ledPinStartPWM(uint16_t value) {
+    if (ws2811TCH == NULL) {
+        return;
+    }
+
+	if ( !pwmMode ) {
+	    timerPWMStopDMA(ws2811TCH);
+        //FIXME: implement method to release DMA
+        ws2811TCH->dma->owner = OWNER_FREE;
+
+        ledConfigurePWM();
+    }
+	*timerCCR(ws2811TCH) = value;
+}
+
+void ledPinStopPWM(void) {
+    if (ws2811TCH == NULL || !pwmMode ) {
+        return;
+    }
+
+    if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) {
+		*timerCCR(ws2811TCH) = 100;
+        return;
+    } else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) {
+		*timerCCR(ws2811TCH) = 0;
+        return;
+    } 
+    pwmMode = false;
+
+    if (!ledConfigureDMA()) {
+        ws2811Initialised = false;
+    }
+}
+
+
 #endif

src/main/drivers/light_ws2811strip.h

@@ -17,23 +17,41 @@
 
 #pragma once
 #include "common/color.h"
+#include "config/parameter_group.h"
 
 #define WS2811_LED_STRIP_LENGTH 32
 #define WS2811_BITS_PER_LED 24
-#define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay
+#define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay 
 
 #define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH)
 
-#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH)   // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
+#define WS2811_DMA_BUFFER_SIZE (WS2811_DELAY_BUFFER_LENGTH + WS2811_DATA_BUFFER_SIZE + 1)   // leading bytes (reset low 302us) + data bytes LEDS*3  + 1 byte(keep line high optionally)
 
 #define WS2811_TIMER_HZ         2400000
 #define WS2811_CARRIER_HZ       800000
 
+typedef enum {
+    LED_PIN_PWM_MODE_SHARED_LOW = 0,
+    LED_PIN_PWM_MODE_SHARED_HIGH = 1,
+    LED_PIN_PWM_MODE_LOW = 2,
+    LED_PIN_PWM_MODE_HIGH = 3
+} led_pin_pwm_mode_e;
+
+typedef struct ledPinConfig_s {
+    uint8_t led_pin_pwm_mode;  //led_pin_pwm_mode_e
+} ledPinConfig_t;
+
+PG_DECLARE(ledPinConfig_t, ledPinConfig);
+
 void ws2811LedStripInit(void);
 void ws2811LedStripHardwareInit(void);
 void ws2811LedStripDMAEnable(void);
 bool ws2811LedStripDMAInProgress(void);
 
+//value 0...100
+void ledPinStartPWM(uint16_t value);
+void ledPinStopPWM(void);
+
 void ws2811UpdateStrip(void);
 
 void setLedHsv(uint16_t index, const hsvColor_t *color);

src/main/drivers/osd_symbols.h

@@ -58,7 +58,7 @@
 //                                  0x26  // 038 ASCII &
 #define SYM_VTX_POWER               0x27  // 039 VTx Power
 //                                  0x28  // 040 to 062 ASCII
-#define SYM_AH_NM       		    0x3F  // 063 Ah/NM
+#define SYM_AH_NM                   0x3F  // 063 Ah/NM
 //                                  0x40  // 064 to 095 ASCII
 #define SYM_MAH_NM_0                0x60  // 096 mAh/NM left
 #define SYM_MAH_NM_1                0x61  // 097 mAh/NM right
@@ -78,7 +78,7 @@
 #define SYM_WH                      0x6D  // 109 WH
 #define SYM_WH_KM                   0x6E  // 110 WH/KM
 #define SYM_WH_MI                   0x6F  // 111 WH/MI
-#define SYM_WH_NM       		    0x70  // 112 Wh/NM
+#define SYM_WH_NM                   0x70  // 112 Wh/NM
 #define SYM_WATT                    0x71  // 113 WATTS
 #define SYM_MW                      0x72  // 114 mW
 #define SYM_KILOWATT                0x73  // 115 kW
@@ -159,6 +159,7 @@
 #define SYM_HEADING_W               0xCB  // 203 Heading Graphic west
 #define SYM_HEADING_DIVIDED_LINE    0xCC  // 204 Heading Graphic
 #define SYM_HEADING_LINE            0xCD  // 205 Heading Graphic
+
 #define SYM_MAX                     0xCE  // 206 MAX symbol
 #define SYM_PROFILE                 0xCF  // 207 Profile symbol
 #define SYM_SWITCH_INDICATOR_LOW    0xD0  // 208 Switch High
@@ -175,10 +176,10 @@
 #define SYM_FLIGHT_HOURS_REMAINING  0xDB  // 219 Flight time (hours) remaining
 #define SYM_GROUND_COURSE           0xDC  // 220 Ground course
 #define SYM_ALERT                   0xDD  // 221 General alert symbol
-
 #define SYM_TERRAIN_FOLLOWING       0xFB  // 251 Terrain following (also Alt adjust)
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
+
 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo
 #define SYM_LOGO_WIDTH              10
 #define SYM_LOGO_HEIGHT             4
@@ -225,9 +226,10 @@
 #define SYM_HUD_SIGNAL_3            0x163 // 355 Hud signal icon 75%
 #define SYM_HUD_SIGNAL_4            0x164 // 356 Hud signal icon 100%
 
-#define SYM_HOME_DIST 	            0x165 // 357 DIST
+#define SYM_HOME_DIST               0x165 // 357 DIST
 #define SYM_AH_CH_CENTER            0x166 // 358 Crossair center
 #define SYM_FLIGHT_DIST_REMAINING   0x167 // 359 Flight distance reminaing
+#define SYM_ODOMETER                0x168 // 360 Odometer
 
 #define SYM_AH_CH_TYPE3             0x190 // 400 to 402, crosshair 3
 #define SYM_AH_CH_TYPE4             0x193 // 403 to 405, crosshair 4
@@ -261,8 +263,13 @@
 #define SYM_SERVO_PAN_IS_OFFSET_L   0x1C7 // 455 Pan servo is offset left
 #define SYM_SERVO_PAN_IS_OFFSET_R   0x1C8 // 456 Pan servo is offset right
 
+#define SYM_PILOT_LOGO_SML_L        0x1D5 // 469 small Pilot logo Left
+#define SYM_PILOT_LOGO_SML_C        0x1D6 // 470 small Pilot logo Centre
+#define SYM_PILOT_LOGO_SML_R        0x1D7 // 471 small Pilot logo Right
+#define SYM_PILOT_LOGO_LRG_START    0x1D8 // 472 to 511, Pilot logo
+
 #else
 
-#define TEMP_SENSOR_SYM_COUNT 0
+#define TEMP_SENSOR_SYM_COUNT       0
 
 #endif // USE_OSD

src/main/drivers/timer_impl_hal.c

@@ -81,13 +81,16 @@ void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz)
     TIM_HandleTypeDef * timHandle = tch->timCtx->timHandle;
     TIM_TypeDef * timer = tch->timCtx->timDef->tim;
 
-    if (timHandle->Instance == timer) {
+    uint16_t period1 = (period - 1) & 0xffff;
+    uint16_t prescaler1 = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1;
+
+    if (timHandle->Instance == timer && timHandle->Init.Prescaler == prescaler1 && timHandle->Init.Period == period1) {
         return;
     }
 
     timHandle->Instance = timer;
-    timHandle->Init.Prescaler = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1;
+    timHandle->Init.Prescaler = prescaler1;
-    timHandle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
+    timHandle->Init.Period = period1; // AKA TIMx_ARR
     timHandle->Init.RepetitionCounter = 0;
     timHandle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
     timHandle->Init.CounterMode = TIM_COUNTERMODE_UP;
@@ -565,6 +568,15 @@ void impl_timerPWMStartDMA(TCH_t * tch)
 
 void impl_timerPWMStopDMA(TCH_t * tch)
 {
-    (void)tch;
+    const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
-    // FIXME
+    DMA_TypeDef *dmaBase = tch->dma->dma;
+
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
+        LL_DMA_DisableStream(dmaBase, streamLL);
+        DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
+    }
+    tch->dmaState = TCH_DMA_IDLE;
+
+    HAL_TIM_Base_Start(tch->timCtx->timHandle);
 }

src/main/drivers/timer_impl_stdperiph.c

@@ -301,7 +301,12 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     TIM_CtrlPWMOutputs(timer, ENABLE);
     TIM_ARRPreloadConfig(timer, ENABLE);
 
-    TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
+    if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
+        TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable);
+    } else {
+        TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
+    }
+    
     TIM_Cmd(timer, ENABLE);
 
     dmaInit(tch->dma, OWNER_TIMER, 0);
@@ -382,7 +387,12 @@ bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, vo
     TIM_CtrlPWMOutputs(timer, ENABLE);
     TIM_ARRPreloadConfig(timer, ENABLE);
 
-    TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
+    if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
+        TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable);
+    } else {
+        TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
+    }
+
     TIM_Cmd(timer, ENABLE);
 
     if (!tch->timCtx->dmaBurstRef) {
@@ -506,5 +516,6 @@ void impl_timerPWMStopDMA(TCH_t * tch)
 {
     DMA_Cmd(tch->dma->ref, DISABLE);
     TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE);
+    tch->dmaState = TCH_DMA_IDLE;
     TIM_Cmd(tch->timHw->tim, ENABLE);
 }

src/main/drivers/timer_impl_stdperiph_at32.c

@@ -404,6 +404,6 @@ void impl_timerPWMStopDMA(TCH_t * tch)
 {
     dma_channel_enable(tch->dma->ref,FALSE);
     tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE);
+    tch->dmaState = TCH_DMA_IDLE;
     tmr_counter_enable(tch->timHw->tim, TRUE);
-
 }

src/main/fc/cli.c

@@ -68,6 +68,7 @@ bool cliMode = false;
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
 #include "drivers/vtx_common.h"
+#include "drivers/light_ws2811strip.h"
 
 #include "fc/fc_core.h"
 #include "fc/cli.h"
@@ -1688,6 +1689,20 @@ static void cliModeColor(char *cmdline)
         cliPrintLinef("mode_color %u %u %u", modeIdx, funIdx, color);
     }
 }
+
+static void cliLedPinPWM(char *cmdline)
+{
+    int i;
+
+    if (isEmpty(cmdline)) {
+        ledPinStopPWM();
+        cliPrintLine("PWM stopped");
+    } else {       
+        i = fastA2I(cmdline);
+        ledPinStartPWM(i);
+        cliPrintLinef("PWM started: %d%%",i);
+    }
+}
 #endif
 
 static void cliDelay(char* cmdLine) {
@@ -4035,6 +4050,7 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
 #ifdef USE_LED_STRIP
     CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed),
+    CLI_COMMAND_DEF("ledpinpwm", "start/stop PWM on LED pin, 0..100 duty ratio", "[<value>]\r\n", cliLedPinPWM),
 #endif
     CLI_COMMAND_DEF("map", "configure rc channel order", "[<map>]", cliMap),
     CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory),

src/main/fc/config.c

@@ -64,6 +64,7 @@
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/failsafe.h"
+#include "flight/ez_tune.h"
 
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
@@ -305,6 +306,7 @@ static void activateConfig(void)
 {
     activateControlRateConfig();
     activateBatteryProfile();
+    activateMixerConfig();
 
     resetAdjustmentStates();
 
@@ -425,6 +427,9 @@ bool setConfigProfile(uint8_t profileIndex)
     systemConfigMutable()->current_profile_index = profileIndex;
     // set the control rate profile to match
     setControlRateProfile(profileIndex);
+#ifdef USE_EZ_TUNE
+    ezTuneUpdate();
+#endif
     return ret;
 }
 
@@ -486,7 +491,6 @@ bool setConfigMixerProfile(uint8_t profileIndex)
         profileIndex = 0;
     }
     systemConfigMutable()->current_mixer_profile_index = profileIndex;
-    // setMixerProfile(profileIndex);
     return ret;
 }
 

src/main/fc/controlrate_profile.c

@@ -44,6 +44,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
                 .rcMid8 = SETTING_THR_MID_DEFAULT,
                 .rcExpo8 = SETTING_THR_EXPO_DEFAULT,
                 .dynPID = SETTING_TPA_RATE_DEFAULT,
+                .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT,
                 .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT,
                 .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT
             },

src/main/fc/controlrate_profile_config_struct.h

@@ -29,6 +29,7 @@ typedef struct controlRateConfig_s {
         uint8_t rcMid8;
         uint8_t rcExpo8;
         uint8_t dynPID;
+        bool dynPID_on_YAW;
         uint16_t pa_breakpoint;                // Breakpoint where TPA is activated
         uint16_t fixedWingTauMs;               // Time constant of airplane TPA PT1-filter
     } throttle;

src/main/fc/fc_core.c

@@ -106,6 +106,7 @@ enum {
 #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000
 #define EMERGENCY_ARMING_COUNTER_STEP_MS 1000
 #define EMERGENCY_ARMING_MIN_ARM_COUNT 10
+#define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000
 
 timeDelta_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 static timeUs_t flightTime = 0;
@@ -120,6 +121,7 @@ uint8_t motorControlEnable = false;
 static bool isRXDataNew;
 static disarmReason_t lastDisarmReason = DISARM_NONE;
 timeUs_t lastDisarmTimeUs = 0;
+timeMs_t emergRearmStabiliseTimeout = 0;
 
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;
@@ -315,11 +317,11 @@ static void updateArmingStatus(void)
 
         /* CHECK: Do not allow arming if Servo AutoTrim is enabled */
         if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
-	       ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
+           ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
-	    }
+        }
         else {
-	       DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
+           DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
-	    }
+        }
 
 #ifdef USE_DSHOT
         /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */
@@ -435,6 +437,7 @@ void disarm(disarmReason_t disarmReason)
         lastDisarmReason = disarmReason;
         lastDisarmTimeUs = micros();
         DISABLE_ARMING_FLAG(ARMED);
+        DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
 
 #ifdef USE_BLACKBOX
         if (feature(FEATURE_BLACKBOX)) {
@@ -505,14 +508,27 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
     return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
 }
 
-#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
+bool emergInflightRearmEnabled(void)
+{
+    /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */
+    timeMs_t currentTimeMs = millis();
+    emergRearmStabiliseTimeout = 0;
 
-void releaseSharedTelemetryPorts(void) {
+    if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) ||
-    serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+        (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
-    while (sharedPort) {
+        return false;
-        mspSerialReleasePortIfAllocated(sharedPort);
-        sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
     }
+
+    // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying
+    bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f;
+
+    if (isProbablyStillFlying() || mcDisarmVertVelCheck) {
+        emergRearmStabiliseTimeout = currentTimeMs + 5000;  // activate Angle mode for 5s after rearm to help stabilise craft
+        ENABLE_STATE(IN_FLIGHT_EMERG_REARM);
+        return true;
+    }
+
+    return false;   // craft doesn't appear to be flying, don't allow emergency rearm
 }
 
 void tryArm(void)
@@ -538,9 +554,10 @@ void tryArm(void)
 #endif
 
 #ifdef USE_PROGRAMMING_FRAMEWORK
-    if (!isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
+    if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() ||
+        LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
 #else
-    if (!isArmingDisabled() || emergencyArmingIsEnabled()) {
+    if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) {
 #endif
         // If nav_extra_arming_safety was bypassed we always
         // allow bypassing it even without the sticks set
@@ -558,11 +575,14 @@ void tryArm(void)
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
         //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
         ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
-        logicConditionReset();
 
+        if (!STATE(IN_FLIGHT_EMERG_REARM)) {
+            resetLandingDetectorActiveState();  // reset landing detector after arming to avoid false detection before flight
+            logicConditionReset();
 #ifdef USE_PROGRAMMING_FRAMEWORK
-        programmingPidReset();
+            programmingPidReset();
 #endif
+        }
 
         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
@@ -595,6 +615,16 @@ void tryArm(void)
     }
 }
 
+#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
+
+void releaseSharedTelemetryPorts(void) {
+    serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+    while (sharedPort) {
+        mspSerialReleasePortIfAllocated(sharedPort);
+        sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+    }
+}
+
 void processRx(timeUs_t currentTimeUs)
 {
     // Calculate RPY channel data
@@ -645,9 +675,12 @@ void processRx(timeUs_t currentTimeUs)
         processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
     }
 
+    // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
+    bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
+    bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
     bool canUseHorizonMode = true;
 
-    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
+    if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) {
         // bumpless transfer to Level mode
         canUseHorizonMode = false;
 
@@ -727,6 +760,8 @@ void processRx(timeUs_t currentTimeUs)
         } else {
             DISABLE_FLIGHT_MODE(MANUAL_MODE);
         }
+    }else{
+        DISABLE_FLIGHT_MODE(MANUAL_MODE);
     }
 
     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
@@ -814,7 +849,6 @@ void processRx(timeUs_t currentTimeUs)
         }
     }
 #endif
-
 }
 
 // Function for loop trigger
@@ -871,7 +905,14 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     if (!ARMING_FLAG(ARMED)) {
         armTime = 0;
 
-        processDelayedSave();
+        // Delay saving for 0.5s to allow other functions to process save actions on disarm
+        if (currentTimeUs - lastDisarmTimeUs > USECS_PER_SEC / 2) {
+            processDelayedSave();
+        }
+    }
+
+    if (armTime > 1 * USECS_PER_SEC) {     // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose
+        DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
     }
 
 #if defined(SITL_BUILD)
@@ -923,15 +964,15 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
 
 #ifdef USE_SIMULATOR
-	if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
+    if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-	    if (isServoOutputEnabled()) {
+        if (isServoOutputEnabled()) {
-	        writeServos();
+            writeServos();
-	    }
+        }
 
-	    if (motorControlEnable) {
+        if (motorControlEnable) {
-	        writeMotors();
+            writeMotors();
-	    }
+        }
-	}
+    }
 #else
     if (isServoOutputEnabled()) {
         writeServos();

src/main/fc/fc_init.c

@@ -96,6 +96,7 @@
 #include "flight/power_limits.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
+#include "flight/ez_tune.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
@@ -515,6 +516,10 @@ void init(void)
     owInit();
 #endif
 
+#ifdef USE_EZ_TUNE
+    ezTuneUpdate();
+#endif
+
     if (!sensorsAutodetect()) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);

src/main/fc/fc_msp.c

@@ -78,6 +78,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
+#include "flight/ez_tune.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -461,6 +462,7 @@ 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));
         }
@@ -522,6 +524,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU8(dst, -1);
         #endif
         }
+        if(MAX_MIXER_PROFILE_COUNT==1) break;
+        for (int i = 0; i < MAX_SERVO_RULES; i++) {
+            sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel);
+            sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource);
+            sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate);
+            sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed);
+        #ifdef USE_PROGRAMMING_FRAMEWORK
+            sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId);
+        #else
+            sbufWriteU8(dst, -1);
+        #endif
+        }
         break;
 #ifdef USE_PROGRAMMING_FRAMEWORK
     case MSP2_INAV_LOGIC_CONDITIONS:
@@ -567,11 +581,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
     case MSP2_COMMON_MOTOR_MIXER:
         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
-            sbufWriteU16(dst, primaryMotorMixer(i)->throttle * 1000);
+            sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->throttle + 2.0f, 0.0f, 4.0f) * 1000);
             sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000);
             sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000);
             sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000);
         }
+        if (MAX_MIXER_PROFILE_COUNT==1) break;
+        for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+            sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].throttle + 2.0f, 0.0f, 4.0f) * 1000);
+            sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].roll + 2.0f, 0.0f, 4.0f) * 1000);
+            sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].pitch + 2.0f, 0.0f, 4.0f) * 1000);
+            sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].yaw + 2.0f, 0.0f, 4.0f) * 1000);
+        }
         break;
 
     case MSP_MOTOR:
@@ -697,6 +718,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
             sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255));
         }
+        #ifdef USE_EZ_TUNE
+            ezTuneUpdate();
+        #endif
         break;
 
     case MSP_PIDNAMES:
@@ -1582,6 +1606,38 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 #endif
 
+#ifdef USE_EZ_TUNE
+
+    case MSP2_INAV_EZ_TUNE:
+        {
+            sbufWriteU8(dst, ezTune()->enabled);
+            sbufWriteU16(dst, ezTune()->filterHz);
+            sbufWriteU8(dst, ezTune()->axisRatio);
+            sbufWriteU8(dst, ezTune()->response);
+            sbufWriteU8(dst, ezTune()->damping);
+            sbufWriteU8(dst, ezTune()->stability);
+            sbufWriteU8(dst, ezTune()->aggressiveness);
+            sbufWriteU8(dst, ezTune()->rate);
+            sbufWriteU8(dst, ezTune()->expo);
+        }
+        break;
+#endif
+
+#ifdef USE_RATE_DYNAMICS
+
+    case MSP2_INAV_RATE_DYNAMICS:
+        {
+            sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityCenter);
+            sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityEnd);
+            sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionCenter);
+            sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionEnd);
+            sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightCenter);
+            sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightEnd);
+        }
+        break;
+
+#endif
+
     default:
         return false;
     }
@@ -2085,7 +2141,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
     case MSP2_COMMON_SET_MOTOR_MIXER:
         sbufReadU8Safe(&tmp_u8, src);
         if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) {
-            primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f);
+            primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
             primaryMotorMixerMutable(tmp_u8)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
             primaryMotorMixerMutable(tmp_u8)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
             primaryMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
@@ -2979,6 +3035,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
         break;
 
+    case MSP2_INAV_SELECT_MIXER_PROFILE:
+        if (!ARMING_FLAG(ARMED) && sbufReadU8Safe(&tmp_u8, src)) {
+                setConfigMixerProfileAndWriteEEPROM(tmp_u8);
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+
 #ifdef USE_TEMPERATURE_SENSOR
     case MSP2_INAV_SET_TEMP_SENSOR_CONFIG:
         if (dataSize == sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS) {
@@ -3040,6 +3104,51 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;
 #endif
 
+#ifdef USE_EZ_TUNE
+
+    case MSP2_INAV_EZ_TUNE_SET:
+        {
+            if (dataSize == 10) {
+                ezTuneMutable()->enabled = sbufReadU8(src);
+                ezTuneMutable()->filterHz = sbufReadU16(src);
+                ezTuneMutable()->axisRatio = sbufReadU8(src);
+                ezTuneMutable()->response = sbufReadU8(src);
+                ezTuneMutable()->damping = sbufReadU8(src);
+                ezTuneMutable()->stability = sbufReadU8(src);
+                ezTuneMutable()->aggressiveness = sbufReadU8(src);
+                ezTuneMutable()->rate = sbufReadU8(src);
+                ezTuneMutable()->expo = sbufReadU8(src);
+
+                ezTuneUpdate();
+            } else {
+                return MSP_RESULT_ERROR;
+            }
+        }
+        break;
+
+#endif
+
+#ifdef USE_RATE_DYNAMICS
+
+    case MSP2_INAV_SET_RATE_DYNAMICS:
+
+        if (dataSize == 6) {
+            ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityCenter = sbufReadU8(src);
+            ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityEnd = sbufReadU8(src);
+            ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionCenter = sbufReadU8(src);
+            ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src);
+            ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src);
+            ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src);
+            
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+
+        break;    
+
+#endif
+
+
     default:
         return MSP_RESULT_ERROR;
     }
@@ -3223,6 +3332,8 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src)
         sbufWriteU8(dst, 0);
         sbufWriteU8(dst, 0);
         break;
+    case EZ_TUNE_VALUE:
+        FALLTHROUGH;
     case PROFILE_VALUE:
         FALLTHROUGH;
     case CONTROL_RATE_VALUE:
@@ -3286,7 +3397,7 @@ bool isOSDTypeSupportedBySimulator(void)
 {
 #ifdef USE_OSD
     displayPort_t *osdDisplayPort = osdGetDisplayPort();
-    return (osdDisplayPort && osdDisplayPort->cols == 30 && (osdDisplayPort->rows == 13 || osdDisplayPort->rows == 16));
+	return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar);
 #else
     return false;
 #endif
@@ -3298,18 +3409,25 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 	//scan displayBuffer iteratively
 	//no more than 80+3+2 bytes output in single run
 	//0 and 255 are special symbols
-	//255 - font bank switch
+	//255 [char] - font bank switch
-	//0  - font bank switch, blink switch and character repeat
+	//0 [flags,count] [char] - font bank switch, blink switch and character repeat
+    //original 0 is sent as 32
+    //original 0xff, 0x100 and 0x1ff are forcibly sent inside command 0
 
 	static uint8_t osdPos_y = 0;
 	static uint8_t osdPos_x = 0;
 
+    //indicate new format hitl 1.4.0
+	sbufWriteU8(dst, 255);  
 
 	if (isOSDTypeSupportedBySimulator())
 	{
 		displayPort_t *osdDisplayPort = osdGetDisplayPort();
 
-		sbufWriteU8(dst, osdPos_y | (osdDisplayPort->rows == 16 ? 128: 0));
+		sbufWriteU8(dst, osdDisplayPort->rows);
+		sbufWriteU8(dst, osdDisplayPort->cols);
+
+		sbufWriteU8(dst, osdPos_y);
 		sbufWriteU8(dst, osdPos_x);
 
 		int bytesCount = 0;
@@ -3320,7 +3438,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 		bool blink = false;
 		int count = 0;
 
-		int processedRows = 16;
+		int processedRows = osdDisplayPort->rows;
 
 		while (bytesCount < 80) //whole response should be less 155 bytes at worst.
 		{
@@ -3331,7 +3449,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 			while ( true )
 			{
 				displayReadCharWithAttr(osdDisplayPort, osdPos_x, osdPos_y, &c, &attr);
-				if (c == 0 || c == 255) c = 32;
+				if (c == 0) c = 32;
 
 				//REVIEW: displayReadCharWithAttr() should return mode with _TEXT_ATTRIBUTES_BLINK_BIT !
 				//for max7456 it returns mode with MAX7456_MODE_BLINK instead (wrong)
@@ -3346,7 +3464,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 					lastChar = c;
 					blink1 = blink2;
 				}
-				else if (lastChar != c || blink2 != blink1 || count == 63)
+				else if ((lastChar != c) || (blink2 != blink1) || (count == 63))
 				{
 					break;
 				}
@@ -3354,12 +3472,12 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 				count++;
 
 				osdPos_x++;
-				if (osdPos_x == 30)
+				if (osdPos_x == osdDisplayPort->cols)
 				{
 					osdPos_x = 0;
 					osdPos_y++;
 					processedRows--;
-					if (osdPos_y == 16)
+					if (osdPos_y == osdDisplayPort->rows)
 					{
 						osdPos_y = 0;
 					}
@@ -3367,6 +3485,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 			}
 
 			uint8_t cmd = 0;
+            uint8_t lastCharLow = (uint8_t)(lastChar & 0xff);
 			if (blink1 != blink)
 			{
 				cmd |= 128;//switch blink attr
@@ -3382,27 +3501,27 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 
 			if (count == 1 && cmd == 64)
 			{
-				sbufWriteU8(dst, 255);  //short command for bank switch
+				sbufWriteU8(dst, 255);  //short command for bank switch with char following
 				sbufWriteU8(dst, lastChar & 0xff);
 				bytesCount += 2;
 			}
-			else if (count > 2 || cmd !=0 )
+			else if ((count > 2) || (cmd !=0) || (lastChar == 255) || (lastChar == 0x100) || (lastChar == 0x1ff))
 			{
 				cmd |= count;  //long command for blink/bank switch and symbol repeat
 				sbufWriteU8(dst, 0);
 				sbufWriteU8(dst, cmd);
-				sbufWriteU8(dst, lastChar & 0xff);
+				sbufWriteU8(dst, lastCharLow);
 				bytesCount += 3;
 			}
 			else if (count == 2)  //cmd == 0 here
 			{
-				sbufWriteU8(dst, lastChar & 0xff);
+				sbufWriteU8(dst, lastCharLow);
-				sbufWriteU8(dst, lastChar & 0xff);
+				sbufWriteU8(dst, lastCharLow);
 				bytesCount+=2;
 			}
 			else
 			{
-				sbufWriteU8(dst, lastChar & 0xff);
+				sbufWriteU8(dst, lastCharLow);
 				bytesCount++;
 			}
 
@@ -3416,7 +3535,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 	}
 	else
 	{
-		sbufWriteU8(dst, 255);
+		sbufWriteU8(dst, 0);
 	}
 }
 #endif
@@ -3550,6 +3669,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                 }
 #endif
                 ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
+                ENABLE_STATE(ACCELEROMETER_CALIBRATED);
                 LOG_DEBUG(SYSTEM, "Simulator enabled");
             }
 
@@ -3624,15 +3744,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                     sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
                 }
 
-#if defined(USE_FAKE_BATT_SENSOR)
                 if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) {
-                    fakeBattSensorSetVbat(sbufReadU8(src) * 10);
+                    simulatorData.vbat = sbufReadU8(src);
                 } else {
-#endif
+                    simulatorData.vbat = SIMULATOR_FULL_BATTERY;
-                    fakeBattSensorSetVbat((uint16_t)(SIMULATOR_FULL_BATTERY * 10.0f));
-#if defined(USE_FAKE_BATT_SENSOR)
                 }
-#endif
 
                 if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
                     simulatorData.airSpeed = sbufReadU16(src);

src/main/fc/fc_msp_box.c

@@ -202,7 +202,7 @@ void initActiveBoxIds(void)
     //Camstab mode is enabled always
     ADD_ACTIVE_BOX(BOXCAMSTAB);
 
-    if (STATE(MULTIROTOR)) {
+    if (STATE(MULTIROTOR) || platformTypeConfigured(PLATFORM_MULTIROTOR) || platformTypeConfigured(PLATFORM_TRICOPTER)) {
         if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) {
             ADD_ACTIVE_BOX(BOXHEADFREE);
             ADD_ACTIVE_BOX(BOXHEADADJ);
@@ -244,13 +244,13 @@ void initActiveBoxIds(void)
 #endif
         }
 
-        if (STATE(AIRPLANE)) {
+        if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) {
             ADD_ACTIVE_BOX(BOXSOARING);
         }
     }
 
 #ifdef USE_MR_BRAKING_MODE
-    if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
+    if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || platformTypeConfigured(PLATFORM_MULTIROTOR)) {
         ADD_ACTIVE_BOX(BOXBRAKING);
     }
 #endif
@@ -259,11 +259,12 @@ void initActiveBoxIds(void)
         ADD_ACTIVE_BOX(BOXNAVALTHOLD);
     }
 
-    if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
+    if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) || 
+        platformTypeConfigured(PLATFORM_AIRPLANE) || platformTypeConfigured(PLATFORM_ROVER) || platformTypeConfigured(PLATFORM_BOAT)) {
         ADD_ACTIVE_BOX(BOXMANUAL);
     }
 
-    if (STATE(AIRPLANE)) {
+    if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) {
         if (!feature(FEATURE_FW_LAUNCH)) {
            ADD_ACTIVE_BOX(BOXNAVLAUNCH);
         }

src/main/fc/fc_tasks.c

@@ -62,6 +62,7 @@
 #include "io/osd.h"
 #include "io/serial.h"
 #include "io/rcdevice_cam.h"
+#include "io/osd_joystick.h"
 #include "io/smartport_master.h"
 #include "io/vtx.h"
 #include "io/vtx_msp.h"
@@ -393,8 +394,12 @@ void fcTasksInit(void)
 #endif
 #endif
 #ifdef USE_RCDEVICE
+#ifdef USE_LED_STRIP
+    setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled() || osdJoystickEnabled());
+#else
     setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
 #endif
+#endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
     setTaskEnabled(TASK_PROGRAMMING_FRAMEWORK, true);
 #endif

src/main/fc/runtime_config.c

@@ -179,7 +179,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
 
 #ifdef USE_SIMULATOR
 simulatorData_t simulatorData = {
-	.flags = 0,
+    .flags = 0,
-	.debugIndex = 0
+    .debugIndex = 0,
+    .vbat = 0
 };
 #endif

src/main/fc/runtime_config.h

@@ -142,6 +142,7 @@ typedef enum {
     FW_HEADING_USE_YAW                  = (1 << 24),
     ANTI_WINDUP_DEACTIVATED             = (1 << 25),
     LANDING_DETECTED                    = (1 << 26),
+    IN_FLIGHT_EMERG_REARM               = (1 << 27),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))
@@ -173,7 +174,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void);
 
 #define SIMULATOR_MSP_VERSION  2     // Simulator MSP version
 #define SIMULATOR_BARO_TEMP    25    // °C
-#define SIMULATOR_FULL_BATTERY 12.6f // Volts
+#define SIMULATOR_FULL_BATTERY 126   // Volts*10
 #define SIMULATOR_HAS_OPTION(flag) ((simulatorData.flags & flag) != 0)
 
 typedef enum {

src/main/fc/settings.c

@@ -237,6 +237,8 @@ static uint16_t getValueOffset(const setting_t *value)
         return value->offset + sizeof(pidProfile_t) * getConfigProfile();
     case CONTROL_RATE_VALUE:
         return value->offset + sizeof(controlRateConfig_t) * getConfigProfile();
+	case EZ_TUNE_VALUE:
+		return value->offset + sizeof(ezTuneSettings_t) * getConfigProfile();
     case BATTERY_CONFIG_VALUE:
         return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile();
 	case MIXER_CONFIG_VALUE:

src/main/fc/settings.h

@@ -35,6 +35,7 @@ typedef enum {
     CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET),
     BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET),
     MIXER_CONFIG_VALUE = (4 << SETTING_SECTION_OFFSET),
+    EZ_TUNE_VALUE = (5 << SETTING_SECTION_OFFSET)
 } setting_section_e;
 
 typedef enum {

src/main/fc/settings.yaml

@@ -193,6 +193,9 @@ tables:
   - name: nav_mc_althold_throttle
     values: ["STICK", "MID_STICK", "HOVER"]
     enum: navMcAltHoldThrottle_e    
+  - name: led_pin_pwm_mode
+    values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
+    enum: led_pin_pwm_mode_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -1051,7 +1054,7 @@ groups:
         max: PWM_RANGE_MAX
       - name: nav_mc_hover_thr
         description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
-        default_value: 1500
+        default_value: 1300
         field: nav.mc.hover_throttle
         min: 1000
         max: 2000
@@ -1273,7 +1276,7 @@ groups:
         min: 0
         max: 100
       - name: tpa_rate
-        description: "Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate."
+        description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate."
         default_value: 0
         field: throttle.dynPID
         min: 0
@@ -1284,6 +1287,11 @@ groups:
         field: throttle.pa_breakpoint
         min: PWM_RANGE_MIN
         max: PWM_RANGE_MAX
+      - name: tpa_on_yaw
+        description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors."
+        type: bool
+        field: throttle.dynPID_on_YAW
+        default_value: OFF
       - name: fw_tpa_time_constant
         description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details."
         default_value: 1500
@@ -1458,7 +1466,7 @@ groups:
         type: bool
       - name: ahrs_inertia_comp_method
         description: "Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop"
-        default_value: VELNED
+        default_value: ADAPTIVE
         field: inertia_comp_method
         table: imu_inertia_comp_method
 
@@ -1498,6 +1506,65 @@ groups:
         min: 0
         max: 99
 
+  - name: PG_EZ_TUNE
+    headers: ["flight/ez_tune.h"]
+    type: ezTuneSettings_t
+    value_type: EZ_TUNE_VALUE
+    members:
+      - name: ez_enabled
+        description: "Enables EzTune feature"
+        default_value: OFF
+        field: enabled
+        type: bool
+      - name: ez_filter_hz
+        description: "EzTune filter cutoff frequency"
+        default_value: 110
+        field: filterHz
+        min: 10
+        max: 300
+      - name: ez_axis_ratio
+        description: "EzTune axis ratio"
+        default_value: 110
+        field: axisRatio
+        min: 25
+        max: 175
+      - name: ez_response
+        description: "EzTune response"
+        default_value: 100
+        field: response
+        min: 0
+        max: 200
+      - name: ez_damping
+        description: "EzTune damping"
+        default_value: 100
+        field: damping
+        min: 0
+        max: 200
+      - name: ez_stability
+        description: "EzTune stability"
+        default_value: 100
+        field: stability
+        min: 0
+        max: 200
+      - name: ez_aggressiveness
+        description: "EzTune aggressiveness"
+        default_value: 100
+        field: aggressiveness
+        min: 0
+        max: 200
+      - name: ez_rate
+        description: "EzTune rate"
+        default_value: 100
+        field: rate
+        min: 0
+        max: 200
+      - name: ez_expo
+        description: "EzTune expo"
+        default_value: 100
+        field: expo
+        min: 0
+        max: 200
+
   - name: PG_RPM_FILTER_CONFIG
     headers: ["flight/rpm_filter.h"]
     condition: USE_RPM_FILTER
@@ -1857,12 +1924,6 @@ groups:
         default_value: 0
         min: 0
         max: 200
-      - name: fw_iterm_throw_limit
-        description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely."
-        default_value: 165
-        field: fixedWingItermThrowLimit
-        min: FW_ITERM_THROW_LIMIT_MIN
-        max: FW_ITERM_THROW_LIMIT_MAX
       - name: fw_reference_airspeed
         description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present."
         default_value: 1500
@@ -1911,6 +1972,12 @@ groups:
         field: itermWindupPointPercent
         min: 0
         max: 90
+      - name: pid_iterm_limit_percent
+        description: "Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely."
+        default_value: 33
+        field: pidItermLimitPercent
+        min: 0
+        max: 200
       - name: rate_accel_limit_roll_pitch
         description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting."
         default_value: 0
@@ -3505,66 +3572,95 @@ groups:
         max: 6
         default_value: 3
 
-      - name: osd_mah_used_precision
+      - name: osd_mah_precision
-        description: Number of digits used to display mAh used.
+        description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity
-        field: mAh_used_precision
+        field: mAh_precision
         min: 4
         max: 6
         default_value: 4
 
+      - name: osd_use_pilot_logo
+        description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511
+        field: use_pilot_logo
+        type: bool
+        default_value: OFF
+
+      - name: osd_inav_to_pilot_logo_spacing
+        description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen.
+        field: inav_to_pilot_logo_spacing
+        min: 0
+        max: 20
+        default_value: 8
+
+      - name: osd_arm_screen_display_time
+        description: Amount of time to display the arm screen [ms]
+        field: arm_screen_display_time
+        min: 1000
+        max: 5000
+        default_value: 1500
+  
       - name: osd_switch_indicator_zero_name
         description: "Character to use for OSD switch incicator 0."
         field: osd_switch_indicator0_name
         type: string
         max: 5
         default_value: "FLAP"
+
       - name: osd_switch_indicator_one_name
         description: "Character to use for OSD switch incicator 1."
         field: osd_switch_indicator1_name
         type: string
         max: 5
         default_value: "GEAR"
+
       - name: osd_switch_indicator_two_name
         description: "Character to use for OSD switch incicator 2."
         field: osd_switch_indicator2_name
         type: string
         max: 5
         default_value: "CAM"
+
       - name: osd_switch_indicator_three_name
         description: "Character to use for OSD switch incicator 3."
         field: osd_switch_indicator3_name
         type: string
         max: 5
         default_value: "LIGT"
+
       - name: osd_switch_indicator_zero_channel
         description: "RC Channel to use for OSD switch indicator 0."
         field: osd_switch_indicator0_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
+
       - name: osd_switch_indicator_one_channel
         description: "RC Channel to use for OSD switch indicator 1."
         field: osd_switch_indicator1_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
+
       - name: osd_switch_indicator_two_channel
         description: "RC Channel to use for OSD switch indicator 2."
         field: osd_switch_indicator2_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
+
       - name: osd_switch_indicator_three_channel
         description: "RC Channel to use for OSD switch indicator 3."
         field: osd_switch_indicator3_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
+
       - name: osd_switch_indicators_align_left
         description: "Align text to left of switch indicators"
         field: osd_switch_indicators_align_left
         type: bool
         default_value: ON
+
       - name: osd_system_msg_display_time
         description: System message display cycle time for multiple messages (milliseconds).
         field: system_msg_display_time
@@ -3938,3 +4034,55 @@ groups:
         default_value: 1.2
         field: attnFilterCutoff
         max: 100
+
+  - name: PG_LEDPIN_CONFIG
+    type: ledPinConfig_t
+    headers: ["drivers/light_ws2811strip.h"]
+    members:
+      - name: led_pin_pwm_mode
+        condition: USE_LED_STRIP
+        description: "PWM mode of LED pin."
+        default_value: "SHARED_LOW"
+        field: led_pin_pwm_mode
+        table: led_pin_pwm_mode
+
+  - name: PG_OSD_JOYSTICK_CONFIG
+    type: osdJoystickConfig_t
+    headers: ["io/osd_joystick.h"]
+    condition: USE_RCDEVICE && USE_LED_STRIP
+    members:
+      - name: osd_joystick_enabled
+        description: "Enable OSD Joystick emulation"
+        default_value: OFF
+        field: osd_joystick_enabled
+        type: bool
+      - name: osd_joystick_down
+        description: "PWM value for DOWN key"
+        default_value: 0
+        field: osd_joystick_down
+        min: 0
+        max: 100
+      - name: osd_joystick_up
+        description: "PWM value for UP key"
+        default_value: 48
+        field: osd_joystick_up
+        min: 0
+        max: 100
+      - name: osd_joystick_left
+        description: "PWM value for LEFT key"
+        default_value: 63
+        field: osd_joystick_left
+        min: 0
+        max: 100
+      - name: osd_joystick_right
+        description: "PWM value for RIGHT key"
+        default_value: 28
+        field: osd_joystick_right
+        min: 0
+        max: 100
+      - name: osd_joystick_enter
+        description: "PWM value for ENTER key"
+        default_value: 75
+        field: osd_joystick_enter
+        min: 0
+        max: 100

src/main/fc/stats.h

@@ -3,8 +3,8 @@
 #ifdef USE_STATS
 
 typedef struct statsConfig_s {
-    uint32_t stats_total_time; // [s]
+    uint32_t stats_total_time; // [Seconds]
-    uint32_t stats_total_dist; // [m]
+    uint32_t stats_total_dist; // [Metres]
 #ifdef USE_ADC
     uint32_t stats_total_energy; // deciWatt hour (x0.1Wh)
 #endif

src/main/flight/ez_tune.c

@@ -0,0 +1,143 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "fc/config.h"
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "flight/ez_tune.h"
+
+#include "fc/settings.h"
+#include "flight/pid.h"
+#include "sensors/gyro.h"
+#include "fc/controlrate_profile.h"
+
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0);
+
+PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
+    .enabled = SETTING_EZ_ENABLED_DEFAULT,
+    .filterHz = SETTING_EZ_FILTER_HZ_DEFAULT,
+    .axisRatio = SETTING_EZ_AXIS_RATIO_DEFAULT,
+    .response = SETTING_EZ_RESPONSE_DEFAULT,
+    .damping = SETTING_EZ_DAMPING_DEFAULT,
+    .stability = SETTING_EZ_STABILITY_DEFAULT,
+    .aggressiveness = SETTING_EZ_AGGRESSIVENESS_DEFAULT,
+    .rate = SETTING_EZ_RATE_DEFAULT,
+    .expo = SETTING_EZ_EXPO_DEFAULT,
+);
+
+#define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 }
+#define EZ_TUNE_PID_YAW_DEFAULT { 45, 80, 0, 100 }
+
+#define EZ_TUNE_YAW_SCALE 0.5f
+
+static float computePt1FilterDelayMs(uint8_t filterHz) {
+    return 1000.0f / (2.0f * M_PIf * filterHz);
+}
+
+static float getYawPidScale(float input) {
+    const float normalized = (input - 100) * 0.01f;
+
+    return 1.0f + (normalized * 0.5f); 
+}
+
+/**
+ * Update INAV settings based on current EZTune settings
+ * This has to be called every time control profile is changed, or EZTune settings are changed
+ */
+void ezTuneUpdate(void) {
+    if (ezTune()->enabled) {
+
+        // Setup filtering
+        //Set Dterm LPF
+        pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50);
+        pidProfileMutable()->dterm_lpf_type = FILTER_PT2;
+
+        //Set main gyro filter
+        gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz;
+        gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1;
+
+        //Set anti-aliasing filter
+        gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT;
+        gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1;
+
+        //Enable Smith predictor
+        pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz);
+
+#ifdef USE_DYNAMIC_FILTERS
+        //Enable dynamic notch
+        gyroConfigMutable()->dynamicGyroNotchEnabled = 1;
+        gyroConfigMutable()->dynamicGyroNotchQ = 250;
+        gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT);
+        gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D;
+#endif
+
+#ifdef USE_GYRO_KALMAN
+        //Make sure Kalman filter is enabled
+        gyroConfigMutable()->kalmanEnabled = 1;
+        if (ezTune()->filterHz < 150) {
+            gyroConfigMutable()->kalman_q = 200;
+        } else {
+            gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400);
+        }
+#endif
+
+        //Disable dynamic LPF
+        gyroConfigMutable()->useDynamicLpf = 0;
+
+        //Setup PID controller
+
+        const uint8_t pidDefaults[4] = EZ_TUNE_PID_RP_DEFAULT;
+        const uint8_t pidDefaultsYaw[4] = EZ_TUNE_PID_YAW_DEFAULT;
+        const float pitchRatio = ezTune()->axisRatio / 100.0f;
+
+        //Roll
+        pidProfileMutable()->bank_mc.pid[PID_ROLL].P = pidDefaults[0] * ezTune()->response / 100.0f;
+        pidProfileMutable()->bank_mc.pid[PID_ROLL].I = pidDefaults[1] * ezTune()->stability / 100.0f;
+        pidProfileMutable()->bank_mc.pid[PID_ROLL].D = pidDefaults[2] * ezTune()->damping / 100.0f;
+        pidProfileMutable()->bank_mc.pid[PID_ROLL].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f;
+
+        //Pitch
+        pidProfileMutable()->bank_mc.pid[PID_PITCH].P = pidDefaults[0] * ezTune()->response / 100.0f * pitchRatio;
+        pidProfileMutable()->bank_mc.pid[PID_PITCH].I = pidDefaults[1] * ezTune()->stability / 100.0f * pitchRatio;
+        pidProfileMutable()->bank_mc.pid[PID_PITCH].D = pidDefaults[2] * ezTune()->damping / 100.0f * pitchRatio;
+        pidProfileMutable()->bank_mc.pid[PID_PITCH].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f * pitchRatio;
+    
+        //Yaw
+        pidProfileMutable()->bank_mc.pid[PID_YAW].P = pidDefaultsYaw[0] * getYawPidScale(ezTune()->response);
+        pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability);
+        pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping);
+        pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness);
+
+        //Setup rates
+        ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = scaleRange(ezTune()->rate, 0, 200, 30, 90);
+        ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = scaleRange(ezTune()->rate, 0, 200, 30, 90);
+        ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = scaleRange(ezTune()->rate, 0, 200, 30, 90) - 10;
+
+        ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100);
+        ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100);
+
+    }
+}

src/main/flight/ez_tune.h

@@ -0,0 +1,43 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "config/parameter_group.h"
+
+typedef struct ezTuneSettings_s {
+    uint8_t enabled;
+    uint16_t filterHz;
+    uint8_t axisRatio;
+    uint8_t response;
+    uint8_t damping;
+    uint8_t stability;
+    uint8_t aggressiveness;
+    uint8_t rate;
+    uint8_t expo;
+} ezTuneSettings_t;
+
+PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune);
+
+void ezTuneUpdate(void);

src/main/flight/imu.c

@@ -723,28 +723,25 @@ static void imuCalculateEstimatedAttitude(float dT)
     }
     if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE))
     {
+        //pick the best centrifugal acceleration between velned and turnrate
         fpVector3_t compansatedGravityBF_velned;
         vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
-        float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS);
+        float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS);
 
         fpVector3_t compansatedGravityBF_turnrate;
         vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
-        float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS);
+        float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS);
-        if (velned_magnitude > turnrate_magnitude)
+
-        {
+        compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned;
-            compansatedGravityBF = compansatedGravityBF_turnrate;
-        }
-        else
-        {
-            compansatedGravityBF = compansatedGravityBF_velned;
-        }
     }
-    else if (imuConfig()->inertia_comp_method == COMPMETHOD_VELNED && isGPSTrustworthy())
+    else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy())
     {
+        //velned centrifugal force compensation, quad will use this method
         vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
     }
     else if (STATE(AIRPLANE))
     {
+        //turnrate centrifugal force compensation
         vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
     }
     else

src/main/flight/mixer_profile.c

@@ -23,6 +23,7 @@
 #include "fc/runtime_config.h"
 #include "fc/settings.h"
 #include "fc/rc_modes.h"
+#include "fc/cli.h"
 
 #include "programming/logic_condition.h"
 #include "navigation/navigation.h"
@@ -34,7 +35,7 @@ int currentMixerProfileIndex;
 bool isMixerTransitionMixing;
 bool isMixerTransitionMixing_requested;
 mixerProfileAT_t mixerProfileAT;
-int nextProfileIndex;
+int nextMixerProfileIndex;
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1);
 
@@ -77,11 +78,15 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
     }
 }
 
-void mixerConfigInit(void)
+void activateMixerConfig(){
-{
     currentMixerProfileIndex = getConfigMixerProfile();
     currentMixerConfig = *mixerConfig();
-    nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT;
+    nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT;
+}
+
+void mixerConfigInit(void)
+{
+    activateMixerConfig();
     servosInit();
     mixerUpdateStateFlags();
     mixerInit();
@@ -103,6 +108,14 @@ void setMixerProfileAT(void)
     mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100;
 }
 
+bool platformTypeConfigured(flyingPlatformType_e platformType)
+{   
+    if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){
+        return false;
+    }
+    return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType;
+}
+
 bool checkMixerATRequired(mixerProfileATRequest_e required_action)
 {
     //return false if mixerAT condition is not required or setting is not valid
@@ -158,7 +171,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action)
             isMixerTransitionMixing_requested = true;
             if (millis() > mixerProfileAT.transitionTransEndTime){
                 isMixerTransitionMixing_requested = false;
-                outputProfileHotSwitch(nextProfileIndex);
+                outputProfileHotSwitch(nextMixerProfileIndex);
                 mixerProfileAT.phase = MIXERAT_PHASE_IDLE;
                 reprocessState = true;
                 //transition is done
@@ -183,8 +196,9 @@ bool checkMixerProfileHotSwitchAvalibility(void)
 }
 
 void outputProfileUpdateTask(timeUs_t currentTimeUs)
-{
+{   
     UNUSED(currentTimeUs);
+    if(cliMode) return;
     bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE;
     // transition mode input for servo mix and motor mix
     if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse))

src/main/flight/mixer_profile.h

@@ -54,6 +54,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action);
 
 extern mixerConfig_t currentMixerConfig;
 extern int currentMixerProfileIndex;
+extern int nextMixerProfileIndex;
 extern bool isMixerTransitionMixing;
 #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config))
 #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig())
@@ -71,7 +72,9 @@ static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index)
 #define mixerMotorMixersByIndex(index) (mixerProfiles(index)->MotorMixers)
 #define mixerServoMixersByIndex(index) (mixerProfiles(index)->ServoMixers)
 
+bool platformTypeConfigured(flyingPlatformType_e platformType);
 bool outputProfileHotSwitch(int profile_index);
 bool checkMixerProfileHotSwitchAvalibility(void);
+void activateMixerConfig(void);
 void mixerConfigInit(void);
 void outputProfileUpdateTask(timeUs_t currentTimeUs);

src/main/flight/pid.c

@@ -161,7 +161,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
 
 #define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
-#define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f
+#define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f
 #define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER
 #define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE
 
@@ -264,8 +264,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT,
         .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT,
         .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT,
+        .pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT,
 
-        .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT,
         .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
         .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
         .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
@@ -531,7 +531,7 @@ void updatePIDCoefficients(void)
             pidState[axis].kT  = 0.0f;
         }
         else {
-            const float axisTPA = (axis == FD_YAW) ? 1.0f : tpaFactor;
+            const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor;
             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA;
             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER;
             pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA;
@@ -762,8 +762,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 
     applyItermLimiting(pidState);
 
-    if (pidProfile()->fixedWingItermThrowLimit != 0) {
+    if (pidProfile()->pidItermLimitPercent != 0){
-        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
+        float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f;
+        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
     }
 
     axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
@@ -837,6 +838,12 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
 
     pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
                              + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
+    
+    if (pidProfile()->pidItermLimitPercent != 0){
+        float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f;
+        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
+    }
+
 
     // Don't grow I-term if motors are at their limit
     applyItermLimiting(pidState);
@@ -1033,7 +1040,7 @@ void checkItermLimitingActive(pidState_t *pidState)
         shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
     } else
     {
-        shouldActivate = mixerIsOutputSaturated();
+        shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
     }
 
     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
@@ -1072,7 +1079,7 @@ void FAST_CODE pidController(float dT)
     // In case Yaw override is active, we engage the Heading Hold state
     if (isFlightAxisAngleOverrideActive(FD_YAW)) {
         headingHoldState = HEADING_HOLD_ENABLED;
-        headingHoldTarget = getFlightAxisAngleOverride(FD_YAW, 0);
+        headingHoldTarget = DECIDEGREES_TO_DEGREES(getFlightAxisAngleOverride(FD_YAW, 0));
     }
 
     if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) {
@@ -1130,7 +1137,7 @@ void FAST_CODE pidController(float dT)
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
     }
 
-    if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees) {
+    if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) {
         pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
     }
 
@@ -1243,7 +1250,7 @@ void pidInit(void)
     navPidInit(
         &fixedWingLevelTrimController,
         0.0f,
-        (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
+        (float)pidProfile()->fixedWingLevelTrimGain / 100.0f,
         0.0f,
         0.0f,
         2.0f,
@@ -1255,47 +1262,52 @@ void pidInit(void)
 const pidBank_t * pidBank(void) {
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
 }
+
 pidBank_t * pidBankMutable(void) {
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
 
+bool isFixedWingLevelTrimActive(void)
+{
+    return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
+           (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
+           !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
+           !navigationIsControllingAltitude();
+}
+
 void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
 {
     if (!STATE(AIRPLANE)) {
         return;
     }
 
-    static timeUs_t previousUpdateTimeUs;
+    static bool previousArmingState = false;
-    static bool previousArmingState;
-    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
 
-    /*
+    if (ARMING_FLAG(ARMED)) {
-     * On every ARM reset the controller
+        if (!previousArmingState) {     // On every ARM reset the controller
-     */
+            navPidReset(&fixedWingLevelTrimController);
-    if (ARMING_FLAG(ARMED) && !previousArmingState) {
+        }
-        navPidReset(&fixedWingLevelTrimController);
+    } else if (previousArmingState) {   // On disarm update the default value
-    }
-
-    /*
-     * On disarm update the default value
-     */
-    if (!ARMING_FLAG(ARMED) && previousArmingState) {
         pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
     }
+    previousArmingState = ARMING_FLAG(ARMED);
+
+    // return if not active or disarmed
+    if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
+        return;
+    }
+
+    static timeUs_t previousUpdateTimeUs;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+    previousUpdateTimeUs = currentTimeUs;
 
     /*
      * Prepare flags for the PID controller
      */
     pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
 
-    //Iterm should freeze when sticks are deflected
+    // Iterm should freeze when conditions for setting level trim aren't met
-    if (
+    if (!isFixedWingLevelTrimActive()) {
-        !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
-        areSticksDeflected() ||
-        (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) ||
-        FLIGHT_MODE(SOARING_MODE) ||
-        navigationIsControllingAltitude()
-    ) {
         flags |= PID_FREEZE_INTEGRATOR;
     }
 
@@ -1313,8 +1325,6 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
 
     DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
-
-    previousArmingState = !!ARMING_FLAG(ARMED);
 }
 
 float getFixedWingLevelTrim(void)

src/main/flight/pid.h

@@ -31,10 +31,6 @@
 #define HEADING_HOLD_RATE_LIMIT_MAX 250
 #define HEADING_HOLD_RATE_LIMIT_DEFAULT 90
 
-#define FW_ITERM_THROW_LIMIT_DEFAULT 165
-#define FW_ITERM_THROW_LIMIT_MIN 0
-#define FW_ITERM_THROW_LIMIT_MAX 500
-
 #define AXIS_ACCEL_MIN_LIMIT        50
 
 #define HEADING_HOLD_ERROR_LPF_FREQ 2
@@ -106,8 +102,8 @@ typedef struct pidProfile_s {
     pidBank_t bank_mc;
 
     uint8_t dterm_lpf_type;                 // Dterm LPF type: PT1, BIQUAD
-    uint16_t dterm_lpf_hz;                  
+    uint16_t dterm_lpf_hz;
-    
+
     uint8_t yaw_lpf_hz;
 
     uint8_t heading_hold_rate_limit;        // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
@@ -121,15 +117,15 @@ typedef struct pidProfile_s {
 
     uint16_t pidSumLimit;
     uint16_t pidSumLimitYaw;
+    uint16_t pidItermLimitPercent;
 
     // Airplane-specific parameters
-    uint16_t    fixedWingItermThrowLimit;
     float       fixedWingReferenceAirspeed;     // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
     uint16_t    fixedWingYawItermBankFreeze;       // Freeze yaw Iterm when bank angle is more than this many degrees
-    
+
     float       navVelXyDTermLpfHz;
     uint8_t navVelXyDtermAttenuation;       // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
     uint8_t navVelXyDtermAttenuationStart;  // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity
@@ -221,5 +217,6 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
 
+bool isFixedWingLevelTrimActive(void);
 void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
 float getFixedWingLevelTrim(void);

src/main/flight/rth_estimator.c

@@ -209,8 +209,8 @@ float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
 // returns meters
 float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
 
-    // Fixed wing only for now
+    // Fixed wing only for now, and must be armed
-    if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) {
+    if (!STATE(AIRPLANE) || !ARMING_FLAG(ARMED)) {
         return -1;
     }
 

src/main/flight/servos.c

@@ -103,8 +103,14 @@ void pgResetFn_servoParams(servoParam_t *instance)
 int16_t servo[MAX_SUPPORTED_SERVOS];
 
 static uint8_t servoRuleCount = 0;
+static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
+
+/*
+//Was used to keep track of servo rules in all mixer_profile, In order to Apply mixer speed limit when rules turn off
 static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];
-static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];// if true, the rule is used by current servo mixer
+static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; // if true, the rule is used by current servo mixer
+*/
+
 static bool servoOutputEnabled;
 
 static bool mixerUsesServos;
@@ -115,7 +121,7 @@ static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
 static bool servoFilterIsSet;
 
 static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS];
-static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];
+static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES];
 
 STATIC_FASTRAM pt1Filter_t rotRateFilter;
 STATIC_FASTRAM pt1Filter_t targetRateFilter;
@@ -137,6 +143,33 @@ void servoComputeScalingFactors(uint8_t servoIndex) {
     servoMetadata[servoIndex].scaleMin = (servoParams(servoIndex)->middle - servoParams(servoIndex)->min) / 500.0f;
 }
 
+void computeServoCount(void)
+{
+    static bool firstRun = true;
+    if (!firstRun) {
+        return;
+    }
+    minServoIndex = 255;
+    maxServoIndex = 0;
+    for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) {
+        for (int i = 0; i < MAX_SERVO_RULES; i++) {
+            // check if done
+            if (mixerServoMixersByIndex(j)[i].rate == 0){
+                break;
+            }
+            if (mixerServoMixersByIndex(j)[i].targetChannel < minServoIndex) {
+                minServoIndex = mixerServoMixersByIndex(j)[i].targetChannel;
+            }
+
+            if (mixerServoMixersByIndex(j)[i].targetChannel > maxServoIndex) {
+                maxServoIndex = mixerServoMixersByIndex(j)[i].targetChannel;
+            }
+            mixerUsesServos = true;
+        }
+    }
+    firstRun = false;
+}
+
 void servosInit(void)
 {
     // give all servos a default command
@@ -147,12 +180,12 @@ void servosInit(void)
     /*
      * load mixer
      */
+    computeServoCount();
     loadCustomServoMixer();
 
     // If there are servo rules after all, update variables
-    if (servoRuleCount > 0) {
+    if (mixerUsesServos) {
         servoOutputEnabled = true;
-        mixerUsesServos = true;
     }
 
     for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
@@ -161,8 +194,8 @@ void servosInit(void)
 }
 
 int getServoCount(void)
-{
+{   
-    if (servoRuleCount) {
+    if (mixerUsesServos) {
         return 1 + maxServoIndex - minServoIndex;
     }
     else {
@@ -173,30 +206,17 @@ int getServoCount(void)
 void loadCustomServoMixer(void)
 {
     servoRuleCount = 0;
-    minServoIndex = 255;
-    maxServoIndex = 0;
     memset(currentServoMixer, 0, sizeof(currentServoMixer));
 
-    for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) {
+    // load custom mixer into currentServoMixer
-        const servoMixer_t* tmp_customServoMixers = &mixerServoMixersByIndex(j)[0];
+    for (int i = 0; i < MAX_SERVO_RULES; i++) {
-        // load custom mixer into currentServoMixer
+        // check if done
-        for (int i = 0; i < MAX_SERVO_RULES; i++) {
+        if (customServoMixers(i)->rate == 0){
-            // check if done
+            break;
-            if (tmp_customServoMixers[i].rate == 0)
-                break;
-
-            if (tmp_customServoMixers[i].targetChannel < minServoIndex) {
-                minServoIndex = tmp_customServoMixers[i].targetChannel;
-            }
-
-            if (tmp_customServoMixers[i].targetChannel > maxServoIndex) {
-                maxServoIndex = tmp_customServoMixers[i].targetChannel;
-            }
-
-            memcpy(&currentServoMixer[servoRuleCount], &tmp_customServoMixers[i], sizeof(servoMixer_t));
-            currentServoMixerActivative[servoRuleCount] = j==currentMixerProfileIndex;
-            servoRuleCount++;
         }
+        currentServoMixer[servoRuleCount] = *customServoMixers(i);
+        servoSpeedLimitFilter[servoRuleCount].state = 0;
+        servoRuleCount++;
     }
 }
 
@@ -353,9 +373,6 @@ void servoMixer(float dT)
             inputRaw = 0;
         }
     #endif
-        if (!currentServoMixerActivative[i]) {
-            inputRaw = 0;
-        }
         /*
          * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s:
          * 0 = no limiting
@@ -368,20 +385,6 @@ void servoMixer(float dT)
         servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100;
     }
 
-    /*
-     * When not armed, apply servo low position to all outputs that include a throttle or stabilizet throttle in the mix
-     */
-    if (!ARMING_FLAG(ARMED)) {
-        for (int i = 0; i < servoRuleCount; i++) {
-            const uint8_t target = currentServoMixer[i].targetChannel;
-            const uint8_t from = currentServoMixer[i].inputSource;
-
-            if (from == INPUT_STABILIZED_THROTTLE || from == INPUT_RC_THROTTLE) {
-                servo[target] = motorConfig()->mincommand;
-            }
-        }
-    }
-
     for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 
         /*
@@ -412,6 +415,20 @@ void servoMixer(float dT)
          */
         servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max);
     }
+
+    /*
+     * When not armed, apply servo low position to all outputs that include a throttle or stabilizet throttle in the mix
+     */
+    if (!ARMING_FLAG(ARMED)) {
+        for (int i = 0; i < servoRuleCount; i++) {
+            const uint8_t target = currentServoMixer[i].targetChannel;
+            const uint8_t from = currentServoMixer[i].inputSource;
+
+            if (from == INPUT_STABILIZED_THROTTLE || from == INPUT_RC_THROTTLE) {
+                servo[target] = motorConfig()->mincommand;
+            }
+        }
+    }
 }
 
 #define SERVO_AUTOTRIM_TIMER_MS     2000
@@ -438,7 +455,6 @@ void processServoAutotrimMode(void)
                 if (ARMING_FLAG(ARMED)) {
                     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
                         for (int i = 0; i < servoRuleCount; i++) {
-                            if (!currentServoMixerActivative[i]) {continue;}
                                 // Reset servo middle accumulator
                             const uint8_t target = currentServoMixer[i].targetChannel;
                             const uint8_t source = currentServoMixer[i].inputSource;
@@ -461,7 +477,6 @@ void processServoAutotrimMode(void)
                 if (ARMING_FLAG(ARMED)) {
                     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
                         for (int i = 0; i < servoRuleCount; i++) {
-                            if (!currentServoMixerActivative[i]) {continue;}
                             const uint8_t target = currentServoMixer[i].targetChannel;
                             const uint8_t source = currentServoMixer[i].inputSource;
                             if (source == axis) {
@@ -474,7 +489,6 @@ void processServoAutotrimMode(void)
                     if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) {
                         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
                             for (int i = 0; i < servoRuleCount; i++) {
-                                if (!currentServoMixerActivative[i]) {continue;}
                                 const uint8_t target = currentServoMixer[i].targetChannel;
                                 const uint8_t source = currentServoMixer[i].inputSource;
                                 if (source == axis) {
@@ -508,7 +522,6 @@ void processServoAutotrimMode(void)
         if (trimState == AUTOTRIM_SAVE_PENDING) {
             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
                 for (int i = 0; i < servoRuleCount; i++) {
-                    if (!currentServoMixerActivative[i]) {continue;}
                     const uint8_t target = currentServoMixer[i].targetChannel;
                     const uint8_t source = currentServoMixer[i].inputSource;
                     if (source == axis) {

src/main/io/displayport_msp_bf_compat.c

@@ -96,10 +96,31 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
 
     case SYM_AH_DECORATION_DOWN:
         return BF_SYM_AH_DECORATION_DOWN;
-
-    case SYM_DIRECTION:
-        return BF_SYM_DIRECTION;
 */
+    case SYM_DIRECTION:
+        return BF_SYM_ARROW_NORTH;
+    
+    case SYM_DIRECTION + 1: // NE pointing arrow
+        return BF_SYM_ARROW_7;
+
+    case SYM_DIRECTION + 2: // E pointing arrow
+        return BF_SYM_ARROW_EAST;
+
+    case SYM_DIRECTION + 3: // SE pointing arrow
+        return BF_SYM_ARROW_3;
+
+    case SYM_DIRECTION + 4: // S pointing arrow
+        return BF_SYM_ARROW_SOUTH;
+
+    case SYM_DIRECTION + 5: // SW pointing arrow
+        return BF_SYM_ARROW_15;
+
+    case SYM_DIRECTION + 6: // W pointing arrow
+        return BF_SYM_ARROW_WEST;
+
+    case SYM_DIRECTION + 7: // NW pointing arrow
+        return BF_SYM_ARROW_11;
+
     case SYM_VOLT:
         return BF_SYM_VOLT;
 
@@ -187,13 +208,9 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
     case SYM_ALT_M:
         return BF_SYM_M;
 
-/*
-    case SYM_TRIP_DIST:
-        return BF_SYM_TRIP_DIST;
-
     case SYM_TOTAL:
-        return BF_SYM_TOTAL;
+        return BF_SYM_TOTAL_DISTANCE;
-
+/*
     case SYM_ALT_KM:
         return BF_SYM_ALT_KM;
 
@@ -226,20 +243,19 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
 /*
     case SYM_NM:
         return BF_SYM_NM;
-
+*/
     case SYM_WIND_HORIZONTAL:
-        return BF_SYM_WIND_HORIZONTAL;
+        return 'W';     // W for wind
 
+/*
     case SYM_WIND_VERTICAL:
         return BF_SYM_WIND_VERTICAL;
 
     case SYM_3D_KT:
         return BF_SYM_3D_KT;
-
-
-    case SYM_AIR:
-        return BF_SYM_AIR;
 */
+    case SYM_AIR:
+        return 'A';     // A for airspeed
 
     case SYM_3D_KMH:
         return BF_SYM_KPH;
@@ -334,10 +350,12 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
 
     case SYM_PITCH_DOWN:
         return BF_SYM_PITCH_DOWN;
+ */
 
     case SYM_GFORCE:
-        return BF_SYM_GFORCE;
+        return 'G';
 
+/*
     case SYM_GFORCE_X:
         return BF_SYM_GFORCE_X;
 

src/main/io/displayport_msp_osd.c

@@ -42,6 +42,7 @@
 #include "drivers/osd_symbols.h"
 
 #include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
 
 #include "io/osd.h"
 #include "io/displayport_msp.h"
@@ -113,6 +114,10 @@ static void checkVtxPresent(void)
     if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) {
         vtxActive = false;
     }
+
+    if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { 
+        vtxActive = true;
+    }
 }
 
 static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len)

src/main/io/osd.h

@@ -274,15 +274,17 @@ typedef enum {
     OSD_PILOT_NAME,
     OSD_PAN_SERVO_CENTRED,
     OSD_MULTI_FUNCTION,
+    OSD_ODOMETER,
+    OSD_PILOT_LOGO,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
 typedef enum {
     OSD_UNIT_IMPERIAL,
     OSD_UNIT_METRIC,
-    OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph
+    OSD_UNIT_METRIC_MPH,    // Old UK units, all metric except speed in mph
-    OSD_UNIT_UK, // Show everything in imperial, temperature in C
+    OSD_UNIT_UK,            // Show everything in imperial, temperature in C
-    OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C
+    OSD_UNIT_GA,            // General Aviation: Knots, Nautical Miles, Feet, Degrees C
 
     OSD_UNIT_MAX = OSD_UNIT_GA,
 } osd_unit_e;
@@ -343,111 +345,112 @@ PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig);
 
 typedef struct osdConfig_s {
     // Alarms
-    uint8_t rssi_alarm; // rssi %
+    uint8_t         rssi_alarm;                         // rssi %
-    uint16_t time_alarm; // fly minutes
+    uint16_t        time_alarm;                         // fly minutes
-    uint16_t alt_alarm; // positive altitude in m
+    uint16_t        alt_alarm;                          // positive altitude in m
-    uint16_t dist_alarm; // home distance in m
+    uint16_t        dist_alarm;                         // home distance in m
-    uint16_t neg_alt_alarm; // abs(negative altitude) in m
+    uint16_t        neg_alt_alarm;                      // abs(negative altitude) in m
-    uint8_t current_alarm; // current consumption in A
+    uint8_t         current_alarm;                      // current consumption in A
-    int16_t imu_temp_alarm_min;
+    int16_t         imu_temp_alarm_min;
-    int16_t imu_temp_alarm_max;
+    int16_t         imu_temp_alarm_max;
-    int16_t esc_temp_alarm_min;
+    int16_t         esc_temp_alarm_min;
-    int16_t esc_temp_alarm_max;
+    int16_t         esc_temp_alarm_max;
-    float gforce_alarm;
+    float           gforce_alarm;
-    float gforce_axis_alarm_min;
+    float           gforce_axis_alarm_min;
-    float gforce_axis_alarm_max;
+    float           gforce_axis_alarm_max;
 #ifdef USE_SERIALRX_CRSF
-    int8_t snr_alarm; //CRSF SNR alarm in dB
+    int8_t          snr_alarm;                          //CRSF SNR alarm in dB
-    int8_t link_quality_alarm;
+    int8_t          link_quality_alarm;
-    int16_t rssi_dbm_alarm; // in dBm
+    int16_t         rssi_dbm_alarm;                     // in dBm
-    int16_t rssi_dbm_max;  // Perfect RSSI. Set to High end of curve. RSSI at 100%
+    int16_t         rssi_dbm_max;                       // Perfect RSSI. Set to High end of curve. RSSI at 100%
-    int16_t rssi_dbm_min;   // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0%
+    int16_t         rssi_dbm_min;                       // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0%
 #endif
 #ifdef USE_BARO
-    int16_t baro_temp_alarm_min;
+    int16_t         baro_temp_alarm_min;
-    int16_t baro_temp_alarm_max;
+    int16_t         baro_temp_alarm_max;
 #endif
 #ifdef USE_TEMPERATURE_SENSOR
     osd_alignment_e temp_label_align;
 #endif
 #ifdef USE_PITOT
-    float airspeed_alarm_min;
+    float           airspeed_alarm_min;
-    float airspeed_alarm_max;
+    float           airspeed_alarm_max;
 #endif
 
-    videoSystem_e video_system;
+    videoSystem_e   video_system;
-    uint8_t row_shiftdown;
+    uint8_t         row_shiftdown;
-    int16_t msp_displayport_fullframe_interval;
+    int16_t         msp_displayport_fullframe_interval;
 
     // Preferences
-    uint8_t main_voltage_decimals;
+    uint8_t         main_voltage_decimals;
-    uint8_t ahi_reverse_roll;
+    uint8_t         ahi_reverse_roll;
-    uint8_t ahi_max_pitch;
+    uint8_t         ahi_max_pitch;
-    uint8_t crosshairs_style; // from osd_crosshairs_style_e
+    uint8_t         crosshairs_style;                   // from osd_crosshairs_style_e
-    int8_t horizon_offset;
+    int8_t          horizon_offset;
-    int8_t camera_uptilt;
+    int8_t          camera_uptilt;
-    bool ahi_camera_uptilt_comp;
+    bool            ahi_camera_uptilt_comp;
-    uint8_t camera_fov_h;
+    uint8_t         camera_fov_h;
-    uint8_t camera_fov_v;
+    uint8_t         camera_fov_v;
-    uint8_t hud_margin_h;
+    uint8_t         hud_margin_h;
-    uint8_t hud_margin_v;
+    uint8_t         hud_margin_v;
-    bool hud_homing;
+    bool            hud_homing;
-    bool hud_homepoint;
+    bool            hud_homepoint;
-    uint8_t hud_radar_disp;
+    uint8_t         hud_radar_disp;
-    uint16_t hud_radar_range_min;
+    uint16_t        hud_radar_range_min;
-    uint16_t hud_radar_range_max;
+    uint16_t        hud_radar_range_max;
-    uint8_t hud_radar_alt_difference_display_time;
+    uint8_t         hud_radar_alt_difference_display_time;
-    uint8_t hud_radar_distance_display_time;
+    uint8_t         hud_radar_distance_display_time;
-    uint8_t hud_wp_disp;
+    uint8_t         hud_wp_disp;
 
-    uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
+    uint8_t         left_sidebar_scroll;                // from osd_sidebar_scroll_e
-    uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e
+    uint8_t         right_sidebar_scroll;               // from osd_sidebar_scroll_e
-    uint8_t sidebar_scroll_arrows;
+    uint8_t         sidebar_scroll_arrows;
 
-    uint8_t units; // from osd_unit_e
+    uint8_t         units;                              // from osd_unit_e
-    uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
+    uint8_t         stats_energy_unit;                  // from osd_stats_energy_unit_e
-    uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e
+    uint8_t         stats_min_voltage_unit;             // from osd_stats_min_voltage_unit_e
-    uint8_t stats_page_auto_swap_time;   // stats page auto swap interval time (seconds)
+    uint8_t         stats_page_auto_swap_time;          // stats page auto swap interval time (seconds)
 
 #ifdef USE_WIND_ESTIMATOR
-    bool    estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
+    bool            estimations_wind_compensation;      // use wind compensation for estimated remaining flight/distance
-#endif
+#endif	
-
+    uint8_t         coordinate_digits;
-    uint8_t coordinate_digits;
+    bool            osd_failsafe_switch_layout;
-
+    uint8_t         plus_code_digits;                   // Number of digits to use in OSD_PLUS_CODE
-    bool    osd_failsafe_switch_layout;
+    uint8_t         plus_code_short;
-    uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
+    uint8_t         ahi_style;
-    uint8_t plus_code_short;
+    uint8_t         force_grid;                         // Force a pixel based OSD to use grid mode.
-    uint8_t ahi_style;
+    uint8_t         ahi_bordered;                       // Only used by the AHI widget
-    uint8_t force_grid;                         // Force a pixel based OSD to use grid mode.
+    uint8_t         ahi_width;                          // In pixels, only used by the AHI widget
-    uint8_t ahi_bordered;                       // Only used by the AHI widget
+    uint8_t         ahi_height;                         // In pixels, only used by the AHI widget
-    uint8_t ahi_width;                          // In pixels, only used by the AHI widget
+    int8_t          ahi_vertical_offset;                // Offset from center in pixels. Positive moves the AHI down. Widget only.
-    uint8_t ahi_height;                         // In pixels, only used by the AHI widget
+    int8_t          sidebar_horizontal_offset;          // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges.
-    int8_t  ahi_vertical_offset;                // Offset from center in pixels. Positive moves the AHI down. Widget only.
+    uint8_t         left_sidebar_scroll_step;           // How many units each sidebar step represents. 0 means the default value for the scroll type.
-    int8_t  sidebar_horizontal_offset;          // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges.
+    uint8_t         right_sidebar_scroll_step;          // Same as left_sidebar_scroll_step, but for the right sidebar.
-    uint8_t left_sidebar_scroll_step;           // How many units each sidebar step represents. 0 means the default value for the scroll type.
+    bool            osd_home_position_arm_screen;
-    uint8_t right_sidebar_scroll_step;          // Same as left_sidebar_scroll_step, but for the right sidebar.
+    uint8_t         pan_servo_index;                    // Index of the pan servo used for home direction offset
-    bool    osd_home_position_arm_screen;
+    int8_t          pan_servo_pwm2centideg;             // Centidegrees of servo rotation per us pwm
-    uint8_t pan_servo_index;                    // Index of the pan servo used for home direction offset
+    uint8_t         pan_servo_offcentre_warning;        // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred
-    int8_t  pan_servo_pwm2centideg;             // Centidegrees of servo rotation per us pwm
+    bool            pan_servo_indicator_show_degrees;   // Show the degrees of offset for the pan servo
-    uint8_t pan_servo_offcentre_warning;        // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred
+    uint8_t         crsf_lq_format;
-    bool    pan_servo_indicator_show_degrees;   // Show the degrees of offset for the pan servo
+    uint8_t         sidebar_height;                     // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
-    uint8_t crsf_lq_format;
+    uint8_t         telemetry;                          // use telemetry on displayed pixel line 0
-    uint8_t sidebar_height;                     // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
+    uint8_t         esc_rpm_precision;                  // Number of characters used for the RPM numbers.
-    uint8_t telemetry; 				            // use telemetry on displayed pixel line 0
+    uint16_t        system_msg_display_time;            // system message display time for multiple messages (ms)
-    uint8_t esc_rpm_precision;                  // Number of characters used for the RPM numbers.
+    uint8_t         mAh_precision;                 // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
-    uint16_t system_msg_display_time;           // system message display time for multiple messages (ms)
+    uint8_t         ahi_pitch_interval;                 // redraws AHI at set pitch interval (Not pixel OSD)
-    uint8_t mAh_used_precision;                 // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
+    char            osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0.
-    uint8_t ahi_pitch_interval;                 // redraws AHI at set pitch interval (Not pixel OSD)
+    uint8_t         osd_switch_indicator0_channel;      // RC Channel to use for switch indicator 0.
-    char    osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1];      // Name to use for switch indicator 0.
+    char            osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1.
-    uint8_t osd_switch_indicator0_channel;     // RC Channel to use for switch indicator 0.
+    uint8_t         osd_switch_indicator1_channel;      // RC Channel to use for switch indicator 1.
-    char    osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1];      // Name to use for switch indicator 1.
+    char            osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2.
-    uint8_t osd_switch_indicator1_channel;     // RC Channel to use for switch indicator 1.
+    uint8_t         osd_switch_indicator2_channel;      // RC Channel to use for switch indicator 2.
-    char    osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1];      // Name to use for switch indicator 2.
+    char            osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3.
-    uint8_t osd_switch_indicator2_channel;     // RC Channel to use for switch indicator 2.
+    uint8_t         osd_switch_indicator3_channel;      // RC Channel to use for switch indicator 3.
-    char    osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1];      // Name to use for switch indicator 3.
+    bool            osd_switch_indicators_align_left;   // Align switch indicator name to left of the switch.
-    uint8_t osd_switch_indicator3_channel;     // RC Channel to use for switch indicator 3.
+    bool            use_pilot_logo;                     // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
-    bool    osd_switch_indicators_align_left;   // Align switch indicator name to left of the switch.
+    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
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);
@@ -483,7 +486,7 @@ void osdStartedSaveProcess(void);
 void osdShowEEPROMSavedNotification(void);
 
 void osdCrosshairPosition(uint8_t *x, uint8_t *y);
-bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
+bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros);
 void osdFormatAltitudeSymbol(char *buff, int32_t alt);
 void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max);
 // Returns a heading angle in degrees normalized to [0, 360).

src/main/io/osd_hud.c

@@ -256,18 +256,18 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
             case OSD_UNIT_IMPERIAL:
                 {
                     if (poiType == 1) {
-                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 4, 4);
+                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 4, 4, false);
                     } else {
-                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3);
+                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3, false);
                     }
                 }
                 break;
             case OSD_UNIT_GA:
                 {
                     if (poiType == 1) {
-                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4);
+                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4, false);
                     } else {
-                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3);
+                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3, false);
                     }
                 }
                 break;
@@ -278,9 +278,9 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
             case OSD_UNIT_METRIC:
                 {
                     if (poiType == 1) {
-                        osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 4, 4);
+                        osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 4, 4, false);
                     } else {
-                        osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3);
+                        osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3, false);
                     }
                 }
                 break;

src/main/io/osd_joystick.c

@@ -0,0 +1,74 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "common/crc.h"
+#include "common/maths.h"
+#include "common/streambuf.h"
+#include "common/utils.h"
+
+#include "build/build_config.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+#include "fc/settings.h"
+#include "fc/runtime_config.h"
+
+#include "drivers/time.h"
+#include "drivers/light_ws2811strip.h"
+
+#include "io/serial.h"
+#include "io/rcdevice.h"
+
+#include "osd_joystick.h"
+
+#ifdef USE_RCDEVICE
+#ifdef USE_LED_STRIP
+
+
+PG_REGISTER_WITH_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig, PG_OSD_JOYSTICK_CONFIG, 0);
+
+PG_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig,
+    .osd_joystick_enabled = SETTING_OSD_JOYSTICK_ENABLED_DEFAULT,
+    .osd_joystick_down = SETTING_OSD_JOYSTICK_DOWN_DEFAULT,
+    .osd_joystick_up = SETTING_OSD_JOYSTICK_UP_DEFAULT,
+    .osd_joystick_left = SETTING_OSD_JOYSTICK_LEFT_DEFAULT,
+    .osd_joystick_right = SETTING_OSD_JOYSTICK_RIGHT_DEFAULT,
+    .osd_joystick_enter = SETTING_OSD_JOYSTICK_ENTER_DEFAULT
+);
+
+bool osdJoystickEnabled(void) {
+    return osdJoystickConfig()->osd_joystick_enabled;
+}
+
+
+void osdJoystickSimulate5KeyButtonPress(uint8_t operation) {
+    switch (operation) {
+        case RCDEVICE_CAM_KEY_ENTER:
+            ledPinStartPWM( osdJoystickConfig()->osd_joystick_enter );
+            break;
+        case RCDEVICE_CAM_KEY_LEFT:
+            ledPinStartPWM( osdJoystickConfig()->osd_joystick_left );
+            break;
+        case RCDEVICE_CAM_KEY_UP:
+            ledPinStartPWM( osdJoystickConfig()->osd_joystick_up );
+            break;
+        case RCDEVICE_CAM_KEY_RIGHT:
+            ledPinStartPWM( osdJoystickConfig()->osd_joystick_right );
+            break;
+        case RCDEVICE_CAM_KEY_DOWN:
+            ledPinStartPWM( osdJoystickConfig()->osd_joystick_down );
+            break;
+    }
+}
+
+
+void osdJoystickSimulate5KeyButtonRelease(void) {
+    ledPinStopPWM();
+}
+
+
+#endif
+#endif

src/main/io/osd_joystick.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "config/parameter_group.h"
+
+#ifdef USE_RCDEVICE 
+#ifdef USE_LED_STRIP
+
+typedef struct osdJoystickConfig_s {
+    bool osd_joystick_enabled;
+    uint8_t osd_joystick_down;
+    uint8_t osd_joystick_up;
+    uint8_t osd_joystick_left;
+    uint8_t osd_joystick_right;
+    uint8_t osd_joystick_enter;
+} osdJoystickConfig_t;
+
+PG_DECLARE(osdJoystickConfig_t, osdJoystickConfig);
+
+bool osdJoystickEnabled(void);
+
+// 5 key osd cable simulation
+void osdJoystickSimulate5KeyButtonPress(uint8_t operation);
+void osdJoystickSimulate5KeyButtonRelease(void);
+
+#endif
+#endif

src/main/io/osd_utils.c

@@ -38,7 +38,7 @@ int digitCount(int32_t value)
 }
 
 
-bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length)
+bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros)
 {
     char *ptr = buff;
     char *dec;
@@ -86,7 +86,11 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
     // Done counting. Time to write the characters.
     // Write spaces at the start
     while (remaining > 0) {
-        *ptr = SYM_BLANK;
+        if (leadingZeros) 
+            *ptr = '0';
+        else
+            *ptr = SYM_BLANK;
+
         ptr++;
         remaining--;
     }
@@ -98,7 +102,11 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
         // Add any needed remaining leading spaces
         while(rem_spaces > 0)
         {
-            *ptr = SYM_BLANK;
+            if (leadingZeros) 
+                *ptr = '0';
+            else
+                *ptr = SYM_BLANK;
+
             ptr++;
             remaining--;
             rem_spaces--;

src/main/io/osd_utils.h

@@ -33,6 +33,6 @@ int digitCount(int32_t value);
  * of the same length. If the value doesn't fit into the provided length
  * it will be divided by scale and true will be returned.
  */
-bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
+bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros);
 
 #endif

src/main/io/rcdevice_cam.c

@@ -29,6 +29,7 @@
 
 #include "io/beeper.h"
 #include "io/rcdevice_cam.h"
+#include "io/osd_joystick.h"
 
 #include "rx/rx.h"
 
@@ -47,6 +48,14 @@ bool waitingDeviceResponse = false;
 
 static bool isFeatureSupported(uint8_t feature)
 {
+#ifndef UNIT_TEST
+#ifdef USE_LED_STRIP
+    if (!rcdeviceIsEnabled() && osdJoystickEnabled() ) {
+        return true;
+    }
+#endif
+#endif
+
     if (camDevice->info.features & feature) {
         return true;
     }
@@ -72,6 +81,7 @@ static void rcdeviceCameraControlProcess(void)
             }
 
             uint8_t behavior = RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION;
+            uint8_t behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION;
             switch (i) {
             case BOXCAMERA1:
                 if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON)) {
@@ -81,11 +91,13 @@ static void rcdeviceCameraControlProcess(void)
                     if (!ARMING_FLAG(ARMED)) {
                         behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN;
                     }
+                    behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN;
                 }
                 break;
             case BOXCAMERA2:
                 if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON)) {
                     behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN;
+                    behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN;
                 }
                 break;
             case BOXCAMERA3:
@@ -94,16 +106,43 @@ static void rcdeviceCameraControlProcess(void)
                     if (!ARMING_FLAG(ARMED)) {
                         behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE;
                     }
+                    behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE;
                 }
                 break;
             default:
                 break;
             }
-            if (behavior != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) {
+            if ((behavior != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) && rcdeviceIsEnabled()) {
                 runcamDeviceSimulateCameraButton(camDevice, behavior);
                 switchStates[switchIndex].isActivated = true;
             }
+#ifndef UNIT_TEST
+#ifdef USE_LED_STRIP
+            else if ((behavior1 != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) && osdJoystickEnabled()) {
+                switch (behavior1) {
+                        case RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN:
+                            osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_ENTER);
+                            break;
+                        case RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN:
+                            osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_UP);
+                            break;
+                        case RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE:
+                            osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_DOWN);
+                            break;
+                    }
+                switchStates[switchIndex].isActivated = true;
+            }
+#endif
+#endif
+            UNUSED(behavior1);
         } else {
+#ifndef UNIT_TEST
+#ifdef USE_LED_STRIP
+            if (osdJoystickEnabled() && switchStates[switchIndex].isActivated) {
+                osdJoystickSimulate5KeyButtonRelease();
+            }
+#endif
+#endif
             switchStates[switchIndex].isActivated = false;
         }
     }
@@ -225,14 +264,24 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
     }
 #endif
 
-    if (camDevice->serialPort == 0 || ARMING_FLAG(ARMED)) {
+    if (ARMING_FLAG(ARMED)) {
         return;
     }
 
     if (isButtonPressed) {
         if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) {
-            rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+            if ( rcdeviceIsEnabled() ) {
-            waitingDeviceResponse = true;
+                rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+                waitingDeviceResponse = true;
+            }
+#ifndef UNIT_TEST
+#ifdef USE_LED_STRIP
+            else if (osdJoystickEnabled()) {
+                osdJoystickSimulate5KeyButtonRelease();
+                isButtonPressed = false;
+            }
+#endif
+#endif
         }
     } else {
         if (waitingDeviceResponse) {
@@ -266,16 +315,33 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
         }
 
         if (key != RCDEVICE_CAM_KEY_NONE) {
-            rcdeviceSend5KeyOSDCableSimualtionEvent(key);
+            if ( rcdeviceIsEnabled() ) {
+                rcdeviceSend5KeyOSDCableSimualtionEvent(key);
+                waitingDeviceResponse = true;
+            }
+#ifndef UNIT_TEST
+#ifdef USE_LED_STRIP
+            else if (osdJoystickEnabled()) {
+                if ( key == RCDEVICE_CAM_KEY_CONNECTION_OPEN ) {
+                    rcdeviceInMenu = true;
+                } else if ( key == RCDEVICE_CAM_KEY_CONNECTION_CLOSE ) {
+                    rcdeviceInMenu = false;
+                } else {
+                    osdJoystickSimulate5KeyButtonPress(key);
+                }
+            }
+#endif
+#endif
             isButtonPressed = true;
-            waitingDeviceResponse = true;
         }
     }
 }
 
 void rcdeviceUpdate(timeUs_t currentTimeUs)
 {
-    rcdeviceReceive(currentTimeUs);
+    if ( rcdeviceIsEnabled() ) {
+        rcdeviceReceive(currentTimeUs);
+    }
 
     rcdeviceCameraControlProcess();
 

src/main/io/vtx_tramp.c

@@ -575,16 +575,27 @@ const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] =
 const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT]         = { 25, 200, 800 };
 const char * const trampPowerNames_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800" };
 
+const uint16_t trampPowerTable_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT]         = { 25, 200, 2000 };
+const char * const trampPowerNames_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "2000" };
+
 static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
 {
     switch (vtxSettingsConfig()->frequencyGroup) {
         case FREQUENCYGROUP_1G3:
-            vtxState.metadata.powerTablePtr  = trampPowerTable_1G3_800;
+            if (maxPower >= 2000) {
-            vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
+               vtxState.metadata.powerTablePtr  = trampPowerTable_1G3_2000;
+               vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
             
-            impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800;
+               impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_2000;
-            impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
+               impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
+            }
+            else {
+               vtxState.metadata.powerTablePtr  = trampPowerTable_1G3_800;
+               vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
             
+               impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800;
+               impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
+            }
             impl_vtxDevice.capability.bandCount = VTX_TRAMP_1G3_BAND_COUNT;
             impl_vtxDevice.capability.channelCount = VTX_TRAMP_1G3_CHANNEL_COUNT;
             impl_vtxDevice.capability.bandNames = (char **)vtx1G3BandNames;

src/main/msp/msp_protocol_v2_inav.h

@@ -92,3 +92,10 @@
 #define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049
 
+#define MSP2_INAV_RATE_DYNAMICS                 0x2060
+#define MSP2_INAV_SET_RATE_DYNAMICS             0x2061
+
+#define MSP2_INAV_EZ_TUNE                       0x2070
+#define MSP2_INAV_EZ_TUNE_SET                   0x2071
+
+#define MSP2_INAV_SELECT_MIXER_PROFILE          0x2080

src/main/navigation/navigation.c

@@ -62,6 +62,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
 #include "sensors/battery.h"
+#include "sensors/gyro.h"
 
 #include "programming/global_variables.h"
 
@@ -223,6 +224,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
 static navWapointHeading_t wpHeadingControl;
 navigationPosControl_t posControl;
 navSystemStatus_t NAV_Status;
+static bool landingDetectorIsActive;
 
 EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 
@@ -949,7 +951,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
         }
     },
-    
+
     /** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/
     [NAV_STATE_MIXERAT_INITIALIZE] = {
         .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE,
@@ -992,7 +994,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .onEvent = {
             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
-            
+
         }
     },
 };
@@ -1514,7 +1516,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
     if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
-    
+
     if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){
         return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
     }
@@ -2671,11 +2673,15 @@ bool findNearestSafeHome(void)
  *-----------------------------------------------------------*/
 void updateHomePosition(void)
 {
-    // Disarmed and have a valid position, constantly update home
+    // Disarmed and have a valid position, constantly update home before first arm (depending on setting)
+    // Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm)
+    static bool setHome = false;
+    navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING;
+
     if (!ARMING_FLAG(ARMED)) {
         if (posControl.flags.estPosStatus >= EST_USABLE) {
             const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
-            bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
+            setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
             switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
                 case NAV_RESET_NEVER:
                     break;
@@ -2686,24 +2692,16 @@ void updateHomePosition(void)
                     setHome = true;
                     break;
             }
-            if (setHome) {
-#if defined(USE_SAFE_HOME)
-                findNearestSafeHome();
-#endif
-                setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-                // save the current location in case it is replaced by a safehome or HOME_RESET
-                posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
-            }
         }
     }
     else {
         static bool isHomeResetAllowed = false;
-
         // If pilot so desires he may reset home position to current position
         if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
             if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
-                const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+                homeUpdateFlags = 0;
-                setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
+                homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+                setHome = true;
                 isHomeResetAllowed = false;
             }
         }
@@ -2718,6 +2716,22 @@ void updateHomePosition(void)
             posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
             updateHomePositionCompatibility();
         }
+
+        setHome &= !STATE(IN_FLIGHT_EMERG_REARM);   // prevent reset following emerg in flight rearm
+    }
+
+    if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) {
+#if defined(USE_SAFE_HOME)
+        findNearestSafeHome();
+#endif
+        setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
+
+        if (ARMING_FLAG(ARMED) && positionEstimationConfig()->reset_altitude_type == NAV_RESET_ON_EACH_ARM) {
+            posControl.rthState.homePosition.pos.z = 0;     // force to 0 if reference altitude also reset every arm
+        }
+        // save the current location in case it is replaced by a safehome or HOME_RESET
+        posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
+        setHome = false;
     }
 }
 
@@ -2890,6 +2904,9 @@ static void updateNavigationFlightStatistics(void)
     }
 }
 
+/*
+ * Total travel distance in cm
+ */
 uint32_t getTotalTravelDistance(void)
 {
     return lrintf(posControl.totalTripDistance);
@@ -2960,14 +2977,15 @@ void updateLandingStatus(timeMs_t currentTimeMs)
     }
     lastUpdateTimeMs = currentTimeMs;
 
-    static bool landingDetectorIsActive;
-
     DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
     DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED));
 
     if (!ARMING_FLAG(ARMED)) {
+        if (STATE(LANDING_DETECTED)) {
+            landingDetectorIsActive = false;
+        }
         resetLandingDetector();
-        landingDetectorIsActive = false;
+
         if (!IS_RC_MODE_ACTIVE(BOXARM)) {
             DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
         }
@@ -3004,11 +3022,28 @@ void resetLandingDetector(void)
     posControl.flags.resetLandingDetector = true;
 }
 
+void resetLandingDetectorActiveState(void)
+{
+    landingDetectorIsActive = false;
+}
+
 bool isFlightDetected(void)
 {
     return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying();
 }
 
+bool isProbablyStillFlying(void)
+{
+    bool inFlightSanityCheck;
+    if (STATE(MULTIROTOR)) {
+        inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f;
+    } else {
+        inFlightSanityCheck = isGPSHeadingValid();
+    }
+
+    return landingDetectorIsActive && inFlightSanityCheck;
+}
+
 /*-----------------------------------------------------------
  * Z-position controller
  *-----------------------------------------------------------*/
@@ -3927,14 +3962,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         // CRUISE has priority over COURSE_HOLD and AH
-        if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) {
+        if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
             if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
                 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
         }
 
         // PH has priority over COURSE_HOLD
         // CRUISE has priority on AH
-        if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) {
+        if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
             if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
                 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
             }
@@ -3948,12 +3983,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
                 return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
         }
-    }
+    } else {
-    else {
         canActivateWaypoint = false;
 
         // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight)
-        canActivateLaunchMode = isNavLaunchEnabled();
+        canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid()));
     }
 
     return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@@ -4062,13 +4096,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
         }
     }
 
-        /*
+    /*
-         * Don't allow arming if any of JUMP waypoint has invalid settings
+     * Don't allow arming if any of JUMP waypoint has invalid settings
-         * First WP can't be JUMP
+     * First WP can't be JUMP
-         * Can't jump to immediately adjacent WPs (pointless)
+     * Can't jump to immediately adjacent WPs (pointless)
-         * Can't jump beyond WP list
+     * Can't jump beyond WP list
-         * Only jump to geo-referenced WP types
+     * Only jump to geo-referenced WP types
-         */
+     */
     if (posControl.waypointCount) {
         for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++){
             if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){

src/main/navigation/navigation.h

@@ -614,6 +614,8 @@ const char * fixedWingLaunchStateMessage(void);
 float calculateAverageSpeed(void);
 
 void updateLandingStatus(timeMs_t currentTimeMs);
+bool isProbablyStillFlying(void);
+void resetLandingDetectorActiveState(void);
 
 const navigationPIDControllers_t* getNavigationPIDControllers(void);
 

src/main/navigation/navigation_fixedwing.c

@@ -695,7 +695,7 @@ bool isFixedWingFlying(void)
     bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f;
     bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING;
 
-    return (isImuHeadingValid() && throttleCondition && velCondition) || launchCondition;
+    return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition;
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation_pos_estimator.c

@@ -54,6 +54,7 @@
 #include "sensors/opflow.h"
 
 navigationPosEstimator_t posEstimator;
+static float initialBaroAltitudeOffset = 0.0f;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5);
 
@@ -113,6 +114,25 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur
 
 static bool shouldResetReferenceAltitude(void)
 {
+    /* Reference altitudes reset constantly when disarmed.
+     * On arming ref altitudes saved as backup in case of emerg in flight rearm
+     * If emerg in flight rearm active ref altitudes reset to backup values to avoid unwanted altitude reset */
+
+    static float backupInitialBaroAltitudeOffset = 0.0f;
+    static int32_t backupGpsOriginAltitude = 0;
+    static bool emergRearmResetCheck = false;
+
+    if (ARMING_FLAG(ARMED) && emergRearmResetCheck) {
+        if (STATE(IN_FLIGHT_EMERG_REARM)) {
+            initialBaroAltitudeOffset = backupInitialBaroAltitudeOffset;
+            posControl.gpsOrigin.alt = backupGpsOriginAltitude;
+        } else {
+            backupInitialBaroAltitudeOffset = initialBaroAltitudeOffset;
+            backupGpsOriginAltitude = posControl.gpsOrigin.alt;
+        }
+    }
+    emergRearmResetCheck = !ARMING_FLAG(ARMED);
+
     switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) {
         case NAV_RESET_NEVER:
             return false;
@@ -325,7 +345,6 @@ void onNewGPSData(void)
  */
 void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
 {
-    static float initialBaroAltitudeOffset = 0.0f;
     float newBaroAlt = baroCalculateAltitude();
 
     /* If we are required - keep altitude at zero */

src/main/programming/logic_condition.c

@@ -56,6 +56,7 @@
 
 #include "io/vtx.h"
 #include "drivers/vtx_common.h"
+#include "drivers/light_ws2811strip.h"
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 4);
 
@@ -475,6 +476,17 @@ static int logicConditionCompute(
             }
             break;
 
+#ifdef LED_PIN
+        case LOGIC_CONDITION_LED_PIN_PWM:
+            if (operandA >=0 && operandA <= 100) {
+                ledPinStartPWM((uint8_t)operandA);
+            } else {
+                ledPinStopPWM();
+            }
+            return operandA;
+            break;
+#endif
+        
 #ifdef USE_GPS_FIX_ESTIMATION
         case LOGIC_CONDITION_DISABLE_GPS_FIX:
             if (operandA > 0) {
@@ -720,6 +732,10 @@ static int logicConditionGetFlightOperandValue(int operand) {
             return constrain(attitude.values.pitch / 10, -180, 180);
             break;
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW: // deg
+            return constrain(attitude.values.yaw / 10, 0, 360);
+            break;
+
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1
             return ARMING_FLAG(ARMED) ? 1 : 0;
             break;

src/main/programming/logic_condition.h

@@ -81,10 +81,9 @@ typedef enum {
     LOGIC_CONDITION_TIMER                       = 49,
     LOGIC_CONDITION_DELTA                       = 50,
     LOGIC_CONDITION_APPROX_EQUAL                = 51,
-#ifdef USE_GPS_FIX_ESTIMATION
+    LOGIC_CONDITION_LED_PIN_PWM                 = 52,
-    LOGIC_CONDITION_DISABLE_GPS_FIX             = 52,
+    LOGIC_CONDITION_DISABLE_GPS_FIX             = 53,
-#endif
+    LOGIC_CONDITION_LAST                        = 54,
-    LOGIC_CONDITION_LAST                        = 53,
 } logicOperation_e;
 
 typedef enum logicOperandType_s {
@@ -138,8 +137,9 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2                      // 35
     LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2                             // 36
     LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int                   // 37
-    LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int              // 39
+    LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int              // 38
-    LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1           // 40
+    LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1           // 39
+    LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg                     // 40
 } logicFlightOperands_e;
 
 typedef enum {

src/main/rx/jetiexbus.c

@@ -63,7 +63,12 @@
 #define JETIEXBUS_BAUDRATE 125000                       // EX Bus 125000; EX Bus HS 250000 not supported
 #define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
 #define JETIEXBUS_MIN_FRAME_GAP     1000
-#define JETIEXBUS_CHANNEL_COUNT     16                  // most Jeti TX transmit 16 channels
+
+#ifdef USE_24CHANNELS
+#define JETIEXBUS_CHANNEL_COUNT 24
+#else
+#define JETIEXBUS_CHANNEL_COUNT 16
+#endif
 
 
 #define EXBUS_START_CHANNEL_FRAME       (0x3E)
@@ -153,6 +158,7 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
 
     static timeUs_t jetiExBusTimeLast = 0;
     static uint8_t *jetiExBusFrame;
+    static uint8_t jetiExBusFrameMaxSize;
     const timeUs_t now = microsISR();
 
     // Check if we shall reset frame position due to time
@@ -169,11 +175,13 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
         case EXBUS_START_CHANNEL_FRAME:
             jetiExBusFrameState = EXBUS_STATE_IN_PROGRESS;
             jetiExBusFrame = jetiExBusChannelFrame;
+            jetiExBusFrameMaxSize = EXBUS_MAX_CHANNEL_FRAME_SIZE;
             break;
 
         case EXBUS_START_REQUEST_FRAME:
             jetiExBusRequestState = EXBUS_STATE_IN_PROGRESS;
             jetiExBusFrame = jetiExBusRequestFrame;
+            jetiExBusFrameMaxSize = EXBUS_MAX_CHANNEL_FRAME_SIZE;
             break;
 
         default:
@@ -181,6 +189,15 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
         }
     }
 
+    if (jetiExBusFramePosition == jetiExBusFrameMaxSize) {
+        // frame overrun
+        jetiExBusFrameReset();
+        jetiExBusFrameState = EXBUS_STATE_ZERO;
+        jetiExBusRequestState = EXBUS_STATE_ZERO;
+
+        return;
+    }
+
     // Store in frame copy
     jetiExBusFrame[jetiExBusFramePosition] = (uint8_t)c;
     jetiExBusFramePosition++;

src/main/rx/rx.h

@@ -83,7 +83,11 @@ typedef enum {
     SERIALRX_FBUS,
 } rxSerialReceiverType_e;
 
-#define MAX_SUPPORTED_RC_CHANNEL_COUNT              18
+#ifdef USE_24CHANNELS
+#define MAX_SUPPORTED_RC_CHANNEL_COUNT 26
+#else
+#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18
+#endif
 
 #define NON_AUX_CHANNEL_COUNT 4
 #define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT)

src/main/sensors/battery.c

@@ -84,7 +84,7 @@ static bool batteryUseCapacityThresholds = false;
 static bool batteryFullWhenPluggedIn = false;
 static bool profileAutoswitchDisable = false;
 
-static uint16_t vbat = 0;                       // battery voltage in 0.1V steps (filtered)
+static uint16_t vbat = 0;                       // battery voltage in 0.01V steps (filtered)
 static uint16_t powerSupplyImpedance = 0;       // calculated impedance in milliohm
 static uint16_t sagCompensatedVBat = 0;         // calculated no load vbat
 static bool powerSupplyImpedanceIsValid = false;
@@ -297,6 +297,14 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
             vbat = 0;
             break;
     }
+
+#ifdef USE_SIMULATOR
+    if (ARMING_FLAG(SIMULATOR_MODE_HITL) && SIMULATOR_HAS_OPTION(HITL_SIMULATE_BATTERY)) {
+        vbat = ((uint16_t)simulatorData.vbat)*10;
+        return;
+    }
+#endif
+
     if (justConnected) {
         pt1FilterReset(&vbatFilterState, vbat);
     } else {

src/main/target/AIKONF4/target.h

@@ -86,14 +86,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             DEFAULT_I2C_BUS
-#define USE_MAG_AK8963
+#define USE_MAG_ALL
-#define USE_MAG_AK8975
-#define USE_MAG_HMC5883
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
 
 #define RANGEFINDER_I2C_BUS     DEFAULT_I2C_BUS
 #define TEMPERATURE_I2C_BUS     DEFAULT_I2C_BUS

src/main/target/AIRBOTF4/target.h

@@ -41,14 +41,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
-#define USE_MAG_HMC5883
+#define USE_MAG_ALL
-#define USE_MAG_AK8963
-#define USE_MAG_AK8975
-#define USE_MAG_MAG3110
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_LIS3MDL
 
 #define USE_BARO
 #define BARO_I2C_BUS             BUS_I2C2
@@ -139,4 +132,4 @@
 #define TARGET_IO_PORTD         0xffff
 
 #define USE_DSHOT
-#define USE_ESC_SENSOR
+#define USE_ESC_SENSOR

src/main/target/AIRBOTF7/target.h

@@ -70,12 +70,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
-#define USE_MAG_HMC5883
+#define USE_MAG_ALL
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
 #define PITOT_I2C_BUS           BUS_I2C1

src/main/target/ALIENFLIGHTF4/target.h

@@ -46,13 +46,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
-#define USE_MAG_MPU9250
+#define USE_MAG_ALL
-#define USE_MAG_HMC5883
-#define USE_MAG_MAG3110
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
 

src/main/target/ALIENFLIGHTNGF7/target.h

@@ -44,14 +44,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
-#define USE_MAG_AK8963
+#define USE_MAG_ALL
-#define USE_MAG_MPU9250
-#define USE_MAG_HMC5883
-#define USE_MAG_MAG3110
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_LIS3MDL
 
 #define AK8963_CS_PIN           PC15
 #define AK8963_SPI_BUS          BUS_SPI3

src/main/target/ANYFC/target.h

@@ -35,12 +35,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
-#define USE_MAG_HMC5883
+#define USE_MAG_ALL
-#define USE_MAG_MAG3110
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
 

src/main/target/ANYFCF7/target.h

@@ -35,12 +35,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
-#define USE_MAG_HMC5883
+#define USE_MAG_ALL
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
 

src/main/target/ANYFCM7/target.h

@@ -35,12 +35,7 @@
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
-#define USE_MAG_HMC5883
+#define USE_MAG_ALL
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C2
 

src/main/target/AOCODARCF405AIO/target.h

@@ -98,13 +98,7 @@
 // Mag 
 #define USE_MAG
 #define MAG_I2C_BUS          BUS_I2C1
-#define USE_MAG_HMC5883
+#define USE_MAG_ALL
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
-#define USE_MAG_MLX90393
 
 /*** Onboard Flash ***/    
 #define USE_SPI_DEVICE_3

src/main/target/AOCODARCF4V2/target.h

@@ -60,13 +60,7 @@
 //*********** Magnetometer / Compass *************
 #define USE_MAG
 #define MAG_I2C_BUS                     DEFAULT_I2C_BUS
-
+#define USE_MAG_ALL
-#define USE_MAG_AK8975
-#define USE_MAG_HMC5883
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
 
 // ******* SERIAL ********
 #define USE_VCP