Merge branch 'development' into dzikuvx-mr-cruise-experiments

2025-07-20 06:45:14 +03:00 · 2018-07-04 17:08:56 +02:00 · 2018-07-04 17:08:56 +02:00 · 1231df9654
commit 1231df9654
parent 46ff1e31e6 3f115185b9
160 changed files with 6506 additions and 2781 deletions
--- a/20
+++ b/20
@ -131,6 +131,7 @@ FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk
 FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' )

 FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
+FC_VER_SUFFIX ?=

 BUILD_DATE = $(shell date +%Y%m%d)

@ -266,13 +267,16 @@ CPPCHECK        = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
 #
 # Things we will build
 #
-ifeq ($(BUILD_SUFFIX),)
-TARGET_BIN      = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).bin
-TARGET_HEX      = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).hex
-else
-TARGET_BIN      = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)_$(BUILD_SUFFIX).bin
-TARGET_HEX      = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)_$(BUILD_SUFFIX).hex
+TARGET_BIN	:= $(BIN_DIR)/$(FORKNAME)_$(FC_VER)
+ifneq ($(FC_VER_SUFFIX),)
+    TARGET_BIN	:= $(TARGET_BIN)-$(FC_VER_SUFFIX)
 endif
+TARGET_BIN	:= $(TARGET_BIN)_$(TARGET)
+ifneq ($(BUILD_SUFFIX),)
+    TARGET_BIN	:= $(TARGET_BIN)_$(BUILD_SUFFIX)
+endif
+TARGET_BIN	:= $(TARGET_BIN).bin
+TARGET_HEX	= $(TARGET_BIN:.bin=.hex)

 TARGET_OBJ_DIR  = $(OBJECT_DIR)/$(TARGET)
 TARGET_ELF      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf
@ -386,7 +390,7 @@ clean:

 ## clean_test        : clean up all temporary / machine-generated files (tests)
 clean_test:
-	$(V0) cd src/test && $(MAKE) clean || true
+	$(V0) cd src/test && $(MAKE) clean

 ## clean_<TARGET>    : clean up one specific target
 $(CLEAN_TARGETS) :
@ -461,7 +465,7 @@ targets:

 ## test              : run the cleanflight test suite
 test:
-	$(V0) cd src/test && $(MAKE) test || true
+	$(V0) cd src/test && $(MAKE) test

 # rebuild everything when makefile changes
 # Make the generated files and the build stamp order only prerequisites,
--- a/docs/Battery.md
+++ b/docs/Battery.md
@ -28,23 +28,42 @@ On the first battery connection is always advisable to use a current limiter dev

 See the [Sparky board chapter](Board - Sparky.md).

-## Configuration
+## Voltage measurement

-Enable the `VBAT` feature.
+Enable the `VBAT` feature to enable the measurement of the battery voltage and the use of the voltage based OSD battery gauge, voltage based and energy based battery alarms.

-Configure min/max cell voltages using the following CLI setting:
+### Calibration

-`vbat_scale` - Adjust this to match actual measured battery voltage to reported value.
+`vbat_scale` - Adjust this setting to match actual measured battery voltage to reported value. Increasing this value increases the measured voltage.

-`vbat_max_cell_voltage` - Maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, i.e. 430 = 4.30V
+### Voltage measurement source

-`set vbat_warning_cell_voltage` - Warning voltage per cell; this triggers battery-out alarms, in 0.01V units, i.e. 340 = 3.40V
+Two voltage sources are available: raw voltage and sag compensated voltage. The raw voltage is the voltage directly measured at the battery while the sag compensated voltage is calculated by an algorithm aiming to provide a stable voltage source for gauges, telemetry and alarms. When the current drawn from a battery varies the provided voltage also varies due to the internal resistance of the battery, it is called sag. The sag can often trigger the battery alarms before the battery is empty and if you are relying on the battery voltage to know the charge state of your battery you have to land or cut the throttle to know the real, without load, battery voltage. The sag compensation algorithm simulates a battery with zero internal resistance and provides a stable reading independent from the drawn current.

-`vbat_min_cell_voltage` - Minimum voltage per cell; this triggers battery-out alarms, in 0.01V units, i.e. 330 = 3.30V
+You can select the voltage source used for battery alarms and telemetry with the `bat_voltage_source` setting. It can be set to either `RAW` for using raw battery voltage or `SAG_COMP` for using the calculated sag compensated voltage.
+
+You can see an illustration of the sag compensation algorithm in action in the following graph:
+
+![Voltage graph](assets/images/sag_compensated_battery_voltage_plot.png)
+
+### Voltage based OSD gauge and alarms
+
+Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Each profile stores the following voltage settings:
+
+`bat_cells` - Specify the number of cells of your battery. Allows the automatic selection of the battery profile when set to a value greater than 0. Set to 0 (default) for auto-detecting the number of cells (see next setting)
+
+`vbat_cell_detect_voltage` - Maximum voltage per cell, used for auto-detecting the number of cells of the battery. Should be higher than maximum cell voltage to take into account possible drift in measured voltage and keep cell count detection accurate (0.01V unit, i.e. 430 = 4.30V)
+
+`vbat_max_cell_voltage` - Maximum voltage per cell when the battery is fully charged. Used for the OSD voltage based battery gauge (0.01V unit, i.e. 420 = 4.20V)
+
+`vbat_warning_cell_voltage` - Cell warning voltage. A cell voltage bellow this value triggers the first (short beeps) voltage based battery alarm if used and also the blinking of the OSD voltage indicator if the battery capacity is not used instead (see bellow) (0.01V unit, i.e. 370 = 3.70V)
+
+`vbat_min_cell_voltage` - Cell minimum voltage. A cell voltage bellow this value triggers the second (long beeps) voltage based battery alarm if used and the OSD gauge will display 0% if the battery capacity is not used instead (see bellow) (0.01V unit, i.e. 350 = 3.50V)

 e.g.

 ```
+battery_profile 1
 set vbat_scale = 1100
 set vbat_max_cell_voltage = 430
 set vbat_warning_cell_voltage = 340
@ -172,3 +191,124 @@ set battery_capacity_critical = 440     // the battery critical alarm will sound
 ```

 Note that in this example even though your warning capacity (`battery_capacity_warning`) is set to 30% (660mAh), since 440mAh (`battery_capacity_critical`) is considered empty (0% left), the OSD capacity related items will only start to blink when the remaining battery percentage shown on the OSD is below 12%: (`battery_capacity_warning`-`battery_capacity_critical`)*100/(`battery_capacity`-`battery_capacity_critical`)=(660-440)*100/(2200-440)=12.5
+
+
+## Battery profiles
+
+Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): `bat_cells`, `vbat_cell_detect_voltage`, `vbat_max_cell_voltage`, `vbat_warning_cell_voltage`, `vbat_min_cell_voltage`, `battery_capacity_unit`, `battery_capacity`, `battery_capacity_warning`, `battery_capacity_critical`
+
+To enable the automatic battery profile switching based on battery voltage enable the `BAT_PROF_AUTOSWITCH` feature. For a profile to be automatically selected the number of cells of the battery needs to be specified (>0).
+
+### Battery profiles configuration examples
+
+#### Simple example
+
+In this example we want to use two different type of batteries for the same aircraft and switch manually between them. The first battery is a Li-Po (4.20V/cell) and the second battery is a Li-Ion (4.10V/cell).
+
+```
+battery_profile 1
+
+set bat_cells = 0
+set vbat_max_cell_voltage = 420
+set vbat_warning_cell_voltage = 370
+set vbat_min_cell_voltage = 340
+
+
+battery_profile 2
+
+set bat_cells = 0
+set vbat_max_cell_voltage = 410
+set vbat_warning_cell_voltage = 280
+set vbat_min_cell_voltage = 250
+```
+
+#### Simple example with automatic profile switching
+
+In this example we want to use two different batteries for the same aircraft and automatically switch between them when the battery is plugged in. The first battery is a Li-Po 2200mAh 3S and the second battery is a LiPo 1500mAh 4S. Since the iNav defaults for the cell detection voltage and max voltage are adequate for standard LiPo batteries they will not be modified. The warning and minimum voltage are not modified either in this example but you can set them to the value you like. Since we are using battery capacities only the warning voltage (kept at default in this example) will be used and only for triggering the battery voltage indicator blinking in the OSD.
+
+```
+feature BAT_PROF_AUTOSWITCH
+
+
+battery_profile 1
+
+set bat_cells = 3
+set battery_capacity_unit = MAH
+set battery_capacity = 2200
+set battery_capacity_warning = 440
+set battery_capacity_critical = 220
+
+
+battery_profile 2
+
+set bat_cells = 4
+set battery_capacity_unit = MAH
+set battery_capacity = 1500
+set battery_capacity_warning = 300
+set battery_capacity_critical = 150
+```
+
+#### Advanced automatic switching example
+
+Profile 1 is for a 3S 2200mAh Li-Po pack (max 4.20V/cell), profile 2 for a 3S 4000mAh Li-Ion pack (max 4.10V/cell) and profile 3 for a 4S 1500mAh Li-Po pack (max 4.20V/cell).
+With this configuration if the battery plugged in is less than 12.36V (3 x 4.12) the profile 2 will be automatically selected else if the battery voltage is less than 12.66V (3 x 4.22) the profile 1 will be automatically selected else if the battery voltage is less 17.20V (4 x 4.3) the profile 3 will be automatically selected. If a matching profile can't be found the last selected profile is used.
+
+```
+feature BAT_PROF_AUTOSWITCH
+
+
+battery_profile 1
+
+set bat_cells = 3
+set vbat_cell_detect_voltage = 422
+set vbat_max_cell_voltage = 420
+set vbat_warning_cell_voltage = 350
+set vbat_min_cell_voltage = 330
+set battery_capacity = 2200
+set battery_capacity_warning = 440
+set battery_capacity_critical = 220
+
+
+battery_profile 2
+
+set bat_cells = 3
+set vbat_cell_detect_voltage = 412
+set vbat_max_cell_voltage = 410
+set vbat_warning_cell_voltage = 300
+set vbat_min_cell_voltage = 280
+set battery_capacity = 4000
+set battery_capacity_warning = 800
+set battery_capacity_critical = 400
+
+
+battery_profile 3
+
+set bat_cells = 4
+set vbat_cell_detect_voltage = 430
+set vbat_max_cell_voltage = 420
+set vbat_warning_cell_voltage = 350
+set vbat_min_cell_voltage = 330
+set battery_capacity = 1500
+set battery_capacity_warning = 300
+set battery_capacity_critical = 150
+```
+
+## Remaining flight time and flight distance estimation
+
+The estimated remaining flight time and flight distance estimations can be displayed on the OSD (for fixed wing only for the moment). They are calculated from the GPS distance from home, remaining battery capacity and average power draw. They are taking into account the requested altitude change and heading to home change after altitude change following the switch to RTH. They are also taking into account the estimated wind if `osd_use_wind_compensation` is set to `ON`. When the timer and distance indicator reach 0 they will blink and you need to go home in a straight line manually or by engaging RTH. You should be left with at least `rth_energy_margin`% of battery left when arriving home if the cruise speed and power are set correctly (see bellow).
+
+To use this feature the following conditions need to be met:
+- The `VBAT`, `CURRENT_METER` and `GPS` features need to be enabled
+- The battery capacity needs to be specified in mWh (`battery_capacity` setting > 0 and `battery_capacity_unit` set to `MWH`)
+- The average ground speed of the aircraft without wind at cruise throttle needs to be set (`nav_fw_cruise_speed` setting in cm/s)
+- The average power draw at zero throttle needs to be specified (`idle_power` setting in 0.01W unit)
+- The average power draw at cruise throttle needs to be specified (`cruise_power` setting in 0.01W unit)
+- The battery needs to be full when plugged in (voltage >= (`vbat_max_cell_voltage` - 100mV) * cells)
+
+It is advised to set `nav_fw_cruise_speed` a bit lower than the real speed and `cruise_power` 10% higher than the power at cruise throttle to ensure variations in throttle during cruise won't cause the aircraft to draw more energy than estimated.
+
+If `---` is displayed during flight instead of the remaining flight time/distance it means at least one of the above conditions aren't met. If the OSD element is blinking and the digits are replaced by the horizontal wind symbol it means that the estimated horizontal wind is too strong to be able to return home at `nav_fw_cruise_speed`.
+
+## Automatic throttle compensation based on battery voltage
+
+Automatic throttle compensation based on battery voltage can be used by enabling the `THR_VBAT_COMP` feature. It is working like this: used_throttle = requested_throttle * battery_max_voltage / sag_compensated_voltage.
--- a/docs/Board
+++ b/docs/Board
@ -38,7 +38,7 @@
 * PPM and UART6 can be used together when S.BUS jumper is removed (close to PPM/SBUS connector)
 * Uses target **OMNIBUSF4V3**

-### Omnibus F4 v4
+### Omnibus F4 v4/v5

 * Switching voltage regulator - solves problem of overheating BEC
 * SD Card slot instead of flash memory
@ -143,10 +143,14 @@ Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount
 | CH5   | RX                    |
 | CH6   | TX                    |

-## SoftwareSerial
-
 ![Omnibus F4 Pro SmartPort using SoftwareSerial](assets/images/omnibusf4pro_ss.png)

+### Omnibus F4 v3/v4/v5 SoftwareSerial Connections
+
+The SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin.
+When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software (can be used for e.g. transmitting one-way telemetry).
+
+
 # Wiring diagrams for Omnibus F4 Pro

 Following diagrams applies to _Pro_ version with integrated current meter and JST connectors only
--- a/docs/Cli.md
+++ b/docs/Cli.md
@ -101,7 +101,6 @@ Re-apply any new defaults as desired.
 |  acc_task_frequency  | 500 | Determines accelerometer task frequency in async_mode = ALL. Depending on UAV type this frequency can be lowered to conserve CPU resources as long as vibrations are not a problem. |
 |  attitude_task_frequency  | 250 | Determines attitude task frequency when async_mode = ALL |
 |  async_mode  | NONE | Enables asynchronous mode processing for gyro/accelerometer and attitude computations. Allowed modes: NONE -> default behavior, all calculations are executed in main PID loop. GYRO -> gyro sampling and filtering is detached from main PID loop. PID loop runs based on looptime while gyro sampling uses gyro_sync_denom and gyro_lpf combination to determine its frequency. ALL -> in this mode, gyro, accelerometer and attitude are running as separate tasks. Accelerometer task frequency is determined by acc_task_frequency, attitude task frequency by attitude_task_frequency. In this mode ANGLE and HORIZON, as well GPS assisted flight modes (including PosHold) performance might be lowered. |
-|  mid_rc  | 1500 | This is an important number to set in order to avoid trimming receiver/transmitter. Most standard receivers will have this at 1500, however Futaba transmitters will need this set to 1520. A way to find out if this needs to be changed, is to clear all trim/subtrim on transmitter, and connect to GUI. Note the value most channels idle at - this should be the number to choose. Once midrc is set, use subtrim on transmitter to make sure all channels (except throttle of course) are centered at midrc value. |
 |  min_check  | 1100 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. |
 |  max_check  | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. |
 |  rssi_channel  | 0 | RX channel containing the RSSI signal |
@ -121,6 +120,7 @@ Re-apply any new defaults as desired.
 |  fixed_wing_auto_arm  | OFF | Auto-arm fixed wing aircraft on throttle above min_throttle, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
 |  disarm_kill_switch  | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. |
 |  auto_disarm_delay  | 5 | Delay before automatic disarming when using stick arming and MOTOR_STOP. This does not apply when using FIXED_WING |
+|  switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] |
 |  small_angle  | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm.  |
 |  reboot_character  | 82 | Special character used to trigger reboot |
 |  gps_provider  | UBLOX | Which GPS protocol to be used |
@ -135,6 +135,7 @@ Re-apply any new defaults as desired.
 |  inav_use_gps_velned  | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. |
 |  inav_gps_delay  | 200 | GPS position and velocity data usually arrive with a delay. This parameter defines this delay. Default (200) should be reasonable for most GPS receivers. |
 |  inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) |
+|  inav_reset_home | EACH_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM |
 |  inav_max_surface_altitude  | 200 | Max allowed altitude for surface following mode. [cm] |
 |  inav_w_z_baro_p  | 0.350 | Weight of barometer measurements in estimated altitude and climb rate |
 |  inav_w_z_gps_p  | 0.200 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes |
@ -146,6 +147,7 @@ Re-apply any new defaults as desired.
 |  inav_w_acc_bias  | 0.010 | Weight for accelerometer drift estimation |
 |  inav_max_eph_epv  | 1000.000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] |
 |  inav_baro_epv  | 100.000 | Uncertainty value for barometric sensor [cm] |
+|  name  | Empty string | Craft name |
 |  nav_disarm_on_landing  | OFF | If set to ON, iNav disarms the FC after landing |
 |  nav_use_midthr_for_althold  | OFF | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude |
 |  nav_extra_arming_safety  | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured |
@ -168,17 +170,20 @@ Re-apply any new defaults as desired.
 |  nav_rth_climb_ignore_emerg  | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. |
 |  nav_rth_alt_mode  | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details |
 |  nav_rth_altitude  | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) |
+|  nav_rth_home_altitude  | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] |
 |  nav_rth_abort_threshold  | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 |  nav_mc_bank_angle  | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
 |  nav_mc_hover_thr  | 1500 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. |
 |  nav_mc_auto_disarm_delay  | 2000 |  |
 |  nav_fw_cruise_thr  | 1400 | Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )  |
+|  nav_fw_cruise_speed  | 0 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s |
+|  nav_fw_allow_manual_thr_increase  | OFF | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing |
 |  nav_fw_min_thr  | 1200 | Minimum throttle for flying wing in GPS assisted modes |
 |  nav_fw_max_thr  | 1700 | Maximum throttle for flying wing in GPS assisted modes |
 |  nav_fw_bank_angle  | 20 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll |
 |  nav_fw_climb_angle  | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
 |  nav_fw_dive_angle  | 15 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
-|  nav_fw_pitch2thr  | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes |
+|  nav_fw_pitch2thr  | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
 |  nav_fw_loiter_radius  | 5000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] |
 |  nav_fw_launch_velocity  | 300 | Forward velocity threshold for swing-launch detection [cm/s] |
 |  nav_fw_launch_accel  | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
@ -194,6 +199,7 @@ Re-apply any new defaults as desired.
 |  nav_fw_launch_min_time | 0 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. |
 |  nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. |
 |  nav_fw_land_dive_angle  | 2 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees |
+|  nav_fw_cruise_yaw_rate  | 20 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]|
 |  serialrx_provider  | SPEK1024 | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |
 |  serialrx_halfduplex  | OFF | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire |
 |  serialrx_inverted     | OFF | Reverse the serial inversion of the  serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). |
@ -213,7 +219,10 @@ Re-apply any new defaults as desired.
 |  ltm_update_rate  | NORMAL | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. |
 |  battery_capacity  | 0 | Battery capacity in mAH. This value is used in conjunction with the current meter to determine remaining battery capacity. |
 |  vbat_scale  | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. |
-|  vbat_max_cell_voltage  | 424 | Maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 424 (4.24V) |
+|  bat_voltage_src  | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` |
+|  bat_cells  | 0 | Number of cells of the battery (0 = autodetect), see battery documentation |
+|  vbat_cell_detect_voltage  | 430 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V |
+|  vbat_max_cell_voltage  | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V |
 |  vbat_min_cell_voltage  | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) |
 |  vbat_warning_cell_voltage  | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) |
 |  current_meter_scale  | 400 | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. |
@ -222,6 +231,9 @@ Re-apply any new defaults as desired.
 |  battery_capacity_warning | 0 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. |
 |  battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. |
 |  battery_capacity_unit | MAH | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). |
+|  cruise_power  | 0 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit |
+|  idle_power  | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit |
+|  rth_energy_margin | 5 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation |
 |  multiwii_current_meter_output  | OFF | Default current output via MSP is in 0.01A steps. Setting this to 1 causes output in default multiwii scaling (1mA steps) |
 |  current_meter_type  | ADC | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. |
 |  align_gyro  | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. |
@ -233,6 +245,7 @@ Re-apply any new defaults as desired.
 |  align_mag_roll  | 0 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. |
 |  align_mag_pitch  | 0 | Same as align_mag_roll, but for the pitch axis. |
 |  align_mag_yaw  | 0 | Same as align_mag_roll, but for the yaw axis. |
+|  align_opflow | 5 | Optical flow module alignment (default CW0_DEG_FLIP) |
 |  gyro_hardware_lpf  | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. |
 |  moron_threshold  | 32 | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. |
 |  imu_dcm_kp  | 2500 | Inertial Measurement Unit KP Gain for accelerometer measurements |
@ -276,38 +289,18 @@ Re-apply any new defaults as desired.
 |  blackbox_device  | SPIFLASH | Selection of where to write blackbox data |
 |  sdcard_detect_inverted  | `TARGET dependent` | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. |
 |  ledstrip_visual_beeper  | OFF |  |
-|  osd_video_system     | 0     |  |
-|  osd_row_shiftdown    | 0     |  |
+|  osd_video_system     | AUTO   | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` |
+|  osd_row_shiftdown    | 0     | Number of rows to shift the OSD display (increase if top rows are cut off) |
 |  osd_units            | METRIC| IMPERIAL, METRIC, UK |
 |  osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) |
 |  osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. |
-|  osd_wh_drawn_pos | |  |
-|  osd_bat_remaining_capacity_pos | |  |
-|  osd_bat_remaining_percent_pos | |  |
-|  osd_efficiency_mah_pos | |  |
-|  osd_efficiency_wh_pos | |  |
-|  osd_rssi_alarm       | 20    |  |
-|  osd_time_alarm       | 10    |  |
-|  osd_alt_alarm        | 100   |  |
-|  osd_main_voltage_pos | 0     |  |
-|  osd_rssi_pos         | 0     |  |
-|  osd_flytimer_pos     | 0     |  |
-|  osd_ontime_pos       | 0     |  |
-|  osd_flymode_pos      | 0     |  |
-|  osd_throttle_pos     | 0     |  |
-|  osd_vtx_channel_pos  | 0     |  |
-|  osd_crosshairs       | 0     |  |
-|  osd_artificial_horizon  | 0  |  |
-|  osd_current_draw_pos | 0     |  |
-|  osd_mah_drawn_pos    | 0     |  |
-|  osd_craft_name_pos   | 0     |  |
-|  osd_gps_speed_pos    | 0     |  |
-|  osd_gps_sats_pos     | 0     |  |
-|  osd_altitude_pos     | 0     |  |
-|  osd_pid_roll_pos     | 0     |  |
-|  osd_pid_pitch_pos    | 0     |  |
-|  osd_pid_yaw_pos      | 0     |  |
-|  osd_power_pos        | 0     |  |
+|  osd_rssi_alarm       | 20    | Value bellow which to make the OSD RSSI indicator blink |
+|  osd_time_alarm       | 10    | Value above which to make the OSD flight time indicator blink (minutes) |
+|  osd_dist_alarm       | 1000  | Value above which to make the OSD distance from home indicator blink (meters) |
+|  osd_alt_alarm        | 100   | Value above which to make the OSD relative altitude indicator blink (meters) |
+|  osd_neg_alt_alarm    | 5    | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
+|  osd_estimations_wind_compensation  | ON | Use wind estimation for remaining flight time/distance estimation |
+|  osd_attitude_angle_decimals | 0 | Chose the numbers of decimals displayed by OSD attitude angle elements [0-1] |
 |  magzero_x  | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. |
 |  magzero_y  | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. |
 |  magzero_z  | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. |
@ -375,6 +368,7 @@ Re-apply any new defaults as desired.
 | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point    |
 |  max_angle_inclination_rll  | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° |
 |  max_angle_inclination_pit  | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° |
+|  fw_min_throttle_down_pitch  | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle |
 |  gyro_lpf_hz  | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 |  acc_lpf_hz  | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 |  dterm_lpf_hz  | 40 |  |
@ -417,6 +411,15 @@ Re-apply any new defaults as desired.
 |  model_preview_type        |  -1  | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons.  |
 |  tz_offset  | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs |
 |  tz_automatic_dst  | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`.  |
+|  vtx_band  | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. |
+|  vtx_channel  | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. |
+|  vtx_freq  | 5740 | Set the VTX frequency using raw MHz. This parameter is ignored unless `vtx_band` is 0. |
+|  vtx_halfduplex  | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. |
+|  vtx_low_power_disarm  | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. |
+|  vtx_pit_mode_freq  | Frequency to use (in MHz) when the VTX is in pit mode. |
+|  vtx_power  | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. |
+| motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] |
+| motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] |

 This Markdown table is made by MarkdwonTableMaker addon for google spreadsheet.
 Original Spreadsheet used to make this table can be found here https://docs.google.com/spreadsheets/d/1ubjYdMGmZ2aAMUNYkdfe3hhIF7wRfIjcuPOi_ysmp00/edit?usp=sharing
--- a/docs/Controls.md
+++ b/docs/Controls.md
@ -28,6 +28,9 @@ The stick positions are combined to activate different functions:
 | Profile 1                     | LOW      | LOW     | CENTER | LOW    |
 | Profile 2                     | LOW      | LOW     | HIGH   | CENTER |
 | Profile 3                     | LOW      | LOW     | CENTER | HIGH   |
+| Battery profile 1             | HIGH     | LOW     | CENTER | LOW    |
+| Battery profile 2             | HIGH     | LOW     | HIGH   | CENTER |
+| Battery profile 3             | HIGH     | LOW     | CENTER | HIGH   |
 | Calibrate Gyro                | LOW      | LOW     | LOW    | CENTER |
 | Calibrate Acc                 | HIGH     | LOW     | LOW    | CENTER |
 | Calibrate Mag/Compass         | HIGH     | HIGH    | LOW    | CENTER |
--- a/docs/assets/images/StickPositions.png
+++ b/docs/assets/images/StickPositions.png
--- a/docs/assets/images/StickPositions.svg
+++ b/docs/assets/images/StickPositions.svg
--- a/docs/assets/images/sag_compensated_battery_voltage_plot.png
+++ b/docs/assets/images/sag_compensated_battery_voltage_plot.png
--- a/make/source.mk
+++ b/make/source.mk
@ -74,6 +74,7 @@ COMMON_SRC = \
            flight/mixer.c \
            flight/pid.c \
            flight/pid_autotune.c \
+            flight/rth_estimator.c \
            flight/servos.c \
            flight/wind_estimator.c \
            io/beeper.c \
@ -123,6 +124,7 @@ COMMON_SRC = \
            blackbox/blackbox_encoding.c \
            blackbox/blackbox_io.c \
            cms/cms.c \
+            cms/cms_menu_battery.c \
            cms/cms_menu_blackbox.c \
            cms/cms_menu_builtin.c \
            cms/cms_menu_imu.c \
@ -163,12 +165,15 @@ COMMON_SRC = \
            navigation/navigation_geo.c \
            navigation/navigation_multicopter.c \
            navigation/navigation_pos_estimator.c \
+            navigation/navigation_pos_estimator_agl.c \
+            navigation/navigation_pos_estimator_flow.c \
            sensors/barometer.c \
            sensors/pitotmeter.c \
            sensors/rangefinder.c \
            sensors/opflow.c \
            telemetry/crsf.c \
            telemetry/frsky.c \
+            telemetry/frsky_d.c \
            telemetry/hott.c \
            telemetry/ibus_shared.c \
            telemetry/ibus.c \
@ -177,6 +182,7 @@ COMMON_SRC = \
            telemetry/msp_shared.c \
            telemetry/smartport.c \
            telemetry/telemetry.c \
+            io/vtx.c \
            io/vtx_string.c \
            io/vtx_smartaudio.c \
            io/vtx_tramp.c \
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@ -193,6 +193,34 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
    {"axisD",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
    {"axisD",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
    {"axisD",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
+    {"fwAltP",     -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"fwAltI",     -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"fwAltD",     -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"fwAltOut",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"fwPosP",     -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"fwPosI",     -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"fwPosD",     -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"fwPosOut",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
+    {"mcPosAxisP",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcPosAxisP",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcPosAxisP",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisP",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisP",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisP",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisI",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisI",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisI",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisD",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisD",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisD",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisOut",0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisOut",1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisOut",2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcSurfaceP", -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcSurfaceI", -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcSurfaceD", -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcSurfaceOut",-1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+
    /* rcData are encoded together as a group: */
    {"rcData",      0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
    {"rcData",      1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
@ -309,6 +337,8 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
    {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},

    {"hwHealthStatus",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
+    {"powerSupplyImpedance",  -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
+    {"sagCompensatedVBat",    -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 };

 typedef enum BlackboxState {
@ -337,6 +367,18 @@ typedef struct blackboxMainState_s {
    int32_t axisPID_D[XYZ_AXIS_COUNT];
    int32_t axisPID_Setpoint[XYZ_AXIS_COUNT];

+    int32_t mcPosAxisP[XYZ_AXIS_COUNT];
+    int32_t mcVelAxisPID[3][XYZ_AXIS_COUNT];
+    int32_t mcVelAxisOutput[XYZ_AXIS_COUNT];
+
+    int32_t mcSurfacePID[3];
+    int32_t mcSurfacePIDOutput;
+
+    int32_t fwAltPID[3];
+    int32_t fwAltPIDOutput;
+    int32_t fwPosPID[3];
+    int32_t fwPosPIDOutput;
+
    int16_t rcData[4];
    int16_t rcCommand[4];
    int16_t gyroADC[XYZ_AXIS_COUNT];
@ -392,6 +434,8 @@ typedef struct blackboxSlowState_s {
    bool rxSignalReceived;
    bool rxFlightChannelsValid;
    int32_t hwHealthStatus;
+    uint16_t powerSupplyImpedance;
+    uint16_t sagCompensatedVBat;
 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()

 //From rc_controls.c
@ -506,7 +550,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
        return feature(FEATURE_VBAT);

    case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
-        return feature(FEATURE_CURRENT_METER) && batteryConfig()->current.type == CURRENT_SENSOR_ADC;
+        return feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC;

    case FLIGHT_LOG_FIELD_CONDITION_SURFACE:
 #ifdef USE_RANGEFINDER
@ -515,6 +559,20 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
        return false;
 #endif

+    case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
+#ifdef USE_NAV
+        return STATE(FIXED_WING);
+#else
+        return false;
+#endif
+
+    case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
+#ifdef USE_NAV
+        return !STATE(FIXED_WING);
+#else
+        return false;
+#endif
+
    case FLIGHT_LOG_FIELD_CONDITION_RSSI:
        return rxConfig()->rssi_channel > 0 || feature(FEATURE_RSSI_ADC);

@ -601,6 +659,27 @@ static void writeIntraframe(void)
        }
    }

+    if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
+        blackboxWriteSignedVBArray(blackboxCurrent->fwAltPID, 3);
+        blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput);
+        blackboxWriteSignedVBArray(blackboxCurrent->fwPosPID, 3);
+        blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput);
+    }
+
+    if (testBlackboxCondition(CONDITION(MC_NAV))) {
+
+        blackboxWriteSignedVBArray(blackboxCurrent->mcPosAxisP, XYZ_AXIS_COUNT);
+
+        for (int i = 0; i < 3; i++) {
+            blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisPID[i], XYZ_AXIS_COUNT);
+        }
+
+        blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisOutput, XYZ_AXIS_COUNT);
+
+        blackboxWriteSignedVBArray(blackboxCurrent->mcSurfacePID, 3);
+        blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput);
+    }
+
    // Write raw stick positions
    blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4);

@ -773,6 +852,38 @@ static void writeInterframe(void)
        }
    }

+    if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
+
+        arraySubInt32(deltas, blackboxCurrent->fwAltPID, blackboxLast->fwAltPID, 3);
+        blackboxWriteSignedVBArray(deltas, 3);
+
+        blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput);
+
+        arraySubInt32(deltas, blackboxCurrent->fwPosPID, blackboxLast->fwPosPID, 3);
+        blackboxWriteSignedVBArray(deltas, 3);
+
+        blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput);
+
+    }
+
+    if (testBlackboxCondition(CONDITION(MC_NAV))) {
+        arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT);
+        blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
+
+        for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+            arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT);
+            blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
+        }
+
+        arraySubInt32(deltas, blackboxCurrent->mcVelAxisOutput, blackboxLast->mcVelAxisOutput, XYZ_AXIS_COUNT);
+        blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
+
+        arraySubInt32(deltas, blackboxCurrent->mcSurfacePID, blackboxLast->mcSurfacePID, 3);
+        blackboxWriteSignedVBArray(deltas, 3);
+
+        blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput);
+    }
+
    /*
     * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
     * can pack multiple values per byte:
@ -907,6 +1018,9 @@ static void writeSlowFrame(void)

    blackboxWriteUnsignedVB(slowHistory.hwHealthStatus);

+    blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance);
+    blackboxWriteUnsignedVB(slowHistory.sagCompensatedVBat);
+
    blackboxSlowFrameIterationTimer = 0;
 }

@ -927,6 +1041,8 @@ static void loadSlowState(blackboxSlowState_t *slow)
                           (getHwGPSStatus()            << 2 * 4) |
                           (getHwRangefinderStatus()    << 2 * 5) |
                           (getHwPitotmeterStatus()     << 2 * 6);
+    slow->powerSupplyImpedance = getPowerSupplyImpedance();
+    slow->sagCompensatedVBat = getBatterySagCompensatedVoltage();
 }

 /**
@ -1102,7 +1218,7 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
    blackboxWriteUnsignedVB(gpsSol.numSat);
    blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[0]);
    blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[1]);
-    blackboxWriteUnsignedVB(gpsSol.llh.alt / 100); // meters
+    blackboxWriteSignedVB(gpsSol.llh.alt / 100); // meters
    blackboxWriteUnsignedVB(gpsSol.groundSpeed);
    blackboxWriteUnsignedVB(gpsSol.groundCourse);
    blackboxWriteUnsignedVB(gpsSol.hdop);
@ -1124,6 +1240,10 @@ static void loadMainState(timeUs_t currentTimeUs)

    blackboxCurrent->time = currentTimeUs;

+#ifdef USE_NAV
+    const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+#endif
+
    for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
        blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i];
        blackboxCurrent->axisPID_P[i] = axisPID_P[i];
@ -1134,7 +1254,34 @@ static void loadMainState(timeUs_t currentTimeUs)
 #ifdef USE_MAG
        blackboxCurrent->magADC[i] = mag.magADC[i];
 #endif
+#ifdef USE_NAV
+        if (!STATE(FIXED_WING)) {
+            blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained / 10);
+            blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional / 10);
+            blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral / 10);
+            blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative / 10);
        }
+#endif
+    }
+
+#ifdef USE_NAV
+    if (STATE(FIXED_WING)) {
+        blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional / 10);
+        blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral / 10);
+        blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative / 10);
+        blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_alt.output_constrained / 10);
+
+        blackboxCurrent->fwPosPID[0] = lrintf(nav_pids->fw_nav.proportional / 10);
+        blackboxCurrent->fwPosPID[1] = lrintf(nav_pids->fw_nav.integral / 10);
+        blackboxCurrent->fwPosPID[2] = lrintf(nav_pids->fw_nav.derivative / 10);
+        blackboxCurrent->fwPosPIDOutput = lrintf(nav_pids->fw_nav.output_constrained / 10);
+    } else {
+        blackboxCurrent->mcSurfacePID[0] = lrintf(nav_pids->surface.proportional / 10);
+        blackboxCurrent->mcSurfacePID[1] = lrintf(nav_pids->surface.integral / 10);
+        blackboxCurrent->mcSurfacePID[2] = lrintf(nav_pids->surface.derivative / 10);
+        blackboxCurrent->mcSurfacePIDOutput = lrintf(nav_pids->surface.output_constrained / 10);
+    }
+#endif

    for (int i = 0; i < 4; i++) {
        blackboxCurrent->rcData[i] = rcData[i];
@ -1355,21 +1502,21 @@ static bool blackboxWriteSysinfo(void)

        BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
            if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
-                blackboxPrintfHeaderLine("vbat_scale", "%u", batteryConfig()->voltage.scale / 10);
+                blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage_scale / 10);
            } else {
                xmitState.headerIndex += 2; // Skip the next two vbat fields too
            }
            );
-        BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u",           batteryConfig()->voltage.cellMin / 10,
-                                                                            batteryConfig()->voltage.cellWarning / 10,
-                                                                            batteryConfig()->voltage.cellMax / 10);
+        BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u",           currentBatteryProfile->voltage.cellMin / 10,
+                                                                            currentBatteryProfile->voltage.cellWarning / 10,
+                                                                            currentBatteryProfile->voltage.cellMax / 10);
        BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u",                         vbatReference);

        BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
            //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
            if (feature(FEATURE_CURRENT_METER)) {
-                blackboxPrintfHeaderLine("currentMeter", "%d,%d",           batteryConfig()->current.offset,
-                                                                            batteryConfig()->current.scale);
+                blackboxPrintfHeaderLine("currentMeter", "%d,%d",           batteryMetersConfig()->current.offset,
+                                                                            batteryMetersConfig()->current.scale);
            }
            );

--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@ -40,6 +40,8 @@ typedef enum FlightLogFieldCondition {
    FLIGHT_LOG_FIELD_CONDITION_VBAT,
    FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC,
    FLIGHT_LOG_FIELD_CONDITION_SURFACE,
+    FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV,
+    FLIGHT_LOG_FIELD_CONDITION_MC_NAV,
    FLIGHT_LOG_FIELD_CONDITION_RSSI,

    FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@ -16,6 +16,8 @@
 */

 #include <stddef.h>
+#include <stdint.h>
+#include <stdbool.h>

 #define DEBUG16_VALUE_COUNT 4
 extern int16_t debug[DEBUG16_VALUE_COUNT];
@ -54,12 +56,17 @@ typedef enum {
    DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
    DEBUG_AGL,
    DEBUG_FLOW_RAW,
+    DEBUG_FLOW,
    DEBUG_SBUS,
    DEBUG_FPORT,
    DEBUG_ALWAYS,
    DEBUG_STAGE2,
    DEBUG_WIND_ESTIMATOR,
    DEBUG_SAG_COMP_VOLTAGE,
+    DEBUG_VIBE,
+    DEBUG_CRUISE,
+    DEBUG_REM_FLIGHT_TIME,
+    DEBUG_SMARTAUDIO,
    DEBUG_COUNT
 } debugType_e;

--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@ -430,7 +430,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t
            const OSD_SETTING_t *ptr = p->data;
            const setting_t *var = &settingsTable[ptr->val];
            int32_t value;
-            const void *valuePointer = setting_get_value_pointer(var);
+            const void *valuePointer = settingGetValuePointer(var);
            switch (SETTING_TYPE(var)) {
                case VAR_UINT8:
                    value = *(uint8_t *)valuePointer;
@ -453,6 +453,9 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t
                    // a data type yet.
                    ftoa(*(float *)valuePointer, buff);
                    break;
+                case VAR_STRING:
+                    strncpy(buff, valuePointer, sizeof(buff));
+                    break;
            }
            if (buff[0] == '\0') {
                const char *suffix = NULL;
@ -1006,13 +1009,13 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
            if (p->data) {
                const OSD_SETTING_t *ptr = p->data;
                const setting_t *var = &settingsTable[ptr->val];
-                setting_min_t min = setting_get_min(var);
-                setting_max_t max = setting_get_max(var);
+                setting_min_t min = settingGetMin(var);
+                setting_max_t max = settingGetMax(var);
                float step = ptr->step ?: 1;
                if (key != KEY_RIGHT) {
                    step = -step;
                }
-                const void *valuePointer = setting_get_value_pointer(var);
+                const void *valuePointer = settingGetValuePointer(var);
                switch (SETTING_TYPE(var)) {
                    case VAR_UINT8:
                        {
@ -1057,6 +1060,8 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
                            break;
                        }
                        break;
+                    case VAR_STRING:
+                        break;
                }
                SET_PRINTVALUE(p, currentCtx.cursorRow);
                if (p->func) {
--- a/src/main/cms/cms_menu_battery.c
+++ b/src/main/cms/cms_menu_battery.c
@ -0,0 +1,152 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_CMS
+
+#include "common/utils.h"
+
+#include "config/feature.h"
+
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+#include "cms/cms_menu_misc.h"
+
+#include "fc/config.h"
+#include "fc/rc_controls.h"
+#include "fc/settings.h"
+
+#include "sensors/battery.h"
+
+// Battery menu
+
+static uint8_t battDispProfileIndex;
+static uint8_t battProfileIndex;
+static bool featureProfAutoswitchEnabled;
+static char battProfileIndexString[] = " p";
+
+
+static long cmsx_menuBattery_onEnter(const OSD_Entry *from)
+{
+    UNUSED(from);
+
+    battProfileIndex = getConfigBatteryProfile();
+    battDispProfileIndex = battProfileIndex + 1;
+    battProfileIndexString[1] = '0' + battDispProfileIndex;
+    featureProfAutoswitchEnabled = feature(FEATURE_BAT_PROFILE_AUTOSWITCH);
+
+    return 0;
+}
+
+static long cmsx_menuBattery_onExit(const OSD_Entry *self)
+{
+    UNUSED(self);
+
+    setConfigBatteryProfile(battProfileIndex);
+    activateBatteryProfile();
+
+    if (featureProfAutoswitchEnabled) {
+        featureSet(FEATURE_BAT_PROFILE_AUTOSWITCH);
+    } else {
+        featureClear(FEATURE_BAT_PROFILE_AUTOSWITCH);
+    }
+
+    return 0;
+}
+
+static long cmsx_onBatteryProfileIndexChange(displayPort_t *displayPort, const void *ptr)
+{
+    UNUSED(displayPort);
+    UNUSED(ptr);
+
+    battProfileIndex = battDispProfileIndex - 1;
+    battProfileIndexString[1] = '0' + battDispProfileIndex;
+    batteryDisableProfileAutoswitch();
+
+    return 0;
+}
+
+static long cmsx_menuBattSettings_onEnter(const OSD_Entry *from)
+{
+    UNUSED(from);
+
+    setConfigBatteryProfile(battProfileIndex);
+
+    return 0;
+}
+
+static const OSD_Entry menuBattSettingsEntries[]=
+{
+    OSD_LABEL_DATA_ENTRY("-- BATT SETTINGS --", battProfileIndexString),
+
+#ifdef USE_ADC
+    OSD_SETTING_ENTRY("CELLS", SETTING_BAT_CELLS),
+    OSD_SETTING_ENTRY("CELL DET.", SETTING_VBAT_CELL_DETECT_VOLTAGE),
+    OSD_SETTING_ENTRY("CELL MAX", SETTING_VBAT_MAX_CELL_VOLTAGE),
+    OSD_SETTING_ENTRY("CELL WARN", SETTING_VBAT_WARNING_CELL_VOLTAGE),
+    OSD_SETTING_ENTRY("CELL MIN", SETTING_VBAT_MIN_CELL_VOLTAGE),
+#endif /* USE_ADC */
+    OSD_SETTING_ENTRY("CAP UNIT", SETTING_BATTERY_CAPACITY_UNIT),
+    OSD_SETTING_ENTRY("CAPACITY", SETTING_BATTERY_CAPACITY),
+    OSD_SETTING_ENTRY("CAP WARN", SETTING_BATTERY_CAPACITY_WARNING),
+    OSD_SETTING_ENTRY("CAP CRIT", SETTING_BATTERY_CAPACITY_CRITICAL),
+
+    OSD_BACK_ENTRY,
+    OSD_END_ENTRY
+};
+
+static CMS_Menu cmsx_menuBattSettings = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "XBATT",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = cmsx_menuBattSettings_onEnter,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = menuBattSettingsEntries
+};
+
+static OSD_Entry menuBatteryEntries[]=
+{
+    OSD_LABEL_ENTRY("-- BATTERY --"),
+
+#ifdef USE_ADC
+    OSD_BOOL_ENTRY("PROF AUTOSWITCH", &featureProfAutoswitchEnabled),
+#endif
+    OSD_UINT8_CALLBACK_ENTRY("PROF", cmsx_onBatteryProfileIndexChange, (&(const OSD_UINT8_t){ &battDispProfileIndex, 1, MAX_BATTERY_PROFILE_COUNT, 1})),
+    OSD_SUBMENU_ENTRY("SETTINGS", &cmsx_menuBattSettings),
+
+    OSD_BACK_ENTRY,
+    OSD_END_ENTRY
+};
+
+CMS_Menu cmsx_menuBattery = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "XBATT",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = cmsx_menuBattery_onEnter,
+    .onExit = cmsx_menuBattery_onExit,
+    .onGlobalExit = NULL,
+    .entries = menuBatteryEntries
+};
+
+#endif // CMS
--- a/src/main/cms/cms_menu_battery.h
+++ b/src/main/cms/cms_menu_battery.h
@ -0,0 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+extern CMS_Menu cmsx_menuBattery;
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/src/main/cms/cms_menu_blackbox.c
@ -39,6 +39,7 @@
 #include "drivers/time.h"

 #include "fc/config.h"
+#include "fc/settings.h"

 #include "io/flashfs.h"

--- a/src/main/cms/cms_menu_builtin.c
+++ b/src/main/cms/cms_menu_builtin.c
@ -46,6 +46,7 @@
 #include "cms/cms_menu_vtx.h"
 #include "cms/cms_menu_osd.h"
 #include "cms/cms_menu_ledstrip.h"
+#include "cms/cms_menu_battery.h"
 #include "cms/cms_menu_misc.h"

 // VTX supplied menus
@ -111,11 +112,11 @@ static const OSD_Entry menuFeaturesEntries[] =
 #if defined(VTX) || defined(USE_RTC6705)
    OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtx),
 #endif // VTX || USE_RTC6705
-#if defined(VTX_CONTROL)
-#if defined(VTX_SMARTAUDIO)
+#if defined(USE_VTX_CONTROL)
+#if defined(USE_VTX_SMARTAUDIO)
    OSD_SUBMENU_ENTRY("VTX SA", &cmsx_menuVtxSmartAudio),
 #endif
-#if defined(VTX_TRAMP)
+#if defined(USE_VTX_TRAMP)
    OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp),
 #endif
 #endif // VTX_CONTROL
@ -150,6 +151,7 @@ static const OSD_Entry menuMainEntries[] =
    OSD_SUBMENU_ENTRY("OSD LAYOUTS", &cmsx_menuOsdLayout),
    OSD_SUBMENU_ENTRY("ALARMS", &cmsx_menuAlarms),
 #endif
+    OSD_SUBMENU_ENTRY("BATTERY", &cmsx_menuBattery),
    OSD_SUBMENU_ENTRY("FC&FW INFO", &menuInfo),
    OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMisc),

--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@ -39,6 +39,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
+#include "fc/settings.h"

 #include "navigation/navigation.h"

@ -77,15 +78,6 @@ static long cmsx_menuImu_onEnter(const OSD_Entry *from)
    return 0;
 }

-static long cmsx_menuImu_onExit(const OSD_Entry *self)
-{
-    UNUSED(self);
-
-    setConfigProfile(profileIndex);
-
-    return 0;
-}
-
 static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr)
 {
    UNUSED(displayPort);
@ -93,6 +85,7 @@ static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *pt

    profileIndex = tmpProfileIndex - 1;
    profileIndexString[1] = '0' + tmpProfileIndex;
+    setConfigProfile(profileIndex);

    return 0;
 }
@ -483,7 +476,7 @@ const CMS_Menu cmsx_menuImu = {
    .GUARD_type = OME_MENU,
 #endif
    .onEnter = cmsx_menuImu_onEnter,
-    .onExit = cmsx_menuImu_onExit,
+    .onExit = NULL,
    .onGlobalExit = NULL,
    .entries = cmsx_menuImuEntries,
 };
--- a/src/main/cms/cms_menu_misc.c
+++ b/src/main/cms/cms_menu_misc.c
@ -32,6 +32,7 @@

 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/settings.h"

 #include "rx/rx.h"

@ -86,19 +87,10 @@ static const OSD_Entry menuMiscEntries[]=
    OSD_LABEL_ENTRY("-- MISC --"),

    OSD_SETTING_ENTRY("MIN THR", SETTING_MIN_THROTTLE),
-#ifdef USE_ADC
-    OSD_SETTING_ENTRY("VBATCELL MAX", SETTING_VBAT_MAX_CELL_VOLTAGE),
-    OSD_SETTING_ENTRY("VBATCELL WARN", SETTING_VBAT_WARNING_CELL_VOLTAGE),
-    OSD_SETTING_ENTRY("VBATCELL MIN", SETTING_VBAT_MIN_CELL_VOLTAGE),
-    OSD_SETTING_ENTRY("BAT CAP UNIT", SETTING_BATTERY_CAPACITY_UNIT),
-    OSD_SETTING_ENTRY("BAT CAPACITY", SETTING_BATTERY_CAPACITY),
-    OSD_SETTING_ENTRY("BAT CAP WARN", SETTING_BATTERY_CAPACITY_WARNING),
-    OSD_SETTING_ENTRY("BAT CAP CRIT", SETTING_BATTERY_CAPACITY_CRITICAL),
-#ifdef USE_OSD
+#if defined(USE_OSD) && defined(USE_ADC)
    OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
    OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
-#endif /* USE_OSD */
-#endif /* USE_ADC */
+#endif

    OSD_SUBMENU_ENTRY("RC PREV", &cmsx_menuRcPreview),

--- a/src/main/cms/cms_menu_navigation.c
+++ b/src/main/cms/cms_menu_navigation.c
@ -35,6 +35,7 @@
 #include "cms/cms_types.h"

 #include "fc/runtime_config.h"
+#include "fc/settings.h"

 #include "navigation/navigation.h"

--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@ -150,19 +150,23 @@ static const OSD_Entry menuOsdElemsEntries[] =
    OSD_ELEMENT_ENTRY("MAIN BATTERY", OSD_MAIN_BATT_VOLTAGE),
    OSD_ELEMENT_ENTRY("MAIN BATT SC", OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE),
    OSD_ELEMENT_ENTRY("CELL VOLTAGE", OSD_MAIN_BATT_CELL_VOLTAGE),
+    OSD_ELEMENT_ENTRY("CELL VOLT. SC", OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE),
+    OSD_ELEMENT_ENTRY("BAT IMPEDANCE", OSD_POWER_SUPPLY_IMPEDANCE),
    OSD_ELEMENT_ENTRY("CROSSHAIRS", OSD_CROSSHAIRS),
    OSD_ELEMENT_ENTRY("HORIZON", OSD_ARTIFICIAL_HORIZON),
    OSD_ELEMENT_ENTRY("HORIZON SIDEBARS", OSD_HORIZON_SIDEBARS),
    OSD_ELEMENT_ENTRY("ON TIME", OSD_ONTIME),
    OSD_ELEMENT_ENTRY("FLY TIME", OSD_FLYTIME),
    OSD_ELEMENT_ENTRY("ON/FLY TIME", OSD_ONTIME_FLYTIME),
+    OSD_ELEMENT_ENTRY("REM. TIME", OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH),
+    OSD_ELEMENT_ENTRY("REM. DIST", OSD_REMAINING_DISTANCE_BEFORE_RTH),
    OSD_ELEMENT_ENTRY("TIME (HOUR)", OSD_RTC_TIME),
    OSD_ELEMENT_ENTRY("FLY MODE", OSD_FLYMODE),
    OSD_ELEMENT_ENTRY("NAME", OSD_CRAFT_NAME),
    OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS),
    OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR),
    OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES),
-#ifdef VTX_COMMON
+#ifdef USE_VTX_COMMON
    OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL),
 #endif // VTX
    OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW),
@ -175,6 +179,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
    OSD_ELEMENT_ENTRY("BATT % REM", OSD_BATTERY_REMAINING_PERCENT),
 #ifdef USE_GPS
    OSD_ELEMENT_ENTRY("HOME DIR", OSD_HOME_DIR),
+    OSD_ELEMENT_ENTRY("HOME HEAD. ERR", OSD_HOME_HEADING_ERROR),
    OSD_ELEMENT_ENTRY("HOME DIST", OSD_HOME_DIST),
    OSD_ELEMENT_ENTRY("TRIP DIST", OSD_TRIP_DIST),
    OSD_ELEMENT_ENTRY("GPS SPEED", OSD_GPS_SPEED),
@ -185,6 +190,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
 #endif // GPS
    OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
    OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),
+    OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR),
+    OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT),
 #if defined(USE_BARO) || defined(USE_GPS)
    OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO),
    OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM),
@ -198,9 +205,38 @@ static const OSD_Entry menuOsdElemsEntries[] =
    OSD_ELEMENT_ENTRY("MAP TAKE OFF", OSD_MAP_TAKEOFF),
    OSD_ELEMENT_ENTRY("RADAR", OSD_RADAR),
 #endif
+    OSD_ELEMENT_ENTRY("EXPO", OSD_RC_EXPO),
+    OSD_ELEMENT_ENTRY("YAW EXPO", OSD_RC_YAW_EXPO),
+    OSD_ELEMENT_ENTRY("THR EXPO", OSD_THROTTLE_EXPO),
+    OSD_ELEMENT_ENTRY("ROLL RATE", OSD_ROLL_RATE),
+    OSD_ELEMENT_ENTRY("PITCH RATE", OSD_PITCH_RATE),
+    OSD_ELEMENT_ENTRY("YAW RATE", OSD_YAW_RATE),
+    OSD_ELEMENT_ENTRY("M EXPO", OSD_MANUAL_RC_EXPO),
+    OSD_ELEMENT_ENTRY("M YAW EXPO", OSD_MANUAL_RC_YAW_EXPO),
+    OSD_ELEMENT_ENTRY("M ROLL RATE", OSD_MANUAL_ROLL_RATE),
+    OSD_ELEMENT_ENTRY("M PITCH RATE", OSD_MANUAL_PITCH_RATE),
+    OSD_ELEMENT_ENTRY("M YAW RATE", OSD_MANUAL_YAW_RATE),
+    OSD_ELEMENT_ENTRY("CRUISE THR", OSD_NAV_FW_CRUISE_THR),
+    OSD_ELEMENT_ENTRY("PITCH TO THR", OSD_NAV_FW_PITCH2THR),
    OSD_ELEMENT_ENTRY("ROLL PIDS", OSD_ROLL_PIDS),
    OSD_ELEMENT_ENTRY("PITCH PIDS", OSD_PITCH_PIDS),
    OSD_ELEMENT_ENTRY("YAW PIDS", OSD_YAW_PIDS),
+    OSD_ELEMENT_ENTRY("LEVEL PIDS", OSD_LEVEL_PIDS),
+    OSD_ELEMENT_ENTRY("POSXY PIDS", OSD_POS_XY_PIDS),
+    OSD_ELEMENT_ENTRY("POSZ PIDS", OSD_POS_Z_PIDS),
+    OSD_ELEMENT_ENTRY("VELXY PIDS", OSD_VEL_XY_PIDS),
+    OSD_ELEMENT_ENTRY("VELZ PIDS", OSD_VEL_Z_PIDS),
+    OSD_ELEMENT_ENTRY("HEAD P", OSD_HEADING_P),
+    OSD_ELEMENT_ENTRY("ALIGN ROLL", OSD_BOARD_ALIGN_ROLL),
+    OSD_ELEMENT_ENTRY("ALIGN PITCH", OSD_BOARD_ALIGN_PITCH),
+    OSD_ELEMENT_ENTRY("0THR PITCH", OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE),
+
+    OSD_ELEMENT_ENTRY("FW ALT PID OUT", OSD_FW_ALT_PID_OUTPUTS),
+    OSD_ELEMENT_ENTRY("FW POS PID OUT", OSD_FW_POS_PID_OUTPUTS),
+    OSD_ELEMENT_ENTRY("MC VELX PID OUT", OSD_MC_VEL_X_PID_OUTPUTS),
+    OSD_ELEMENT_ENTRY("MC VELY PID OUT", OSD_MC_VEL_Y_PID_OUTPUTS),
+    OSD_ELEMENT_ENTRY("MC VELZ PID OUT", OSD_MC_VEL_Z_PID_OUTPUTS),
+    OSD_ELEMENT_ENTRY("MC POS PID OUT", OSD_MC_POS_XYZ_P_OUTPUTS),

    OSD_ELEMENT_ENTRY("ATTI PITCH", OSD_ATTITUDE_PITCH),
    OSD_ELEMENT_ENTRY("ATTI ROLL", OSD_ATTITUDE_ROLL),
@ -212,7 +248,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
    OSD_END_ENTRY,
 };

-#if defined(VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT)
+#if defined(USE_VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT)
 // All CMS OSD elements should be enabled in this case. The menu has 3 extra
 // elements (label, back and end), but there's an OSD element that we intentionally
 // don't show here (OSD_DEBUG).
--- a/src/main/cms/cms_menu_vtx_smartaudio.c
+++ b/src/main/cms/cms_menu_vtx_smartaudio.c
@ -1,27 +1,31 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of Cleanflight and Betaflight.
 *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
 *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
 */

 #include <stdbool.h>
 #include <stdint.h>
+#include <ctype.h>
 #include <string.h>

 #include "platform.h"

-#if defined(USE_CMS) && defined(VTX_SMARTAUDIO)
+#if defined(USE_CMS) && defined(USE_VTX_SMARTAUDIO)

 #include "common/printf.h"
 #include "common/utils.h"
@ -36,6 +40,7 @@

 #include "io/vtx_string.h"
 #include "io/vtx_smartaudio.h"
+#include "io/vtx.h"

 // Interface to CMS

@ -65,8 +70,10 @@ uint16_t saCmsDeviceFreq = 0;

 uint8_t  saCmsDeviceStatus = 0;
 uint8_t  saCmsPower;
-uint8_t  saCmsPitFMode;         // In-Range or Out-Range
+uint8_t  saCmsPitFMode;          // Undef(0), In-Range(1) or Out-Range(2)
+
 uint8_t  saCmsFselMode;          // Channel(0) or User defined(1)
+uint8_t  saCmsFselModeNew;       // Channel(0) or User defined(1)

 uint16_t saCmsORFreq = 0;       // POR frequency
 uint16_t saCmsORFreqNew;        // POR frequency
@ -74,44 +81,33 @@ uint16_t saCmsORFreqNew;        // POR frequency
 uint16_t saCmsUserFreq = 0;     // User defined frequency
 uint16_t saCmsUserFreqNew;      // User defined frequency

-void saCmsUpdate(void)
-{
-// XXX Take care of pit mode update somewhere???
-
-    if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) {
-        // This is a first valid response to GET_SETTINGS.
-        saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE;
-
-        saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0;
-
-        saCmsBand = (saDevice.channel / 8) + 1;
-        saCmsChan = (saDevice.channel % 8) + 1;
-        saCmsFreqRef = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8];
-
-        saCmsDeviceFreq = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8];
-
-        if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) {
-            saCmsRFState = SACMS_TXMODE_ACTIVE;
-        } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
-            saCmsRFState = SACMS_TXMODE_PIT_INRANGE;
-        } else {
-            saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE;
-        }
-
-        if (saDevice.version == 2) {
-            saCmsPower = saDevice.power + 1; // XXX Take care V1
-        } else {
-            saCmsPower = saDacToPowerIndex(saDevice.power) + 1;
-        }
-    }
-}
-
 static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self);
 static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self);
 static long saCmsConfigBandByGvar(displayPort_t *, const void *self);
 static long saCmsConfigChanByGvar(displayPort_t *, const void *self);
 static long saCmsConfigPowerByGvar(displayPort_t *, const void *self);

+static bool saCmsUpdateCopiedState(void)
+{
+    if (saDevice.version == 0)
+        return false;
+
+    if (saCmsDeviceStatus == 0 && saDevice.version != 0)
+        saCmsDeviceStatus = saDevice.version;
+    if (saCmsORFreq == 0 && saDevice.orfreq != 0)
+        saCmsORFreq = saDevice.orfreq;
+    if (saCmsUserFreq == 0 && saDevice.freq != 0)
+        saCmsUserFreq = saDevice.freq;
+
+    if (saDevice.version == 2) {
+        if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
+            saCmsPitFMode = 1;
+        else
+            saCmsPitFMode = 0;
+    }
+    return true;
+}
+
 static bool saCmsDrawStatusString(char *buf, unsigned bufsize)
 {
    const char *defaultString = "- -- ---- ---";
@ -124,21 +120,10 @@ static bool saCmsDrawStatusString(char *buf, unsigned bufsize)

    strcpy(buf, defaultString);

-    if (saDevice.version == 0)
+    if (!saCmsUpdateCopiedState()) {
+        // VTX is not ready
        return true;
-
-    // XXX These should be done somewhere else
-    if (saCmsDeviceStatus == 0 && saDevice.version != 0)
-        saCmsDeviceStatus = saDevice.version;
-    if (saCmsORFreq == 0 && saDevice.orfreq != 0)
-        saCmsORFreq = saDevice.orfreq;
-    if (saCmsUserFreq == 0 && saDevice.freq != 0)
-        saCmsUserFreq = saDevice.freq;
-
-    if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
-        saCmsPitFMode = 1;
-    else
-        saCmsPitFMode = 0;
+    }

    buf[0] = "-FR"[saCmsOpmodel];

@ -176,6 +161,47 @@ static bool saCmsDrawStatusString(char *buf, unsigned bufsize)
    return true;
 }

+void saCmsUpdate(void)
+{
+// XXX Take care of pit mode update somewhere???
+    if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) {
+        // This is a first valid response to GET_SETTINGS.
+        saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE;
+
+        saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0;
+
+        saCmsBand = vtxSettingsConfig()->band;
+        saCmsChan = vtxSettingsConfig()->channel;
+        saCmsFreqRef = vtxSettingsConfig()->freq;
+        saCmsDeviceFreq = saCmsFreqRef;
+
+        if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) {
+            saCmsRFState = SACMS_TXMODE_ACTIVE;
+        } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
+            saCmsRFState = SACMS_TXMODE_PIT_INRANGE;
+        } else {
+            saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE;
+        }
+
+        saCmsPower = vtxSettingsConfig()->power;
+
+        // if user-freq mode then track possible change
+        if (saCmsFselMode && vtxSettingsConfig()->freq) {
+            saCmsUserFreq = vtxSettingsConfig()->freq;
+        }
+
+        saCmsFselModeNew = saCmsFselMode;   //init mode for menu
+    }
+
+    saCmsUpdateCopiedState();
+}
+
+void saCmsResetOpmodel()
+{
+    // trigger data refresh in 'saCmsUpdate()'
+    saCmsOpmodel = SACMS_OPMODEL_UNDEF;
+}
+
 static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self)
 {
    UNUSED(pDisp);
@ -243,8 +269,9 @@ static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
        return 0;
    }

-    if (saCmsOpmodel == SACMS_OPMODEL_FREE)
-        saSetPowerByIndex(saCmsPower - 1);
+    if (saCmsOpmodel == SACMS_OPMODEL_FREE && !saDeferred) {
+        vtxSettingsConfigMutable()->power = saCmsPower;
+    }

    return 0;
 }
@ -254,9 +281,21 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
    UNUSED(pDisp);
    UNUSED(self);

+    if (saDevice.version == 1) {
+        // V1 device doesn't support PIT mode; bounce back.
+        saCmsPitFMode = 0;
+        return 0;
+    }
+
    dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode));

    if (saCmsPitFMode == 0) {
+        // Bounce back
+        saCmsPitFMode = 1;
+        return 0;
+    }
+
+    if (saCmsPitFMode == 1) {
        saSetMode(SA_MODE_SET_IN_RANGE_PITMODE);
    } else {
        saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE);
@ -272,6 +311,12 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
    UNUSED(pDisp);
    UNUSED(self);

+    if (saDevice.version == 1) {
+        if (saCmsOpmodel != SACMS_OPMODEL_FREE)
+            saCmsOpmodel = SACMS_OPMODEL_FREE;
+        return 0;
+    }
+
    uint8_t opmodel = saCmsOpmodel;

    dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel));
@ -288,7 +333,7 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
        saCmsConfigPitFModeByGvar(pDisp, self);

        // Direct frequency mode is not available in RACE opmodel
-        saCmsFselMode = 0;
+        saCmsFselModeNew = 0;
        saCmsConfigFreqModeByGvar(pDisp, self);
    } else {
        // Trying to go back to unknown state; bounce back
@ -298,6 +343,7 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
    return 0;
 }

+#ifdef USE_EXTENDED_CMS_MENUS
 static const char * const saCmsDeviceStatusNames[] = {
    "OFFL",
    "ONL V1",
@ -332,20 +378,13 @@ static const CMS_Menu saCmsMenuStats = {
    .onGlobalExit = NULL,
    .entries = saCmsMenuStatsEntries
 };
+#endif /* USE_EXTENDED_CMS_MENUS */

-static const OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames };
+static const OSD_TAB_t saCmsEntBand = { &saCmsBand, VTX_SMARTAUDIO_BAND_COUNT, vtx58BandNames };

-static const OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChannelNames };
+static const OSD_TAB_t saCmsEntChan = { &saCmsChan, VTX_SMARTAUDIO_CHANNEL_COUNT, vtx58ChannelNames };

-static const char * const saCmsPowerNames[] = {
-    "---",
-    "25 ",
-    "200",
-    "500",
-    "800",
-};
-
-static const OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames};
+static const OSD_TAB_t saCmsEntPower = { &saCmsPower, VTX_SMARTAUDIO_POWER_COUNT, saPowerNames};

 static const char * const saCmsOpmodelNames[] = {
    "----",
@ -359,6 +398,7 @@ static const char * const saCmsFselModeNames[] = {
 };

 static const char * const saCmsPitFModeNames[] = {
+    "---",
    "PIR",
    "POR"
 };
@ -372,19 +412,15 @@ static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
    UNUSED(pDisp);
    UNUSED(self);

-    if (saCmsFselMode == 0) {
-        // CHAN
-        saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
-    } else {
-        // USER: User frequency mode is only available in FREE opmodel.
-        if (saCmsOpmodel == SACMS_OPMODEL_FREE) {
-            saSetFreq(saCmsUserFreq);
-        } else {
-            // Bounce back
-            saCmsFselMode = 0;
-        }
+    // if trying to do user frequency mode in RACE opmodel then
+    // revert because user-freq only available in FREE opmodel
+    if (saCmsFselModeNew != 0 && saCmsOpmodel != SACMS_OPMODEL_FREE) {
+        saCmsFselModeNew = 0;
    }

+    // don't call 'saSetBandAndChannel()' / 'saSetFreq()' here,
+    // wait until SET / 'saCmsCommence()' is activated
+
    sacms_SetupTopMenu(NULL);

    return 0;
@ -395,12 +431,22 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self)
    UNUSED(pDisp);
    UNUSED(self);

+    const vtxSettingsConfig_t prevSettings = {
+        .band = vtxSettingsConfig()->band,
+        .channel = vtxSettingsConfig()->channel,
+        .freq = vtxSettingsConfig()->freq,
+        .power = vtxSettingsConfig()->power,
+        .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
+    };
+    vtxSettingsConfig_t newSettings = prevSettings;
+
    if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
        // Race model
        // Setup band, freq and power.

-        saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
-
+        newSettings.band = saCmsBand;
+        newSettings.channel = saCmsChan;
+        newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
        // If in pit mode, cancel it.

        if (saCmsPitFMode == 0)
@ -410,13 +456,26 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self)
    } else {
        // Freestyle model
        // Setup band and freq / user freq
-        if (saCmsFselMode == 0)
-            saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
-        else
-            saSetFreq(saCmsUserFreq);
+        if (saCmsFselModeNew == 0) {
+            newSettings.band = saCmsBand;
+            newSettings.channel = saCmsChan;
+            newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
+        } else {
+            saSetMode(0);    //make sure FREE mode is setup
+            newSettings.band = 0;
+            newSettings.freq = saCmsUserFreq;
+        }
    }

-    saSetPowerByIndex(saCmsPower - 1);
+    newSettings.power = saCmsPower;
+
+    if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) {
+        vtxSettingsConfigMutable()->band = newSettings.band;
+        vtxSettingsConfigMutable()->channel = newSettings.channel;
+        vtxSettingsConfigMutable()->power = newSettings.power;
+        vtxSettingsConfigMutable()->freq = newSettings.freq;
+        saveConfigAndNotify();
+    }

    return MENU_CHAIN_BACK;
 }
@ -425,6 +484,9 @@ static long saCmsSetPORFreqOnEnter(const OSD_Entry *from)
 {
    UNUSED(from);

+    if (saDevice.version == 1)
+        return MENU_CHAIN_BACK;
+
    saCmsORFreqNew = saCmsORFreq;

    return 0;
@ -435,7 +497,7 @@ static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self)
    UNUSED(pDisp);
    UNUSED(self);

-    saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT);
+    saSetPitFreq(saCmsORFreqNew);

    return 0;
 }
@ -473,7 +535,6 @@ static long saCmsConfigUserFreq(displayPort_t *pDisp, const void *self)
    UNUSED(self);

    saCmsUserFreq = saCmsUserFreqNew;
-    //saSetFreq(saCmsUserFreq);

    return MENU_CHAIN_BACK;
 }
@ -526,7 +587,7 @@ static const CMS_Menu saCmsMenuUserFreq =
    .entries = saCmsMenuUserFreqEntries,
 };

-static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames };
+static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselModeNew, 1, saCmsFselModeNames };

 static const OSD_Entry saCmsMenuConfigEntries[] =
 {
@ -536,7 +597,9 @@ static const OSD_Entry saCmsMenuConfigEntries[] =
    { "FSEL MODE", OME_TAB,  {.func = saCmsConfigFreqModeByGvar}, &saCmsEntFselMode, DYNAMIC },
    OSD_TAB_CALLBACK_ENTRY("PIT FMODE", saCmsConfigPitFModeByGvar, &saCmsEntPitFMode),
    { "POR FREQ",  OME_Submenu, {.menufunc = saCmsORFreqGetString}, (void *)&saCmsMenuPORFreq, OPTSTRING },
+#ifdef USE_EXTENDED_CMS_MENUS
    OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats),
+#endif /* USE_EXTENDED_CMS_MENUS */

    OSD_BACK_ENTRY,
    OSD_END_ENTRY,
@ -614,7 +677,9 @@ static const OSD_Entry saCmsMenuOfflineEntries[] =
    OSD_LABEL_ENTRY("- VTX SMARTAUDIO -"),

    OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString),
+#ifdef USE_EXTENDED_CMS_MENUS
    OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats),
+#endif /* USE_EXTENDED_CMS_MENUS */

    OSD_BACK_ENTRY,
    OSD_END_ENTRY,
@ -627,7 +692,7 @@ static long sacms_SetupTopMenu(const OSD_Entry *from)
    UNUSED(from);

    if (saCmsDeviceStatus) {
-        if (saCmsFselMode == 0)
+        if (saCmsFselModeNew == 0)
            cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
        else
            cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries;
--- a/src/main/cms/cms_menu_vtx_smartaudio.h
+++ b/src/main/cms/cms_menu_vtx_smartaudio.h
@ -1,18 +1,21 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of Cleanflight and Betaflight.
 *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
 *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
 */

 #pragma once
--- a/src/main/cms/cms_menu_vtx_tramp.c
+++ b/src/main/cms/cms_menu_vtx_tramp.c
@ -1,27 +1,31 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of Cleanflight and Betaflight.
 *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
 *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
 */

+#include <ctype.h>
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>

 #include "platform.h"

-#if defined(USE_CMS) && defined(VTX_TRAMP)
+#if defined(USE_CMS) && defined(USE_VTX_TRAMP)

 #include "common/printf.h"
 #include "common/utils.h"
@ -35,6 +39,7 @@

 #include "io/vtx_string.h"
 #include "io/vtx_tramp.h"
+#include "io/vtx.h"

 static bool trampCmsDrawStatusString(char *buf, unsigned bufsize)
 {
@ -48,7 +53,8 @@ static bool trampCmsDrawStatusString(char *buf, unsigned bufsize)

    strcpy(buf, defaultString);

-    if (!trampIsAvailable()) {
+    vtxDevice_t *vtxDevice = vtxCommonDevice();
+    if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_TRAMP || !vtxCommonDeviceIsReady(vtxDevice)) {
        return true;
    }

@ -65,8 +71,7 @@ static bool trampCmsDrawStatusString(char *buf, unsigned bufsize)

    if (trampData.power) {
        tfp_sprintf(&buf[9], " %c%3d", (trampData.power == trampData.configuredPower) ? ' ' : '*', trampData.power);
-    }
-    else {
+    } else {
        tfp_sprintf(&buf[9], " ----");
    }

@ -78,13 +83,13 @@ uint8_t trampCmsBand = 1;
 uint8_t trampCmsChan = 1;
 uint16_t trampCmsFreqRef;

-static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames };
+static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, VTX_TRAMP_BAND_COUNT, vtx58BandNames };

-static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames };
+static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, VTX_TRAMP_CHANNEL_COUNT, vtx58ChannelNames };

 static uint8_t trampCmsPower = 1;

-static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames };
+static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, sizeof(trampPowerTable), trampPowerNames };

 static void trampCmsUpdateFreqRef(void)
 {
@ -164,6 +169,13 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self)
    // If it fails, the user should retry later
    trampCommitChanges();

+    // update'vtx_' settings
+    vtxSettingsConfigMutable()->band = trampCmsBand;
+    vtxSettingsConfigMutable()->channel = trampCmsChan;
+    vtxSettingsConfigMutable()->power = trampCmsPower;
+    vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(trampCmsBand, trampCmsChan);
+
+    saveConfigAndNotify();

    return MENU_CHAIN_BACK;
 }
@ -177,7 +189,7 @@ static void trampCmsInitSettings(void)
    trampCmsPitMode = trampData.pitMode + 1;

    if (trampData.configuredPower > 0) {
-        for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
+        for (uint8_t i = 0; i < VTX_TRAMP_POWER_COUNT; i++) {
            if (trampData.configuredPower <= trampPowerTable[i]) {
                trampCmsPower = i + 1;
                break;
@ -194,7 +206,8 @@ static long trampCmsOnEnter(const OSD_Entry *from)
    return 0;
 }

-static const OSD_Entry trampCmsMenuCommenceEntries[] = {
+static const OSD_Entry trampCmsMenuCommenceEntries[] =
+{
    OSD_LABEL_ENTRY("CONFIRM"),
    OSD_FUNC_CALL_ENTRY("YES", trampCmsCommence),

--- a/src/main/cms/cms_types.h
+++ b/src/main/cms/cms_types.h
@ -24,8 +24,6 @@

 #include <stdint.h>

-#include "fc/settings.h"
-
 //type of elements

 typedef enum
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@ -49,6 +49,14 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
    pt1FilterInitRC(filter, 1.0f / (2.0f * M_PIf * f_cut), dT);
 }

+void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
+    filter->RC = tau;
+}
+
+float pt1FilterGetLastOutput(pt1Filter_t *filter) {
+    return filter->state;
+}
+
 float pt1FilterApply(pt1Filter_t *filter, float input)
 {
    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@ -57,6 +57,8 @@ float nullFilterApply(void *filter, float input);

 void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
+void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
+float pt1FilterGetLastOutput(pt1Filter_t *filter);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
 float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt);
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@ -153,7 +153,9 @@ float cos_approx(float x);
 float atan2_approx(float y, float x);
 float acos_approx(float x);
 #define tan_approx(x)       (sin_approx(x) / cos_approx(x))
+#define asin_approx(x)      (M_PIf / 2 - acos_approx(x))
 #else
+#define asin_approx(x)      asinf(x)
 #define sin_approx(x)       sinf(x)
 #define cos_approx(x)       cosf(x)
 #define atan2_approx(y,x)   atan2f(y,x)
--- a/src/main/common/time.h
+++ b/src/main/common/time.h
@ -41,6 +41,13 @@ typedef uint32_t timeUs_t;
 #define MILLISECS_PER_SEC 1000
 #define USECS_PER_SEC (1000 * 1000)

+#define HZ2US(hz)   (1000000 / (hz))
+#define US2S(us)    ((us) * 1e-6f)
+#define US2MS(us)   ((us) * 1e-3f)
+#define MS2US(ms)   ((ms) * 1000)
+#define MS2S(ms)    ((ms) * 1e-3f)
+#define HZ2S(hz)    US2S(HZ2US(hz))
+
 static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }

 typedef enum {
--- a/src/main/common/vector.h
+++ b/src/main/common/vector.h
@ -41,6 +41,13 @@ typedef struct {
 void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles);
 void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a);

+static inline void vectorZero(fpVector3_t * v)
+{ 
+    v->x = 0.0f;
+    v->y = 0.0f;
+    v->z = 0.0f;
+}
+
 static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
 {
    fpVector3_t r;
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@ -27,7 +27,7 @@
 //#define PG_SENSOR_ALIGNMENT_CONFIG 8 -- NOT USED in iNav
 //#define PG_SENSOR_TRIMS 9 -- NOT USED in iNav
 #define PG_GYRO_CONFIG 10
-#define PG_BATTERY_CONFIG 11
+#define PG_BATTERY_PROFILES 11
 #define PG_CONTROL_RATE_PROFILES 12
 #define PG_SERIAL_CONFIG 13
 #define PG_PID_PROFILE 14
@ -63,6 +63,7 @@
 #define PG_SERVO_PARAMS 42
 //#define PG_RX_FAILSAFE_CHANNEL_CONFIG 43
 #define PG_RX_CHANNEL_RANGE_CONFIG 44
+#define PG_BATTERY_METERS_CONFIG 45
 //#define PG_MODE_COLOR_CONFIG 45
 //#define PG_SPECIAL_COLOR_CONFIG 46
 //#define PG_PILOT_CONFIG 47
@ -80,6 +81,9 @@
 //#define PG_DRIVER_PWM_RX_CONFIG 100
 //#define PG_DRIVER_FLASHCHIP_CONFIG 101

+// cleanflight v2 specific parameter group ids start at 256
+#define PG_VTX_SETTINGS_CONFIG 259
+
 // iNav specific parameter group ids start at 1000
 #define PG_INAV_START 1000
 #define PG_PITOTMETER_CONFIG 1000
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@ -262,7 +262,7 @@ void max7456ReInit(void)


 //here we init only CS and try to init MAX for first time
-void max7456Init(const vcdProfile_t *pVcdProfile)
+void max7456Init(const videoSystem_e videoSystem)
 {
    max7456dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD);

@ -275,7 +275,7 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
    // force soft reset on Max7456
    busWrite(max7456dev, MAX7456ADD_VM0, MAX7456_RESET);

-    videoSignalCfg = pVcdProfile->video_system;
+    videoSignalCfg = videoSystem;

    // Set screenbuffer to all blanks
    for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) {
--- a/src/main/drivers/max7456.h
+++ b/src/main/drivers/max7456.h
@ -18,6 +18,7 @@
 #pragma once

 #include <stdbool.h>
+#include "drivers/vcd.h"

 #ifndef WHITEBRIGHTNESS
  #define WHITEBRIGHTNESS 0x01
@ -43,7 +44,7 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC };
 extern uint16_t maxScreenSize;

 struct vcdProfile_s;
-void    max7456Init(const struct vcdProfile_s *vcdProfile);
+void    max7456Init(const videoSystem_e videoSystem);
 void    max7456DrawScreenPartial(void);
 void    max7456WriteNvm(uint8_t char_address, const uint8_t *font_data);
 uint8_t max7456GetRowsCount(void);
--- a/src/main/drivers/max7456_symbols.h
+++ b/src/main/drivers/max7456_symbols.h
@ -23,7 +23,6 @@

 #ifdef USE_MAX7456

-// Character Symbols
 #define SYM_BLANK 0x20

 // Satellite Graphics
@ -156,6 +155,9 @@
 #define SYM_MAIN_BATT 0x97
 #define SYM_VID_BAT   0xBF

+// Used for battery impedance
+#define SYM_MILLIOHM 0x3F
+
 // Unit Icon´s (Metric)
 #define SYM_MS          0x9F
 #define SYM_KMH         0xA1
--- a/src/main/drivers/opflow/opflow.h
+++ b/src/main/drivers/opflow/opflow.h
@ -31,7 +31,7 @@ struct opflowDev_s;

 typedef struct opflowData_s {
    timeDelta_t deltaTime;      // Integration timeframe of motionX/Y
-    int32_t     flowRateRaw[2]; // Flow rotation in raw sensor uints (per deltaTime interval)
+    int32_t     flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
    int16_t     quality;
 } opflowData_t;

--- a/src/main/drivers/opflow/opflow_fake.c
+++ b/src/main/drivers/opflow/opflow_fake.c
@ -43,6 +43,7 @@ void fakeOpflowSet(timeDelta_t deltaTime, int32_t flowRateX, int32_t flowRateY,
    fakeData.deltaTime = deltaTime;
    fakeData.flowRateRaw[0] = flowRateX;
    fakeData.flowRateRaw[1] = flowRateY;
+    fakeData.flowRateRaw[2] = 0;
    fakeData.quality = quality;
 }

--- a/src/main/drivers/serial.h
+++ b/src/main/drivers/serial.h
@ -41,7 +41,8 @@ typedef enum portOptions_t {
     * to actual data bytes.
     */
    SERIAL_BIDIR_OD      = 0 << 4,
-    SERIAL_BIDIR_PP      = 1 << 4
+    SERIAL_BIDIR_PP      = 1 << 4,
+    SERIAL_BIDIR_NOPULL  = 1 << 5, // disable pulls in BIDIR RX mode
 } portOptions_t;

 typedef void (*serialReceiveCallbackPtr)(uint16_t data, void *rxCallbackData);   // used by serial drivers to return frames to app
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@ -130,9 +130,11 @@ static void serialEnableCC(softSerial_t *softSerial)
 static void serialInputPortActivate(softSerial_t *softSerial)
 {
    if (softSerial->port.options & SERIAL_INVERTED) {
-        IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction);
+        const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_PD;
+        IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction);
    } else {
-        IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction);
+        const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_UP;
+        IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction);
    }

    softSerial->rxActive = true;
--- a/src/main/drivers/vcd.h
+++ b/src/main/drivers/vcd.h
@ -19,12 +19,8 @@

 // Video Character Display parameters

-typedef struct vcdProfile_s {
-    uint8_t video_system;
-} vcdProfile_t;
-
-enum VIDEO_SYSTEMS {
+typedef enum {
    VIDEO_SYSTEM_AUTO = 0,
    VIDEO_SYSTEM_PAL,
    VIDEO_SYSTEM_NTSC
-};
+} videoSystem_e;
--- a/src/main/drivers/vtx_common.c
+++ b/src/main/drivers/vtx_common.c
@ -25,42 +25,52 @@
 #include "platform.h"
 #include "build/debug.h"

-#if defined(VTX_COMMON)
+#if defined(USE_VTX_COMMON)

 #include "vtx_common.h"

-vtxDevice_t *vtxDevice = NULL;
+static vtxDevice_t *commonVtxDevice = NULL;

 void vtxCommonInit(void)
 {
 }

-// Whatever registered last will win
-
-void vtxCommonRegisterDevice(vtxDevice_t *pDevice)
+void vtxCommonSetDevice(vtxDevice_t *vtxDevice)
 {
-    vtxDevice = pDevice;
+    commonVtxDevice = vtxDevice;
 }

-void vtxCommonProcess(timeUs_t currentTimeUs)
+vtxDevice_t *vtxCommonDevice(void)
 {
-    if (!vtxDevice)
-        return;
-
-    if (vtxDevice->vTable->process)
-        vtxDevice->vTable->process(currentTimeUs);
+    return commonVtxDevice;
 }

-vtxDevType_e vtxCommonGetDeviceType(void)
+void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 {
-    if (!vtxDevice || !vtxDevice->vTable->getDeviceType)
+    if (vtxDevice && vtxDevice->vTable->process) {
+        vtxDevice->vTable->process(vtxDevice, currentTimeUs);
+    }
+}
+
+vtxDevType_e vtxCommonGetDeviceType(vtxDevice_t *vtxDevice)
+{
+    if (!vtxDevice || !vtxDevice->vTable->getDeviceType) {
        return VTXDEV_UNKNOWN;
+    }

-    return vtxDevice->vTable->getDeviceType();
+    return vtxDevice->vTable->getDeviceType(vtxDevice);
+}
+
+bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice)
+{
+    if (vtxDevice && vtxDevice->vTable->isReady) {
+        return vtxDevice->vTable->isReady(vtxDevice);
+    }
+    return false;
 }

 // band and channel are 1 origin
-void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel)
+void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 {
    if (!vtxDevice)
        return;
@ -68,12 +78,13 @@ void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel)
    if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount))
        return;
    
-    if (vtxDevice->vTable->setBandAndChannel)
-        vtxDevice->vTable->setBandAndChannel(band, channel);
+    if (vtxDevice->vTable->setBandAndChannel) {
+        vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);
+    }
 }

 // index is zero origin, zero = power off completely
-void vtxCommonSetPowerByIndex(uint8_t index)
+void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
    if (!vtxDevice)
        return;
@ -81,59 +92,64 @@ void vtxCommonSetPowerByIndex(uint8_t index)
    if (index > vtxDevice->capability.powerCount)
        return;
    
-    if (vtxDevice->vTable->setPowerByIndex)
-        vtxDevice->vTable->setPowerByIndex(index);
+    if (vtxDevice->vTable->setPowerByIndex) {
+        vtxDevice->vTable->setPowerByIndex(vtxDevice, index);
+    }
 }

 // on = 1, off = 0
-void vtxCommonSetPitMode(uint8_t onoff)
+void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 {
-    if (!vtxDevice)
-        return;
-
-    if (vtxDevice->vTable->setPitMode)
-        vtxDevice->vTable->setPitMode(onoff);
+    if (vtxDevice && vtxDevice->vTable->setPitMode) {
+        vtxDevice->vTable->setPitMode(vtxDevice, onoff);
+    }
 }

-bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
+void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
 {
-    if (!vtxDevice)
-        return false;
+    if (vtxDevice && vtxDevice->vTable->setFrequency) {
+        vtxDevice->vTable->setFrequency(vtxDevice, frequency);
+    }
+}

-    if (vtxDevice->vTable->getBandAndChannel)
-        return vtxDevice->vTable->getBandAndChannel(pBand, pChannel);
-    else
+bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
+{
+    if (vtxDevice && vtxDevice->vTable->getBandAndChannel) {
+        return vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel);
+    }
    return false;
 }

-bool vtxCommonGetPowerIndex(uint8_t *pIndex)
+bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex)
 {
-    if (!vtxDevice)
-        return false;
-
-    if (vtxDevice->vTable->getPowerIndex)
-        return vtxDevice->vTable->getPowerIndex(pIndex);
-    else
+    if (vtxDevice && vtxDevice->vTable->getPowerIndex) {
+        return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex);
+    }
    return false;
 }

-bool vtxCommonGetPitMode(uint8_t *pOnOff)
+bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!vtxDevice)
-        return false;
-
-    if (vtxDevice->vTable->getPitMode)
-        return vtxDevice->vTable->getPitMode(pOnOff);
-    else
+    if (vtxDevice && vtxDevice->vTable->getPitMode) {
+        return vtxDevice->vTable->getPitMode(vtxDevice, pOnOff);
+    }
    return false;
 }

-bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability)
+bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
 {
-    if (!vtxDevice)
+    if (vtxDevice && vtxDevice->vTable->getFrequency) {
+        return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency);
+    }
    return false;
+}

+bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability)
+{
+    if (vtxDevice) {
        memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t));
        return true;
    }
+    return false;
+}
 #endif
--- a/src/main/drivers/vtx_common.h
+++ b/src/main/drivers/vtx_common.h
@ -19,6 +19,53 @@

 #include "common/time.h"

+#define VTX_SETTINGS_NO_BAND        0 // used for custom frequency selection mode
+#define VTX_SETTINGS_MIN_BAND       1
+#define VTX_SETTINGS_MAX_BAND       5
+#define VTX_SETTINGS_MIN_CHANNEL    1
+#define VTX_SETTINGS_MAX_CHANNEL    8
+
+#define VTX_SETTINGS_BAND_COUNT     (VTX_SETTINGS_MAX_BAND - VTX_SETTINGS_MIN_BAND + 1)
+#define VTX_SETTINGS_CHANNEL_COUNT  (VTX_SETTINGS_MAX_CHANNEL - VTX_SETTINGS_MIN_CHANNEL + 1)
+
+#define VTX_SETTINGS_DEFAULT_BAND               4
+#define VTX_SETTINGS_DEFAULT_CHANNEL            1
+#define VTX_SETTINGS_DEFAULT_FREQ               5740
+#define VTX_SETTINGS_DEFAULT_PITMODE_FREQ       0
+#define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM   0
+
+#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0             //min freq (in MHz) for 'vtx_freq' setting
+#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999          //max freq (in MHz) for 'vtx_freq' setting
+
+#if defined(USE_VTX_RTC6705)
+
+#include "drivers/vtx_rtc6705.h"
+
+#endif
+
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
+
+#define VTX_SETTINGS_POWER_COUNT        5
+#define VTX_SETTINGS_DEFAULT_POWER      1
+#define VTX_SETTINGS_MIN_POWER          0
+#define VTX_SETTINGS_MIN_USER_FREQ      5000
+#define VTX_SETTINGS_MAX_USER_FREQ      5999
+#define VTX_SETTINGS_FREQCMD
+
+#elif defined(USE_VTX_RTC6705)
+
+#define VTX_SETTINGS_POWER_COUNT    VTX_RTC6705_POWER_COUNT
+#define VTX_SETTINGS_DEFAULT_POWER  VTX_RTC6705_DEFAULT_POWER
+#define VTX_SETTINGS_MIN_POWER      VTX_RTC6705_MIN_POWER
+
+#endif
+
+#define VTX_SETTINGS_MAX_POWER      (VTX_SETTINGS_POWER_COUNT - 1)
+
+// check value for MSP_SET_VTX_CONFIG to determine if value is encoded
+// band/channel or frequency in MHz (3 bits for band and 3 bits for channel)
+#define VTXCOMMON_MSP_BANDCHAN_CHKVAL ((uint16_t)((7 << 3) + 7))
+
 typedef enum {
    VTXDEV_UNSUPPORTED = 0, // reserved for MSP
    VTXDEV_RTC6705    = 1,
@ -58,17 +105,19 @@ typedef struct vtxDevice_s {
 // {set,get}PitMode: 0 = OFF, 1 = ON

 typedef struct vtxVTable_s {
-    void (*process)(uint32_t currentTimeUs);
-    vtxDevType_e (*getDeviceType)(void);
-    bool (*isReady)(void);
+    void (*process)(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs);
+    vtxDevType_e (*getDeviceType)(const vtxDevice_t *vtxDevice);
+    bool (*isReady)(const vtxDevice_t *vtxDevice);

-    void (*setBandAndChannel)(uint8_t band, uint8_t channel);
-    void (*setPowerByIndex)(uint8_t level);
-    void (*setPitMode)(uint8_t onoff);
+    void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
+    void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level);
+    void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff);
+    void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq);

-    bool (*getBandAndChannel)(uint8_t *pBand, uint8_t *pChannel);
-    bool (*getPowerIndex)(uint8_t *pIndex);
-    bool (*getPitMode)(uint8_t *pOnOff);
+    bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
+    bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
+    bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
+    bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
 } vtxVTable_t;

 // 3.1.0
@ -77,15 +126,19 @@ typedef struct vtxVTable_s {
 // - It is *NOT* RF on/off control ?

 void vtxCommonInit(void);
-void vtxCommonRegisterDevice(vtxDevice_t *pDevice);
+void vtxCommonSetDevice(vtxDevice_t *vtxDevice);
+vtxDevice_t *vtxCommonDevice(void);

 // VTable functions
-void vtxCommonProcess(timeUs_t currentTimeUs);
-uint8_t vtxCommonGetDeviceType(void);
-void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel);
-void vtxCommonSetPowerByIndex(uint8_t level);
-void vtxCommonSetPitMode(uint8_t onoff);
-bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel);
-bool vtxCommonGetPowerIndex(uint8_t *pIndex);
-bool vtxCommonGetPitMode(uint8_t *pOnOff);
-bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability);
+void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs);
+vtxDevType_e vtxCommonGetDeviceType(vtxDevice_t *vtxDevice);
+bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice);
+void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
+void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index);
+void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff);
+void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency);
+bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
+bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex);
+bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff);
+bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
+bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability);
--- a/src/main/drivers/vtx_rtc6705.c
+++ b/src/main/drivers/vtx_rtc6705.c
@ -27,7 +27,7 @@

 #include "platform.h"

-#if defined(VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI)
+#if defined(USE_VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI)

 #include "common/maths.h"

@ -118,7 +118,7 @@ static IO_t vtxCLKPin       = IO_NONE;


 // Define variables
-static const uint32_t channelArray[RTC6705_BAND_COUNT][RTC6705_CHANNEL_COUNT] = {
+static const uint32_t channelArray[VTX_RTC6705_BAND_COUNT][VTX_RTC6705_CHANNEL_COUNT] = {
    { RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 },
    { RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 },
    { RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 },
@ -200,8 +200,8 @@ static void rtc6705Transfer(uint32_t command)
 */
 void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    band = constrain(band, 0, RTC6705_BAND_COUNT - 1);
-    channel = constrain(channel, 0, RTC6705_CHANNEL_COUNT - 1);
+    band = constrain(band, 0, VTX_RTC6705_BAND_COUNT - 1);
+    channel = constrain(channel, 0, VTX_RTC6705_CHANNEL_COUNT - 1);

    spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);

@ -215,7 +215,7 @@ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
 */
 void rtc6705SetFreq(uint16_t frequency)
 {
-    frequency = constrain(frequency, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX);
+    frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX);

    uint32_t val_hex = 0;

@ -235,7 +235,7 @@ void rtc6705SetFreq(uint16_t frequency)

 void rtc6705SetRFPower(uint8_t rf_power)
 {
-    rf_power = constrain(rf_power, 0, RTC6705_RF_POWER_COUNT - 1);
+    rf_power = constrain(rf_power, 0, VTX_RTC6705_POWER_COUNT - 1);

    spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);

--- a/src/main/drivers/vtx_rtc6705.h
+++ b/src/main/drivers/vtx_rtc6705.h
@ -26,14 +26,21 @@

 #include <stdint.h>

-#define RTC6705_BAND_COUNT          5
-#define RTC6705_CHANNEL_COUNT       8
-#define RTC6705_RF_POWER_COUNT      2
+#define VTX_RTC6705_BAND_COUNT      5
+#define VTX_RTC6705_CHANNEL_COUNT   8
+#define VTX_RTC6705_POWER_COUNT     3
+#define VTX_RTC6705_DEFAULT_POWER   1

-#define RTC6705_FREQ_MIN            5600
-#define RTC6705_FREQ_MAX            5950
+#if defined(RTC6705_POWER_PIN)
+#define VTX_RTC6705_MIN_POWER   0
+#else
+#define VTX_RTC6705_MIN_POWER   1
+#endif

-#define RTC6705_BOOT_DELAY 350 // milliseconds
+#define VTX_RTC6705_FREQ_MIN    5600
+#define VTX_RTC6705_FREQ_MAX    5950
+
+#define VTX_RTC6705_BOOT_DELAY  350 // milliseconds

 void rtc6705IOInit(void);
 void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel);
--- a/src/main/drivers/vtx_rtc6705_soft_spi.c
+++ b/src/main/drivers/vtx_rtc6705_soft_spi.c
@ -20,7 +20,7 @@

 #include "platform.h"

-#if defined(VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI)
+#if defined(USE_VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI)

 #include "drivers/bus_spi.h"
 #include "drivers/io.h"
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@ -108,7 +108,7 @@ extern uint8_t __config_end;
 #include "sensors/opflow.h"
 #include "sensors/sensors.h"

-#include "telemetry/frsky.h"
+#include "telemetry/frsky_d.h"
 #include "telemetry/telemetry.h"
 #include "build/debug.h"

@ -135,11 +135,9 @@ static void cliAssert(char *cmdline);
 static void cliBootlog(char *cmdline);
 #endif

-static const char* const emptyName = "-";
-
 // sync this with features_e
 static const char * const featureNames[] = {
-    "RX_PPM", "VBAT", "TX_PROF_SEL", "", "MOTOR_STOP",
+    "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
    "", "SOFTSERIAL", "GPS", "",
    "", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
    "RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
@ -229,11 +227,12 @@ static void cliPutp(void *p, char ch)
 typedef enum {
    DUMP_MASTER = (1 << 0),
    DUMP_PROFILE = (1 << 1),
-    DUMP_RATES = (1 << 2),
-    DUMP_ALL = (1 << 3),
-    DO_DIFF = (1 << 4),
-    SHOW_DEFAULTS = (1 << 5),
-    HIDE_UNUSED = (1 << 6)
+    DUMP_BATTERY_PROFILE = (1 << 2),
+    DUMP_RATES = (1 << 3),
+    DUMP_ALL = (1 << 4),
+    DO_DIFF = (1 << 5),
+    SHOW_DEFAULTS = (1 << 6),
+    HIDE_UNUSED = (1 << 7)
 } dumpFlags_e;

 static void cliPrintfva(const char *format, va_list va)
@ -329,11 +328,15 @@ static void printValuePointer(const setting_t *var, const void *valuePointer, ui
        cliPrintf("%s", ftoa(*(float *)valuePointer, buf));
        if (full) {
            if (SETTING_MODE(var) == MODE_DIRECT) {
-                cliPrintf(" %s", ftoa((float)setting_get_min(var), buf));
-                cliPrintf(" %s", ftoa((float)setting_get_max(var), buf));
+                cliPrintf(" %s", ftoa((float)settingGetMin(var), buf));
+                cliPrintf(" %s", ftoa((float)settingGetMax(var), buf));
            }
        }
        return; // return from case for float only
+
+    case VAR_STRING:
+        cliPrintf("%s", (const char *)valuePointer);
+        return;
    }

    switch (SETTING_MODE(var)) {
@ -344,7 +347,7 @@ static void printValuePointer(const setting_t *var, const void *valuePointer, ui
            cliPrintf("%d", value);
        if (full) {
            if (SETTING_MODE(var) == MODE_DIRECT) {
-                cliPrintf(" %d %u", setting_get_min(var), setting_get_max(var));
+                cliPrintf(" %d %u", settingGetMin(var), settingGetMax(var));
            }
        }
        break;
@ -352,17 +355,17 @@ static void printValuePointer(const setting_t *var, const void *valuePointer, ui
        if (var->config.lookup.tableIndex < LOOKUP_TABLE_COUNT) {
            cliPrintf(settingLookupTables[var->config.lookup.tableIndex].values[value]);
        } else {
-            setting_get_name(var, buf);
+            settingGetName(var, buf);
            cliPrintLinef("VALUE %s OUT OF RANGE", buf);
        }
        break;
    }
 }

-static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault)
+static bool valuePtrEqualsDefault(const setting_t *value, const void *ptr, const void *ptrDefault)
 {
    bool result = false;
-    switch (type & SETTING_TYPE_MASK) {
+    switch (SETTING_TYPE(value)) {
    case VAR_UINT8:
        result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault;
        break;
@ -386,6 +389,10 @@ static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptr
    case VAR_FLOAT:
        result = *(float *)ptr == *(float *)ptrDefault;
        break;
+
+    case VAR_STRING:
+        result = strncmp(ptr, ptrDefault, settingGetStringMaxLength(value) + 1) == 0;
+        break;
    }
    return result;
 }
@ -397,14 +404,14 @@ static void dumpPgValue(const setting_t *value, uint8_t dumpMask)
    const char *defaultFormat = "#set %s = ";
    // During a dump, the PGs have been backed up to their "copy"
    // regions and the actual values have been reset to its
-    // defaults. This means that setting_get_value_pointer() will
-    // return the default value while setting_get_copy_value_pointer()
+    // defaults. This means that settingGetValuePointer() will
+    // return the default value while settingGetCopyValuePointer()
    // will return the actual value.
-    const void *valuePointer = setting_get_copy_value_pointer(value);
-    const void *defaultValuePointer = setting_get_value_pointer(value);
-    const bool equalsDefault = valuePtrEqualsDefault(value->type, valuePointer, defaultValuePointer);
+    const void *valuePointer = settingGetCopyValuePointer(value);
+    const void *defaultValuePointer = settingGetValuePointer(value);
+    const bool equalsDefault = valuePtrEqualsDefault(value, valuePointer, defaultValuePointer);
    if (((dumpMask & DO_DIFF) == 0) || !equalsDefault) {
-        setting_get_name(value, name);
+        settingGetName(value, name);
        if (dumpMask & SHOW_DEFAULTS && !equalsDefault) {
            cliPrintf(defaultFormat, name);
            printValuePointer(value, defaultValuePointer, 0);
@ -429,7 +436,7 @@ static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask)

 static void cliPrintVar(const setting_t *var, uint32_t full)
 {
-    const void *ptr = setting_get_value_pointer(var);
+    const void *ptr = settingGetValuePointer(var);

    printValuePointer(var, ptr, full);
 }
@ -437,10 +444,15 @@ static void cliPrintVar(const setting_t *var, uint32_t full)
 static void cliPrintVarRange(const setting_t *var)
 {
    switch (SETTING_MODE(var)) {
-    case (MODE_DIRECT):
-        cliPrintLinef("Allowed range: %d - %u", setting_get_min(var), setting_get_max(var));
+    case MODE_DIRECT:
+        if (SETTING_TYPE(var) == VAR_STRING) {
+           cliPrintLinef("Max. length: %u", settingGetStringMaxLength(var));
           break;
-    case (MODE_LOOKUP): {
+        }
+        cliPrintLinef("Allowed range: %d - %u", settingGetMin(var), settingGetMax(var));
+        break;
+    case MODE_LOOKUP:
+    {
        const lookupTableEntry_t *tableEntry = &settingLookupTables[var->config.lookup.tableIndex];
        cliPrint("Allowed values:");
        for (uint32_t i = 0; i < tableEntry->valueCount ; i++) {
@ -460,9 +472,9 @@ typedef union {
    float float_value;
 } int_float_value_t;

-static void cliSetVar(const setting_t *var, const int_float_value_t value)
+static void cliSetIntFloatVar(const setting_t *var, const int_float_value_t value)
 {
-    void *ptr = setting_get_value_pointer(var);
+    void *ptr = settingGetValuePointer(var);

    switch (SETTING_TYPE(var)) {
    case VAR_UINT8:
@ -482,6 +494,10 @@ static void cliSetVar(const setting_t *var, const int_float_value_t value)
    case VAR_FLOAT:
        *(float *)ptr = (float)value.float_value;
        break;
+
+    case VAR_STRING:
+        // Handled by cliSet directly
+        break;
    }
 }

@ -2122,24 +2138,6 @@ static void cliMotor(char *cmdline)
    cliPrintLinef("motor %d: %d", motor_index, motor_disarmed[motor_index]);
 }

-static void printName(uint8_t dumpMask, const systemConfig_t * sConfig)
-{
-    bool equalsDefault = strlen(sConfig->name) == 0;
-    cliDumpPrintLinef(dumpMask, equalsDefault, "name %s", equalsDefault ? emptyName : sConfig->name);
-}
-
-static void cliName(char *cmdline)
-{
-    int32_t len = strlen(cmdline);
-    if (len > 0) {
-        memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfigMutable()->name));
-        if (strncmp(cmdline, emptyName, len)) {
-            strncpy(systemConfigMutable()->name, cmdline, MIN(len, MAX_NAME_LENGTH));
-        }
-    }
-    printName(DUMP_MASTER, systemConfig());
-}
-
 #ifdef PLAY_SOUND
 static void cliPlaySound(char *cmdline)
 {
@ -2203,6 +2201,33 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask)
    dumpAllValues(CONTROL_RATE_VALUE, dumpMask);
 }

+static void cliBatteryProfile(char *cmdline)
+{
+    // CLI profile index is 1-based
+    if (isEmpty(cmdline)) {
+        cliPrintLinef("battery_profile %d", getConfigBatteryProfile() + 1);
+        return;
+    } else {
+        const int i = fastA2I(cmdline) - 1;
+        if (i >= 0 && i < MAX_PROFILE_COUNT) {
+            setConfigBatteryProfileAndWriteEEPROM(i);
+            cliBatteryProfile("");
+        }
+    }
+}
+
+static void cliDumpBatteryProfile(uint8_t profileIndex, uint8_t dumpMask)
+{
+    if (profileIndex >= MAX_BATTERY_PROFILE_COUNT) {
+        // Faulty values
+        return;
+    }
+    setConfigBatteryProfile(profileIndex);
+    cliPrintHashLine("battery_profile");
+    cliPrintLinef("battery_profile %d\r\n", getConfigBatteryProfile() + 1);
+    dumpAllValues(BATTERY_CONFIG_VALUE, dumpMask);
+}
+
 static void cliSave(char *cmdline)
 {
    UNUSED(cmdline);
@ -2232,7 +2257,7 @@ static void cliGet(char *cmdline)

    for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) {
        val = &settingsTable[i];
-        if (setting_name_contains(val, name, cmdline)) {
+        if (settingNameContains(val, name, cmdline)) {
            cliPrintf("%s = ", name);
            cliPrintVar(val, 0);
            cliPrintLinefeed();
@ -2264,7 +2289,7 @@ static void cliSet(char *cmdline)
        cliPrintLine("Current settings:");
        for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) {
            val = &settingsTable[i];
-            setting_get_name(val, name);
+            settingGetName(val, name);
            cliPrintf("%s = ", name);
            cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
            cliPrintLinefeed();
@ -2287,17 +2312,21 @@ static void cliSet(char *cmdline)
        for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) {
            val = &settingsTable[i];
            // ensure exact match when setting to prevent setting variables with shorter names
-            if (setting_name_exact_match(val, name, cmdline, variableNameLength)) {
+            if (settingNameIsExactMatch(val, name, cmdline, variableNameLength)) {
+                const setting_type_e type = SETTING_TYPE(val);
+                if (type == VAR_STRING) {
+                    settingSetString(val, eqptr, strlen(eqptr));
+                    return;
+                }
+                const setting_mode_e mode = SETTING_MODE(val);
                bool changeValue = false;
                int_float_value_t tmp = {0};
-                const int mode = SETTING_MODE(val);
-                const int type = SETTING_TYPE(val);
                switch (mode) {
                case MODE_DIRECT: {
                        if (*eqptr != 0 && strspn(eqptr, "0123456789.+-") == strlen(eqptr)) {
                            float valuef = fastA2F(eqptr);
                            // note: compare float values
-                            if (valuef >= (float)setting_get_min(val) && valuef <= (float)setting_get_max(val)) {
+                            if (valuef >= (float)settingGetMin(val) && valuef <= (float)settingGetMax(val)) {

                                if (type == VAR_FLOAT)
                                    tmp.float_value = valuef;
@ -2327,7 +2356,7 @@ static void cliSet(char *cmdline)
                }

                if (changeValue) {
-                    cliSetVar(val, tmp);
+                    cliSetIntFloatVar(val, tmp);

                    cliPrintf("%s set to ", name);
                    cliPrintVar(val, 0);
@ -2583,6 +2612,8 @@ static void printConfig(const char *cmdline, bool doDiff)
        dumpMask = DUMP_MASTER; // only
    } else if ((options = checkCommand(cmdline, "profile"))) {
        dumpMask = DUMP_PROFILE; // only
+    } else if ((options = checkCommand(cmdline, "battery_profile"))) {
+        dumpMask = DUMP_BATTERY_PROFILE; // only
    } else if ((options = checkCommand(cmdline, "all"))) {
        dumpMask = DUMP_ALL;   // all profiles and rates
    } else {
@ -2594,11 +2625,13 @@ static void printConfig(const char *cmdline, bool doDiff)
    }

    const int currentProfileIndexSave = getConfigProfile();
+    const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
    backupConfigs();
    // reset all configs to defaults to do differencing
    resetConfigs();
    // restore the profile indices, since they should not be reset for proper comparison
    setConfigProfile(currentProfileIndexSave);
+    setConfigBatteryProfile(currentBatteryProfileIndexSave);

    if (checkCommand(options, "showdefaults")) {
        dumpMask = dumpMask | SHOW_DEFAULTS;   // add default values as comments for changed values
@ -2644,9 +2677,6 @@ static void printConfig(const char *cmdline, bool doDiff)
        cliPrintHashLine("map");
        printMap(dumpMask, &rxConfig_Copy, rxConfig());

-        cliPrintHashLine("name");
-        printName(dumpMask, &systemConfig_Copy);
-
        cliPrintHashLine("serial");
        printSerial(dumpMask, &serialConfig_Copy, serialConfig());

@ -2681,23 +2711,35 @@ static void printConfig(const char *cmdline, bool doDiff)
        if (dumpMask & DUMP_ALL) {
            // dump all profiles
            const int currentProfileIndexSave = getConfigProfile();
+            const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
            for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
                cliDumpProfile(ii, dumpMask);
            }
+            for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) {
+                cliDumpBatteryProfile(ii, dumpMask);
+            }
            setConfigProfile(currentProfileIndexSave);
+            setConfigBatteryProfile(currentBatteryProfileIndexSave);
+
            cliPrintHashLine("restore original profile selection");
            cliPrintLinef("profile %d", currentProfileIndexSave + 1);
+            cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1);

            cliPrintHashLine("save configuration\r\nsave");
        } else {
-            // dump just the current profile
+            // dump just the current profiles
            cliDumpProfile(getConfigProfile(), dumpMask);
+            cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
        }
    }

    if (dumpMask & DUMP_PROFILE) {
        cliDumpProfile(getConfigProfile(), dumpMask);
    }
+
+    if (dumpMask & DUMP_BATTERY_PROFILE) {
+        cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
+    }
    // restore configs from copies
    restoreConfigs();
 }
@ -2760,9 +2802,9 @@ const clicmd_t cmdTable[] = {
    CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
    CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu),
    CLI_COMMAND_DEF("diff", "list configuration changes from default",
-        "[master|profile|rates|all] {showdefaults}", cliDiff),
+        "[master|battery_profile|profile|rates|all] {showdefaults}", cliDiff),
    CLI_COMMAND_DEF("dump", "dump configuration",
-        "[master|profile|rates|all] {showdefaults}", cliDump),
+        "[master|battery_profile|profile|rates|all] {showdefaults}", cliDump),
 #ifdef USE_RX_ELERES
    CLI_COMMAND_DEF("eleres_bind", NULL, NULL, cliEleresBind),
 #endif // USE_RX_ELERES
@ -2790,12 +2832,13 @@ const clicmd_t cmdTable[] = {
    CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory),
    CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix),
    CLI_COMMAND_DEF("motor",  "get/set motor", "<index> [<value>]", cliMotor),
-    CLI_COMMAND_DEF("name", "name of craft", NULL, cliName),
 #ifdef PLAY_SOUND
    CLI_COMMAND_DEF("play_sound", NULL, "[<index>]\r\n", cliPlaySound),
 #endif
    CLI_COMMAND_DEF("profile", "change profile",
        "[<index>]", cliProfile),
+    CLI_COMMAND_DEF("battery_profile", "change battery profile",
+        "[<index>]", cliBatteryProfile),
 #if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES)
    CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
 #endif
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@ -109,6 +109,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG,

 PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
    .current_profile_index = 0,
+    .current_battery_profile_index = 0,
    .debug_mode = DEBUG_NONE,
    .i2c_speed = I2C_SPEED_400KHZ,
    .cpuUnderclock = 0,
@ -222,7 +223,7 @@ void validateAndFixConfig(void)
 #endif

    // Disable unused features
-    featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_2 | FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 );
+    featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 );

 #if defined(DISABLE_RX_PWM_FEATURE) || !defined(USE_RX_PWM)
    if (rxConfig()->receiverType == RX_TYPE_PWM) {
@ -367,6 +368,7 @@ void resetConfigs(void)
 static void activateConfig(void)
 {
    activateControlRateConfig();
+    activateBatteryProfile();

    resetAdjustmentStates();

@ -396,6 +398,7 @@ void readEEPROM(void)
    }

    setConfigProfile(getConfigProfile());
+    setConfigBatteryProfile(getConfigBatteryProfile());

    validateAndFixConfig();
    activateConfig();
@ -464,6 +467,35 @@ void setConfigProfileAndWriteEEPROM(uint8_t profileIndex)
    beeperConfirmationBeeps(profileIndex + 1);
 }

+uint8_t getConfigBatteryProfile(void)
+{
+    return systemConfig()->current_battery_profile_index;
+}
+
+bool setConfigBatteryProfile(uint8_t profileIndex)
+{
+    bool ret = true; // return true if current_battery_profile_index has changed
+    if (systemConfig()->current_battery_profile_index == profileIndex) {
+        ret =  false;
+    }
+    if (profileIndex >= MAX_BATTERY_PROFILE_COUNT) {// sanity check
+        profileIndex = 0;
+    }
+    systemConfigMutable()->current_battery_profile_index = profileIndex;
+    setBatteryProfile(profileIndex);
+    return ret;
+}
+
+void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex)
+{
+    if (setConfigBatteryProfile(profileIndex)) {
+        // profile has changed, so ensure current values saved before new profile is loaded
+        writeEEPROM();
+        readEEPROM();
+    }
+    beeperConfirmationBeeps(profileIndex + 1);
+}
+
 void beeperOffSet(uint32_t mask)
 {
    beeperConfigMutable()->beeper_off_flags |= mask;
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@ -43,10 +43,10 @@ typedef enum {
 } asyncMode_e;

 typedef enum {
-    FEATURE_UNUSED_1 = 1 << 0,          // RX_PPM
+    FEATURE_THR_VBAT_COMP = 1 << 0,
    FEATURE_VBAT = 1 << 1,
    FEATURE_TX_PROF_SEL = 1 << 2,       // Profile selection by TX stick command
-    FEATURE_UNUSED_2 = 1 << 3,          // RX_SERIAL
+    FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
    FEATURE_MOTOR_STOP = 1 << 4,
    NOT_USED_10 = 1 << 5,               // was FEATURE_SERVO_TILT
    FEATURE_SOFTSERIAL = 1 << 6,
@ -81,6 +81,7 @@ typedef struct systemConfig_s {
    uint16_t accTaskFrequency;
    uint16_t attitudeTaskFrequency;
    uint8_t current_profile_index;
+    uint8_t current_battery_profile_index;
    uint8_t asyncMode;
    uint8_t debug_mode;
    uint8_t i2c_speed;
@ -134,6 +135,10 @@ uint8_t getConfigProfile(void);
 bool setConfigProfile(uint8_t profileIndex);
 void setConfigProfileAndWriteEEPROM(uint8_t profileIndex);

+uint8_t getConfigBatteryProfile(void);
+bool setConfigBatteryProfile(uint8_t profileIndex);
+void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex);
+
 bool canSoftwareSerialBeUsed(void);
 void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch);

--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@ -94,6 +94,7 @@ enum {
 #define GYRO_WATCHDOG_DELAY 100  // Watchdog for boards without interrupt for gyro

 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;

 float dT;

@ -373,6 +374,8 @@ void tryArm(void)
            return;
        }

+        lastDisarmReason = DISARM_NONE;
+
        ENABLE_ARMING_FLAG(ARMED);
        ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
        headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
@ -480,9 +483,10 @@ void processRx(timeUs_t currentTimeUs)

    updateActivatedModes();

-    if ((!cliMode) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
-        updateAdjustmentStates();
-        processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile));
+    if (!cliMode) {
+        bool canUseRxData = rxIsReceivingSignal() && !FLIGHT_MODE(FAILSAFE_MODE);
+        updateAdjustmentStates(canUseRxData);
+        processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
    }

    bool canUseHorizonMode = true;
@ -702,6 +706,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
    cycleTime = getTaskDeltaTime(TASK_SELF);
    dT = (float)cycleTime * 0.000001f;

+    if (ARMING_FLAG(ARMED) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))) {
+        flightTime += cycleTime;
+    }
+
 #ifdef USE_ASYNC_GYRO_PROCESSING
    if (getAsyncMode() == ASYNC_MODE_NONE) {
        taskGyro(currentTimeUs);
@ -823,3 +831,8 @@ void taskUpdateRxMain(timeUs_t currentTimeUs)
    processRx(currentTimeUs);
    isRXDataNew = true;
 }
+
+// returns seconds
+float getFlightTime() {
+    return (float)(flightTime / 1000) / 1000;
+}
--- a/src/main/fc/fc_core.h
+++ b/src/main/fc/fc_core.h
@ -17,6 +17,8 @@

 #pragma once

+#include "common/time.h"
+
 typedef enum disarmReason_e {
    DISARM_NONE         = 0,
    DISARM_TIMEOUT      = 1,
@ -29,6 +31,7 @@ typedef enum disarmReason_e {
    DISARM_REASON_COUNT
 } disarmReason_t;

+
 void handleInflightCalibrationStickPosition(void);

 void disarm(disarmReason_t disarmReason);
@ -36,3 +39,4 @@ void tryArm(void);
 disarmReason_t getDisarmReason(void);

 bool isCalibrating(void);
+float getFlightTime();
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@ -103,6 +103,7 @@
 #include "io/rcdevice_cam.h"
 #include "io/serial.h"
 #include "io/displayport_msp.h"
+#include "io/vtx.h"
 #include "io/vtx_control.h"
 #include "io/vtx_smartaudio.h"
 #include "io/vtx_tramp.h"
@ -302,7 +303,7 @@ void init(void)
    pwm_params.useParallelPWM = (rxConfig()->receiverType == RX_TYPE_PWM);
    pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
    pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
-        && batteryConfig()->current.type == CURRENT_SENSOR_ADC;
+        && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC;
    pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
    pwm_params.usePPM = (rxConfig()->receiverType == RX_TYPE_PPM);
    pwm_params.useSerialRx = (rxConfig()->receiverType == RX_TYPE_SERIAL);
@ -473,7 +474,7 @@ void init(void)
        adc_params.adcFunctionChannel[ADC_RSSI] = adcChannelConfig()->adcFunctionChannel[ADC_RSSI];
    }

-    if (feature(FEATURE_CURRENT_METER) && batteryConfig()->current.type == CURRENT_SENSOR_ADC) {
+    if (feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC) {
        adc_params.adcFunctionChannel[ADC_CURRENT] =  adcChannelConfig()->adcFunctionChannel[ADC_CURRENT];
    }

@ -557,9 +558,7 @@ void init(void)
    if (feature(FEATURE_OSD)) {
 #if defined(USE_MAX7456)
        // If there is a max7456 chip for the OSD then use it
-        static vcdProfile_t vcdProfile;
-        vcdProfile.video_system = osdConfig()->video_system;
-        osdDisplayPort = max7456DisplayPortInit(&vcdProfile);
+        osdDisplayPort = max7456DisplayPortInit(osdConfig()->video_system);
 #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
        osdDisplayPort = displayPortMspInit();
 #endif
@ -653,20 +652,23 @@ void init(void)
    pitotStartCalibration();
 #endif

-#ifdef VTX_CONTROL
+#ifdef USE_VTX_CONTROL
    vtxControlInit();

+#if defined(USE_VTX_COMMON)
    vtxCommonInit();
+    vtxInit();
+#endif

-#ifdef VTX_SMARTAUDIO
+#ifdef USE_VTX_SMARTAUDIO
    vtxSmartAudioInit();
 #endif

-#ifdef VTX_TRAMP
+#ifdef USE_VTX_TRAMP
    vtxTrampInit();
 #endif

-#endif // VTX_CONTROL
+#endif // USE_VTX_CONTROL

    // Now that everything has powered up the voltage and cell count be determined.
    if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@ -76,6 +76,8 @@
 #include "io/osd.h"
 #include "io/serial.h"
 #include "io/serial_4way.h"
+#include "io/vtx.h"
+#include "io/vtx_string.h"

 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
@ -419,7 +421,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
            sbufWriteU16(dst, packSensorStatus());
            sbufWriteU16(dst, averageSystemLoadPercent);
-            sbufWriteU8(dst, getConfigProfile());
+            sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile());
            sbufWriteU32(dst, armingFlags);
            sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
        }
@ -539,16 +541,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        break;

    case MSP2_INAV_ANALOG:
+        // Bit 1: battery full, Bit 2: use capacity threshold, Bit 3-4: battery state, Bit 5-8: battery cell count
+        sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2) | (getBatteryCellCount() << 4));
        sbufWriteU16(dst, getBatteryVoltage());
-        sbufWriteU8(dst, getBatteryCellCount());
-        sbufWriteU8(dst, calculateBatteryPercentage());
-        sbufWriteU16(dst, constrain(getPower(), 0, 0x7FFFFFFF));           // power draw
-        sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
-        sbufWriteU16(dst, (uint16_t)constrain(getMWhDrawn(), 0, 0xFFFF)); // milliWatt hours drawn from battery
-        sbufWriteU16(dst, getRSSI());
-        sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
-        sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2));
+        sbufWriteU16(dst, getAmperage()); // send amperage in 0.01 A steps
+        sbufWriteU32(dst, getPower());    // power draw
+        sbufWriteU32(dst, getMAhDrawn()); // milliamp hours drawn from battery
+        sbufWriteU32(dst, getMWhDrawn()); // milliWatt hours drawn from battery
        sbufWriteU32(dst, getBatteryRemainingCapacity());
+        sbufWriteU8(dst, calculateBatteryPercentage());
+        sbufWriteU16(dst, getRSSI());
        break;

    case MSP_ARMING_CONFIG:
@ -670,10 +672,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF

        sbufWriteU16(dst, compassConfig()->mag_declination / 10);

-        sbufWriteU8(dst, batteryConfig()->voltage.scale / 10);
-        sbufWriteU8(dst, batteryConfig()->voltage.cellMin / 10);
-        sbufWriteU8(dst, batteryConfig()->voltage.cellMax / 10);
-        sbufWriteU8(dst, batteryConfig()->voltage.cellWarning / 10);
+        sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10);
+        sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
+        sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
+        sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
        break;

    case MSP2_INAV_MISC:
@ -698,32 +700,36 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF

        sbufWriteU16(dst, compassConfig()->mag_declination / 10);

-        sbufWriteU16(dst, batteryConfig()->voltage.scale);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellDetect);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellMin);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellMax);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellWarning);
+        sbufWriteU16(dst, batteryMetersConfig()->voltage_scale);
+        sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
+        sbufWriteU8(dst, currentBatteryProfile->cells);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);

-        sbufWriteU32(dst, batteryConfig()->capacity.value);
-        sbufWriteU32(dst, batteryConfig()->capacity.warning);
-        sbufWriteU32(dst, batteryConfig()->capacity.critical);
-        sbufWriteU8(dst, batteryConfig()->capacity.unit);
+        sbufWriteU32(dst, currentBatteryProfile->capacity.value);
+        sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
+        sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
+        sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
        break;

    case MSP2_INAV_BATTERY_CONFIG:
-        sbufWriteU16(dst, batteryConfig()->voltage.scale);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellDetect);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellMin);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellMax);
-        sbufWriteU16(dst, batteryConfig()->voltage.cellWarning);
+        sbufWriteU16(dst, batteryMetersConfig()->voltage_scale);
+        sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
+        sbufWriteU8(dst, currentBatteryProfile->cells);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
+        sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);

-        sbufWriteU16(dst, batteryConfig()->current.offset);
-        sbufWriteU16(dst, batteryConfig()->current.scale);
+        sbufWriteU16(dst, batteryMetersConfig()->current.offset);
+        sbufWriteU16(dst, batteryMetersConfig()->current.scale);

-        sbufWriteU32(dst, batteryConfig()->capacity.value);
-        sbufWriteU32(dst, batteryConfig()->capacity.warning);
-        sbufWriteU32(dst, batteryConfig()->capacity.critical);
-        sbufWriteU8(dst, batteryConfig()->capacity.unit);
+        sbufWriteU32(dst, currentBatteryProfile->capacity.value);
+        sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
+        sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
+        sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
        break;

    case MSP_MOTOR_PINS:
@ -809,17 +815,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        break;

    case MSP_VOLTAGE_METER_CONFIG:
-        sbufWriteU8(dst, batteryConfig()->voltage.scale / 10);
-        sbufWriteU8(dst, batteryConfig()->voltage.cellMin / 10);
-        sbufWriteU8(dst, batteryConfig()->voltage.cellMax / 10);
-        sbufWriteU8(dst, batteryConfig()->voltage.cellWarning / 10);
+        sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10);
+        sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
+        sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
+        sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
        break;

    case MSP_CURRENT_METER_CONFIG:
-        sbufWriteU16(dst, batteryConfig()->current.scale);
-        sbufWriteU16(dst, batteryConfig()->current.offset);
-        sbufWriteU8(dst, batteryConfig()->current.type);
-        sbufWriteU16(dst, constrain(batteryConfig()->capacity.value, 0, 0xFFFF));
+        sbufWriteU16(dst, batteryMetersConfig()->current.scale);
+        sbufWriteU16(dst, batteryMetersConfig()->current.offset);
+        sbufWriteU8(dst, batteryMetersConfig()->current.type);
+        sbufWriteU16(dst, constrain(currentBatteryProfile->capacity.value, 0, 0xFFFF));
        break;

    case MSP_MIXER:
@ -879,8 +885,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        sbufWriteU16(dst, boardAlignment()->pitchDeciDegrees);
        sbufWriteU16(dst, boardAlignment()->yawDeciDegrees);

-        sbufWriteU16(dst, batteryConfig()->current.scale);
-        sbufWriteU16(dst, batteryConfig()->current.offset);
+        sbufWriteU16(dst, batteryMetersConfig()->current.scale);
+        sbufWriteU16(dst, batteryMetersConfig()->current.offset);
        break;

    case MSP_CF_SERIAL_CONFIG:
@ -964,7 +970,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
        sbufWriteU8(dst, osdConfig()->units);
        sbufWriteU8(dst, osdConfig()->rssi_alarm);
-        sbufWriteU16(dst, batteryConfig()->capacity.warning);
+        sbufWriteU16(dst, currentBatteryProfile->capacity.warning);
        sbufWriteU16(dst, osdConfig()->time_alarm);
        sbufWriteU16(dst, osdConfig()->alt_alarm);
        sbufWriteU16(dst, osdConfig()->dist_alarm);
@ -1266,30 +1272,32 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        }
        break;

-#if defined(VTX_COMMON)
+#if defined(USE_VTX_COMMON)
    case MSP_VTX_CONFIG:
        {
-            uint8_t deviceType = vtxCommonGetDeviceType();
-            if (deviceType != VTXDEV_UNKNOWN) {
+            vtxDevice_t *vtxDevice = vtxCommonDevice();
+            if (vtxDevice) {

-                uint8_t band=0, channel=0;
-                vtxCommonGetBandAndChannel(&band,&channel);
-
-                uint8_t powerIdx=0; // debug
-                vtxCommonGetPowerIndex(&powerIdx);
+                uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice);

+                // Return band, channel and power from vtxSettingsConfig_t
+                // since the VTX might be configured but temporarily offline.
                uint8_t pitmode = 0;
-                vtxCommonGetPitMode(&pitmode);
+                vtxCommonGetPitMode(vtxDevice, &pitmode);

                sbufWriteU8(dst, deviceType);
-                sbufWriteU8(dst, band);
-                sbufWriteU8(dst, channel);
-                sbufWriteU8(dst, powerIdx);
+                sbufWriteU8(dst, vtxSettingsConfig()->band);
+                sbufWriteU8(dst, vtxSettingsConfig()->channel);
+                sbufWriteU8(dst, vtxSettingsConfig()->power);
                sbufWriteU8(dst, pitmode);
+
+                // Betaflight < 4 doesn't send these fields
+                sbufWriteU8(dst, vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0);
+                sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
                // future extensions here...
            }
            else {
-                sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
+                sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured
            }
        }
        break;
@ -1327,16 +1335,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        break;

 #if defined(USE_OSD)
-    case MSP2_INAV_OSD_LAYOUTS:
-        sbufWriteU8(dst, OSD_LAYOUT_COUNT);
-        sbufWriteU8(dst, OSD_ITEM_COUNT);
-        for (unsigned ii = 0; ii < OSD_LAYOUT_COUNT; ii++) {
-            for (unsigned jj = 0; jj < OSD_ITEM_COUNT; jj++) {
-                sbufWriteU16(dst, osdConfig()->item_pos[ii][jj]);
-            }
-        }
-        break;
-
    case MSP2_INAV_OSD_ALARMS:
        sbufWriteU8(dst, osdConfig()->rssi_alarm);
        sbufWriteU16(dst, osdConfig()->time_alarm);
@ -1426,6 +1424,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 {
    uint8_t tmp_u8;
    uint16_t tmp_u16;
+    batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile;

    const unsigned int dataSize = sbufBytesRemaining(src);

@ -1636,16 +1635,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        sbufReadU16(src);
 #endif

-        batteryConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
-        batteryConfigMutable()->voltage.cellMin = sbufReadU8(src) * 10;         // vbatlevel_warn1 in MWC2.3 GUI
-        batteryConfigMutable()->voltage.cellMax = sbufReadU8(src) * 10;         // vbatlevel_warn2 in MWC2.3 GUI
-        batteryConfigMutable()->voltage.cellWarning = sbufReadU8(src) * 10;     // vbatlevel when buzzer starts to alert
+        batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10;
+        currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10;         // vbatlevel_warn1 in MWC2.3 GUI
+        currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10;         // vbatlevel_warn2 in MWC2.3 GUI
+        currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10;     // vbatlevel when buzzer starts to alert
        } else
            return MSP_RESULT_ERROR;
        break;

    case MSP2_INAV_SET_MISC:
-        if (dataSize == 39) {
+        if (dataSize == 41) {
            sbufReadU16(src);       // midrc

            motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
@ -1674,18 +1673,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
            sbufReadU16(src);
 #endif

-            batteryConfigMutable()->voltage.scale = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellDetect = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellMin = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellMax = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellWarning = sbufReadU16(src);
+            batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src);
+            batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
+            currentBatteryProfileMutable->cells = sbufReadU8(src);
+            currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
+            currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
+            currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
+            currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);

-            batteryConfigMutable()->capacity.value = sbufReadU32(src);
-            batteryConfigMutable()->capacity.warning = sbufReadU32(src);
-            batteryConfigMutable()->capacity.critical = sbufReadU32(src);
-            batteryConfigMutable()->capacity.unit = sbufReadU8(src);
-            if ((batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
-                batteryConfigMutable()->capacity.unit = BAT_CAPACITY_UNIT_MAH;
+            currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
+            currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
+            currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
+            currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
+            if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
+                batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
+                return MSP_RESULT_ERROR;
+            }
+            if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
+                currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
                return MSP_RESULT_ERROR;
            }
        } else
@ -1693,22 +1698,28 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        break;

    case MSP2_INAV_SET_BATTERY_CONFIG:
-        if (dataSize == 27) {
-            batteryConfigMutable()->voltage.scale = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellDetect = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellMin = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellMax = sbufReadU16(src);
-            batteryConfigMutable()->voltage.cellWarning = sbufReadU16(src);
+        if (dataSize == 29) {
+            batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src);
+            batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
+            currentBatteryProfileMutable->cells = sbufReadU8(src);
+            currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
+            currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
+            currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
+            currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);

-            batteryConfigMutable()->current.offset = sbufReadU16(src);
-            batteryConfigMutable()->current.scale = sbufReadU16(src);
+            batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
+            batteryMetersConfigMutable()->current.scale = sbufReadU16(src);

-            batteryConfigMutable()->capacity.value = sbufReadU32(src);
-            batteryConfigMutable()->capacity.warning = sbufReadU32(src);
-            batteryConfigMutable()->capacity.critical = sbufReadU32(src);
-            batteryConfigMutable()->capacity.unit = sbufReadU8(src);
-            if ((batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
-                batteryConfigMutable()->capacity.unit = BAT_CAPACITY_UNIT_MAH;
+            currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
+            currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
+            currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
+            currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
+            if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
+                batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
+                return MSP_RESULT_ERROR;
+            }
+            if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
+                currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
                return MSP_RESULT_ERROR;
            }
        } else
@ -2100,7 +2111,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #endif
                osdConfigMutable()->units = sbufReadU8(src);
                osdConfigMutable()->rssi_alarm = sbufReadU8(src);
-                batteryConfigMutable()->capacity.warning = sbufReadU16(src);
+                currentBatteryProfileMutable->capacity.warning = sbufReadU16(src);
                osdConfigMutable()->time_alarm = sbufReadU16(src);
                osdConfigMutable()->alt_alarm = sbufReadU16(src);
                // Won't be read if they weren't provided
@ -2138,36 +2149,44 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        break;
 #endif // USE_OSD

-#if defined(VTX_COMMON)
+#if defined(USE_VTX_COMMON)
    case MSP_SET_VTX_CONFIG:
-        if (dataSize >= 4) {
-            tmp_u16 = sbufReadU16(src);
-            const uint8_t band    = (tmp_u16 / 8) + 1;
-            const uint8_t channel = (tmp_u16 % 8) + 1;
-
-            if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
-                uint8_t current_band=0, current_channel=0;
-                vtxCommonGetBandAndChannel(&current_band,&current_channel);
-                if ((current_band != band) || (current_channel != channel))
-                    vtxCommonSetBandAndChannel(band,channel);
-
-                if (sbufBytesRemaining(src) < 2)
-                    break;
-
-                uint8_t power = sbufReadU8(src);
-                uint8_t current_power = 0;
-                vtxCommonGetPowerIndex(&current_power);
-                if (current_power != power)
-                    vtxCommonSetPowerByIndex(power);
-
-                uint8_t pitmode = sbufReadU8(src);
-                uint8_t current_pitmode = 0;
-                vtxCommonGetPitMode(&current_pitmode);
-                if (current_pitmode != pitmode)
-                    vtxCommonSetPitMode(pitmode);
+        if (dataSize >= 2) {
+            vtxDevice_t *vtxDevice = vtxCommonDevice();
+            if (vtxDevice) {
+                if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
+                    uint16_t newFrequency = sbufReadU16(src);
+                    if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
+                        const uint8_t newBand = (newFrequency / 8) + 1;
+                        const uint8_t newChannel = (newFrequency % 8) + 1;
+                        vtxSettingsConfigMutable()->band = newBand;
+                        vtxSettingsConfigMutable()->channel = newChannel;
+                        vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
+                    } else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) {  //value is frequency in MHz. Ignore it if it's invalid
+                        vtxSettingsConfigMutable()->band = 0;
+                        vtxSettingsConfigMutable()->channel = 0;
+                        vtxSettingsConfigMutable()->freq = newFrequency;
                    }
-        } else
+
+                    if (sbufBytesRemaining(src) > 1) {
+                        vtxSettingsConfigMutable()->power = sbufReadU8(src);
+                        // Delegate pitmode to vtx directly
+                        const uint8_t newPitmode = sbufReadU8(src);
+                        uint8_t currentPitmode = 0;
+                        vtxCommonGetPitMode(vtxDevice, &currentPitmode);
+                        if (currentPitmode != newPitmode) {
+                            vtxCommonSetPitMode(vtxDevice, newPitmode);
+                        }
+
+                        if (sbufBytesRemaining(src) > 0) {
+                            vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
+                        }
+                    }
+                }
+            }
+        } else {
            return MSP_RESULT_ERROR;
+        }
        break;
 #endif

@ -2245,20 +2264,20 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)

    case MSP_SET_VOLTAGE_METER_CONFIG:
        if (dataSize >= 4) {
-            batteryConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
-            batteryConfigMutable()->voltage.cellMin = sbufReadU8(src) * 10;
-            batteryConfigMutable()->voltage.cellMax = sbufReadU8(src) * 10;
-            batteryConfigMutable()->voltage.cellWarning = sbufReadU8(src) * 10;
+            batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10;
+            currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10;
+            currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10;
+            currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10;
        } else
            return MSP_RESULT_ERROR;
        break;

    case MSP_SET_CURRENT_METER_CONFIG:
        if (dataSize >= 7) {
-            batteryConfigMutable()->current.scale = sbufReadU16(src);
-            batteryConfigMutable()->current.offset = sbufReadU16(src);
-            batteryConfigMutable()->current.type = sbufReadU8(src);
-            batteryConfigMutable()->capacity.value = sbufReadU16(src);
+            batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
+            batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
+            batteryMetersConfigMutable()->current.type = sbufReadU8(src);
+            currentBatteryProfileMutable->capacity.value = sbufReadU16(src);
        } else
            return MSP_RESULT_ERROR;
        break;
@ -2342,8 +2361,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
            boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src); // board_align_pitch
            boardAlignmentMutable()->yawDeciDegrees = sbufReadU16(src); // board_align_yaw

-            batteryConfigMutable()->current.scale = sbufReadU16(src);
-            batteryConfigMutable()->current.offset = sbufReadU16(src);
+            batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
+            batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
        } else
            return MSP_RESULT_ERROR;
        break;
@ -2539,6 +2558,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        navConfigMutable()->mc.braking_bank_angle = sbufReadU8(src);
        break;
        
+    case MSP2_INAV_SELECT_BATTERY_PROFILE:
+        if (!ARMING_FLAG(ARMED)) {
+            if (sbufReadU8Safe(&tmp_u8, src))
+                setConfigBatteryProfileAndWriteEEPROM(tmp_u8);
+        }
+        break;
+
    default:
        return MSP_RESULT_ERROR;
    }
@ -2563,7 +2589,7 @@ static const setting_t *mspReadSettingName(sbuf_t *src)
            return NULL;
        }
    }
-    return setting_find(name);
+    return settingFind(name);
 }

 static bool mspSettingCommand(sbuf_t *dst, sbuf_t *src)
@ -2573,8 +2599,8 @@ static bool mspSettingCommand(sbuf_t *dst, sbuf_t *src)
        return false;
    }

-    const void *ptr = setting_get_value_pointer(setting);
-    size_t size = setting_get_value_size(setting);
+    const void *ptr = settingGetValuePointer(setting);
+    size_t size = settingGetValueSize(setting);
    sbufWriteDataSafe(dst, ptr, size);
    return true;
 }
@ -2588,10 +2614,10 @@ static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src)
        return false;
    }

-    setting_min_t min = setting_get_min(setting);
-    setting_max_t max = setting_get_max(setting);
+    setting_min_t min = settingGetMin(setting);
+    setting_max_t max = settingGetMax(setting);

-    void *ptr = setting_get_value_pointer(setting);
+    void *ptr = settingGetValuePointer(setting);
    switch (SETTING_TYPE(setting)) {
        case VAR_UINT8:
            {
@ -2665,11 +2691,80 @@ static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src)
                *((float*)ptr) = val;
            }
            break;
+        case VAR_STRING:
+            {
+                settingSetString(setting, (const char*)sbufPtr(src), sbufBytesRemaining(src));
+            }
+            break;
    }

    return true;
 }

+bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret)
+{
+    switch (cmdMSP) {
+
+#if defined(USE_NAV)
+    case MSP_WP:
+        mspFcWaypointOutCommand(dst, src);
+        *ret = MSP_RESULT_ACK;
+        break;
+#endif
+
+#if defined(USE_FLASHFS)
+    case MSP_DATAFLASH_READ:
+        mspFcDataFlashReadCommand(dst, src);
+        *ret = MSP_RESULT_ACK;
+        break;
+#endif
+
+    case MSP2_COMMON_SETTING:
+        *ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
+        break;
+
+    case MSP2_COMMON_SET_SETTING:
+        *ret = mspSetSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
+        break;
+
+#if defined(USE_OSD)
+    case MSP2_INAV_OSD_LAYOUTS:
+        if (sbufBytesRemaining(src) >= 1) {
+            uint8_t layout = sbufReadU8(src);
+            if (layout >= OSD_LAYOUT_COUNT) {
+                *ret = MSP_RESULT_ERROR;
+                break;
+            }
+            if (sbufBytesRemaining(src) >= 2) {
+                // Asking for an specific item in a layout
+                uint16_t item = sbufReadU16(src);
+                if (item >= OSD_ITEM_COUNT) {
+                    *ret = MSP_RESULT_ERROR;
+                    break;
+                }
+                sbufWriteU16(dst, osdConfig()->item_pos[layout][item]);
+            } else {
+                // Asking for an specific layout
+                for (unsigned ii = 0; ii < OSD_ITEM_COUNT; ii++) {
+                    sbufWriteU16(dst, osdConfig()->item_pos[layout][ii]);
+                }
+            }
+        } else {
+            // Return the number of layouts and items
+            sbufWriteU8(dst, OSD_LAYOUT_COUNT);
+            sbufWriteU8(dst, OSD_ITEM_COUNT);
+        }
+        *ret = MSP_RESULT_ACK;
+        break;
+#endif
+
+    default:
+        // Not handled
+        return false;
+    }
+    return true;
+}
+
 static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
 {
    UNUSED(src);
@ -2696,7 +2791,7 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
 */
 mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 {
-    int ret = MSP_RESULT_ACK;
+    mspResult_e ret = MSP_RESULT_ACK;
    sbuf_t *dst = &reply->buf;
    sbuf_t *src = &cmd->buf;
    const uint16_t cmdMSP = cmd->cmd;
@ -2712,23 +2807,11 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
        mspFc4waySerialCommand(dst, src, mspPostProcessFn);
        ret = MSP_RESULT_ACK;
 #endif
-#ifdef USE_NAV
-    } else if (cmdMSP == MSP_WP) {
-        mspFcWaypointOutCommand(dst, src);
-        ret = MSP_RESULT_ACK;
-#endif
-#ifdef USE_FLASHFS
-    } else if (cmdMSP == MSP_DATAFLASH_READ) {
-        mspFcDataFlashReadCommand(dst, src);
-        ret = MSP_RESULT_ACK;
-#endif
-    } else if (cmdMSP == MSP2_COMMON_SETTING) {
-        ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
-    } else if (cmdMSP == MSP2_COMMON_SET_SETTING) {
-        ret = mspSetSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
    } else {
+        if (!mspFCProcessInOutCommand(cmdMSP, dst, src, &ret)) {
            ret = mspFcProcessInCommand(cmdMSP, src);
        }
+    }

    // Process DONT_REPLY flag
    if (cmd->flags & MSP_FLAG_DONT_REPLY) {
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@ -79,7 +79,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
    { BOXOSDALT1, "OSD ALT 1", 42 },
    { BOXOSDALT2, "OSD ALT 2", 43 },
    { BOXOSDALT3, "OSD ALT 3", 44 },
-    { BOXBRAKING, "MC BRAKING", 45 },
+    { BOXNAVCRUISE, "NAV CRUISE", 45 },
+    { BOXBRAKING, "MC BRAKING", 46 },
    { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };

@ -184,12 +185,24 @@ void initActiveBoxIds(void)
        activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
    }

-    if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
+    const bool navReadyQuads = !STATE(FIXED_WING) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
+    const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
+    const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
+    if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) {
        activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
+    }
+
+    if (navReadyQuads || navReadyPlanes) {
        activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
        activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
        activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
+
+        if (feature(FEATURE_GPS)) {
            activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
+            if (STATE(FIXED_WING)) {
+                activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
+            }
+        }
    }

    if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
@ -292,6 +305,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)),        BOXFAILSAFE);
    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)),     BOXNAVALTHOLD);
    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)),     BOXNAVPOSHOLD);
+    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)),      BOXNAVCRUISE);
    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)),         BOXNAVRTH);
    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)),          BOXNAVWP);
    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)),     BOXAIRMODE);
@ -331,7 +345,7 @@ uint16_t packSensorStatus(void)
            IS_ENABLED(sensors(SENSOR_MAG))     << 2 |
            IS_ENABLED(sensors(SENSOR_GPS))     << 3 |
            IS_ENABLED(sensors(SENSOR_RANGEFINDER))   << 4 |
-            //IS_ENABLED(sensors(SENSOR_OPFLOW))  << 5 |
+            IS_ENABLED(sensors(SENSOR_OPFLOW))  << 5 |
            IS_ENABLED(sensors(SENSOR_PITOT))   << 6;

    // Hardware failure indication bit
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@ -32,7 +32,6 @@
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/stack_check.h"
-#include "drivers/vtx_common.h"

 #include "fc/cli.h"
 #include "fc/config.h"
@ -58,6 +57,7 @@
 #include "io/pwmdriver_i2c.h"
 #include "io/serial.h"
 #include "io/rcdevice_cam.h"
+#include "io/vtx.h"

 #include "msp/msp_serial.h"

@ -108,7 +108,7 @@ void taskUpdateBattery(timeUs_t currentTimeUs)
        batteryUpdate(BatMonitoringTimeSinceLastServiced);
    if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
        powerMeterUpdate(BatMonitoringTimeSinceLastServiced);
-        sagCompensatedVBatUpdate(currentTimeUs);
+        sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced);
    }
 #endif
    batMonitoringLastServiced = currentTimeUs;
@ -273,19 +273,6 @@ void taskUpdateOsd(timeUs_t currentTimeUs)
 }
 #endif

-#ifdef VTX_CONTROL
-// Everything that listens to VTX devices
-void taskVtxControl(timeUs_t currentTimeUs)
-{
-    if (ARMING_FLAG(ARMED))
-        return;
-
-#ifdef VTX_COMMON
-    vtxCommonProcess(currentTimeUs);
-#endif
-}
-#endif
-
 void fcTasksInit(void)
 {
    schedulerInit();
@ -369,8 +356,8 @@ void fcTasksInit(void)
 #ifdef USE_OPTICAL_FLOW
    setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
 #endif
-#ifdef VTX_CONTROL
-#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP)
+#ifdef USE_VTX_CONTROL
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
    setTaskEnabled(TASK_VTXCTRL, true);
 #endif
 #endif
@ -616,10 +603,10 @@ cfTask_t cfTasks[TASK_COUNT] = {
    },
 #endif

-#ifdef VTX_CONTROL
+#ifdef USE_VTX_CONTROL
    [TASK_VTXCTRL] = {
        .taskName = "VTXCTRL",
-        .taskFunc = taskVtxControl,
+        .taskFunc = vtxUpdate,
        .desiredPeriod = TASK_PERIOD_HZ(5),          // 5Hz @200msec
        .staticPriority = TASK_PRIORITY_IDLE,
    },
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@ -41,6 +41,7 @@

 #include "navigation/navigation.h"

+#include "flight/mixer.h"
 #include "flight/pid.h"

 #include "io/beeper.h"
@ -58,6 +59,15 @@ static uint8_t adjustmentStateMask = 0;

 #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))

+#define RC_ADJUSTMENT_EXPO_MIN 0
+#define RC_ADJUSTMENT_EXPO_MAX 100
+
+#define RC_ADJUSTMENT_MANUAL_RATE_MIN 0
+#define RC_ADJUSTMENT_MANUAL_RATE_MAX 100
+
+#define RC_ADJUSTMENT_PID_MIN 0
+#define RC_ADJUSTMENT_PID_MAX 200
+
 // sync with adjustmentFunction_e
 static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
    {
@ -184,6 +194,74 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
        .adjustmentFunction = ADJUSTMENT_PITCH_BOARD_ALIGNMENT,
        .mode = ADJUSTMENT_MODE_STEP,
        .data = { .stepConfig = { .step = 5 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_LEVEL_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_LEVEL_I,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_LEVEL_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_POS_XY_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_POS_XY_I,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_POS_XY_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_POS_Z_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_POS_Z_I,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_POS_Z_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_HEADING_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_VEL_XY_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_VEL_XY_I,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_VEL_XY_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_VEL_Z_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_VEL_Z_I,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_VEL_Z_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 5 }}
 #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
    }, {
        .adjustmentFunction = ADJUSTMENT_PROFILE,
@ -246,6 +324,35 @@ static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustm
 }
 #endif

+static void applyAdjustmentU8(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta, int low, int high)
+{
+    int newValue = constrain((int)(*val) + delta, low, high);
+    *val = newValue;
+    blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue);
+}
+
+static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta, int low, int high)
+{
+    int newValue = constrain((int)(*val) + delta, low, high);
+    *val = newValue;
+    blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue);
+}
+
+static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
+{
+    applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_EXPO_MIN, RC_ADJUSTMENT_EXPO_MAX);
+}
+
+static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
+{
+    return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX);
+}
+
+static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
+{
+    applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
+}
+
 static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
 {
    if (delta > 0) {
@ -253,39 +360,25 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
    } else {
        beeperConfirmationBeeps(1);
    }
-    int newValue;
    switch (adjustmentFunction) {
        case ADJUSTMENT_RC_EXPO:
-            newValue = constrain((int)controlRateConfig->stabilized.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            controlRateConfig->stabilized.rcExpo8 = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
+            applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlRateConfig->stabilized.rcExpo8, delta);
            break;
        case ADJUSTMENT_RC_YAW_EXPO:
-            newValue = constrain((int)controlRateConfig->stabilized.rcYawExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            controlRateConfig->stabilized.rcYawExpo8 = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_YAW_EXPO, newValue);
+            applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta);
            break;
        case ADJUSTMENT_MANUAL_RC_EXPO:
-            newValue = constrain((int)controlRateConfig->manual.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            controlRateConfig->manual.rcExpo8 = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_RC_EXPO, newValue);
+            applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlRateConfig->manual.rcExpo8, delta);
            break;
        case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
-            newValue = constrain((int)controlRateConfig->manual.rcYawExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            controlRateConfig->manual.rcYawExpo8 = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_RC_YAW_EXPO, newValue);
+            applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta);
            break;
        case ADJUSTMENT_THROTTLE_EXPO:
-            newValue = constrain((int)controlRateConfig->throttle.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
-            controlRateConfig->throttle.rcExpo8 = newValue;
-            generateThrottleCurve(controlRateConfig);
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
+            applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlRateConfig->throttle.rcExpo8, delta);
            break;
        case ADJUSTMENT_PITCH_ROLL_RATE:
        case ADJUSTMENT_PITCH_RATE:
-            newValue = constrain((int)controlRateConfig->stabilized.rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
-            controlRateConfig->stabilized.rates[FD_PITCH] = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
+            applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
            if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
                schedulePidGainsUpdate();
                break;
@ -294,41 +387,29 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_RATE:
-            newValue = constrain((int)controlRateConfig->stabilized.rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
-            controlRateConfig->stabilized.rates[FD_ROLL] = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
+            applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
        case ADJUSTMENT_MANUAL_ROLL_RATE:
-            newValue = constrain((int)controlRateConfig->manual.rates[FD_ROLL] + delta, 0, 100);
-            controlRateConfig->manual.rates[FD_ROLL] = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_ROLL_RATE, newValue);
+            applyAdjustmentManualRate(ADJUSTMENT_MANUAL_ROLL_RATE, &controlRateConfig->manual.rates[FD_ROLL], delta);
            if (adjustmentFunction == ADJUSTMENT_MANUAL_ROLL_RATE)
                break;
            // follow though for combined ADJUSTMENT_MANUAL_PITCH_ROLL_RATE
            FALLTHROUGH;
        case ADJUSTMENT_MANUAL_PITCH_RATE:
-            newValue = constrain((int)controlRateConfig->manual.rates[FD_PITCH] + delta, 0, 100);
-            controlRateConfig->manual.rates[FD_PITCH] = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_PITCH_RATE, newValue);
+            applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta);
            break;
        case ADJUSTMENT_YAW_RATE:
-            newValue = constrain((int)controlRateConfig->stabilized.rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
-            controlRateConfig->stabilized.rates[FD_YAW] = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
+            applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_MANUAL_YAW_RATE:
-            newValue = constrain((int)controlRateConfig->manual.rates[FD_YAW] + delta, 0, 100);
-            controlRateConfig->manual.rates[FD_YAW] = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_YAW_RATE, newValue);
+            applyAdjustmentManualRate(ADJUSTMENT_MANUAL_YAW_RATE, &controlRateConfig->manual.rates[FD_YAW], delta);
            break;
        case ADJUSTMENT_PITCH_ROLL_P:
        case ADJUSTMENT_PITCH_P:
-            newValue = constrain((int)pidBank()->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_PITCH].P = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
+            applyAdjustmentPID(ADJUSTMENT_PITCH_P, &pidBankMutable()->pid[PID_PITCH].P, delta);
            if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
                schedulePidGainsUpdate();
                break;
@ -337,16 +418,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_P:
-            newValue = constrain((int)pidBank()->pid[PID_ROLL].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_ROLL].P = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
+            applyAdjustmentPID(ADJUSTMENT_ROLL_P, &pidBankMutable()->pid[PID_ROLL].P, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_PITCH_ROLL_I:
        case ADJUSTMENT_PITCH_I:
-            newValue = constrain((int)pidBank()->pid[PID_PITCH].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_PITCH].I = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
+            applyAdjustmentPID(ADJUSTMENT_PITCH_I, &pidBankMutable()->pid[PID_PITCH].I, delta);
            if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
                schedulePidGainsUpdate();
                break;
@ -355,16 +432,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_I:
-            newValue = constrain((int)pidBank()->pid[PID_ROLL].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_ROLL].I = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
+            applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_PITCH_ROLL_D:
        case ADJUSTMENT_PITCH_D:
-            newValue = constrain((int)pidBank()->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_PITCH].D = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
+            applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
            if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
                schedulePidGainsUpdate();
                break;
@ -373,38 +446,26 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_D:
-            newValue = constrain((int)pidBank()->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_ROLL].D = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
+            applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_YAW_P:
-            newValue = constrain((int)pidBank()->pid[PID_YAW].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_YAW].P = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
+            applyAdjustmentPID(ADJUSTMENT_YAW_P, &pidBankMutable()->pid[PID_YAW].P, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_YAW_I:
-            newValue = constrain((int)pidBank()->pid[PID_YAW].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_YAW].I = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
+            applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_YAW_D:
-            newValue = constrain((int)pidBank()->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            pidBankMutable()->pid[PID_YAW].D = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
+            applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_NAV_FW_CRUISE_THR:
-            newValue = constrain((int16_t)navConfig()->fw.cruise_throttle + delta, 1000, 2000);
-            navConfigMutable()->fw.cruise_throttle = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_NAV_FW_CRUISE_THR, newValue);
+            applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000);
            break;
        case ADJUSTMENT_NAV_FW_PITCH2THR:
-            newValue = constrain((int8_t)navConfig()->fw.pitch_to_throttle + delta, 0, 100);
-            navConfigMutable()->fw.pitch_to_throttle = newValue;
-            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_NAV_FW_PITCH2THR, newValue);
+            applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100);
            break;
        case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
            updateBoardAlignment(delta, 0);
@ -414,6 +475,73 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
            updateBoardAlignment(0, delta);
            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_BOARD_ALIGNMENT, boardAlignment()->pitchDeciDegrees);
            break;
+        case ADJUSTMENT_LEVEL_P:
+            applyAdjustmentPID(ADJUSTMENT_LEVEL_P, &pidBankMutable()->pid[PID_LEVEL].P, delta);
+            // TODO: Need to call something to take it into account?
+            break;
+        case ADJUSTMENT_LEVEL_I:
+            applyAdjustmentPID(ADJUSTMENT_LEVEL_I, &pidBankMutable()->pid[PID_LEVEL].I, delta);
+            // TODO: Need to call something to take it into account?
+            break;
+        case ADJUSTMENT_LEVEL_D:
+            applyAdjustmentPID(ADJUSTMENT_LEVEL_D, &pidBankMutable()->pid[PID_LEVEL].D, delta);
+            // TODO: Need to call something to take it into account?
+            break;
+        case ADJUSTMENT_POS_XY_P:
+            applyAdjustmentPID(ADJUSTMENT_POS_XY_P, &pidBankMutable()->pid[PID_POS_XY].P, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_POS_XY_I:
+            applyAdjustmentPID(ADJUSTMENT_POS_XY_I, &pidBankMutable()->pid[PID_POS_XY].I, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_POS_XY_D:
+            applyAdjustmentPID(ADJUSTMENT_POS_XY_D, &pidBankMutable()->pid[PID_POS_XY].D, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_POS_Z_P:
+            applyAdjustmentPID(ADJUSTMENT_POS_Z_P, &pidBankMutable()->pid[PID_POS_Z].P, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_POS_Z_I:
+            applyAdjustmentPID(ADJUSTMENT_POS_Z_I, &pidBankMutable()->pid[PID_POS_Z].I, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_POS_Z_D:
+            applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_HEADING_P:
+            applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta);
+            // TODO: navigationUsePIDs()?
+            break;
+        case ADJUSTMENT_VEL_XY_P:
+            applyAdjustmentPID(ADJUSTMENT_VEL_XY_P, &pidBankMutable()->pid[PID_VEL_XY].P, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_VEL_XY_I:
+            applyAdjustmentPID(ADJUSTMENT_VEL_XY_I, &pidBankMutable()->pid[PID_VEL_XY].I, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_VEL_XY_D:
+            applyAdjustmentPID(ADJUSTMENT_VEL_XY_D, &pidBankMutable()->pid[PID_VEL_XY].D, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_VEL_Z_P:
+            applyAdjustmentPID(ADJUSTMENT_VEL_Z_P, &pidBankMutable()->pid[PID_VEL_Z].P, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_VEL_Z_I:
+            applyAdjustmentPID(ADJUSTMENT_VEL_Z_I, &pidBankMutable()->pid[PID_VEL_Z].I, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_VEL_Z_D:
+            applyAdjustmentPID(ADJUSTMENT_VEL_Z_D, &pidBankMutable()->pid[PID_VEL_Z].D, delta);
+            navigationUsePIDs();
+            break;
+        case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
+            applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX);
+            break;
        default:
            break;
    };
@ -442,12 +570,10 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)

 #define RESET_FREQUENCY_2HZ (1000 / 2)

-void processRcAdjustments(controlRateConfig_t *controlRateConfig)
+void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxData)
 {
    const uint32_t now = millis();

-    const bool canUseRxData = rxIsReceivingSignal();
-
    for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
        adjustmentState_t * const adjustmentState = &adjustmentStates[adjustmentIndex];

@ -508,16 +634,28 @@ void resetAdjustmentStates(void)
    memset(adjustmentStates, 0, sizeof(adjustmentStates));
 }

-void updateAdjustmentStates(void)
+void updateAdjustmentStates(bool canUseRxData)
 {
    for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
        const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index);
-
-        if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
-
        const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];

+        if (canUseRxData && isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
            configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
+        } else {
+            adjustmentState_t * const adjustmentState = &adjustmentStates[index];
+            if (adjustmentState->config == adjustmentConfig) {
+                adjustmentState->config = NULL;
            }
        }
    }
+}
+
+bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction) {
+    for (uint8_t index = 0; index < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; ++index) {
+        if (adjustmentStates[index].config && adjustmentStates[index].config->adjustmentFunction == adjustmentFunction) {
+            return true;
+        }
+    }
+    return false;
+}
--- a/src/main/fc/rc_adjustments.h
+++ b/src/main/fc/rc_adjustments.h
@ -57,8 +57,25 @@ typedef enum {
    ADJUSTMENT_NAV_FW_PITCH2THR         = 29,
    ADJUSTMENT_ROLL_BOARD_ALIGNMENT     = 30,
    ADJUSTMENT_PITCH_BOARD_ALIGNMENT    = 31,
+    ADJUSTMENT_LEVEL_P                  = 32,
+    ADJUSTMENT_LEVEL_I                  = 33,
+    ADJUSTMENT_LEVEL_D                  = 34,
+    ADJUSTMENT_POS_XY_P                 = 35,
+    ADJUSTMENT_POS_XY_I                 = 36,
+    ADJUSTMENT_POS_XY_D                 = 37,
+    ADJUSTMENT_POS_Z_P                  = 38,
+    ADJUSTMENT_POS_Z_I                  = 39,
+    ADJUSTMENT_POS_Z_D                  = 40,
+    ADJUSTMENT_HEADING_P                = 41,
+    ADJUSTMENT_VEL_XY_P                 = 42,
+    ADJUSTMENT_VEL_XY_I                 = 43,
+    ADJUSTMENT_VEL_XY_D                 = 44,
+    ADJUSTMENT_VEL_Z_P                  = 45,
+    ADJUSTMENT_VEL_Z_I                  = 46,
+    ADJUSTMENT_VEL_Z_D                  = 47,
+    ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48,
 #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
-    ADJUSTMENT_PROFILE                  = 32,
+    ADJUSTMENT_PROFILE                  = 49,
 #endif
    ADJUSTMENT_FUNCTION_COUNT // must be last
 } adjustmentFunction_e;
@ -118,6 +135,7 @@ typedef struct adjustmentState_s {
 PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges);

 void resetAdjustmentStates(void);
-void updateAdjustmentStates(void);
+void updateAdjustmentStates(bool canUseRxData);
 struct controlRateConfig_s;
-void processRcAdjustments(struct controlRateConfig_s *controlRateConfig);
+void processRcAdjustments(struct controlRateConfig_s *controlRateConfig, bool canUseRxData);
+bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction);
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@ -63,7 +63,7 @@

 #define AIRMODE_DEADBAND 25
 #define MIN_RC_TICK_INTERVAL_MS             20
-#define DEFAULT_RC_SWITCH_DISARM_DELAY_MS   150     // Wait at least 150ms before disarming via switch
+#define DEFAULT_RC_SWITCH_DISARM_DELAY_MS   250     // Wait at least 250ms before disarming via switch

 stickPositions_e rcStickPositions;

@ -232,7 +232,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
    }

    // actions during not armed
-    int i = 0;

    // GYRO calibration
    if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
@ -257,16 +256,38 @@ void processRcStickPositions(throttleStatus_e throttleStatus)

    // Multiple configuration profiles
    if (feature(FEATURE_TX_PROF_SEL)) {
+
+        uint8_t i = 0;
+
        if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
            i = 1;
        else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
            i = 2;
        else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
            i = 3;
+
        if (i) {
            setConfigProfileAndWriteEEPROM(i - 1);
            return;
        }
+
+        i = 0;
+
+        // Multiple battery configuration profiles
+        if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
+            i = 1;
+        else if (rcSticks == THR_HI + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
+            i = 2;
+        else if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
+            i = 3;
+
+        if (i) {
+            setConfigBatteryProfileAndWriteEEPROM(i - 1);
+            batteryDisableProfileAutoswitch();
+            activateBatteryProfile();
+            return;
+        }
+
    }

    // Save config
@ -331,4 +352,3 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 int32_t getRcStickDeflection(int32_t axis) {
    return MIN(ABS(rcData[axis] - PWM_RANGE_MIDDLE), 500);
 }
-
--- a/src/main/fc/rc_modes.c
+++ b/src/main/fc/rc_modes.c
@ -161,6 +161,7 @@ void updateUsedModeActivationConditionFlags(void)
 #ifdef USE_NAV
    isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
                        isModeActivationConditionPresent(BOXNAVRTH) ||
+                        isModeActivationConditionPresent(BOXNAVCRUISE) ||
                        isModeActivationConditionPresent(BOXNAVWP);
 #endif
 }
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@ -59,7 +59,8 @@ typedef enum {
    BOXOSDALT1      = 32,
    BOXOSDALT2      = 33,
    BOXOSDALT3      = 34,
-    BOXBRAKING      = 35,
+    BOXNAVCRUISE    = 35,
+    BOXBRAKING      = 36,
    CHECKBOX_ITEM_COUNT
 } boxId_e;

--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@ -76,7 +76,7 @@ typedef enum {
    FAILSAFE_MODE   = (1 << 9),
    AUTO_TUNE       = (1 << 10), // old G-Tune
    NAV_WP_MODE     = (1 << 11),
-    UNUSED_MODE2    = (1 << 12),
+    NAV_CRUISE_MODE = (1 << 12),
    FLAPERON        = (1 << 13),
    TURN_ASSISTANT  = (1 << 14),
 } flightModeFlags_e;
--- a/src/main/fc/settings.c
+++ b/src/main/fc/settings.c
@ -10,7 +10,7 @@

 #include "settings_generated.c"

-void setting_get_name(const setting_t *val, char *buf)
+void settingGetName(const setting_t *val, char *buf)
 {
 	uint8_t bpos = 0;
 	uint16_t n = 0;
@ -52,24 +52,24 @@ void setting_get_name(const setting_t *val, char *buf)
 	buf[bpos] = '\0';
 }

-bool setting_name_contains(const setting_t *val, char *buf, const char *cmdline)
+bool settingNameContains(const setting_t *val, char *buf, const char *cmdline)
 {
-	setting_get_name(val, buf);
+	settingGetName(val, buf);
 	return strstr(buf, cmdline) != NULL;
 }

-bool setting_name_exact_match(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length)
+bool settingNameIsExactMatch(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length)
 {
-	setting_get_name(val, buf);
+	settingGetName(val, buf);
 	return sl_strncasecmp(cmdline, buf, strlen(buf)) == 0 && var_name_length == strlen(buf);
 }

-const setting_t *setting_find(const char *name)
+const setting_t *settingFind(const char *name)
 {
 	char buf[SETTING_MAX_NAME_LENGTH];
 	for (int ii = 0; ii < SETTINGS_TABLE_COUNT; ii++) {
 		const setting_t *setting = &settingsTable[ii];
-		setting_get_name(setting, buf);
+		settingGetName(setting, buf);
 		if (strcmp(buf, name) == 0) {
 			return setting;
 		}
@ -77,7 +77,7 @@ const setting_t *setting_find(const char *name)
 	return NULL;
 }

-size_t setting_get_value_size(const setting_t *val)
+size_t settingGetValueSize(const setting_t *val)
 {
 	switch (SETTING_TYPE(val)) {
 		case VAR_UINT8:
@ -92,11 +92,13 @@ size_t setting_get_value_size(const setting_t *val)
 			FALLTHROUGH;
 		case VAR_FLOAT:
 			return 4;
+		case VAR_STRING:
+			return settingGetMax(val);
 	}
 	return 0; // Unreachable
 }

-pgn_t setting_get_pgn(const setting_t *val)
+pgn_t settingGetPgn(const setting_t *val)
 {
 	uint16_t pos = val - (const setting_t *)settingsTable;
 	uint16_t acc = 0;
@ -118,23 +120,25 @@ 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 BATTERY_CONFIG_VALUE:
+        return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile();
    }
    return 0;
 }

-void *setting_get_value_pointer(const setting_t *val)
+void *settingGetValuePointer(const setting_t *val)
 {
-    const pgRegistry_t *pg = pgFind(setting_get_pgn(val));
+    const pgRegistry_t *pg = pgFind(settingGetPgn(val));
    return pg->address + getValueOffset(val);
 }

-const void * setting_get_copy_value_pointer(const setting_t *val)
+const void * settingGetCopyValuePointer(const setting_t *val)
 {
-    const pgRegistry_t *pg = pgFind(setting_get_pgn(val));
+    const pgRegistry_t *pg = pgFind(settingGetPgn(val));
    return pg->copy + getValueOffset(val);
 }

-setting_min_t setting_get_min(const setting_t *val)
+setting_min_t settingGetMin(const setting_t *val)
 {
 	if (SETTING_MODE(val) == MODE_LOOKUP) {
 		return 0;
@ -142,10 +146,37 @@ setting_min_t setting_get_min(const setting_t *val)
 	return settingMinMaxTable[SETTING_INDEXES_GET_MIN(val)];
 }

-setting_max_t setting_get_max(const setting_t *val)
+setting_max_t settingGetMax(const setting_t *val)
 {
 	if (SETTING_MODE(val) == MODE_LOOKUP) {
 		return settingLookupTables[val->config.lookup.tableIndex].valueCount - 1;
 	}
 	return settingMinMaxTable[SETTING_INDEXES_GET_MAX(val)];
 }
+
+const char * settingGetString(const setting_t *val)
+{
+	if (SETTING_TYPE(val) == VAR_STRING) {
+		return settingGetValuePointer(val);
+	}
+	return NULL;
+}
+
+void settingSetString(const setting_t *val, const char *s, size_t size)
+{
+	if (SETTING_TYPE(val) == VAR_STRING) {
+		char *p = settingGetValuePointer(val);
+		size_t copySize = MIN(size, settingGetMax(val));
+		memcpy(p, s, copySize);
+		p[copySize] = '\0';
+	}
+}
+
+setting_max_t settingGetStringMaxLength(const setting_t *val)
+{
+	if (SETTING_TYPE(val) == VAR_STRING) {
+		// Max string length is stored as its max
+		return settingGetMax(val);
+	}
+	return 0;
+}
--- a/src/main/fc/settings.h
+++ b/src/main/fc/settings.h
@ -27,6 +27,7 @@ typedef enum {
    VAR_INT16 = (3 << SETTING_TYPE_OFFSET),
    VAR_UINT32 = (4 << SETTING_TYPE_OFFSET),
    VAR_FLOAT = (5 << SETTING_TYPE_OFFSET), // 0x05
+    VAR_STRING = (6 << SETTING_TYPE_OFFSET) // 0x06
 } setting_type_e;

 typedef enum {
@ -34,6 +35,7 @@ typedef enum {
    MASTER_VALUE = (0 << SETTING_SECTION_OFFSET),
    PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET),
    CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20
+    BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET),
 } setting_section_e;

 typedef enum {
@ -73,28 +75,38 @@ static inline setting_type_e SETTING_TYPE(const setting_t *s) { return s->type &
 static inline setting_section_e SETTING_SECTION(const setting_t *s) { return s->type & SETTING_SECTION_MASK; }
 static inline setting_mode_e SETTING_MODE(const setting_t *s) { return s->type & SETTING_MODE_MASK; }

-void setting_get_name(const setting_t *val, char *buf);
-bool setting_name_contains(const setting_t *val, char *buf, const char *cmdline);
-bool setting_name_exact_match(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length);
+void settingGetName(const setting_t *val, char *buf);
+bool settingNameContains(const setting_t *val, char *buf, const char *cmdline);
+bool settingNameIsExactMatch(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length);
 // Returns a setting_t with the exact name (case sensitive), or
 // NULL if no setting with that name exists.
-const setting_t *setting_find(const char *name);
+const setting_t *settingFind(const char *name);
 // Returns the size in bytes of the setting value.
-size_t setting_get_value_size(const setting_t *val);
-pgn_t setting_get_pgn(const setting_t *val);
+size_t settingGetValueSize(const setting_t *val);
+pgn_t settingGetPgn(const setting_t *val);
 // Returns a pointer to the actual value stored by
 // the setting_t. The returned value might be modified.
-void * setting_get_value_pointer(const setting_t *val);
+void * settingGetValuePointer(const setting_t *val);
 // Returns a pointer to the backed up copy of the value. Note that
 // this will contain random garbage unless a copy of the parameter
 // group for the value has been manually performed. Currently, this
 // is only used by cli.c during config dumps.
-const void * setting_get_copy_value_pointer(const setting_t *val);
+const void * settingGetCopyValuePointer(const setting_t *val);
 // Returns the minimum valid value for the given setting_t. setting_min_t
 // depends on the target and build options, but will always be a signed
 // integer (e.g. intxx_t,)
-setting_min_t setting_get_min(const setting_t *val);
+setting_min_t settingGetMin(const setting_t *val);
 // Returns the maximum valid value for the given setting_t. setting_max_t
 // depends on the target and build options, but will always be an unsigned
 // integer (e.g. uintxx_t,)
-setting_max_t setting_get_max(const setting_t *val);
+setting_max_t settingGetMax(const setting_t *val);
+// Returns the setting value as a const char * iff the setting is of type
+// VAR_STRING. Otherwise it returns NULL.
+const char * settingGetString(const setting_t *val);
+// Sets the value for the given string setting. Size indicates the number of
+// bytes in the string without the '\0' terminator (i.e. its strlen()).
+// If the setting is not of type VAR_STRING, this function does nothing.
+void settingSetString(const setting_t *val, const char *s, size_t size);
+// Returns the max string length (without counting the '\0' terminator)
+// for setting of type VAR_STRING. Otherwise it returns 0.
+setting_max_t settingGetStringMaxLength(const setting_t *val);
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@ -59,6 +59,9 @@ tables:
  - name: osd_stats_energy_unit
    values: ["MAH", "WH"]
    enum: osd_stats_energy_unit_e
+  - name: osd_video_system
+    values: ["AUTO", "PAL", "NTSC"]
+    enum: videoSystem_e
  - name: frsky_unit
    values: ["METRIC", "IMPERIAL"]
    enum: frskyUnit_e
@ -67,7 +70,9 @@ tables:
  - name: i2c_speed
    values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
  - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
+      "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE",
+      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO"]
  - name: async_mode
    values: ["NONE", "GYRO", "ALL"]
  - name: aux_operator
@ -85,6 +90,9 @@ tables:
  - name: bat_capacity_unit
    values: ["MAH", "MWH"]
    enum: batCapacityUnit_e
+  - name: bat_voltage_source
+    values: ["RAW", "SAG_COMP"]
+    enum: batVoltageSource_e
  - name: smartport_fuel_unit
    values: ["PERCENT", "MAH", "MWH"]
    enum: smartportFuelUnit_e
@ -94,6 +102,9 @@ tables:
  - name: tz_automatic_dst
    values: ["OFF", "EU", "USA"]
    enum: tz_automatic_dst_e
+  - name: vtx_low_power_disarm
+    values: ["OFF", "ON", "UNTIL_FIRST_ARM"]
+    enum: vtxLowerPowerDisarm_e

 groups:
  - name: PG_GYRO_CONFIG
@ -423,9 +434,6 @@ groups:
      - name: motor_pwm_protocol
        field: motorPwmProtocol
        table: motor_pwm_protocol
-      - name: throttle_vbat_compensation
-        field: throttleVBatCompensation
-        type: bool

  - name: PG_FAILSAFE_CONFIG
    type: failsafeConfig_t
@ -501,15 +509,53 @@ groups:
        min: -1800
        max: 3600

-  - name: PG_BATTERY_CONFIG
-    type: batteryConfig_t
+  - name: PG_BATTERY_METERS_CONFIG
+    type: batteryMetersConfig_t
    headers: ["sensors/battery.h"]
    members:
      - name: vbat_scale
-        field: voltage.scale
+        field: voltage_scale
        condition: USE_ADC
        min: VBAT_SCALE_MIN
        max: VBAT_SCALE_MAX
+      - name: current_meter_scale
+        field: current.scale
+        min: -10000
+        max: 10000
+      - name: current_meter_offset
+        field: current.offset
+        min: -32768
+        max: 32767
+      - name: current_meter_type
+        field: current.type
+        table: current_sensor
+        type: uint8_t
+      - name: bat_voltage_src
+        field: voltageSource
+        table: bat_voltage_source
+        type: uint8_t
+      - name: cruise_power
+        field: cruise_power
+        min: 0
+        max: 4294967295
+      - name: idle_power
+        field: idle_power
+        min: 0
+        max: 65535
+      - name: rth_energy_margin
+        min: 0
+        max: 100
+
+  - name: PG_BATTERY_PROFILES
+    type: batteryProfile_t
+    headers: ["sensors/battery.h"]
+    value_type: BATTERY_CONFIG_VALUE
+    members:
+      - name: bat_cells
+        field: cells
+        condition: USE_ADC
+        min: 0
+        max: 8
      - name: vbat_cell_detect_voltage
        field: voltage.cellDetect
        condition: USE_ADC
@ -546,18 +592,6 @@ groups:
        field: capacity.unit
        table: bat_capacity_unit
        type: uint8_t
-      - name: current_meter_scale
-        field: current.scale
-        min: -10000
-        max: 10000
-      - name: current_meter_offset
-        field: current.offset
-        min: -3300
-        max: 3300
-      - name: current_meter_type
-        field: current.type
-        table: current_sensor
-        type: uint8_t

  - name: PG_MIXER_CONFIG
    type: mixerConfig_t
@ -579,6 +613,10 @@ groups:
        field: appliedMixerPreset
        min: -1
        max: INT16_MAX
+      - name: fw_min_throttle_down_pitch
+        field: fwMinThrottleDownPitchAngle
+        min: 0
+        max: 450

  - name: PG_MOTOR_3D_CONFIG
    type: flight3DConfig_t
@ -1082,6 +1120,9 @@ groups:
      - name: inav_use_gps_velned
        field: use_gps_velned
        type: bool
+      - name: inav_allow_dead_reckoning
+        field: allow_dead_reckoning
+        type: bool
      - name: inav_reset_altitude
        field: reset_altitude_type
        table: reset_type
@ -1100,6 +1141,14 @@ groups:
        field: w_z_surface_v
        min: 0
        max: 10
+      - name: inav_w_xy_flow_p
+        field: w_xy_flow_p
+        min: 0
+        max: 10
+      - name: inav_w_xy_flow_v
+        field: w_xy_flow_v
+        min: 0
+        max: 10
      - name: inav_w_z_baro_p
        field: w_z_baro_p
        min: 0
@ -1229,6 +1278,9 @@ groups:
      - name: nav_rth_altitude
        field: general.rth_altitude
        max: 65000
+      - name: nav_rth_home_altitude
+        field: general.rth_home_altitude
+        max: 65000

      - name: nav_mc_bank_angle
        field: mc.max_bank_angle
@ -1306,6 +1358,10 @@ groups:
        field: fw.loiter_radius
        min: 0
        max: 10000
+      - name: nav_fw_cruise_speed
+        field: fw.cruise_speed
+        min: 0
+        max: 65535

      - name: nav_fw_land_dive_angle
        field: fw.land_dive_angle
@ -1360,10 +1416,17 @@ groups:
        field: fw.launch_climb_angle
        min: 5
        max: 45
+      - name: nav_fw_cruise_yaw_rate
+        field: fw.cruise_yaw_rate
+        min: 0
+        max: 60
+      - name: nav_fw_allow_manual_thr_increase
+        field: fw.allow_manual_thr_increase
+        type: bool

  - name: PG_TELEMETRY_CONFIG
    type: telemetryConfig_t
-    headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky.h"]
+    headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h"]
    condition: USE_TELEMETRY
    members:
      - name: telemetry_switch
@ -1454,13 +1517,13 @@ groups:

  - name: PG_OSD_CONFIG
    type: osdConfig_t
-    headers: ["io/osd.h"]
+    headers: ["io/osd.h", "drivers/vcd.h"]
    condition: USE_OSD
    members:
      - name: osd_video_system
+        table: osd_video_system
        field: video_system
-        min: 0
-        max: 2
+        type: uint8_t
      - name: osd_row_shiftdown
        field: row_shiftdown
        min: 0
@ -1522,6 +1585,10 @@ groups:
        min: 0
        max: 1

+      - name: osd_estimations_wind_compensation
+        field: estimations_wind_compensation
+        type: bool
+
  - name: PG_SYSTEM_CONFIG
    type: systemConfig_t
    headers: ["fc/config.h"]
@ -1556,6 +1623,7 @@ groups:
      - name: input_filtering_mode
        field: pwmRxInputFilteringMode
        type: bool
+      - name: name

  - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG
    type: modeActivationOperatorConfig_t
@ -1600,3 +1668,43 @@ groups:
      - name: display_force_sw_blink
        field: force_sw_blink
        type: bool
+
+  - name: PG_VTX_CONFIG
+    type: vtxConfig_t
+    headers: ["io/vtx_control.h"]
+    condition: USE_VTX_CONTROL && USE_VTX_COMMON
+    members:
+      - name: vtx_halfduplex
+        field: halfDuplex
+        type: bool
+
+  - name: PG_VTX_SETTINGS_CONFIG
+    type: vtxSettingsConfig_t
+    headers: ["drivers/vtx_common.h", "io/vtx.h"]
+    members:
+      - name: vtx_band
+        field: band
+        min: VTX_SETTINGS_NO_BAND
+        max: VTX_SETTINGS_MAX_BAND
+      - name: vtx_channel
+        field: channel
+        min: VTX_SETTINGS_MIN_CHANNEL
+        max: VTX_SETTINGS_MAX_CHANNEL
+      - name: vtx_power
+        field: power
+        min: VTX_SETTINGS_MIN_POWER
+        max: VTX_SETTINGS_MAX_POWER
+      - name: vtx_low_power_disarm
+        field: lowPowerDisarm
+        table: vtx_low_power_disarm
+        type: uint8_t
+      - name: vtx_freq
+        field: freq
+        min: VTX_SETTINGS_MIN_FREQUENCY_MHZ
+        max: VTX_SETTINGS_MAX_FREQUENCY_MHZ
+        condition: VTX_SETTINGS_FREQCMD
+      - name: vtx_pit_mode_freq
+        field: pitModeFreq
+        min: VTX_SETTINGS_MIN_FREQUENCY_MHZ
+        max: VTX_SETTINGS_MAX_FREQUENCY_MHZ
+        condition: VTX_SETTINGS_FREQCMD
--- a/src/main/fc/stats.c
+++ b/src/main/fc/stats.c
@ -30,8 +30,14 @@ PG_RESET_TEMPLATE(statsConfig_t, statsConfig,

 static uint32_t arm_millis;
 static uint32_t arm_distance_cm;
+
 #ifdef USE_ADC
 static uint32_t arm_mWhDrawn;
+static uint32_t flyingEnergy; // energy drawn during flying up to last disarm (ARMED) mWh
+
+uint32_t getFlyingEnergy() {
+    return flyingEnergy;
+}
 #endif

 void statsOnArm(void)
@ -51,8 +57,11 @@ void statsOnDisarm(void)
            statsConfigMutable()->stats_total_time += dt;   //[s]
            statsConfigMutable()->stats_total_dist += (getTotalTravelDistance() - arm_distance_cm) / 100;   //[m]
 #ifdef USE_ADC
-            if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))
-                statsConfigMutable()->stats_total_energy += getMWhDrawn() - arm_mWhDrawn;
+            if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
+                const uint32_t energy = getMWhDrawn() - arm_mWhDrawn;
+                statsConfigMutable()->stats_total_energy += energy;
+                flyingEnergy += energy;
+            }
 #endif
            writeEEPROM();
        }
--- a/src/main/fc/stats.h
+++ b/src/main/fc/stats.h
@ -11,6 +11,7 @@ typedef struct statsConfig_s {
    uint8_t  stats_enabled;
 } statsConfig_t;

+uint32_t getFlyingEnergy();
 void statsOnArm(void);
 void statsOnDisarm(void);

--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@ -548,6 +548,19 @@ void imuUpdateAccelerometer(void)
 #endif
 }

+void imuCheckVibrationLevels(void)
+{
+    fpVector3_t accVibeLevels;
+
+    accGetVibrationLevels(&accVibeLevels);
+    const uint32_t accClipCount = accGetClipCount();
+
+    DEBUG_SET(DEBUG_VIBE, 0, accVibeLevels.x * 100);
+    DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100);
+    DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100);
+    DEBUG_SET(DEBUG_VIBE, 3, accClipCount);
+}
+
 void imuUpdateAttitude(timeUs_t currentTimeUs)
 {
    /* Calculate dT */
@ -560,6 +573,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
        if (!hilActive) {
            gyroGetMeasuredRotationRate(&imuMeasuredRotationBF);    // Calculate gyro rate in body frame in rad/s
            accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
+            imuCheckVibrationLevels();
            imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
        }
        else {
@ -569,6 +583,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
 #else
        gyroGetMeasuredRotationRate(&imuMeasuredRotationBF);    // Calculate gyro rate in body frame in rad/s
        accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
+        imuCheckVibrationLevels();
        imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
 #endif
    } else {
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@ -38,6 +38,7 @@

 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"

 #include "flight/failsafe.h"
@ -76,7 +77,8 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
    .yaw_jump_prevention_limit = 200,
    .platformType = PLATFORM_MULTIROTOR,
    .hasFlaps = false,
-    .appliedMixerPreset = -1 //This flag is not available in CLI and used by Configurator only
+    .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only
+    .fwMinThrottleDownPitchAngle = 0
 );

 #ifdef BRUSHED_MOTORS
@ -99,7 +101,6 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
    .mincommand = 1000,
    .motorAccelTimeMs = 0,
    .motorDecelTimeMs = 0,
-    .throttleVBatCompensation = false
 );

 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -310,7 +311,7 @@ void mixTable(const float dT)
        throttleMax = motorConfig()->maxthrottle;

        // Throttle compensation based on battery voltage
-        if (motorConfig()->throttleVBatCompensation && STATE(FIXED_WING) && isAmperageConfigured() && feature(FEATURE_VBAT))
+        if (feature(FEATURE_THR_VBAT_COMP) && feature(FEATURE_VBAT) && isAmperageConfigured())
            throttleCommand = MIN(throttleCommand * calculateThrottleCompensationFactor(), throttleMax);
    }

@ -350,17 +351,11 @@ void mixTable(const float dT)
            }

            // Motor stop handling
-            if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED)) {
-                bool failsafeMotorStop = failsafeRequiresMotorStop();
-                bool navMotorStop = !failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE);
-                bool userMotorStop = !navigationIsFlyingAutonomousMode() && !failsafeIsActive() && (rcData[THROTTLE] < rxConfig()->mincheck);
-                if (failsafeMotorStop || navMotorStop || userMotorStop) {
-                    if (feature(FEATURE_3D)) {
-                        motor[i] = PWM_RANGE_MIDDLE;
-                    }
-                    else {
-                        motor[i] = motorConfig()->mincommand;
-                    }
+            if (ARMING_FLAG(ARMED) && (getMotorStatus() != MOTOR_RUNNING)) {
+                if (feature(FEATURE_MOTOR_STOP)) {
+                    motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand);
+                } else {
+                    motor[i] = motorConfig()->minthrottle;
                }
            }
        }
@ -373,3 +368,14 @@ void mixTable(const float dT)
    /* Apply motor acceleration/deceleration limit */
    applyMotorRateLimiting(dT);
 }
+
+motorStatus_e getMotorStatus(void)
+{
+    if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)))
+        return MOTOR_STOPPED_AUTO;
+
+    if ((STATE(FIXED_WING) || !isAirmodeActive()) && (!navigationIsFlyingAutonomousMode()) && (!failsafeIsActive()) && (rcData[THROTTLE] < rxConfig()->mincheck))
+        return MOTOR_STOPPED_USER;
+
+    return MOTOR_RUNNING;
+}
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@ -28,6 +28,8 @@
 #define YAW_JUMP_PREVENTION_LIMIT_LOW 80
 #define YAW_JUMP_PREVENTION_LIMIT_HIGH 500

+#define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450
+
 typedef enum {
    PLATFORM_MULTIROTOR     = 0,
    PLATFORM_AIRPLANE       = 1,
@ -59,6 +61,7 @@ typedef struct mixerConfig_s {
    uint8_t platformType;
    bool hasFlaps;
    int16_t appliedMixerPreset;
+    uint16_t fwMinThrottleDownPitchAngle;
 } mixerConfig_t;

 PG_DECLARE(mixerConfig_t, mixerConfig);
@ -80,17 +83,23 @@ typedef struct motorConfig_s {
    uint8_t  motorPwmProtocol;
    uint16_t motorAccelTimeMs;              // Time limit for motor to accelerate from 0 to 100% throttle [ms]
    uint16_t motorDecelTimeMs;              // Time limit for motor to decelerate from 0 to 100% throttle [ms]
-    bool     throttleVBatCompensation;
 } motorConfig_t;

 PG_DECLARE(motorConfig_t, motorConfig);

+typedef enum {
+    MOTOR_STOPPED_USER,
+    MOTOR_STOPPED_AUTO,
+    MOTOR_RUNNING
+} motorStatus_e;
+
 extern int16_t motor[MAX_SUPPORTED_MOTORS];
 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];

 uint8_t getMotorCount(void);
 float getMotorMixRange(void);
 bool mixerIsOutputSaturated(void);
+motorStatus_e getMotorStatus(void);

 void writeAllMotors(int16_t mc);
 void mixerUsePWMIOConfiguration(void);
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@ -413,7 +413,12 @@ static float calcHorizonRateMagnitude(void)
 static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude)
 {
    // This is ROLL/PITCH, run ANGLE/HORIZON controllers
-    const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
+    float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
+
+    // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
+    if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
+        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
+
    const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);

    float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
--- a/src/main/flight/rth_estimator.c
+++ b/src/main/flight/rth_estimator.c
@ -0,0 +1,201 @@
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+
+#include "fc/config.h"
+#include "fc/fc_core.h"
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/wind_estimator.h"
+
+#include "navigation/navigation.h"
+
+#include "sensors/battery.h"
+
+#include <stdint.h>
+
+#if defined(USE_ADC) && defined(USE_GPS)
+
+/* INPUTS:
+ *   - forwardSpeed (same unit as horizontalWindSpeed)
+ *   - heading degrees
+ *   - horizontalWindSpeed (same unit as forwardSpeed)
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns degrees
+ */
+static float windDriftCompensationAngle(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
+    return RADIANS_TO_DEGREES(asin_approx(-horizontalWindSpeed * sin_approx(DEGREES_TO_RADIANS(windHeading - heading)) / forwardSpeed));
+}
+
+/* INPUTS:
+ *   - forwardSpeed (same unit as horizontalWindSpeed)
+ *   - heading degrees
+ *   - horizontalWindSpeed (same unit as forwardSpeed)
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns (same unit as forwardSpeed and horizontalWindSpeed)
+ */
+static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
+    return forwardSpeed * cos_approx(DEGREES_TO_RADIANS(windDriftCompensationAngle(forwardSpeed, heading, horizontalWindSpeed, windHeading)));
+}
+
+/* INPUTS:
+ *   - heading degrees
+ *   - horizontalWindSpeed
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns same unit as horizontalWindSpeed
+ */
+static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) {
+    return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading));
+}
+
+/* INPUTS:
+ *   - forwardSpeed (same unit as horizontalWindSpeed)
+ *   - heading degrees
+ *   - horizontalWindSpeed (same unit as forwardSpeed)
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns (same unit as forwardSpeed and horizontalWindSpeed)
+ */
+static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
+    return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading);
+}
+
+// returns degrees
+static int8_t RTHAltitudeChangePitchAngle(float altitudeChange) {
+    return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle;
+}
+
+// altitudeChange is in meters
+// idle_power and cruise_power are in deciWatt
+// output is in Watt
+static float estimateRTHAltitudeChangePower(float altitudeChange) {
+    uint16_t altitudeChangeThrottle = navConfig()->fw.cruise_throttle - RTHAltitudeChangePitchAngle(altitudeChange) * navConfig()->fw.pitch_to_throttle;
+    altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
+    const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle);
+    return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
+}
+
+// altitudeChange is in m
+// verticalWindSpeed is in m/s
+// cruise_speed is in cm/s
+// output is in seconds
+static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalWindSpeed) {
+    // Assuming increase in throttle keeps air speed at cruise speed
+    const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed;
+    return altitudeChange / estimatedVerticalSpeed;
+}
+
+// altitudeChange is in m
+// horizontalWindSpeed is in m/s
+// windHeading is in degrees
+// verticalWindSpeed is in m/s
+// cruise_speed is in cm/s
+// output is in meters
+static float estimateRTHAltitudeChangeGroundDistance(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed) {
+    // Assuming increase in throttle keeps air speed at cruise speed
+    float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading);
+    return estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) * estimatedHorizontalSpeed;
+}
+
+// altitudeChange is in m
+// verticalWindSpeed is in m/s
+// output is in Wh
+static uint16_t estimateRTHAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
+    return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
+}
+
+// returns distance in m
+// *heading is in degrees 
+static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) {
+    float estimatedAltitudeChangeGroundDistance = estimateRTHAltitudeChangeGroundDistance(altitudeChange, horizontalWindSpeed, windHeading, verticalWindSpeed);
+    if (altitudeChange > 0) {
+        float headingDiff = DEGREES_TO_RADIANS(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw) - GPS_directionToHome);
+        float triangleAltitude = GPS_distanceToHome * sin_approx(headingDiff);
+        float triangleAltitudeToReturnStart = estimatedAltitudeChangeGroundDistance - GPS_distanceToHome * cos_approx(headingDiff);
+        *heading = RADIANS_TO_DEGREES(atan2_approx(triangleAltitude, triangleAltitudeToReturnStart));
+        return sqrt(sq(triangleAltitude) + sq(triangleAltitudeToReturnStart));
+    } else {
+        *heading = GPS_directionToHome;
+        return GPS_distanceToHome - estimatedAltitudeChangeGroundDistance;
+    }
+}
+
+// returns mWh
+static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
+    // Fixed wing only for now
+    if (!STATE(FIXED_WING))
+        return -1;
+
+    if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isEstimatedWindSpeedValid() && isImuHeadingValid()))
+        return -1;
+
+    uint16_t windHeading; // centidegrees
+    const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
+    const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
+    const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100;
+
+    const float RTH_altitude_change = (RTHAltitude() - getEstimatedActualPosition(Z)) / 100;
+    float RTH_heading; // degrees
+    const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
+    const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, DECIDEGREES_TO_DEGREES(attitude.values.yaw), horizontalWindSpeed, windHeadingDegrees);
+
+    DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_altitude_change * 100));
+    DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100));
+    DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100));
+    DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100));
+
+    if (RTH_speed <= 0)
+        return -2; // wind is too strong
+
+    const uint32_t time_to_home = RTH_distance / RTH_speed; // seconds
+    const uint32_t energy_to_home = estimateRTHAltitudeChangeEnergy(RTH_altitude_change, verticalWindSpeed) * 1000 + heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power) * time_to_home / 360; // mWh
+    const uint32_t energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100; // mWh
+    const int32_t remaining_energy_before_rth = getBatteryRemainingCapacity() - energy_margin_abs - energy_to_home; // mWh
+
+    if (remaining_energy_before_rth < 0) // No energy left = No time left
+        return 0;
+
+    return remaining_energy_before_rth;
+}
+
+// returns seconds
+int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
+
+    const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
+
+    // error: return error code directly
+    if (remainingEnergyBeforeRTH < 0)
+        return remainingEnergyBeforeRTH;
+
+    const int32_t averagePower = calculateAveragePower();
+
+    if (averagePower == 0)
+        return -1;
+
+    const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower;
+
+    if (time_before_rth > 0x7FFFFFFF) // int32 overflow
+        return -1;
+
+    return time_before_rth;
+}
+
+// returns meters
+int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
+
+    const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
+
+    // error: return error code directly
+    if (remainingFlightTimeBeforeRTH < 0)
+        return remainingFlightTimeBeforeRTH;
+
+    return remainingFlightTimeBeforeRTH * calculateAverageSpeed();
+}
+
+#endif
--- a/src/main/flight/rth_estimator.h
+++ b/src/main/flight/rth_estimator.h
@ -0,0 +1,5 @@
+
+#if defined(USE_ADC) && defined(USE_GPS)
+int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount);
+int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
+#endif
--- a/src/main/flight/wind_estimator.h
+++ b/src/main/flight/wind_estimator.h
@ -23,6 +23,7 @@
 #endif
 #endif

+#include "common/axis.h"
 #include "common/time.h"

 bool isEstimatedWindSpeedValid(void);
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@ -158,10 +158,10 @@ static const displayPortVTable_t max7456VTable = {
    .supportedTextAttributes = supportedTextAttributes,
 };

-displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile)
+displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem)
 {
    displayInit(&max7456DisplayPort, &max7456VTable);
-    max7456Init(vcdProfile);
+    max7456Init(videoSystem);
    resync(&max7456DisplayPort);
    return &max7456DisplayPort;
 }
--- a/src/main/io/displayport_max7456.h
+++ b/src/main/io/displayport_max7456.h
@ -19,5 +19,6 @@

 #include "config/parameter_group.h"
 #include "drivers/display.h"
+#include "drivers/vcd.h"

-displayPort_t *max7456DisplayPortInit(const struct vcdProfile_s *vcdProfile);
+displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem);
--- a/src/main/io/opflow_cxof.c
+++ b/src/main/io/opflow_cxof.c
@ -113,6 +113,7 @@ static bool cxofOpflowUpdate(opflowData_t * data)
                    tmpData.deltaTime += (currentTimeUs - previousTimeUs);
                    tmpData.flowRateRaw[0] += pkt->motionX;
                    tmpData.flowRateRaw[1] += pkt->motionY;
+                    tmpData.flowRateRaw[2] = 0;
                    tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14;

                    previousTimeUs = currentTimeUs;
--- a/src/main/io/opflow_msp.c
+++ b/src/main/io/opflow_msp.c
@ -81,6 +81,7 @@ void mspOpflowReceiveNewData(uint8_t * bufferPtr)
    sensorData.deltaTime = currentTimeUs - updatedTimeUs;
    sensorData.flowRateRaw[0] = pkt->motionX;
    sensorData.flowRateRaw[1] = pkt->motionY;
+    sensorData.flowRateRaw[2] = 0;
    sensorData.quality = (int)pkt->quality * 100 / 255;
    hasNewData = true;

--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@ -64,12 +64,17 @@

 #include "fc/fc_core.h"
 #include "fc/config.h"
+#include "fc/controlrate_profile.h"
+#include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
+#include "fc/fc_tasks.h"

 #include "flight/imu.h"
+#include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/rth_estimator.h"
 #include "flight/wind_estimator.h"

 #include "navigation/navigation.h"
@ -77,6 +82,7 @@
 #include "rx/rx.h"

 #include "sensors/battery.h"
+#include "sensors/boardalignment.h"
 #include "sensors/diagnostics.h"
 #include "sensors/sensors.h"
 #include "sensors/pitotmeter.h"
@ -116,7 +122,6 @@
    x; \
 })

-static timeUs_t flyTime = 0;
 static unsigned currentLayout = 0;
 static int layoutOverride = -1;

@ -160,7 +165,7 @@ static displayPort_t *osdDisplayPort;
 #define AH_SIDEBAR_WIDTH_POS 7
 #define AH_SIDEBAR_HEIGHT_POS 3

-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 2);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3);

 static int digitCount(int32_t value)
 {
@ -460,7 +465,7 @@ static inline void osdFormatOnTime(char *buff)

 static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
 {
-    uint32_t seconds = flyTime / 1000000;
+    uint32_t seconds = getFlightTime();
    osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H);
    if (attr && osdConfig()->time_alarm > 0) {
       if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) {
@ -722,7 +727,7 @@ static void osdFormatBatteryChargeSymbol(char *buff)

 static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *attr)
 {
-    if ((getBatteryState() != BATTERY_NOT_PRESENT) && ((batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= batteryConfig()->capacity.warning - batteryConfig()->capacity.critical)) || ((!batteryUsesCapacityThresholds()) && (getBatteryVoltage() <= getBatteryWarningVoltage()))))
+    if ((getBatteryState() != BATTERY_NOT_PRESENT) && ((batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical)) || ((!batteryUsesCapacityThresholds()) && (getBatteryVoltage() <= getBatteryWarningVoltage()))))
        TEXT_ATTRIBUTES_ADD_BLINK(*attr);
 }

@ -749,7 +754,7 @@ static void osdCrosshairsBounds(uint8_t *x, uint8_t *y, uint8_t *length)
 * uses the THR value applied by the system rather than the
 * input value received by the sticks.
 **/
-static void osdFormatThrottlePosition(char *buff, bool autoThr)
+static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr)
 {
    buff[0] = SYM_THR;
    buff[1] = SYM_THR1;
@ -758,6 +763,8 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr)
        buff[0] = SYM_AUTO_THR0;
        buff[1] = SYM_AUTO_THR1;
        thr = rcCommand[THROTTLE];
+        if (isFixedWingAutoThrottleManuallyIncreased())
+            TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
    }
    tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
 }
@ -871,7 +878,7 @@ static int osdGetHeadingAngle(int angle)
 */
 static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t centerSym,
                       uint32_t poiDistance, int16_t poiDirection, uint8_t poiSymbol,
-                       uint16_t *drawn)
+                       uint16_t *drawn, uint32_t *usedScale)
 {
    // TODO: These need to be tested with several setups. We might
    // need to make them configurable.
@ -905,16 +912,20 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
        *drawn = 0;
    }

-    uint32_t scale;
+    uint32_t initialScale;
    float scaleToUnit;
    int scaleUnitDivisor;
    char symUnscaled;
    char symScaled;
    int maxDecimals;
+    const int scaleMultiplier = 2;
+    // We try to reduce the scale when the POI will be around half the distance
+    // between the center and the closers map edge, to avoid too much jumping
+    const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2;

    switch (osdConfig()->units) {
        case OSD_UNIT_IMPERIAL:
-            scale = 16; // 16m ~= 0.01miles
+            initialScale = 16; // 16m ~= 0.01miles
            scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber()
            scaleUnitDivisor = 0;
            symUnscaled = SYM_MI;
@ -924,7 +935,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
        case OSD_UNIT_UK:
            FALLTHROUGH;
        case OSD_UNIT_METRIC:
-            scale = 10; // 10m as initial scale
+            initialScale = 10; // 10m as initial scale
            scaleToUnit = 100; // scale to cm for osdFormatCentiNumber()
            scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m
            symUnscaled = SYM_M;
@ -933,6 +944,15 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
            break;
    }

+    // Try to keep the same scale when getting closer until we draw over the center point
+    uint32_t scale = initialScale;
+    if (*usedScale) {
+        scale = *usedScale;
+        if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) {
+            scale /= scaleMultiplier;
+        }
+    }
+
    if (STATE(GPS_FIX)) {

        int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
@ -941,7 +961,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
        float poiCos = cos_approx(poiAngle);

        // Now start looking for a valid scale that lets us draw everything
-        for (int ii = 0; ii < 50; ii++, scale *= 2) {
+        for (int ii = 0; ii < 50; ii++, scale *= scaleMultiplier) {
            // Calculate location of the aircraft in map
            int points = poiDistance / (float)(scale / charHeight);

@ -993,28 +1013,99 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
    buf[3] = scaled ? symScaled : symUnscaled;
    buf[4] = '\0';
    displayWrite(osdDisplayPort, minX + 1, maxY, buf);
+    *usedScale = scale;
 }

 /* Draws a map with the home in the center and the craft moving around.
 * See osdDrawMap() for reference.
 */
-static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn)
+static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn, uint32_t *usedScale)
 {
-    osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn);
+    osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale);
 }

 /* Draws a map with the aircraft in the center and the home moving around.
 * See osdDrawMap() for reference.
 */
-static void osdDrawRadar(uint16_t *drawn)
+static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale)
 {
    int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading());
    int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180);
-    osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn);
+    osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale);
 }

 #endif

+void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController) {
+    strcpy(buff, label);
+    uint8_t label_len = strlen(label);
+    for (uint8_t i = label_len; i < 5; ++i) buff[i] = ' ';
+    osdFormatCentiNumber(buff + 5, pidController->proportional, 0, 1, 0, 4);
+    buff[9] = ' ';
+    osdFormatCentiNumber(buff + 10, pidController->integrator, 0, 1, 0, 4);
+    buff[14] = ' ';
+    osdFormatCentiNumber(buff + 15, pidController->derivative, 0, 1, 0, 4);
+    buff[19] = ' ';
+    osdFormatCentiNumber(buff + 20, pidController->output_constrained, 0, 1, 0, 4);
+    buff[24] = '\0';
+}
+
+static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t digits, uint8_t decimals)
+{
+    char buff[6];
+    textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
+
+    osdFormatBatteryChargeSymbol(buff);
+    buff[1] = '\0';
+    osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    osdFormatCentiNumber(buff, voltage, 0, decimals, 0, MIN(digits, 4));
+    strcat(buff, "V");
+    if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage()))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
+}
+
+static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, const pid8_t *pid, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
+{
+    textAttributes_t elemAttr;
+    char buff[4];
+
+    displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    tfp_sprintf(buff, "%3d", pid->P);
+    if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    tfp_sprintf(buff, "%3d", pid->I);
+    if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    tfp_sprintf(buff, "%3d", pid->D);
+    if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
+}
+
+static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, const char *str, const uint8_t valueOffset, const float value, const uint8_t valueLength, const uint8_t maxDecimals, adjustmentFunction_e adjFunc) {
+    char buff[8];
+    textAttributes_t elemAttr;
+    displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8));
+    if (isAdjustmentFunctionSelected(adjFunc))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr);
+}
+
 static bool osdDrawSingleElement(uint8_t item)
 {
    uint16_t pos = osdConfig()->item_pos[currentLayout][item];
@ -1040,29 +1131,11 @@ static bool osdDrawSingleElement(uint8_t item)
        }

    case OSD_MAIN_BATT_VOLTAGE:
-        osdFormatBatteryChargeSymbol(buff);
-        buff[1] = '\0';
-        osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
-        displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
-        elemAttr = TEXT_ATTRIBUTES_NONE;
-        osdFormatCentiNumber(buff, getBatteryVoltage(), 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
-        strcat(buff, "V");
-        if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage()))
-            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-        displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
+        osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
        return true;

    case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE:
-        osdFormatBatteryChargeSymbol(buff);
-        buff[1] = '\0';
-        osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
-        displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
-        elemAttr = TEXT_ATTRIBUTES_NONE;
-        osdFormatCentiNumber(buff, getSagCompensatedBatteryVoltage(), 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
-        strcat(buff, "V");
-        if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getSagCompensatedBatteryVoltage() <= getBatteryWarningVoltage()))
-            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-        displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
+        osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
        return true;

    case OSD_CURRENT_DRAW:
@ -1083,18 +1156,18 @@ static bool osdDrawSingleElement(uint8_t item)
        break;

    case OSD_BATTERY_REMAINING_CAPACITY:
-        buff[0] = (batteryConfig()->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH);
+        buff[0] = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH);

-        if (batteryConfig()->capacity.value == 0)
+        if (currentBatteryProfile->capacity.value == 0)
            tfp_sprintf(buff + 1, "NA");
        else if (!batteryWasFullWhenPluggedIn())
            tfp_sprintf(buff + 1, "NF");
-        else if (batteryConfig()->capacity.unit == BAT_CAPACITY_UNIT_MAH)
+        else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH)
            tfp_sprintf(buff + 1, "%-4lu", getBatteryRemainingCapacity());
-        else // batteryConfig()->capacity.unit == BAT_CAPACITY_UNIT_MWH
+        else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH
            osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3);

-        if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= batteryConfig()->capacity.warning - batteryConfig()->capacity.critical))
+        if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical))
            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);

        break;
@ -1104,6 +1177,15 @@ static bool osdDrawSingleElement(uint8_t item)
        osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
        break;

+    case OSD_POWER_SUPPLY_IMPEDANCE:
+        if (isPowerSupplyImpedanceValid())
+            tfp_sprintf(buff, "%3d", getPowerSupplyImpedance());
+        else
+            strcpy(buff, "---");
+        buff[3] = SYM_MILLIOHM;
+        buff[4] = '\0';
+        break;
+
 #ifdef USE_GPS
    case OSD_GPS_SATS:
        buff[0] = SYM_SAT_L;
@ -1128,14 +1210,35 @@ static bool osdDrawSingleElement(uint8_t item)

    case OSD_HOME_DIR:
        {
-            int16_t h = osdGetHeadingAngle(GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()));
-            h = h * 2 / 45;
-
-            buff[0] = SYM_ARROW_UP + h;
+            // There are 16 orientations for the home direction arrow.
+            // so we use 22.5deg per image, where the 1st image is used
+            // for [349, 11], the 2nd for [12, 33], etc...
+            int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading());
+            // Add 11 to the angle, so first character maps to [349, 11]
+            int homeArrowDir = osdGetHeadingAngle(homeDirection + 11);
+            unsigned arrowOffset = homeArrowDir * 2 / 45;
+            buff[0] = SYM_ARROW_UP + arrowOffset;
            buff[1] = 0;
            break;
        }

+    case OSD_HOME_HEADING_ERROR:
+        {
+            buff[0] = SYM_HOME;
+            buff[1] = SYM_HEADING;
+
+            if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) {
+                int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading()))));
+                tfp_sprintf(buff + 2, "%4d", h);
+            } else {
+                strcpy(buff + 2, "----");
+            }
+
+            buff[6] = SYM_DEGREES;
+            buff[7] = '\0';
+            break;
+        }
+
    case OSD_HOME_DIST:
        {
            buff[0] = SYM_HOME;
@ -1168,6 +1271,53 @@ static bool osdDrawSingleElement(uint8_t item)
            buff[5] = '\0';
            break;
        }
+
+    case OSD_CRUISE_HEADING_ERROR:
+        {
+            if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) {
+                displayWrite(osdDisplayPort, elemPosX, elemPosY, "     ");
+                return true;
+            }
+
+            buff[0] = SYM_HEADING;
+
+            if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) {
+                buff[1] = buff[2] = buff[3] = '-';
+            } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
+                int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError()));
+                if (ABS(herr) > 99)
+                    strcpy(buff + 1, ">99");
+                else
+                    tfp_sprintf(buff + 1, "%3d", herr);
+            }
+
+            buff[4] = SYM_DEGREES;
+            buff[5] = '\0';
+            break;
+        }
+
+    case OSD_CRUISE_HEADING_ADJUSTMENT:
+        {
+            int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment()));
+
+            if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
+                displayWrite(osdDisplayPort, elemPosX, elemPosY, "      ");
+                return true;
+            }
+
+            buff[0] = SYM_HEADING;
+
+            if (!ARMING_FLAG(ARMED)) {
+                buff[1] = buff[2] = buff[3] = buff[4] = '-';
+            } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
+                tfp_sprintf(buff + 1, "%4d", heading_adjust);
+            }
+
+            buff[5] = SYM_DEGREES;
+            buff[6] = '\0';
+            break;
+        }
+
    case OSD_GPS_HDOP:
        {
            buff[0] = SYM_HDP_L;
@ -1180,19 +1330,22 @@ static bool osdDrawSingleElement(uint8_t item)
    case OSD_MAP_NORTH:
        {
            static uint16_t drawn = 0;
-            osdDrawHomeMap(0, 'N', &drawn);
+            static uint32_t scale = 0;
+            osdDrawHomeMap(0, 'N', &drawn, &scale);
            return true;
        }
    case OSD_MAP_TAKEOFF:
        {
            static uint16_t drawn = 0;
-            osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn);
+            static uint32_t scale = 0;
+            osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale);
            return true;
        }
    case OSD_RADAR:
        {
            static uint16_t drawn = 0;
-            osdDrawRadar(&drawn);
+            static uint32_t scale = 0;
+            osdDrawRadar(&drawn, &scale);
            return true;
        }
 #endif // GPS
@ -1233,6 +1386,60 @@ static bool osdDrawSingleElement(uint8_t item)
            break;
        }

+    case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH:
+        {
+            static timeUs_t updatedTimestamp = 0;
+            /*static int32_t updatedTimeSeconds = 0;*/
+            timeUs_t currentTimeUs = micros();
+            static int32_t timeSeconds = -1;
+            if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+                timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
+                updatedTimestamp = currentTimeUs;
+            }
+            if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
+                buff[0] = SYM_FLY_M;
+                strcpy(buff + 1, "--:--");
+                updatedTimestamp = 0;
+            } else if (timeSeconds == -2) {
+                // Wind is too strong to come back with cruise throttle
+                buff[0] = SYM_FLY_M;
+                buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL;
+                buff[3] = ':';
+                buff[6] = '\0';
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            } else {
+                osdFormatTime(buff, timeSeconds, SYM_FLY_M, SYM_FLY_H);
+                if (timeSeconds == 0)
+                    TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
+        }
+        break;
+
+    case OSD_REMAINING_DISTANCE_BEFORE_RTH:;
+        static timeUs_t updatedTimestamp = 0;
+        timeUs_t currentTimeUs = micros();
+        static int32_t distanceMeters = -1;
+        if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+            distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
+            updatedTimestamp = currentTimeUs;
+        }
+        buff[0] = SYM_TRIP_DIST;
+        if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
+            buff[1] = SYM_DIST_M;
+            strcpy(buff + 2, "---");
+        } else if (distanceMeters == -2) {
+            // Wind is too strong to come back with cruise throttle
+            buff[1] = SYM_DIST_M;
+            buff[2] = buff[3] = buff[4] = SYM_WIND_HORIZONTAL;
+            buff[5] = '\0';
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        } else {
+            osdFormatDistanceSymbol(buff + 1, distanceMeters * 100);
+            if (distanceMeters == 0)
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        }
+        break;
+
    case OSD_FLYMODE:
        {
            char *p = "ACRO";
@ -1254,17 +1461,22 @@ static bool osdDrawSingleElement(uint8_t item)
                } else {
                    p = " PH ";
                }
+            } else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
+                p = "3CRS";
+            } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
+                p = "CRS ";
            } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
                // If navigationRequiresAngleMode() returns false when ALTHOLD is active,
                // it means it can be combined with ANGLE, HORIZON, ACRO, etc...
                // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
                p = " AH ";
-            } else if (FLIGHT_MODE(NAV_WP_MODE))
+            } else if (FLIGHT_MODE(NAV_WP_MODE)) {
                p = " WP ";
-            else if (FLIGHT_MODE(ANGLE_MODE))
+            } else if (FLIGHT_MODE(ANGLE_MODE)) {
                p = "ANGL";
-            else if (FLIGHT_MODE(HORIZON_MODE))
+            } else if (FLIGHT_MODE(HORIZON_MODE)) {
                p = "HOR ";
+            }

            displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
            return true;
@ -1284,11 +1496,11 @@ static bool osdDrawSingleElement(uint8_t item)

    case OSD_THROTTLE_POS:
    {
-        osdFormatThrottlePosition(buff, false);
+        osdFormatThrottlePosition(buff, false, NULL);
        break;
    }

-#if defined(VTX) || defined(VTX_COMMON)
+#if defined(VTX) || defined(USE_VTX_COMMON)
    case OSD_VTX_CHANNEL:
 #if defined(VTX)
        // FIXME: This doesn't actually work. It's for boards with
@ -1298,8 +1510,21 @@ static bool osdDrawSingleElement(uint8_t item)
        {
            uint8_t band = 0;
            uint8_t channel = 0;
-            vtxCommonGetBandAndChannel(&band, &channel);
-            tfp_sprintf(buff, "CH:%c%s", vtx58BandLetter[band], vtx58ChannelNames[channel]);
+            uint8_t powerIndex = 0;
+            char bandChr = '-';
+            const char *channelStr = "-";
+            char powerChr = '-';
+            vtxDevice_t *vtxDevice = vtxCommonDevice();
+            if (vtxDevice) {
+                if (vtxCommonGetBandAndChannel(vtxDevice, &band, &channel)) {
+                    bandChr = vtx58BandLetter[band];
+                    channelStr = vtx58ChannelNames[channel];
+                }
+                if (vtxCommonGetPowerIndex(vtxDevice, &powerIndex)) {
+                    powerChr = '0' + powerIndex;
+                }
+            }
+            tfp_sprintf(buff, "CH:%c%s:%c", bandChr, channelStr, powerChr);
        }
 #endif
        break;
@ -1327,13 +1552,13 @@ static bool osdDrawSingleElement(uint8_t item)

    case OSD_ATTITUDE_ROLL:
        buff[0] = SYM_ROLL_LEVEL;
-        if (attitude.values.roll)
+        if (ABS(attitude.values.roll) >= (osdConfig()->attitude_angle_decimals ? 1 : 10))
            buff[0] += (attitude.values.roll < 0 ? -1 : 1);
        osdFormatCentiNumber(buff + 1, ABS(attitude.values.roll) * 10, 0, osdConfig()->attitude_angle_decimals, 0, 2 + osdConfig()->attitude_angle_decimals);
        break;

    case OSD_ATTITUDE_PITCH:
-        if (attitude.values.pitch == 0)
+        if (ABS(attitude.values.pitch) < (osdConfig()->attitude_angle_decimals ? 1 : 10))
            buff[0] = 'P';
        else if (attitude.values.pitch > 0)
            buff[0] = SYM_PITCH_DOWN;
@ -1535,20 +1760,176 @@ static bool osdDrawSingleElement(uint8_t item)
 #endif

    case OSD_ROLL_PIDS:
-        {
-            tfp_sprintf(buff, "ROL %3d %3d %3d", pidBank()->pid[PID_ROLL].P, pidBank()->pid[PID_ROLL].I, pidBank()->pid[PID_ROLL].D);
-            break;
-        }
+        osdDisplayPIDValues(elemPosX, elemPosY, "ROL", &pidBank()->pid[PID_ROLL], ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D);
+        return true;

    case OSD_PITCH_PIDS:
+        osdDisplayPIDValues(elemPosX, elemPosY, "PIT", &pidBank()->pid[PID_PITCH], ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D);
+        return true;
+
+    case OSD_YAW_PIDS:
+        osdDisplayPIDValues(elemPosX, elemPosY, "YAW", &pidBank()->pid[PID_YAW], ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D);
+        return true;
+
+    case OSD_LEVEL_PIDS:
+        osdDisplayPIDValues(elemPosX, elemPosY, "LEV", &pidBank()->pid[PID_LEVEL], ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
+        return true;
+
+    case OSD_POS_XY_PIDS:
+        osdDisplayPIDValues(elemPosX, elemPosY, "PXY", &pidBank()->pid[PID_POS_XY], ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
+        return true;
+
+    case OSD_POS_Z_PIDS:
+        osdDisplayPIDValues(elemPosX, elemPosY, "PZ", &pidBank()->pid[PID_POS_Z], ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
+        return true;
+
+    case OSD_VEL_XY_PIDS:
+        if (STATE(FIXED_WING))
+            osdDisplayPIDValues(elemPosX, elemPosY, "VXY", &pidBank()->pid[PID_VEL_XY], ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
+        return true;
+
+    case OSD_VEL_Z_PIDS:
+        if (STATE(FIXED_WING))
+            osdDisplayPIDValues(elemPosX, elemPosY, "VZ", &pidBank()->pid[PID_VEL_Z], ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
+        return true;
+
+    case OSD_HEADING_P:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P);
+        return true;
+
+    case OSD_BOARD_ALIGN_ROLL:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT);
+        return true;
+
+    case OSD_BOARD_ALIGN_PITCH:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT);
+        return true;
+
+    case OSD_RC_EXPO:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO);
+        return true;
+
+    case OSD_RC_YAW_EXPO:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO);
+        return true;
+
+    case OSD_THROTTLE_EXPO:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO);
+        return true;
+
+    case OSD_PITCH_RATE:
+        displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR");
+
+        elemAttr = TEXT_ATTRIBUTES_NONE;
+        tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
+        if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
+        return true;
+
+    case OSD_ROLL_RATE:
+        displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR");
+
+        elemAttr = TEXT_ATTRIBUTES_NONE;
+        tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
+        if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
+        return true;
+
+    case OSD_YAW_RATE:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE);
+        return true;
+
+    case OSD_MANUAL_RC_EXPO:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO);
+        return true;
+
+    case OSD_MANUAL_RC_YAW_EXPO:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO);
+        return true;
+
+    case OSD_MANUAL_PITCH_RATE:
+        displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR");
+
+        elemAttr = TEXT_ATTRIBUTES_NONE;
+        tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]);
+        if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
+        return true;
+
+    case OSD_MANUAL_ROLL_RATE:
+        displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR");
+
+        elemAttr = TEXT_ATTRIBUTES_NONE;
+        tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]);
+        if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
+        return true;
+
+    case OSD_MANUAL_YAW_RATE:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE);
+        return true;
+
+    case OSD_NAV_FW_CRUISE_THR:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
+        return true;
+
+    case OSD_NAV_FW_PITCH2THR:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR);
+        return true;
+
+    case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
+        return true;
+
+    case OSD_FW_ALT_PID_OUTPUTS:
        {
-            tfp_sprintf(buff, "PIT %3d %3d %3d", pidBank()->pid[PID_PITCH].P, pidBank()->pid[PID_PITCH].I, pidBank()->pid[PID_PITCH].D);
+            const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+            osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt);
            break;
        }

-    case OSD_YAW_PIDS:
+    case OSD_FW_POS_PID_OUTPUTS:
        {
-            tfp_sprintf(buff, "YAW %3d %3d %3d", pidBank()->pid[PID_YAW].P, pidBank()->pid[PID_YAW].I, pidBank()->pid[PID_YAW].D);
+            const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+            osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav);
+            break;
+        }
+
+    case OSD_MC_VEL_Z_PID_OUTPUTS:
+        {
+            const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+            osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z]);
+            break;
+        }
+
+    case OSD_MC_VEL_X_PID_OUTPUTS:
+        {
+            const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+            osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X]);
+            break;
+        }
+
+    case OSD_MC_VEL_Y_PID_OUTPUTS:
+        {
+            const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+            osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y]);
+            break;
+        }
+
+    case OSD_MC_POS_XYZ_P_OUTPUTS:
+        {
+            const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
+            strcpy(buff, "POSO ");
+            osdFormatCentiNumber(buff + 5, nav_pids->pos[X].output_constrained, 0, 1, 0, 4);
+            buff[9] = ' ';
+            osdFormatCentiNumber(buff + 10, nav_pids->pos[Y].output_constrained, 0, 1, 0, 4);
+            buff[14] = ' ';
+            osdFormatCentiNumber(buff + 15, nav_pids->pos[Z].output_constrained, 0, 1, 0, 4);
+            buff[19] = '\0';
            break;
        }

@ -1664,23 +2045,19 @@ static bool osdDrawSingleElement(uint8_t item)

    case OSD_MAIN_BATT_CELL_VOLTAGE:
        {
-            uint16_t cellBattCentiVolts = getBatteryAverageCellVoltage();
-            osdFormatBatteryChargeSymbol(buff);
-            buff[1] = '\0';
-            osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
-            displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
-            elemAttr = TEXT_ATTRIBUTES_NONE;
-            osdFormatCentiNumber(buff, cellBattCentiVolts, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 1);
-            strcat(buff, "V");
-            if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage()))
-                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-            displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
+            osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2);
+            return true;
+        }
+
+    case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE:
+        {
+            osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), 3, 2);
            return true;
        }

    case OSD_THROTTLE_POS_AUTO_THR:
        {
-            osdFormatThrottlePosition(buff, true);
+            osdFormatThrottlePosition(buff, true, &elemAttr);
            break;
        }

@ -1862,11 +2239,19 @@ static bool osdDrawSingleElement(uint8_t item)
 static uint8_t osdIncElementIndex(uint8_t elementIndex)
 {
    ++elementIndex;
+
    if (!sensors(SENSOR_ACC)) {
        if (elementIndex == OSD_CROSSHAIRS) {
            elementIndex = OSD_ONTIME;
        }
    }
+
+    if (!feature(FEATURE_VBAT)) {
+        if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
+            elementIndex = OSD_LEVEL_PIDS;
+        }
+    }
+
    if (!feature(FEATURE_CURRENT_METER)) {
        if (elementIndex == OSD_CURRENT_DRAW) {
            elementIndex = OSD_GPS_SPEED;
@ -1874,7 +2259,14 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
        if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) {
            elementIndex = OSD_TRIP_DIST;
        }
+        if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) {
+            elementIndex = OSD_HOME_HEADING_ERROR;
        }
+        if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
+            elementIndex = OSD_LEVEL_PIDS;
+        }
+    }
+
    if (!feature(FEATURE_GPS)) {
        if (elementIndex == OSD_GPS_SPEED) {
            elementIndex = OSD_ALTITUDE;
@ -1888,6 +2280,9 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
        if (elementIndex == OSD_TRIP_DIST) {
            elementIndex = OSD_ATTITUDE_PITCH;
        }
+        if (elementIndex == OSD_MAP_NORTH) {
+            elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE;
+        }
    }

    if (elementIndex == OSD_ITEM_COUNT) {
@ -1917,17 +2312,21 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
    osdConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1);
    osdConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2);
    osdConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1);
+    osdConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
    osdConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);

    osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
    osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
    osdConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
+    osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
+    osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
    osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
    osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(1, 3) | OSD_VISIBLE_FLAG;
    osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
    osdConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5);
    osdConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6);
    osdConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7);
+    osdConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8);

    osdConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5);
    osdConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5);
@ -1950,6 +2349,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
    osdConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
    osdConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;
    osdConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12);
+    osdConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7);
+    osdConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6);

    osdConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
    osdConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
@ -1961,6 +2362,35 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
    osdConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10);
    osdConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11);
    osdConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10);
+    osdConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11);
+    osdConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12);
+
    osdConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1);

    osdConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5);
@ -1982,7 +2412,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
    osdConfig->dist_alarm = 1000;
    osdConfig->neg_alt_alarm = 5;

-    osdConfig->video_system = 0;
+    osdConfig->video_system = VIDEO_SYSTEM_AUTO;

    osdConfig->ahi_reverse_roll = 0;
    osdConfig->crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT;
@ -1993,6 +2423,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
    osdConfig->units = OSD_UNIT_METRIC;
    osdConfig->main_voltage_decimals = 1;
    osdConfig->attitude_angle_decimals = 0;
+
+    osdConfig->estimations_wind_compensation = true;
 }

 static void osdSetNextRefreshIn(uint32_t timeMs) {
@ -2194,7 +2626,7 @@ static void osdShowStats(void)
    }

    displayWrite(osdDisplayPort, statNameX, top, "FLY TIME         :");
-    uint32_t flySeconds = flyTime / 1000000;
+    uint16_t flySeconds = getFlightTime();
    uint16_t flyMinutes = flySeconds / 60;
    flySeconds %= 60;
    uint16_t flyHours = flyMinutes / 60;
@ -2239,8 +2671,6 @@ static void osdShowArmed(void)

 static void osdRefresh(timeUs_t currentTimeUs)
 {
-    static timeUs_t lastTimeUs = 0;
-
    if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu)) {
      displayClearScreen(osdDisplayPort);
      armState = ARMING_FLAG(ARMED);
@ -2261,13 +2691,6 @@ static void osdRefresh(timeUs_t currentTimeUs)
        armState = ARMING_FLAG(ARMED);
    }

-    if (ARMING_FLAG(ARMED)) {
-        timeUs_t deltaT = currentTimeUs - lastTimeUs;
-        flyTime += deltaT;
-    }
-
-    lastTimeUs = currentTimeUs;
-
    if (resumeRefreshAt) {
        // If we already reached he time for the next refresh,
        // or THR is high or PITCH is high, resume refreshing.
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@ -20,6 +20,8 @@
 #include "common/time.h"
 #include "config/parameter_group.h"

+#include "drivers/vcd.h"
+
 #ifndef OSD_ALTERNATE_LAYOUT_COUNT
 #define OSD_ALTERNATE_LAYOUT_COUNT 3
 #endif
@ -80,10 +82,45 @@ typedef enum {
    OSD_MAP_NORTH,
    OSD_MAP_TAKEOFF,
    OSD_RADAR,
-    OSD_DEBUG, // Number 46. Intentionally absent from configurator and CMS. Set it from CLI.
    OSD_WIND_SPEED_HORIZONTAL,
    OSD_WIND_SPEED_VERTICAL,
+    OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,
+    OSD_REMAINING_DISTANCE_BEFORE_RTH,
+    OSD_HOME_HEADING_ERROR,
+    OSD_CRUISE_HEADING_ERROR,
+    OSD_CRUISE_HEADING_ADJUSTMENT,
    OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE,
+    OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE,
+    OSD_POWER_SUPPLY_IMPEDANCE,
+    OSD_LEVEL_PIDS,
+    OSD_POS_XY_PIDS,
+    OSD_POS_Z_PIDS,
+    OSD_VEL_XY_PIDS,
+    OSD_VEL_Z_PIDS,
+    OSD_HEADING_P,
+    OSD_BOARD_ALIGN_ROLL,
+    OSD_BOARD_ALIGN_PITCH,
+    OSD_RC_EXPO,
+    OSD_RC_YAW_EXPO,
+    OSD_THROTTLE_EXPO,
+    OSD_PITCH_RATE,
+    OSD_ROLL_RATE,
+    OSD_YAW_RATE,
+    OSD_MANUAL_RC_EXPO,
+    OSD_MANUAL_RC_YAW_EXPO,
+    OSD_MANUAL_PITCH_RATE,
+    OSD_MANUAL_ROLL_RATE,
+    OSD_MANUAL_YAW_RATE,
+    OSD_NAV_FW_CRUISE_THR,
+    OSD_NAV_FW_PITCH2THR,
+    OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE,
+    OSD_DEBUG, // Intentionally absent from configurator and CMS. Set it from CLI.
+    OSD_FW_ALT_PID_OUTPUTS,
+    OSD_FW_POS_PID_OUTPUTS,
+    OSD_MC_VEL_X_PID_OUTPUTS,
+    OSD_MC_VEL_Y_PID_OUTPUTS,
+    OSD_MC_VEL_Z_PID_OUTPUTS,
+    OSD_MC_POS_XYZ_P_OUTPUTS,
    OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;

@ -121,7 +158,7 @@ typedef struct osdConfig_s {
    uint16_t dist_alarm; // home distance in m
    uint16_t neg_alt_alarm; // abs(negative altitude) in m

-    uint8_t video_system;
+    videoSystem_e video_system;
    uint8_t row_shiftdown;

    // Preferences
@ -135,6 +172,8 @@ typedef struct osdConfig_s {

    uint8_t units; // from osd_unit_e
    uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
+
+    bool    estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
 } osdConfig_t;

 PG_DECLARE(osdConfig_t, osdConfig);
--- a/src/main/io/vtx.c
+++ b/src/main/io/vtx.c
@ -0,0 +1,279 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#if defined(USE_VTX_COMMON)
+
+#include "common/maths.h"
+#include "common/time.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/vtx_common.h"
+
+#include "fc/cli.h"
+#include "fc/config.h"
+#include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
+
+#include "flight/failsafe.h"
+
+#include "io/vtx.h"
+#include "io/vtx_string.h"
+#include "io/vtx_control.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0);
+
+PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig,
+    .band = VTX_SETTINGS_DEFAULT_BAND,
+    .channel = VTX_SETTINGS_DEFAULT_CHANNEL,
+    .power = VTX_SETTINGS_DEFAULT_POWER,
+    .freq = VTX_SETTINGS_DEFAULT_FREQ,
+    .pitModeFreq = VTX_SETTINGS_DEFAULT_PITMODE_FREQ,
+    .lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF,
+);
+
+typedef enum {
+    VTX_PARAM_POWER = 0,
+    VTX_PARAM_BANDCHAN,
+    VTX_PARAM_PITMODE,
+    VTX_PARAM_CONFIRM,
+    VTX_PARAM_COUNT
+} vtxScheduleParams_e;
+
+void vtxInit(void)
+{
+    bool settingsUpdated = false;
+
+    // sync frequency in parameter group when band/channel are specified
+    const uint16_t freq = vtx58_Bandchan2Freq(vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
+    if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) {
+        vtxSettingsConfigMutable()->freq = freq;
+        settingsUpdated = true;
+    }
+
+#if defined(VTX_SETTINGS_FREQCMD)
+    // constrain pit mode frequency
+    if (vtxSettingsConfig()->pitModeFreq) {
+        const uint16_t constrainedPitModeFreq = MAX(vtxSettingsConfig()->pitModeFreq, VTX_SETTINGS_MIN_USER_FREQ);
+        if (constrainedPitModeFreq != vtxSettingsConfig()->pitModeFreq) {
+            vtxSettingsConfigMutable()->pitModeFreq = constrainedPitModeFreq;
+            settingsUpdated = true;
+        }
+    }
+#endif
+
+    if (settingsUpdated) {
+        saveConfigAndNotify();
+    }
+}
+
+static vtxSettingsConfig_t vtxGetSettings(void)
+{
+    vtxSettingsConfig_t settings = {
+        .band = vtxSettingsConfig()->band,
+        .channel = vtxSettingsConfig()->channel,
+        .power = vtxSettingsConfig()->power,
+        .freq = vtxSettingsConfig()->freq,
+        .pitModeFreq = vtxSettingsConfig()->pitModeFreq,
+        .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
+    };
+
+#if 0
+#if defined(VTX_SETTINGS_FREQCMD)
+    if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) {
+        settings.band = 0;
+        settings.freq = settings.pitModeFreq;
+        settings.power = VTX_SETTINGS_DEFAULT_POWER;
+    }
+#endif
+#endif
+
+    if (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
+        ((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) ||
+        (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))) {
+
+        settings.power = VTX_SETTINGS_DEFAULT_POWER;
+    }
+
+    return settings;
+}
+
+static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice)
+{
+    if(!ARMING_FLAG(ARMED)) {
+        uint8_t vtxBand;
+        uint8_t vtxChan;
+        if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
+            const vtxSettingsConfig_t settings = vtxGetSettings();
+            if (vtxBand != settings.band || vtxChan != settings.channel) {
+                vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel);
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+#if defined(VTX_SETTINGS_FREQCMD)
+static bool vtxProcessFrequency(vtxDevice_t *vtxDevice)
+{
+    if(!ARMING_FLAG(ARMED)) {
+        uint16_t vtxFreq;
+        if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) {
+            const vtxSettingsConfig_t settings = vtxGetSettings();
+            if (vtxFreq != settings.freq) {
+                vtxCommonSetFrequency(vtxDevice, settings.freq);
+                return true;
+            }
+        }
+    }
+    return false;
+}
+#endif
+
+static bool vtxProcessPower(vtxDevice_t *vtxDevice)
+{
+    uint8_t vtxPower;
+    if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
+        const vtxSettingsConfig_t settings = vtxGetSettings();
+        if (vtxPower != settings.power) {
+            vtxCommonSetPowerByIndex(vtxDevice, settings.power);
+            return true;
+        }
+    }
+    return false;
+}
+
+static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
+{
+    uint8_t pitOnOff;
+
+    bool        currPmSwitchState = false;
+    static bool prevPmSwitchState = false;
+
+    if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) {
+
+        // Not supported on INAV yet. It might not be that useful.
+#if 0
+        currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE);
+#endif
+
+        if (currPmSwitchState != prevPmSwitchState) {
+            prevPmSwitchState = currPmSwitchState;
+
+            if (currPmSwitchState) {
+#if defined(VTX_SETTINGS_FREQCMD)
+                if (vtxSettingsConfig()->pitModeFreq) {
+                    return false;
+                }
+#endif
+
+#if 0
+                if (isModeActivationConditionPresent(BOXVTXPITMODE)) {
+#endif
+                if (0) {
+                    if (!pitOnOff) {
+                        vtxCommonSetPitMode(vtxDevice, true);
+                        return true;
+                    }
+                }
+            } else {
+                if (pitOnOff) {
+                    vtxCommonSetPitMode(vtxDevice, false);
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+
+static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice)
+{
+    const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings();
+    vtxSettingsConfig_t vtxState = vtxSettingsState;
+
+    if (vtxSettingsState.band) {
+        vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel);
+#if defined(VTX_SETTINGS_FREQCMD)
+    } else {
+        vtxCommonGetFrequency(vtxDevice, &vtxState.freq);
+#endif
+    }
+
+    vtxCommonGetPowerIndex(vtxDevice, &vtxState.power);
+
+    return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t));
+}
+
+void vtxUpdate(timeUs_t currentTimeUs)
+{
+    static uint8_t currentSchedule = 0;
+
+    if (cliMode) {
+        return;
+    }
+
+    vtxDevice_t *vtxDevice = vtxCommonDevice();
+    if (vtxDevice) {
+        // Check input sources for config updates
+        vtxControlInputPoll();
+
+        const uint8_t startingSchedule = currentSchedule;
+        bool vtxUpdatePending = false;
+        do {
+            switch (currentSchedule) {
+                case VTX_PARAM_POWER:
+                    vtxUpdatePending = vtxProcessPower(vtxDevice);
+                    break;
+                case VTX_PARAM_BANDCHAN:
+                    if (vtxGetSettings().band) {
+                        vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice);
+#if defined(VTX_SETTINGS_FREQCMD)
+                    } else {
+                        vtxUpdatePending = vtxProcessFrequency(vtxDevice);
+#endif
+                    }
+                    break;
+                case VTX_PARAM_PITMODE:
+                    vtxUpdatePending = vtxProcessPitMode(vtxDevice);
+                    break;
+                case VTX_PARAM_CONFIRM:
+                    vtxUpdatePending = vtxProcessStateUpdate(vtxDevice);
+                    break;
+                default:
+                    break;
+            }
+            currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
+        } while (!vtxUpdatePending && currentSchedule != startingSchedule);
+
+        if (!ARMING_FLAG(ARMED) || vtxUpdatePending) {
+            vtxCommonProcess(vtxDevice, currentTimeUs);
+        }
+    }
+}
+
+#endif
--- a/src/main/io/vtx.h
+++ b/src/main/io/vtx.h
@ -0,0 +1,49 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "common/time.h"
+
+#include "config/parameter_group.h"
+
+typedef enum {
+    VTX_LOW_POWER_DISARM_OFF = 0,
+    VTX_LOW_POWER_DISARM_ALWAYS = 1,
+    VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM = 2, // Set low power until arming for the first time
+} vtxLowerPowerDisarm_e;
+
+typedef struct vtxSettingsConfig_s {
+    uint8_t band;           // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Racebande
+    uint8_t channel;        // 1-8
+    uint8_t power;          // 0 = lowest
+    uint16_t freq;          // sets freq in MHz if band=0
+    uint16_t pitModeFreq;   // sets out-of-range pitmode frequency
+    uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e
+} vtxSettingsConfig_t;
+
+PG_DECLARE(vtxSettingsConfig_t, vtxSettingsConfig);
+
+void vtxInit(void);
+void vtxUpdate(timeUs_t currentTimeUs);
--- a/src/main/io/vtx_control.c
+++ b/src/main/io/vtx_control.c
@ -37,9 +37,14 @@
 #include "io/vtx_control.h"


-#if defined(VTX_CONTROL) && defined(VTX_COMMON)
+#if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)

-PG_REGISTER(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2);
+
+PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
+//    .vtxChannelActivationConditions = { 0 },
+      .halfDuplex = true,
+);

 static uint8_t locked = 0;

@ -48,6 +53,13 @@ void vtxControlInit(void)
    // NOTHING TO DO
 }

+void vtxControlInputPoll(void)
+{
+  // Check variuos input sources for VTX config updates
+
+  // XXX: None supported in INAV
+}
+
 static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep)
 {
    if (ARMING_FLAG(ARMED)) {
@ -55,9 +67,12 @@ static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep)
    }

    if (!locked) {
+        vtxDevice_t *vtxDevice = vtxCommonDevice();
+        if (vtxDevice) {
            uint8_t band = 0, channel = 0;
-        vtxCommonGetBandAndChannel(&band, &channel);
-        vtxCommonSetBandAndChannel(band + bandStep, channel + channelStep);
+            vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
+            vtxCommonSetBandAndChannel(vtxDevice, band + bandStep, channel + channelStep);
+        }
    }
 }

@ -88,6 +103,8 @@ void vtxUpdateActivatedChannel(void)
    }

    if (!locked) {
+        vtxDevice_t *vtxDevice = vtxCommonDevice();
+        if (vtxDevice) {
            static uint8_t lastIndex = -1;

            for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) {
@ -97,19 +114,26 @@ void vtxUpdateActivatedChannel(void)
                    && index != lastIndex) {
                    lastIndex = index;

-                vtxCommonSetBandAndChannel(vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel);
+                    vtxCommonSetBandAndChannel(vtxDevice, vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel);
                    break;
                }
            }
        }
    }
+}

 void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep)
 {
+    vtxDevice_t *vtxDevice = vtxCommonDevice();
+
+    if (!vtxDevice) {
+        return;
+    }
+
    uint8_t band = 0, channel = 0;
    vtxDeviceCapability_t capability;

-    bool haveAllNeededInfo = vtxCommonGetBandAndChannel(&band, &channel) && vtxCommonGetDeviceCapability(&capability);
+    bool haveAllNeededInfo = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel) && vtxCommonGetDeviceCapability(vtxDevice, &capability);
    if (!haveAllNeededInfo) {
        return;
    }
@ -128,15 +152,21 @@ void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep)
        newBand = capability.bandCount;
    }

-    vtxCommonSetBandAndChannel(newBand, newChannel);
+    vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel);
 }

 void vtxCyclePower(const uint8_t powerStep)
 {
+    vtxDevice_t *vtxDevice = vtxCommonDevice();
+
+    if (!vtxDevice) {
+        return;
+    }
+
    uint8_t power = 0;
    vtxDeviceCapability_t capability;

-    bool haveAllNeededInfo = vtxCommonGetPowerIndex(&power) && vtxCommonGetDeviceCapability(&capability);
+    bool haveAllNeededInfo = vtxCommonGetPowerIndex(vtxDevice, &power) && vtxCommonGetDeviceCapability(vtxDevice, &capability);
    if (!haveAllNeededInfo) {
        return;
    }
@ -148,7 +178,7 @@ void vtxCyclePower(const uint8_t powerStep)
        newPower = capability.powerCount;
    }

-    vtxCommonSetPowerByIndex(newPower);
+    vtxCommonSetPowerByIndex(vtxDevice, newPower);
 }

 #endif
--- a/src/main/io/vtx_control.h
+++ b/src/main/io/vtx_control.h
@ -30,11 +30,13 @@ typedef struct vtxChannelActivationCondition_s {

 typedef struct vtxConfig_s {
    vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT];
+    uint8_t halfDuplex;
 } vtxConfig_t;

 PG_DECLARE(vtxConfig_t, vtxConfig);

 void vtxControlInit(void);
+void vtxControlInputPoll(void);

 void vtxIncrementBand(void);
 void vtxDecrementBand(void);
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@ -1,18 +1,21 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of Cleanflight and Betaflight.
 *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
 *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
 */

 /* Created by jflyper */
@ -20,64 +23,64 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <ctype.h>

 #include "platform.h"

-#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
-#include "build/build_config.h"
+#if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL)
+
 #include "build/debug.h"

 #include "cms/cms.h"
-#include "cms/cms_types.h"
 #include "cms/cms_menu_vtx_smartaudio.h"

-#include "common/axis.h"
+#include "common/maths.h"
 #include "common/printf.h"
 #include "common/utils.h"

-#include "config/parameter_group.h"
-#include "config/parameter_group_ids.h"
-
-#include "drivers/system.h"
-#include "drivers/serial.h"
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"

-#include "fc/rc_controls.h"
-#include "fc/runtime_config.h"
-
-#include "flight/pid.h"
-#include "config/config_master.h"
-
 #include "io/serial.h"
+#include "io/vtx.h"
+#include "io/vtx_control.h"
 #include "io/vtx_smartaudio.h"
 #include "io/vtx_string.h"

-//#define SMARTAUDIO_DEBUG_MONITOR

+// Timing parameters
+// Note that vtxSAProcess() is normally called at 200ms interval
+#define SMARTAUDIO_CMD_TIMEOUT       120    // Time until the command is considered lost
+#define SMARTAUDIO_POLLING_INTERVAL  150    // Minimum time between state polling
+#define SMARTAUDIO_POLLING_WINDOW   1000    // Time window after command polling for state change
+
+//#define USE_SMARTAUDIO_DPRINTF
+//#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1
+
+#ifdef USE_SMARTAUDIO_DPRINTF
+serialPort_t *debugSerialPort = NULL;
+#endif // USE_SMARTAUDIO_DPRINTF

 static serialPort_t *smartAudioSerialPort = NULL;

-#if defined(USE_CMS) || defined(VTX_COMMON)
-static const char * const saPowerNames[] = {
+#if defined(USE_CMS) || defined(USE_VTX_COMMON)
+const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = {
    "---", "25 ", "200", "500", "800",
 };
 #endif

-#ifdef VTX_COMMON
+#ifdef USE_VTX_COMMON
 static const vtxVTable_t saVTable;    // Forward
 static vtxDevice_t vtxSmartAudio = {
    .vTable = &saVTable,
-    .capability = {
-        .bandCount = 5,
-        .channelCount = 8,
-        .powerCount = 4,
-    },
+    .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
+    .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
+    .capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
    .bandNames = (char **)vtx58BandNames,
    .channelNames = (char **)vtx58ChannelNames,
    .powerNames = (char **)saPowerNames,
 };
-#endif // VTX_COMMON
+#endif

 // SmartAudio command and response codes
 enum {
@ -93,10 +96,18 @@ enum {
 // This is not a good design; can't distinguish command from response this way.
 #define SACMD(cmd) (((cmd) << 1) | 1)

+
 #define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE)
 #define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE))
 #define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE))

+
+// convert between 'saDevice.channel' and band/channel values
+#define SA_DEVICE_CHVAL_TO_BAND(val) ((val) / (uint8_t)8)
+#define SA_DEVICE_CHVAL_TO_CHANNEL(val) ((val) % (uint8_t)8)
+#define SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel) ((band) * (uint8_t)8 + (channel))
+
+
 // Statistical counters, for user side trouble shooting.

 smartAudioStat_t saStat = {
@ -109,7 +120,7 @@ smartAudioStat_t saStat = {
    .badcode = 0,
 };

-saPowerTable_t saPowerTable[] = {
+saPowerTable_t saPowerTable[VTX_SMARTAUDIO_POWER_COUNT] = {
    {  25,   7,   0 },
    { 200,  16,   1 },
    { 500,  25,   2 },
@ -169,9 +180,9 @@ static uint8_t CRC8(const uint8_t *data, const int8_t len)
 }


+#ifdef USE_SMARTAUDIO_DPRINTF
 static void saPrintSettings(void)
 {
-#ifdef SMARTAUDIO_DPRINTF
    dprintf(("Current status: version: %d\r\n", saDevice.version));
    dprintf(("  mode(0x%x): fmode=%s", saDevice.mode,  (saDevice.mode & 1) ? "freq" : "chan"));
    dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off"));
@ -184,18 +195,17 @@ static void saPrintSettings(void)
    dprintf(("power: %d ", saDevice.power));
    dprintf(("pitfreq: %d ", saDevice.orfreq));
    dprintf(("\r\n"));
-#endif
 }
+#endif

 int saDacToPowerIndex(int dac)
 {
-    int idx;
-
-    for (idx = 3 ; idx >= 0 ; idx--) {
-        if (saPowerTable[idx].valueV1 <= dac)
-            return(idx);
+    for (int idx = 3 ; idx >= 0 ; idx--) {
+        if (saPowerTable[idx].valueV1 <= dac) {
+            return idx;
        }
-    return(0);
+    }
+    return 0;
 }

 //
@ -208,13 +218,12 @@ uint16_t sa_smartbaud = SMARTBAUD_MIN;
 static int sa_adjdir = 1; // -1=going down, 1=going up
 static int sa_baudstep = 50;

-#define SMARTAUDIO_CMD_TIMEOUT    120
-
 static void saAutobaud(void)
 {
-    if (saStat.pktsent < 10)
+    if (saStat.pktsent < 10) {
        // Not enough samples collected
        return;
+    }

 #if 0
    dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
@ -250,7 +259,7 @@ static void saAutobaud(void)

 // Transport level variables

-static uint32_t sa_lastTransmission = 0;
+static timeUs_t sa_lastTransmissionMs = 0;
 static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
 static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
 static int sa_oslen;         // And associate length
@ -271,8 +280,9 @@ static void saProcessResponse(uint8_t *buf, int len)
    switch (resp) {
    case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
    case SA_CMD_GET_SETTINGS:    // Version 1 Get Settings
-        if (len < 7)
+        if (len < 7) {
            break;
+        }

        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
        saDevice.channel = buf[2];
@ -280,12 +290,10 @@ static void saProcessResponse(uint8_t *buf, int len)
        saDevice.mode = buf[4];
        saDevice.freq = (buf[5] << 8)|buf[6];

-#ifdef SMARTAUDIO_DEBUG_MONITOR
-        debug[0] = saDevice.version * 100 + saDevice.mode;
-        debug[1] = saDevice.channel;
-        debug[2] = saDevice.freq;
-        debug[3] = saDevice.power;
-#endif
+        DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode);
+        DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel);
+        DEBUG_SET(DEBUG_SMARTAUDIO, 2, saDevice.freq);
+        DEBUG_SET(DEBUG_SMARTAUDIO, 3, saDevice.power);
        break;

    case SA_CMD_SET_POWER: // Set Power
@ -295,10 +303,11 @@ static void saProcessResponse(uint8_t *buf, int len)
        break;

    case SA_CMD_SET_FREQ: // Set Frequency
-        if (len < 5)
+        if (len < 5) {
            break;
+        }

-        uint16_t freq = (buf[2] << 8)|buf[3];
+        const uint16_t freq = (buf[2] << 8)|buf[3];

        if (freq & SA_FREQ_GETPIT) {
            saDevice.orfreq = freq & SA_FREQ_MASK;
@ -321,12 +330,17 @@ static void saProcessResponse(uint8_t *buf, int len)
        return;
    }

-    // Debug
-    if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t)))
+    if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
+#ifdef USE_CMS    //if changes then trigger saCms update
+        saCmsResetOpmodel();
+#endif
+#ifdef USE_SMARTAUDIO_DPRINTF    // Debug
        saPrintSettings();
+#endif
+    }
    saDevicePrev = saDevice;

-#ifdef VTX_COMMON
+#ifdef USE_VTX_COMMON
    // Todo: Update states in saVtxDevice?
 #endif

@ -420,16 +434,20 @@ static void saReceiveFramer(uint8_t c)

 static void saSendFrame(uint8_t *buf, int len)
 {
-    int i;
-
+    switch (smartAudioSerialPort->identifier) {
+        case SERIAL_PORT_SOFTSERIAL1:
+        case SERIAL_PORT_SOFTSERIAL2:
+            break;
+        default:
            serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
+            break;
+    }

-    for (i = 0 ; i < len ; i++)
+    for (int i = 0 ; i < len ; i++) {
        serialWrite(smartAudioSerialPort, buf[i]);
+    }

-    serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
-
-    sa_lastTransmission = millis();
+    sa_lastTransmissionMs = millis();
    saStat.pktsent++;
 }

@ -460,10 +478,9 @@ static void saResendCmd(void)

 static void saSendCmd(uint8_t *buf, int len)
 {
-    int i;
-
-    for (i = 0 ; i < len ; i++)
+    for (int i = 0 ; i < len ; i++) {
        sa_osbuf[i] = buf[i];
+    }

    sa_oslen = len;
    sa_outstanding = (buf[2] >> 1);
@ -478,22 +495,11 @@ typedef struct saCmdQueue_s {
    int len;
 } saCmdQueue_t;

-#define SA_QSIZE 4     // 1 heartbeat (GetSettings) + 2 commands + 1 slack
+#define SA_QSIZE 6     // 1 heartbeat (GetSettings) + 2 commands + 1 slack
 static saCmdQueue_t sa_queue[SA_QSIZE];
 static uint8_t sa_qhead = 0;
 static uint8_t sa_qtail = 0;

-#ifdef DPRINTF_SMARTAUDIO
-static int saQueueLength(void)
-{
-    if (sa_qhead >= sa_qtail) {
-        return sa_qhead - sa_qtail;
-    } else {
-        return SA_QSIZE + sa_qhead - sa_qtail;
-    }
-}
-#endif
-
 static bool saQueueEmpty(void)
 {
    return sa_qhead == sa_qtail;
@ -506,8 +512,9 @@ static bool saQueueFull(void)

 static void saQueueCmd(uint8_t *buf, int len)
 {
-    if (saQueueFull())
+    if (saQueueFull()) {
         return;
+    }

    sa_queue[sa_qhead].buf = buf;
    sa_queue[sa_qhead].len = len;
@ -516,8 +523,9 @@ static void saQueueCmd(uint8_t *buf, int len)

 static void saSendQueue(void)
 {
-    if (saQueueEmpty())
+    if (saQueueEmpty()) {
         return;
+    }

    saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
    sa_qtail = (sa_qtail + 1) % SA_QSIZE;
@ -532,9 +540,15 @@ static void saGetSettings(void)
    saQueueCmd(bufGetSettings, 5);
 }

-void saSetFreq(uint16_t freq)
+static bool saValidateFreq(uint16_t freq)
+{
+    return (freq >= VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ && freq <= VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ);
+}
+
+static void saDoDevSetFreq(uint16_t freq)
 {
    static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
+    static uint8_t switchBuf[7];

    if (freq & SA_FREQ_GETPIT) {
        dprintf(("smartAudioSetFreq: GETPIT\r\n"));
@ -548,31 +562,60 @@ void saSetFreq(uint16_t freq)
    buf[5] = freq & 0xff;
    buf[6] = CRC8(buf, 6);

+    // Need to work around apparent SmartAudio bug when going from 'channel'
+    // to 'user-freq' mode, where the set-freq command will fail if the freq
+    // value is unchanged from the previous 'user-freq' mode
+    if ((saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) == 0 && freq == saDevice.freq) {
+        memcpy(&switchBuf, &buf, sizeof(buf));
+        const uint16_t switchFreq = freq + ((freq == VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ) ? -1 : 1);
+        switchBuf[4] = (switchFreq >> 8);
+        switchBuf[5] = switchFreq & 0xff;
+        switchBuf[6] = CRC8(switchBuf, 6);
+
+        saQueueCmd(switchBuf, 7);
+    }
+
    saQueueCmd(buf, 7);
 }

-#if 0
-static void saSetPitFreq(uint16_t freq)
+void saSetFreq(uint16_t freq)
 {
-    saSetFreq(freq | SA_FREQ_SETPIT);
+    saDoDevSetFreq(freq);
 }

+void saSetPitFreq(uint16_t freq)
+{
+    saDoDevSetFreq(freq | SA_FREQ_SETPIT);
+}
+
+#if 0
 static void saGetPitFreq(void)
 {
-    saSetFreq(SA_FREQ_GETPIT);
+    saDoDevSetFreq(SA_FREQ_GETPIT);
 }
 #endif

-void saSetBandAndChannel(uint8_t band, uint8_t channel)
+static bool saValidateBandAndChannel(uint8_t band, uint8_t channel)
+{
+    return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= VTX_SMARTAUDIO_MAX_BAND &&
+             channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
+}
+
+static void saDevSetBandAndChannel(uint8_t band, uint8_t channel)
 {
    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };

-    buf[4] = band * 8 + channel;
+    buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
    buf[5] = CRC8(buf, 5);

    saQueueCmd(buf, 6);
 }

+void saSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    saDevSetBandAndChannel(band, channel);
+}
+
 void saSetMode(int mode)
 {
    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
@ -583,7 +626,7 @@ void saSetMode(int mode)
    saQueueCmd(buf, 6);
 }

-void saSetPowerByIndex(uint8_t index)
+static void saDevSetPowerByIndex(uint8_t index)
 {
    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };

@ -594,17 +637,23 @@ void saSetPowerByIndex(uint8_t index)
        return;
    }

-    if (index > 3)
+    if (index >= VTX_SMARTAUDIO_POWER_COUNT) {
        return;
+    }

    buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2;
    buf[5] = CRC8(buf, 5);
    saQueueCmd(buf, 6);
 }

+void saSetPowerByIndex(uint8_t index)
+{
+    saDevSetPowerByIndex(index);
+}
+
 bool vtxSmartAudioInit(void)
 {
-#ifdef SMARTAUDIO_DPRINTF
+#ifdef USE_SMARTAUDIO_DPRINTF
    // Setup debugSerialPort

    debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, NULL, 115200, MODE_RXTX, 0);
@ -616,24 +665,40 @@ bool vtxSmartAudioInit(void)

    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO);
    if (portConfig) {
-        smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP);
+        portOptions_t portOptions = SERIAL_STOPBITS_2 | SERIAL_BIDIR_NOPULL;
+#if defined(USE_VTX_COMMON)
+        portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR);
+#else
+        portOptions = SERIAL_BIDIR;
+#endif
+
+        smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions);
    }

    if (!smartAudioSerialPort) {
        return false;
    }

-    vtxCommonRegisterDevice(&vtxSmartAudio);
+    vtxCommonSetDevice(&vtxSmartAudio);

    return true;
 }

-void vtxSAProcess(uint32_t now)
-{
-    static bool initialSent = false;
+#define SA_INITPHASE_START         0
+#define SA_INITPHASE_WAIT_SETTINGS 1 // SA_CMD_GET_SETTINGS was sent and waiting for reply.
+#define SA_INITPHASE_WAIT_PITFREQ  2 // SA_FREQ_GETPIT sent and waiting for reply.
+#define SA_INITPHASE_DONE          3

-    if (smartAudioSerialPort == NULL)
+static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
+{
+    UNUSED(vtxDevice);
+    UNUSED(currentTimeUs);
+
+    static char initPhase = SA_INITPHASE_START;
+
+    if (smartAudioSerialPort == NULL) {
        return;
+    }

    while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
        uint8_t c = serialRead(smartAudioSerialPort);
@ -643,72 +708,84 @@ void vtxSAProcess(uint32_t now)
    // Re-evaluate baudrate after each frame reception
    saAutobaud();

-    if (!initialSent) {
+    switch (initPhase) {
+    case SA_INITPHASE_START:
        saGetSettings();
-        saSetFreq(SA_FREQ_GETPIT);
-        saSendQueue();
-        initialSent = true;
-        return;
+        //saSendQueue();
+        initPhase = SA_INITPHASE_WAIT_SETTINGS;
+        break;
+
+    case SA_INITPHASE_WAIT_SETTINGS:
+        // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ,
+        // and put the device into user frequency mode with uninitialized freq.
+        if (saDevice.version) {
+            if (saDevice.version == 2) {
+                saDoDevSetFreq(SA_FREQ_GETPIT);
+                initPhase = SA_INITPHASE_WAIT_PITFREQ;
+            } else {
+                initPhase = SA_INITPHASE_DONE;
+            }
+        }
+        break;
+
+    case SA_INITPHASE_WAIT_PITFREQ:
+        if (saDevice.orfreq) {
+            initPhase = SA_INITPHASE_DONE;
+        }
+        break;
+
+    case SA_INITPHASE_DONE:
+        break;
    }

-    if ((sa_outstanding != SA_CMD_NONE)
-            && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
+    // Command queue control
+
+    timeMs_t nowMs = millis();             // Don't substitute with "currentTimeUs / 1000"; sa_lastTransmissionMs is based on millis().
+    static timeMs_t lastCommandSentMs = 0; // Last non-GET_SETTINGS sent
+
+    if ((sa_outstanding != SA_CMD_NONE) && (nowMs - sa_lastTransmissionMs > SMARTAUDIO_CMD_TIMEOUT)) {
        // Last command timed out
        // dprintf(("process: resending 0x%x\r\n", sa_outstanding));
        // XXX Todo: Resend termination and possible offline transition
        saResendCmd();
+    lastCommandSentMs = nowMs;
    } else if (!saQueueEmpty()) {
        // Command pending. Send it.
        // dprintf(("process: sending queue\r\n"));
        saSendQueue();
-    } else if (now - sa_lastTransmission >= 1000) {
-        // Heart beat for autobauding
-        //dprintf(("process: sending heartbeat\r\n"));
+    lastCommandSentMs = nowMs;
+    } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) {
+    //dprintf(("process: sending status change polling\r\n"));
    saGetSettings();
    saSendQueue();
    }
-
-#ifdef SMARTAUDIO_TEST_VTX_COMMON
-    // Testing VTX_COMMON API
-    {
-        static uint32_t lastMonitorUs = 0;
-        if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000)
-            return;
-
-        static uint8_t monBand;
-        static uint8_t monChan;
-        static uint8_t monPower;
-
-        vtxCommonGetBandAndChannel(&monBand, &monChan);
-        vtxCommonGetPowerIndex(&monPower);
-        debug[0] = monBand;
-        debug[1] = monChan;
-        debug[2] = monPower;
-    }
-#endif
 }

-#ifdef VTX_COMMON
+#ifdef USE_VTX_COMMON
 // Interface to common VTX API

-vtxDevType_e vtxSAGetDeviceType(void)
+vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice)
 {
+    UNUSED(vtxDevice);
    return VTXDEV_SMARTAUDIO;
 }

-bool vtxSAIsReady(void)
+static bool vtxSAIsReady(const vtxDevice_t *vtxDevice)
 {
-    return !(saDevice.version == 0);
+    return vtxDevice!=NULL && !(saDevice.version == 0);
 }

-void vtxSASetBandAndChannel(uint8_t band, uint8_t channel)
+static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 {
-    if (band && channel)
+    UNUSED(vtxDevice);
+    if (saValidateBandAndChannel(band, channel)) {
        saSetBandAndChannel(band - 1, channel - 1);
    }
+}

-void vtxSASetPowerByIndex(uint8_t index)
+static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
+    UNUSED(vtxDevice);
    if (index == 0) {
        // SmartAudio doesn't support power off.
        return;
@ -717,10 +794,11 @@ void vtxSASetPowerByIndex(uint8_t index)
    saSetPowerByIndex(index - 1);
 }

-void vtxSASetPitMode(uint8_t onoff)
+static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 {
-    if (!(vtxSAIsReady() && (saDevice.version == 2)))
+    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
        return;
+    }

    if (onoff) {
        // SmartAudio can not turn pit mode on by software.
@ -729,45 +807,74 @@ void vtxSASetPitMode(uint8_t onoff)

    uint8_t newmode = SA_MODE_CLR_PITMODE;

-    if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE)
+    if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
        newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
+    }

-    if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
+    if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) {
        newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
+    }

    saSetMode(newmode);

    return;
 }

-bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
+static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
 {
-    if (!vtxSAIsReady())
-        return false;
+    UNUSED(vtxDevice);
+    if (saValidateFreq(freq)) {
+        saSetMode(0);        //need to be in FREE mode to set freq
+        saSetFreq(freq);
+    }
+}

-    *pBand = (saDevice.channel / 8) + 1;
-    *pChannel = (saDevice.channel % 8) + 1;
+static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
+{
+    if (!vtxSAIsReady(vtxDevice)) {
+        return false;
+    }
+
+    // if in user-freq mode then report band as zero
+    *pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 :
+        (SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1);
+    *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1;
    return true;
 }

-bool vtxSAGetPowerIndex(uint8_t *pIndex)
+static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 {
-    if (!vtxSAIsReady())
+    if (!vtxSAIsReady(vtxDevice)) {
        return false;
+    }

    *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
    return true;
 }

-bool vtxSAGetPitMode(uint8_t *pOnOff)
+static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!(vtxSAIsReady() && (saDevice.version == 2)))
+    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
        return false;
+    }

    *pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
    return true;
 }

+static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
+{
+    if (!vtxSAIsReady(vtxDevice)) {
+        return false;
+    }
+
+    // if not in user-freq mode then convert band/chan to frequency
+    *pFreq = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? saDevice.freq :
+        vtx58_Bandchan2Freq(SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1,
+        SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1);
+    return true;
+}
+
 static const vtxVTable_t saVTable = {
    .process = vtxSAProcess,
    .getDeviceType = vtxSAGetDeviceType,
@ -775,9 +882,11 @@ static const vtxVTable_t saVTable = {
    .setBandAndChannel = vtxSASetBandAndChannel,
    .setPowerByIndex = vtxSASetPowerByIndex,
    .setPitMode = vtxSASetPitMode,
+    .setFrequency = vtxSASetFreq,
    .getBandAndChannel = vtxSAGetBandAndChannel,
    .getPowerIndex = vtxSAGetPowerIndex,
    .getPitMode = vtxSAGetPitMode,
+    .getFrequency = vtxSAGetFreq,
 };
 #endif // VTX_COMMON

--- a/src/main/io/vtx_smartaudio.h
+++ b/src/main/io/vtx_smartaudio.h
@ -1,18 +1,21 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of Cleanflight and Betaflight.
 *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
 *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
 */

 #pragma once
@ -22,6 +25,19 @@

 #include "platform.h"

+#define VTX_SMARTAUDIO_MIN_BAND 1
+#define VTX_SMARTAUDIO_MAX_BAND 5
+#define VTX_SMARTAUDIO_MIN_CHANNEL 1
+#define VTX_SMARTAUDIO_MAX_CHANNEL 8
+
+#define VTX_SMARTAUDIO_BAND_COUNT (VTX_SMARTAUDIO_MAX_BAND - VTX_SMARTAUDIO_MIN_BAND + 1)
+#define VTX_SMARTAUDIO_CHANNEL_COUNT (VTX_SMARTAUDIO_MAX_CHANNEL - VTX_SMARTAUDIO_MIN_CHANNEL + 1)
+
+#define VTX_SMARTAUDIO_POWER_COUNT 4
+#define VTX_SMARTAUDIO_DEFAULT_POWER 1
+
+#define VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ 5000        //min freq in MHz
+#define VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ 5999        //max freq in MHz

 // opmode flags, GET side
 #define SA_MODE_GET_FREQ_BY_FREQ            1
@ -73,9 +89,11 @@ typedef struct smartAudioStat_s {

 extern smartAudioDevice_t saDevice;
 extern saPowerTable_t saPowerTable[];
+extern const char * const saPowerNames[];
 extern smartAudioStat_t saStat;
-extern bool saDeferred;
+
 extern uint16_t sa_smartbaud;
+extern bool saDeferred;

 int saDacToPowerIndex(int dac);
 void saSetBandAndChannel(uint8_t band, uint8_t channel);
@ -85,17 +103,9 @@ void saSetFreq(uint16_t freq);
 void saSetPitFreq(uint16_t freq);
 bool vtxSmartAudioInit(void);

-#ifdef SMARTAUDIO_DPRINTF
-#ifdef OMNIBUSF4
-#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
-#else
-#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1
-#endif
+#ifdef USE_SMARTAUDIO_DPRINTF
 extern serialPort_t *debugSerialPort;
 #define dprintf(x) if (debugSerialPort) printf x
 #else
 #define dprintf(x)
-#endif // SMARTAUDIO_DPRINTF
-
-int saDacToPowerIndex(int dac);
-bool vtxSmartAudioInit(void);
+#endif // USE_SMARTAUDIO_DPRINTF
--- a/src/main/io/vtx_string.c
+++ b/src/main/io/vtx_string.c
@ -25,9 +25,12 @@
 #include "platform.h"
 #include "build/debug.h"

-#if defined(VTX_COMMON)
+#if defined(USE_VTX_COMMON)

-const uint16_t vtx58frequencyTable[5][8] =
+#define VTX_STRING_BAND_COUNT 5
+#define VTX_STRING_CHAN_COUNT 8
+
+const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] =
 {
    { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A
    { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B
@ -36,7 +39,7 @@ const uint16_t vtx58frequencyTable[5][8] =
    { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand
 };

-const char * const vtx58BandNames[] = {
+const char * const vtx58BandNames[VTX_STRING_BAND_COUNT + 1] = {
    "--------",
    "BOSCAM A",
    "BOSCAM B",
@ -45,9 +48,9 @@ const char * const vtx58BandNames[] = {
    "RACEBAND",
 };

-const char vtx58BandLetter[] = "-ABEFR";
+const char vtx58BandLetter[VTX_STRING_BAND_COUNT + 1] = "-ABEFR";

-const char * const vtx58ChannelNames[] = {
+const char * const vtx58ChannelNames[VTX_STRING_CHAN_COUNT + 1] = {
    "-", "1", "2", "3", "4", "5", "6", "7", "8",
 };

@ -74,4 +77,17 @@ bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
    return false;
 }

+//Converts band and channel values to a frequency (in MHz) value.
+// band: Band value (1 to 5).
+// channel:  Channel value (1 to 8).
+// Returns frequency value (in MHz), or 0 if band/channel out of range.
+uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel)
+{
+    if (band > 0 && band <= VTX_STRING_BAND_COUNT &&
+                          channel > 0 && channel <= VTX_STRING_CHAN_COUNT) {
+        return vtx58frequencyTable[band - 1][channel - 1];
+    }
+    return 0;
+}
+
 #endif
--- a/src/main/io/vtx_string.h
+++ b/src/main/io/vtx_string.h
@ -8,3 +8,4 @@ extern const char * const vtx58ChannelNames[];
 extern const char vtx58BandLetter[];

 bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel);
+uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel);
--- a/src/main/io/vtx_tramp.c
+++ b/src/main/io/vtx_tramp.c
@ -1,61 +1,66 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of Cleanflight and Betaflight.
 *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
 *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
 */

 /* Created by jflyper */

 #include <stdbool.h>
 #include <stdint.h>
+#include <ctype.h>
 #include <string.h>

 #include "platform.h"

-#if defined(VTX_TRAMP) && defined(VTX_CONTROL)
+#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL)

 #include "build/debug.h"

+#include "common/maths.h"
 #include "common/utils.h"

+#include "cms/cms_menu_vtx_tramp.h"
+
 #include "drivers/vtx_common.h"

 #include "io/serial.h"
 #include "io/vtx_tramp.h"
+#include "io/vtx_control.h"
+#include "io/vtx.h"
 #include "io/vtx_string.h"

-#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR)
-
-#if defined(USE_CMS) || defined(VTX_COMMON)
-const uint16_t trampPowerTable[] = {
+#if defined(USE_CMS) || defined(USE_VTX_COMMON)
+const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
    25, 100, 200, 400, 600
 };

-const char * const trampPowerNames[] = {
+const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
    "---", "25 ", "100", "200", "400", "600"
 };
 #endif

-#if defined(VTX_COMMON)
+#if defined(USE_VTX_COMMON)
 static const vtxVTable_t trampVTable; // forward
 static vtxDevice_t vtxTramp = {
    .vTable = &trampVTable,
-    .capability = {
-        .bandCount = 5,
-        .channelCount = 8,
-        .powerCount = sizeof(trampPowerTable),
-    },
+    .capability.bandCount = VTX_TRAMP_BAND_COUNT,
+    .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT,
+    .capability.powerCount = sizeof(trampPowerTable),
    .bandNames = (char **)vtx58BandNames,
    .channelNames = (char **)vtx58ChannelNames,
    .powerNames = (char **)trampPowerNames,
@ -76,38 +81,42 @@ typedef enum {

 trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;

-// TODO: These fields are currently removed by the compiler because they're
-// never read. Decide if we want to use them for something or remove them.
-static uint16_t trampRFFreqMin;
-static uint16_t trampRFFreqMax;
-static uint16_t trampRFPowerMax;
-
+uint32_t trampRFFreqMin;
+uint32_t trampRFFreqMax;
+uint32_t trampRFPowerMax;

 trampData_t trampData;

 // Maximum number of requests sent to try a config change
 #define TRAMP_MAX_RETRIES 2

-uint16_t trampConfFreq = 0;
+uint32_t trampConfFreq = 0;
 uint8_t  trampFreqRetries = 0;

 uint16_t trampConfPower = 0;
 uint8_t  trampPowerRetries = 0;

+static void trampWriteBuf(uint8_t *buf)
+{
+    serialWriteBuf(trampSerialPort, buf, 16);
+}
+
 static uint8_t trampChecksum(uint8_t *trampBuf)
 {
    uint8_t cksum = 0;

-    for (int i = 1 ; i < 14 ; i++)
+    for (int i = 1 ; i < 14 ; i++) {
        cksum += trampBuf[i];
+    }

    return cksum;
 }

 void trampCmdU16(uint8_t cmd, uint16_t param)
 {
-    if (!trampSerialPort)
+    if (!trampSerialPort) {
        return;
+    }

    uint8_t trampReqBuffer[16];
    memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
@ -116,14 +125,26 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
    trampReqBuffer[2] = param & 0xff;
    trampReqBuffer[3] = (param >> 8) & 0xff;
    trampReqBuffer[14] = trampChecksum(trampReqBuffer);
-    serialWriteBuf(trampSerialPort, trampReqBuffer, sizeof(trampReqBuffer));
+    trampWriteBuf(trampReqBuffer);
+}
+
+static bool trampValidateFreq(uint16_t freq)
+{
+    return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
+}
+
+static void trampDevSetFreq(uint16_t freq)
+{
+    trampConfFreq = freq;
+    if (trampConfFreq != trampData.curFreq) {
+        trampFreqRetries = TRAMP_MAX_RETRIES;
+    }
 }

 void trampSetFreq(uint16_t freq)
 {
-    trampConfFreq = freq;
-    if(trampConfFreq != trampData.curFreq)
-        trampFreqRetries = TRAMP_MAX_RETRIES;
+    trampData.setByFreqFlag = true;         //set freq via MHz value
+    trampDevSetFreq(freq);
 }

 void trampSendFreq(uint16_t freq)
@ -131,38 +152,58 @@ void trampSendFreq(uint16_t freq)
    trampCmdU16('F', freq);
 }

+static bool trampValidateBandAndChannel(uint8_t band, uint8_t channel)
+{
+    return (band >= VTX_TRAMP_MIN_BAND && band <= VTX_TRAMP_MAX_BAND &&
+            channel >= VTX_TRAMP_MIN_CHANNEL && channel <= VTX_TRAMP_MAX_CHANNEL);
+}
+
+static void trampDevSetBandAndChannel(uint8_t band, uint8_t channel)
+{
+    trampDevSetFreq(vtx58_Bandchan2Freq(band, channel));
+}
+
 void trampSetBandAndChannel(uint8_t band, uint8_t channel)
 {
-    trampSetFreq(vtx58frequencyTable[band - 1][channel - 1]);
+    trampData.setByFreqFlag = false;        //set freq via band/channel
+    trampDevSetBandAndChannel(band, channel);
 }

 void trampSetRFPower(uint16_t level)
 {
    trampConfPower = level;
-    if(trampConfPower != trampData.power)
+    if (trampConfPower != trampData.power) {
        trampPowerRetries = TRAMP_MAX_RETRIES;
    }
+}

 void trampSendRFPower(uint16_t level)
 {
    trampCmdU16('P', level);
 }

-bool trampIsAvailable(void)
-{
-    return trampStatus != TRAMP_STATUS_BAD_DEVICE && trampStatus != TRAMP_STATUS_OFFLINE;
-}
-
 // return false if error
 bool trampCommitChanges(void)
 {
-    if(trampStatus != TRAMP_STATUS_ONLINE)
+    if (trampStatus != TRAMP_STATUS_ONLINE) {
        return false;
+    }

    trampStatus = TRAMP_STATUS_SET_FREQ_PW;
    return true;
 }

+// return false if index out of range
+static bool trampDevSetPowerByIndex(uint8_t index)
+{
+    if (index > 0 && index <= sizeof(trampPowerTable)) {
+        trampSetRFPower(trampPowerTable[index - 1]);
+        trampCommitChanges();
+        return true;
+    }
+    return false;
+}
+
 void trampSetPitMode(uint8_t onoff)
 {
    trampCmdU16('I', onoff ? 0 : 1);
@ -171,12 +212,12 @@ void trampSetPitMode(uint8_t onoff)
 // returns completed response code
 char trampHandleResponse(void)
 {
-    uint8_t respCode = trampRespBuffer[1];
+    const uint8_t respCode = trampRespBuffer[1];

    switch (respCode) {
    case 'r':
        {
-            uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+            const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
            if (min_freq != 0) {
                trampRFFreqMin = min_freq;
                trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
@ -190,13 +231,17 @@ char trampHandleResponse(void)

    case 'v':
        {
-            uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+            const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
            if (freq != 0) {
                trampData.curFreq = freq;
                trampData.configuredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
                trampData.pitMode = trampRespBuffer[7];
                trampData.power = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
-                vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel);
+
+                // if no band/chan match then make sure set-by-freq mode is flagged
+                if (!vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel)) {
+                    trampData.setByFreqFlag = true;
+                }

                if (trampConfFreq == 0)  trampConfFreq  = trampData.curFreq;
                if (trampConfPower == 0) trampConfPower = trampData.power;
@ -209,7 +254,7 @@ char trampHandleResponse(void)

    case 's':
        {
-            uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
+            const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
            if (temp != 0) {
                trampData.temperature = temp;
                return 's';
@ -238,22 +283,24 @@ static void trampResetReceiver(void)

 static bool trampIsValidResponseCode(uint8_t code)
 {
-    if (code == 'r' || code == 'v' || code == 's')
+    if (code == 'r' || code == 'v' || code == 's') {
        return true;
-    else
+    } else {
        return false;
    }
+}

 // returns completed response code or 0
 static char trampReceive(uint32_t currentTimeUs)
 {
    UNUSED(currentTimeUs);

-    if (!trampSerialPort)
+    if (!trampSerialPort) {
        return 0;
+    }

    while (serialRxBytesWaiting(trampSerialPort)) {
-        uint8_t c = serialRead(trampSerialPort);
+        const uint8_t c = serialRead(trampSerialPort);
        trampRespBuffer[trampReceivePos++] = c;

        switch (trampReceiveState) {
@ -287,6 +334,7 @@ static char trampReceive(uint32_t currentTimeUs)

        default:
            trampResetReceiver();
+            break;
        }
    }

@ -314,19 +362,23 @@ void trampQueryS(void)
    trampQuery('s');
 }

-void vtxTrampProcess(uint32_t currentTimeUs)
+static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 {
-    static uint32_t lastQueryTimeUs = 0;
+    UNUSED(vtxDevice);
+
+    static timeUs_t lastQueryTimeUs = 0;
+    static bool initSettingsDoneFlag = false;

 #ifdef TRAMP_DEBUG
    static uint16_t debugFreqReqCounter = 0;
    static uint16_t debugPowReqCounter = 0;
 #endif

-    if (trampStatus == TRAMP_STATUS_BAD_DEVICE)
+    if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
        return;
+    }

-    char replyCode = trampReceive(currentTimeUs);
+    const char replyCode = trampReceive(currentTimeUs);

 #ifdef TRAMP_DEBUG
    debug[0] = trampStatus;
@ -334,13 +386,21 @@ void vtxTrampProcess(uint32_t currentTimeUs)

    switch (replyCode) {
    case 'r':
-        if (trampStatus <= TRAMP_STATUS_OFFLINE)
+        if (trampStatus <= TRAMP_STATUS_OFFLINE) {
            trampStatus = TRAMP_STATUS_ONLINE;
+
+            // once device is ready enter vtx settings
+            if (!initSettingsDoneFlag) {
+                initSettingsDoneFlag = true;
+                // if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
+            }
+        }
        break;

    case 'v':
-         if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW)
+         if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) {
             trampStatus = TRAMP_STATUS_SET_FREQ_PW;
+         }
         break;
    }

@ -350,15 +410,16 @@ void vtxTrampProcess(uint32_t currentTimeUs)
    case TRAMP_STATUS_ONLINE:
        if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s

-            if (trampStatus == TRAMP_STATUS_OFFLINE)
+            if (trampStatus == TRAMP_STATUS_OFFLINE) {
                trampQueryR();
-            else {
+            } else {
                static unsigned int cnt = 0;
-                if(((cnt++) & 1) == 0)
+                if (((cnt++) & 1) == 0) {
                    trampQueryV();
-                else
+                } else {
                    trampQueryS();
                }
+            }

            lastQueryTimeUs = currentTimeUs;
        }
@ -374,8 +435,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
                debugFreqReqCounter++;
 #endif
                done = false;
-            }
-            else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) {
+            } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) {
                trampSendRFPower(trampConfPower);
                trampPowerRetries--;
 #ifdef TRAMP_DEBUG
@ -389,8 +449,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)

                // delay next status query by 300ms
                lastQueryTimeUs = currentTimeUs + 300 * 1000;
-            }
-            else {
+            } else {
                // everything has been done, let's return to original state
                trampStatus = TRAMP_STATUS_ONLINE;
                // reset configuration value in case it failed (no more retries)
@ -420,55 +479,68 @@ void vtxTrampProcess(uint32_t currentTimeUs)
 }


-#ifdef VTX_COMMON
+#ifdef USE_VTX_COMMON

 // Interface to common VTX API

-vtxDevType_e vtxTrampGetDeviceType(void)
+static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
 {
+    UNUSED(vtxDevice);
    return VTXDEV_TRAMP;
 }

-bool vtxTrampIsReady(void)
+static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
 {
-    return trampStatus > TRAMP_STATUS_OFFLINE;
+    return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE;
 }

-void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel)
+static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 {
-    if (band && channel) {
+    UNUSED(vtxDevice);
+    if (trampValidateBandAndChannel(band, channel)) {
        trampSetBandAndChannel(band, channel);
        trampCommitChanges();
    }
 }

-void vtxTrampSetPowerByIndex(uint8_t index)
+static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
-    if (index) {
-        trampSetRFPower(trampPowerTable[index - 1]);
+    UNUSED(vtxDevice);
+    trampDevSetPowerByIndex(index);
+}
+
+static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
+{
+    UNUSED(vtxDevice);
+    trampSetPitMode(onoff);
+}
+
+static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
+{
+    UNUSED(vtxDevice);
+    if (trampValidateFreq(freq)) {
+        trampSetFreq(freq);
        trampCommitChanges();
    }
 }

-void vtxTrampSetPitMode(uint8_t onoff)
+static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 {
-    trampSetPitMode(onoff);
+    if (!vtxTrampIsReady(vtxDevice)) {
+        return false;
    }

-bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
-{
-    if (!vtxTrampIsReady())
-        return false;
-
-    *pBand = trampData.band;
+    // if in user-freq mode then report band as zero
+    *pBand = trampData.setByFreqFlag ? 0 : trampData.band;
    *pChannel = trampData.channel;
    return true;
 }

-bool vtxTrampGetPowerIndex(uint8_t *pIndex)
+static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 {
-    if (!vtxTrampIsReady())
+    if (!vtxTrampIsReady(vtxDevice)) {
        return false;
+    }

    if (trampData.configuredPower > 0) {
        for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
@ -482,15 +554,26 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
    return true;
 }

-bool vtxTrampGetPitMode(uint8_t *pOnOff)
+static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!vtxTrampIsReady())
+    if (!vtxTrampIsReady(vtxDevice)) {
        return false;
+    }

    *pOnOff = trampData.pitMode;
    return true;
 }

+static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
+{
+    if (!vtxTrampIsReady(vtxDevice)) {
+        return false;
+    }
+
+    *pFreq = trampData.curFreq;
+    return true;
+}
+
 static const vtxVTable_t trampVTable = {
    .process = vtxTrampProcess,
    .getDeviceType = vtxTrampGetDeviceType,
@ -498,9 +581,11 @@ static const vtxVTable_t trampVTable = {
    .setBandAndChannel = vtxTrampSetBandAndChannel,
    .setPowerByIndex = vtxTrampSetPowerByIndex,
    .setPitMode = vtxTrampSetPitMode,
+    .setFrequency = vtxTrampSetFreq,
    .getBandAndChannel = vtxTrampGetBandAndChannel,
    .getPowerIndex = vtxTrampGetPowerIndex,
    .getPitMode = vtxTrampGetPitMode,
+    .getFrequency = vtxTrampGetFreq,
 };

 #endif
@ -510,15 +595,22 @@ bool vtxTrampInit(void)
    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);

    if (portConfig) {
-        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
+        portOptions_t portOptions = 0;
+#if defined(USE_VTX_COMMON)
+        portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
+#else
+        portOptions = SERIAL_BIDIR;
+#endif
+
+        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
    }

    if (!trampSerialPort) {
        return false;
    }

-#if defined(VTX_COMMON)
-    vtxCommonRegisterDevice(&vtxTramp);
+#if defined(USE_VTX_COMMON)
+    vtxCommonSetDevice(&vtxTramp);
 #endif

    return true;
--- a/src/main/io/vtx_tramp.h
+++ b/src/main/io/vtx_tramp.h
@ -37,6 +37,7 @@ extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT];
 extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1];

 typedef struct trampData_s {
+    bool setByFreqFlag;  //false = set via band/channel
    uint8_t band;
    uint8_t channel;
    uint16_t power;       // Actual transmitting power
@ -49,7 +50,6 @@ typedef struct trampData_s {
 extern trampData_t trampData;

 bool vtxTrampInit(void);
-bool trampIsAvailable(void);
 bool trampCommitChanges(void);
 void trampSetPitMode(uint8_t onoff);
 void trampSetBandAndChannel(uint8_t band, uint8_t channel);
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@ -41,3 +41,5 @@
 #define MSP2_INAV_OSD_SET_ALARMS                0x2015
 #define MSP2_INAV_OSD_PREFERENCES               0x2016
 #define MSP2_INAV_OSD_SET_PREFERENCES           0x2017
+
+#define MSP2_INAV_SELECT_BATTERY_PROFILE        0x2018
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@ -41,6 +41,7 @@
 #include "fc/runtime_config.h"

 #include "flight/imu.h"
+#include "flight/mixer.h"
 #include "flight/pid.h"

 #include "io/beeper.h"
@ -107,6 +108,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
        .emerg_descent_rate = 500,    // 5 m/s
        .min_rth_distance = 500,      // If closer than 5m - land immediately
        .rth_altitude = 1000,         // 10m
+        .rth_home_altitude = 0,
        .rth_abort_threshold = 50000, // 500m - should be safe for all aircraft
        .max_terrain_follow_altitude = 100,     // max 1m altitude in terrain following mode
    },
@ -132,6 +134,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
        .max_climb_angle = 20,
        .max_dive_angle = 15,
        .cruise_throttle = 1400,
+        .cruise_speed = 0,         // cm/s
        .max_throttle = 1700,
        .min_throttle = 1200,
        .pitch_to_throttle = 10,   // pwm units per degree of pitch (10pwm units ~ 1% throttle)
@ -152,7 +155,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
        .launch_timeout = 5000,                // ms, timeout for launch procedure
        .launch_max_altitude = 0,              // cm, altitude where to consider launch ended
        .launch_climb_angle = 18,              // 18 degrees
-        .launch_max_angle = 45                 // 45 deg
+        .launch_max_angle = 45,                // 45 deg
+        .cruise_yaw_rate  = 20,                // 20dps
+        .allow_manual_thr_increase = false
    }
 );

@ -181,11 +186,11 @@ static void setupAltitudeController(void);
 static void resetHeadingController(void);
 void resetGCSFlags(void);

-static bool posEstimationHasGlobalReference(void);
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
+void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);

 void initializeRTHSanityChecker(const fpVector3_t * pos);
 bool validateRTHSanityChecker(void);
@ -196,6 +201,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
@ -235,6 +246,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]          = NAV_STATE_WAYPOINT_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_LAUNCH]            = NAV_STATE_LAUNCH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -269,6 +282,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]          = NAV_STATE_WAYPOINT_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -303,6 +318,112 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]          = NAV_STATE_WAYPOINT_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
+        }
+    },
+    /** CRUISE_2D mode ************************************************/
+    [NAV_STATE_CRUISE_2D_INITIALIZE] = {
+        .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE,
+        .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE,
+        .timeoutMs = 0,
+        .stateFlags = NAV_REQUIRE_ANGLE,
+        .mapToFlightModes = NAV_CRUISE_MODE,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_CRUISE_2D_IN_PROGRESS,
+            [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+        }
+    },
+
+    [NAV_STATE_CRUISE_2D_IN_PROGRESS] = {
+        .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS,
+        .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS,
+        .timeoutMs = 10,
+        .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
+        .mapToFlightModes = NAV_CRUISE_MODE,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_CRUISE_2D_IN_PROGRESS,    // re-process the state
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ]        = NAV_STATE_CRUISE_2D_ADJUSTING,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]          = NAV_STATE_WAYPOINT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+        }
+    },
+
+        [NAV_STATE_CRUISE_2D_ADJUSTING] = {
+        .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING,
+        .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING,
+        .timeoutMs = 10,
+        .stateFlags =  NAV_REQUIRE_ANGLE | NAV_RC_POS,
+        .mapToFlightModes = NAV_CRUISE_MODE,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_CRUISE_2D_IN_PROGRESS,
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_CRUISE_2D_ADJUSTING,
+            [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+        }
+    },
+
+    /** CRUISE_3D mode ************************************************/
+    [NAV_STATE_CRUISE_3D_INITIALIZE] = {
+        .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE,
+        .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE,
+        .timeoutMs = 0,
+        .stateFlags = NAV_REQUIRE_ANGLE,
+        .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_CRUISE_3D_IN_PROGRESS,
+            [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+        }
+    },
+    [NAV_STATE_CRUISE_3D_IN_PROGRESS] = {
+        .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS,
+        .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS,
+        .timeoutMs = 10,
+        .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
+        .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_CRUISE_3D_IN_PROGRESS,    // re-process the state
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ]        = NAV_STATE_CRUISE_3D_ADJUSTING,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]          = NAV_STATE_WAYPOINT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+        }
+    },
+
+        [NAV_STATE_CRUISE_3D_ADJUSTING] = {
+        .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING,
+        .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING,
+        .timeoutMs = 10,
+        .stateFlags =  NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
+        .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
+        .mwState = MW_NAV_STATE_NONE,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_CRUISE_3D_IN_PROGRESS,
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_CRUISE_3D_ADJUSTING,
+            [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
        }
    },

@ -338,6 +459,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -356,6 +479,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -375,6 +500,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -392,6 +519,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -493,6 +622,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -514,6 +645,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -533,6 +666,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -564,6 +699,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
        }
    },

@ -748,7 +885,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
        setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);  // This will reset surface offset
    }

-    if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
+    if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) {
        fpVector3_t targetHoldPos;
        calculateInitialHoldPosition(&targetHoldPos);
        setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
@ -772,12 +909,117 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(

    return NAV_FSM_EVENT_NONE;
 }
+/////////////////
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState)
+{
+    const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
+
+    if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
+
+    DEBUG_SET(DEBUG_CRUISE, 0, 1);
+    if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; }  // Switch to IDLE if we do not have an healty position. Try the next iteration.
+
+    if (!(prevFlags & NAV_CTL_POS)) {
+        resetPositionController();
+    }
+
+    posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
+    posControl.cruise.previousYaw = posControl.cruise.yaw;
+    posControl.cruise.lastYawAdjustmentTime = 0;
+
+    return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState)
+{
+    const timeMs_t currentTimeMs = millis();
+
+    if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
+
+    DEBUG_SET(DEBUG_CRUISE, 0, 2);
+    DEBUG_SET(DEBUG_CRUISE, 2, 0);
+
+    if (posControl.flags.isAdjustingPosition) {
+        return NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ;
+    }
+
+    // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
+    if (posControl.flags.isAdjustingHeading) {
+        timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
+        if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
+        float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
+        float centidegsPerIteration = rateTarget * timeDifference / 1000.0f;
+        posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
+        DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
+        posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
+    }
+
+    if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000)
+        posControl.cruise.previousYaw = posControl.cruise.yaw;
+
+    uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
+
+    if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING)
+            || (previousState == NAV_STATE_CRUISE_3D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_3D_ADJUSTING)
+            || posControl.flags.isAdjustingHeading) {
+        calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
+        DEBUG_SET(DEBUG_CRUISE, 2, 1);
+    } else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
+        calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
+        DEBUG_SET(DEBUG_CRUISE, 2, 2);
+    }
+
+    setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY);
+
+    return NAV_FSM_EVENT_NONE;
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+    DEBUG_SET(DEBUG_CRUISE, 0, 3);
+
+    // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
+    if (posControl.flags.isAdjustingPosition) {
+        posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
+        posControl.cruise.lastYawAdjustmentTime = millis();
+        return NAV_FSM_EVENT_NONE;  // reprocess the state
+    }
+
+    resetPositionController();
+
+    return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState)
+{
+    if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
+
+    navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
+
+    return navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(previousState);
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState)
+{
+    navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
+
+    return navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(previousState);
+}
+
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState)
+{
+    navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
+
+    return navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(previousState);
+}

 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
 {
    navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);

-    if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) {
+    if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) {
        // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
        // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
        return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
@ -949,6 +1191,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND

    // If position ok OR within valid timeout - continue
    if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
+
+        // set altitude to go to when landing is not allowed
+        if (navConfig()->general.rth_home_altitude && !navigationRTHAllowsLanding()) {
+            posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude;
+            setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
+        }
+
        // Wait until target heading is reached (with 15 deg margin for error)
        if (STATE(FIXED_WING)) {
            resetLandingDetector();
@ -968,8 +1217,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
                return NAV_FSM_EVENT_NONE;
            }
        }
-    }
-    else {
+
+    } else {
        return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
    }
 }
@ -1384,6 +1633,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
    const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
    const float outValConstrained = constrainf(outVal, outMin, outMax);

+    pid->proportional = newProportional;
+    pid->integral = pid->integrator;
+    pid->derivative = newDerivative;
+    pid->output_constrained = outValConstrained;
+
    /* Update I-term */
    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
        const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
@ -1411,10 +1665,14 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
 void navPidReset(pidController_t *pid)
 {
    pid->reset = true;
+    pid->proportional = 0.0f;
+    pid->integral = 0.0f;
+    pid->derivative = 0.0f;
    pid->integrator = 0.0f;
    pid->last_input = 0.0f;
    pid->dterm_filter_state.state = 0.0f;
    pid->dterm_filter_state.RC = 0.0f;
+    pid->output_constrained = 0.0f;
 }

 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD)
@ -1475,7 +1733,7 @@ bool checkForPositionSensorTimeout(void)
 /*-----------------------------------------------------------
 * Processes an update to XY-position and velocity
 *-----------------------------------------------------------*/
-void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
+void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
 {
    posControl.actualState.abs.pos.x = newX;
    posControl.actualState.abs.pos.y = newY;
@ -1489,13 +1747,24 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f

    posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));

-    if (estimateValid) {
+    // CASE 1: POS & VEL valid
+    if (estPosValid && estVelValid) {
        posControl.flags.estPosStatus = EST_TRUSTED;
+        posControl.flags.estVelStatus = EST_TRUSTED;
        posControl.flags.horizontalPositionDataNew = 1;
        posControl.lastValidPositionTimeMs = millis();
    }
+    // CASE 1: POS invalid, VEL valid
+    else if (!estPosValid && estVelValid) {
+        posControl.flags.estPosStatus = EST_USABLE;     // Pos usable, but not trusted
+        posControl.flags.estVelStatus = EST_TRUSTED;
+        posControl.flags.horizontalPositionDataNew = 1;
+        posControl.lastValidPositionTimeMs = millis();
+    }
+    // CASE 3: can't use pos/vel data
    else {
        posControl.flags.estPosStatus = EST_NONE;
+        posControl.flags.estVelStatus = EST_NONE;
        posControl.flags.horizontalPositionDataNew = 0;
    }

@ -1657,6 +1926,22 @@ static void updateHomePositionCompatibility(void)
    GPS_directionToHome = posControl.homeDirection / 100;
 }

+float RTHAltitude() {
+    switch (navConfig()->general.flags.rth_alt_control_mode) {
+        case NAV_RTH_NO_ALT:
+            return(posControl.actualState.abs.pos.z);
+        case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
+            return(posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude);
+        case NAV_RTH_MAX_ALT:
+            return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z));
+        case NAV_RTH_AT_LEAST_ALT:  // Climb to at least some predefined altitude above home
+            return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z));
+        case NAV_RTH_CONST_ALT:     // Climb/descend to predefined altitude above home
+        default:
+            return(posControl.homePosition.pos.z + navConfig()->general.rth_altitude);
+    }
+}
+
 /*-----------------------------------------------------------
 * Reset home position to current position
 *-----------------------------------------------------------*/
@ -1664,24 +1949,7 @@ static void updateDesiredRTHAltitude(void)
 {
    if (ARMING_FLAG(ARMED)) {
        if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
-            switch (navConfig()->general.flags.rth_alt_control_mode) {
-            case NAV_RTH_NO_ALT:
-                posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
-                break;
-            case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
-                posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
-                break;
-            case NAV_RTH_MAX_ALT:
-                posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z);
-                break;
-            case NAV_RTH_AT_LEAST_ALT:  // Climb to at least some predefined altitude above home
-                posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
-                break;
-            case NAV_RTH_CONST_ALT:     // Climb/descend to predefined altitude above home
-            default:
-                posControl.homeWaypointAbove.pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
-                break;
-            }
+            posControl.homeWaypointAbove.pos.z = RTHAltitude();
        }
    }
    else {
@ -1911,6 +2179,13 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t dista
    farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
 }

+void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance)
+{
+    origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
+    origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
+    origin->z = origin->z;
+}
+
 /*-----------------------------------------------------------
 * NAV land detector
 *-----------------------------------------------------------*/
@ -2208,7 +2483,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
    // WP #255 - special waypoint - directly set desiredPosition
    // Only valid when armed and in poshold mode
    else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
-             ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
+             ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
             (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
        // Convert to local coordinates
        geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
@ -2472,7 +2747,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
 void switchNavigationFlightModes(void)
 {
    const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
-    const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE) & (~enabledNavFlightModes);
+    const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_CRUISE_MODE) & (~enabledNavFlightModes);
    DISABLE_FLIGHT_MODE(disabledFlightModes);
    ENABLE_FLIGHT_MODE(enabledNavFlightModes);
 }
@ -2487,12 +2762,12 @@ static bool canActivateAltHoldMode(void)

 static bool canActivatePosHoldMode(void)
 {
-    return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
+    return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
 }

-static bool posEstimationHasGlobalReference(void)
+static bool canActivateNavigationModes(void)
 {
-    return true;    // For now assume that we always have global coordinates available
+    return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
 }

 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
@ -2510,6 +2785,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
        // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
        bool canActivateAltHold    = canActivateAltHoldMode();
        bool canActivatePosHold    = canActivatePosHoldMode();
+        bool canActivateNavigation = canActivateNavigationModes();

        // LAUNCH mode has priority over any other NAV mode
        if (STATE(FIXED_WING)) {
@ -2536,7 +2812,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
        }

        // RTH/Failsafe_RTH can override MANUAL
-        if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) {
+        if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
            // If we request forced RTH - attempt to activate it no matter what
            // This might switch to emergency landing controller if GPS is unavailable
            canActivateWaypoint = false;    // Block WP mode if we switched to RTH for whatever reason
@ -2550,7 +2826,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
        }

        if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
-            if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
+            if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
                return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
        }
        else {
@ -2563,6 +2839,18 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
                return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
        }

+        // PH has priority over CRUISE
+        // CRUISE has priority on AH
+        if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
+
+            if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
+                return NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D;
+
+            if (FLIGHT_MODE(NAV_CRUISE_MODE) || (canActivatePosHold))
+                return NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D;
+
+        }
+
        if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
            if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
                return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
@ -2603,7 +2891,7 @@ bool navigationRequiresTurnAssistance(void)
    const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
    if (STATE(FIXED_WING)) {
        // For airplanes turn assistant is always required when controlling position
-        return (currentState & NAV_CTL_POS);
+        return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
    }
    else {
        return false;
@ -2641,8 +2929,8 @@ bool navigationTerrainFollowingEnabled(void)

 bool navigationBlockArming(void)
 {
-    const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
-    const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
+    const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
+    const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
    bool shouldBlockArming = false;

    if (!navConfig()->general.flags.extra_arming_safety)
@ -2789,6 +3077,7 @@ void navigationInit(void)

    posControl.flags.estAltStatus = EST_NONE;
    posControl.flags.estPosStatus = EST_NONE;
+    posControl.flags.estVelStatus = EST_NONE;
    posControl.flags.estHeadingStatus = EST_NONE;
    posControl.flags.estAglStatus = EST_NONE;

@ -2854,7 +3143,7 @@ rthState_e getStateOfForcedRTH(void)
 bool navigationIsControllingThrottle(void)
 {
    navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
-    return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
+    return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER));
 }

 bool navigationIsFlyingAutonomousMode(void)
@ -2880,6 +3169,27 @@ int32_t navigationGetHomeHeading(void)
    return posControl.homePosition.yaw;
 }

+// returns m/s
+float calculateAverageSpeed() {
+    return (float)getTotalTravelDistance() / (getFlightTime() * 100);
+}
+
+const navigationPIDControllers_t* getNavigationPIDControllers(void) {
+    return &posControl.pids;
+}
+
+bool isAdjustingPosition(void) {
+    return posControl.flags.isAdjustingPosition;
+}
+
+bool isAdjustingHeading(void) {
+    return posControl.flags.isAdjustingHeading;
+}
+
+int32_t getCruiseHeadingAdjustment(void) {
+    return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
+}
+
 #else // NAV

 #ifdef USE_GPS
@ -2917,7 +3227,7 @@ void onNewGPSData(void)
            int32_t dir;
            GPS_distance_cm_bearing(gpsSol.llh.lat, gpsSol.llh.lon, GPS_home.lat, GPS_home.lon, &dist, &dir);
            GPS_distanceToHome = dist / 100;
-            GPS_directionToHome = dir / 100;
+            GPS_directionToHome = lrintf(dir / 100.0f);
        } else {
            GPS_distanceToHome = 0;
            GPS_directionToHome = 0;
@ -2942,6 +3252,7 @@ int32_t getTotalTravelDistance(void)
 {
    return lrintf(GPS_totalTravelDistance);
 }
+
 #endif

 #endif  // NAV
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@ -17,6 +17,8 @@

 #pragma once

+#include "common/axis.h"
+#include "common/filter.h"
 #include "common/maths.h"
 #include "common/vector.h"

@ -31,6 +33,8 @@ extern gpsLocation_t        GPS_home;
 extern uint16_t             GPS_distanceToHome;        // distance to home point in meters
 extern int16_t              GPS_directionToHome;       // direction to home point in degrees

+extern bool autoThrottleManuallyIncreased;
+
 /* Navigation system updates */
 void onNewGPSData(void);

@ -80,6 +84,7 @@ typedef struct positionEstimationConfig_s {
    uint8_t reset_home_type; // nav_reset_type_e
    uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
    uint8_t use_gps_velned;
+    uint8_t allow_dead_reckoning;

    uint16_t max_surface_altitude;

@ -94,6 +99,9 @@ typedef struct positionEstimationConfig_s {
    float w_xy_gps_p;   // Weight (cutoff frequency) for GPS position measurements
    float w_xy_gps_v;   // Weight (cutoff frequency) for GPS velocity measurements

+    float w_xy_flow_p;
+    float w_xy_flow_v;
+
    float w_z_res_v;    // When velocity sources lost slowly decrease estimated velocity with this weight
    float w_xy_res_v;

@ -132,6 +140,7 @@ typedef struct navConfig_s {
        uint16_t land_slowdown_maxalt;          // Altitude to start lowering descent rate during RTH descend
        uint16_t emerg_descent_rate;            // emergency landing descent rate
        uint16_t rth_altitude;                  // altitude to maintain when RTH is active (depends on rth_alt_control_mode) (cm)
+        uint16_t rth_home_altitude;             // altitude to go to during RTH after the craft reached home (cm)
        uint16_t min_rth_distance;              // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland
        uint16_t rth_abort_threshold;           // Initiate emergency landing if during RTH we get this much [cm] away from home
        uint16_t max_terrain_follow_altitude;   // Max altitude to be used in SURFACE TRACKING mode
@ -156,6 +165,7 @@ typedef struct navConfig_s {
        uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
        uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
        uint16_t cruise_throttle;            // Cruise throttle
+        uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
        uint16_t min_throttle;               // Minimum allowed throttle in auto mode
        uint16_t max_throttle;               // Maximum allowed throttle in auto mode
        uint8_t  pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
@ -173,6 +183,8 @@ typedef struct navConfig_s {
        uint16_t launch_max_altitude;        // cm, altitude where to consider launch ended
        uint8_t  launch_climb_angle;         // Target climb angle for launch (deg)
        uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
+        uint8_t  cruise_yaw_rate;            // Max yaw rate (dps) when CRUISE MODE is enabled
+        bool     allow_manual_thr_increase;
    } fw;
 } navConfig_t;

@ -209,6 +221,46 @@ typedef struct {
    int32_t     yaw;             // deg * 100
 } navWaypointPosition_t;

+typedef struct {
+    float kP;
+    float kI;
+    float kD;
+    float kT;   // Tracking gain (anti-windup)
+} pidControllerParam_t;
+
+typedef struct {
+    float kP;
+} pControllerParam_t;
+
+typedef struct {
+    bool reset;
+    pidControllerParam_t param;
+    pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
+    float integrator;                   // integrator value
+    float last_input;                   // last input for derivative
+
+    float integral;                     // used integral value in output
+    float proportional;                 // used proportional value in output
+    float derivative;                   // used derivative value in output
+    float output_constrained;           // controller output constrained
+} pidController_t;
+
+typedef struct {
+    pControllerParam_t param;
+    float output_constrained;
+} pController_t;
+
+typedef struct navigationPIDControllers_s {
+    /* Multicopter PIDs */
+    pController_t   pos[XYZ_AXIS_COUNT];
+    pidController_t vel[XYZ_AXIS_COUNT];
+    pidController_t surface;
+
+    /* Fixed-wing PIDs */
+    pidController_t fw_alt;
+    pidController_t fw_nav;
+} navigationPIDControllers_t;
+
 /* MultiWii-compatible params for telemetry */
 typedef enum {
    MW_GPS_MODE_NONE = 0,
@ -288,9 +340,20 @@ bool navIsCalibrationComplete(void);
 bool navigationTerrainFollowingEnabled(void);

 /* Access to estimated position and velocity */
+typedef struct {
+    uint8_t altStatus;
+    uint8_t posStatus;
+    uint8_t velStatus;
+    uint8_t aglStatus;
+    fpVector3_t pos;
+    fpVector3_t vel;
+    float agl;
+} navPositionAndVelocity_t;
+
 float getEstimatedActualVelocity(int axis);
 float getEstimatedActualPosition(int axis);
 int32_t getTotalTravelDistance(void);
+void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos);

 /* Waypoint list access functions */
 int getWaypointCount(void);
@ -301,6 +364,8 @@ void resetWaypointList(void);
 bool loadNonVolatileWaypointList(void);
 bool saveNonVolatileWaypointList(void);

+float RTHAltitude();
+
 /* Geodetic functions */
 typedef enum {
    GEO_ALT_ABSOLUTE,
@ -324,6 +389,7 @@ rthState_e getStateOfForcedRTH(void);

 /* Getter functions which return data about the state of the navigation system */
 bool navigationIsControllingThrottle(void);
+bool isFixedWingAutoThrottleManuallyIncreased(void);
 bool navigationIsFlyingAutonomousMode(void);
 /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
 * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
@ -331,6 +397,16 @@ bool navigationIsFlyingAutonomousMode(void);
 bool navigationRTHAllowsLanding(void);

 bool isNavLaunchEnabled(void);
+bool isFixedWingLaunchDetected(void);
+
+float calculateAverageSpeed();
+
+const navigationPIDControllers_t* getNavigationPIDControllers(void);
+
+int32_t navigationGetHeadingError(void);
+int32_t getCruiseHeadingAdjustment(void);
+bool isAdjustingPosition(void);
+bool isAdjustingHeading(void);

 /* Returns the heading recorded when home position was acquired.
 * Note that the navigation system uses deg*100 as unit and angles
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@ -48,6 +48,9 @@
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"

+#include "rx/rx.h"
+
+
 // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
 #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN        1.5f
 #define NAV_FW_MIN_VEL_SPEED_BOOST              700.0f      // 7 m/s
@ -58,6 +61,8 @@
 static bool isPitchAdjustmentValid = false;
 static bool isRollAdjustmentValid = false;
 static float throttleSpeedAdjustment = 0;
+static bool isAutoThrottleManuallyIncreased = false;
+static int32_t navHeadingError;


 /*-----------------------------------------------------------
@ -182,6 +187,10 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
 *-----------------------------------------------------------*/
 bool adjustFixedWingHeadingFromRCInput(void)
 {
+    if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
+        return true;
+    }
+
    return false;
 }

@ -218,7 +227,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
    #define TAN_15DEG    0.26795f
    bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
                                            && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
-                                            && (distanceToActualTarget > 50.0f);
+                                            && (distanceToActualTarget > 50.0f)
+                                            && !FLIGHT_MODE(NAV_CRUISE_MODE);

    // Calculate virtual position for straight movement
    if (needToCalculateCircularLoiter) {
@ -271,29 +281,29 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
    int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);

    // Calculate NAV heading error
-    int32_t headingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
+    navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);

    // Forced turn direction
    // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
-    if (ABS(headingError) > 17000) {
+    if (ABS(navHeadingError) > 17000) {
        forceTurnDirection = true;
    }
-    else if (ABS(headingError) < 9000 && forceTurnDirection) {
+    else if (ABS(navHeadingError) < 9000 && forceTurnDirection) {
        forceTurnDirection = false;
    }

    // If forced turn direction flag is enabled we fix the sign of the direction
    if (forceTurnDirection) {
-        headingError = ABS(headingError);
+        navHeadingError = ABS(navHeadingError);
    }

    // Slow error monitoring (2Hz rate)
    if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
        // Check if error is decreasing over time
-        errorIsDecreasing = (ABS(previousHeadingError) > ABS(headingError));
+        errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));

        // Save values for next iteration
-        previousHeadingError = headingError;
+        previousHeadingError = navHeadingError;
        previousTimeMonitoringUpdate = currentTimeUs;
    }

@ -301,7 +311,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
    const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);

    // Input error in (deg*100), output pitch angle (deg*100)
-    float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + headingError, posControl.actualState.yaw, US2S(deltaMicros),
+    float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros),
                                       -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                        DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                        pidFlags);
@ -445,6 +455,18 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
        }

        uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
+
+        // Manual throttle increase
+        if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
+            if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95)
+                correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle);
+            else
+                correctedThrottleValue = motorConfig()->maxthrottle;
+            isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle);
+        } else {
+            isAutoThrottleManuallyIncreased = false;
+        }
+
        rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle);
    }

@ -471,6 +493,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
 #endif
 }

+bool isFixedWingAutoThrottleManuallyIncreased()
+{
+    return isAutoThrottleManuallyIncreased;
+}
+
 /*-----------------------------------------------------------
 * FixedWing land detector
 *-----------------------------------------------------------*/
@ -527,23 +554,28 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
 #ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY
        // Don't apply anything if ground speed is too low (<3m/s)
        if (posControl.actualState.velXY > 300) {
-            if (navStateFlags & NAV_CTL_ALT)
+#else
+        if (true) {
+#endif
+            if (navStateFlags & NAV_CTL_ALT) {
+                if (getMotorStatus() == MOTOR_STOPPED_USER) {
+                    // Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control
+                    resetFixedWingAltitudeController();
+                    setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
+                } else
                    applyFixedWingAltitudeAndThrottleController(currentTimeUs);
+            }

            if (navStateFlags & NAV_CTL_POS)
                applyFixedWingPositionController(currentTimeUs);
-        }
-        else {
+
+        } else {
            posControl.rcAdjustment[PITCH] = 0;
            posControl.rcAdjustment[ROLL] = 0;
        }
-#else
-        if (navStateFlags & NAV_CTL_ALT)
-            applyFixedWingAltitudeAndThrottleController(currentTimeUs);

-        if (navStateFlags & NAV_CTL_POS)
-            applyFixedWingPositionController(currentTimeUs);
-#endif
+        if (FLIGHT_MODE(NAV_CRUISE_MODE) && posControl.flags.isAdjustingPosition)
+            rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);

        //if (navStateFlags & NAV_CTL_YAW)
        if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS))
@ -551,4 +583,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
    }
 }

+int32_t navigationGetHeadingError(void)
+{
+    return navHeadingError;
+}
+
 #endif  // NAV
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@ -72,6 +72,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
        targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate);
    }

+    posControl.pids.pos[Z].output_constrained = targetVel;
+
    // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
    // if we are decelerating - don't limit (allow better recovery from falling)
    if (ABS(targetVel) > ABS(posControl.desiredState.vel.z)) {
@ -349,6 +351,9 @@ static void updatePositionVelocityController_MC(void)
        newVelTotal = maxSpeed;
    }

+    posControl.pids.pos[X].output_constrained = newVelX;
+    posControl.pids.pos[Y].output_constrained = newVelY;
+
    // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
    const float velHeadFactor = getVelocityHeadingAttenuationFactor();
    const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@ -36,146 +36,22 @@
 #include "drivers/time.h"

 #include "fc/config.h"
-#include "fc/rc_controls.h"
-#include "fc/runtime_config.h"

 #include "flight/imu.h"
-#include "flight/pid.h"

 #include "io/gps.h"

 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
+#include "navigation/navigation_pos_estimator_private.h"

 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
-#include "sensors/boardalignment.h"
 #include "sensors/compass.h"
 #include "sensors/pitotmeter.h"
-#include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
-#include "sensors/sensors.h"

-
-/**
- * Model-identification based position estimator
- * Based on INAV position estimator for PX4 by Anton Babushkin <anton.babushkin@me.com>
- * @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
- */
-#define INAV_GPS_DEFAULT_EPH                200.0f  // 2m GPS HDOP  (gives about 1.6s of dead-reckoning if GPS is temporary lost)
-#define INAV_GPS_DEFAULT_EPV                500.0f  // 5m GPS VDOP
-
-#define INAV_GPS_ACCEPTANCE_EPE             500.0f  // 5m acceptance radius
-
-#define INAV_ACC_BIAS_ACCEPTANCE_VALUE      (GRAVITY_CMSS * 0.25f)   // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway
-
-#define INAV_GPS_GLITCH_RADIUS              250.0f  // 2.5m GPS glitch radius
-#define INAV_GPS_GLITCH_ACCEL               1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
-
-#define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
-#define INAV_PITOT_UPDATE_RATE              10
-
-#define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
-#define INAV_BARO_TIMEOUT_MS                200     // Baro timeout
-#define INAV_SURFACE_TIMEOUT_MS             300     // Surface timeout    (missed 3 readings in a row)
-
-#define INAV_HISTORY_BUF_SIZE               (INAV_POSITION_PUBLISH_RATE_HZ / 2)     // Enough to hold 0.5 sec historical data
-
-typedef struct {
-    timeUs_t    lastTriggeredTime;
-    timeUs_t    deltaTime;
-} navigationTimer_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-#if defined(NAV_GPS_GLITCH_DETECTION)
-    bool        glitchDetected;
-    bool        glitchRecovery;
-#endif
-    fpVector3_t pos;            // GPS position in NEU coordinate system (cm)
-    fpVector3_t vel;            // GPS velocity (cms)
-    float       eph;
-    float       epv;
-} navPositionEstimatorGPS_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    float       alt;            // Raw barometric altitude (cm)
-    float       epv;
-} navPositionEstimatorBARO_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    float       airspeed;            // airspeed (cm/s)
-} navPositionEstimatorPITOT_t;
-
-typedef enum {
-    SURFACE_QUAL_LOW,   // Surface sensor signal lost long ago - most likely surface distance is incorrect
-    SURFACE_QUAL_MID,   // Surface sensor is not available but we can somewhat trust the estimate
-    SURFACE_QUAL_HIGH   // All good
-} navAGLEstimateQuality_e;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    float       alt;            // Raw altitude measurement (cm)
-    float       reliability;
-} navPositionEstimatorSURFACE_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    bool        isValid;
-    float       quality;
-    float       flowRate[2];
-    float       bodyRate[2];
-} navPositionEstimatorFLOW_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-
-    // 3D position, velocity and confidence
-    fpVector3_t pos;
-    fpVector3_t vel;
-    float       eph;
-    float       epv;
-
-    // AGL
-    navAGLEstimateQuality_e aglQual;
-    float                   aglOffset;  // Offset between surface and pos.Z
-    float                   aglAlt;
-    float                   aglVel;
-} navPositionEstimatorESTIMATE_t;
-
-typedef struct {
-    timeUs_t    baroGroundTimeout;
-    float       baroGroundAlt;
-    bool        isBaroGroundValid;
-} navPositionEstimatorSTATE_t;
-
-typedef struct {
-    fpVector3_t     accelNEU;
-    fpVector3_t     accelBias;
-    bool            gravityCalibrationComplete;
-} navPosisitonEstimatorIMU_t;
-
-typedef struct {
-    // Data sources
-    navPositionEstimatorGPS_t   gps;
-    navPositionEstimatorBARO_t  baro;
-    navPositionEstimatorSURFACE_t surface;
-    navPositionEstimatorPITOT_t pitot;
-    navPositionEstimatorFLOW_t  flow;
-
-    // IMU data
-    navPosisitonEstimatorIMU_t  imu;
-
-    // Estimate
-    navPositionEstimatorESTIMATE_t  est;
-
-    // Extra state variables
-    navPositionEstimatorSTATE_t state;
-} navigationPosEstimator_t;
-
-static navigationPosEstimator_t posEstimator;
+navigationPosEstimator_t posEstimator;

 PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3);

@ -186,6 +62,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
        .reset_home_type = NAV_RESET_ON_EACH_ARM,
        .gravity_calibration_tolerance = 5,     // 5 cm/s/s calibration error accepted (0.5% of gravity)
        .use_gps_velned = 1,         // "Disabled" is mandatory with gps_dyn_model = Pedestrian
+        .allow_dead_reckoning = 0,

        .max_surface_altitude = 200,

@ -195,11 +72,14 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
        .w_z_surface_v = 6.100f,

        .w_z_gps_p = 0.2f,
-        .w_z_gps_v = 0.5f,
+        .w_z_gps_v = 0.1f,

        .w_xy_gps_p = 1.0f,
        .w_xy_gps_v = 2.0f,

+        .w_xy_flow_p = 1.0f,
+        .w_xy_flow_v = 2.0f,
+
        .w_z_res_v = 0.5f,
        .w_xy_res_v = 0.5f,

@ -209,25 +89,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
        .baro_epv = 100.0f
 );

-/* Inertial filter, implementation taken from PX4 implementation by Anton Babushkin <rk3dov@gmail.com> */
-static void inavFilterPredict(int axis, float dt, float acc)
-{
-    posEstimator.est.pos.v[axis] += posEstimator.est.vel.v[axis] * dt + acc * dt * dt / 2.0f;
-    posEstimator.est.vel.v[axis] += acc * dt;
-}
-
-static void inavFilterCorrectPos(int axis, float dt, float e, float w)
-{
-    float ewdt = e * w * dt;
-    posEstimator.est.pos.v[axis] += ewdt;
-    posEstimator.est.vel.v[axis] += w * ewdt;
-}
-
-static void inavFilterCorrectVel(int axis, float dt, float e, float w)
-{
-    posEstimator.est.vel.v[axis] += e * w * dt;
-}
-
 #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
 #define getTimerDeltaMicros(tim) ((tim)->deltaTime)
 static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t currentTimeUs)
@ -480,64 +341,6 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
 }
 #endif

-#ifdef USE_RANGEFINDER
-/**
- * Read surface and update alt/vel topic
- *  Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
- */
-#define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
-#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
-#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD   (0.33f)
-#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD  (0.75f)
-
-void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
-{
-    const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
-    float newReliabilityMeasurement = 0;
-
-    posEstimator.surface.lastUpdateTime = currentTimeUs;
-
-    if (newSurfaceAlt >= 0) {
-        if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
-            newReliabilityMeasurement = 1.0f;
-            posEstimator.surface.alt = newSurfaceAlt;
-        }
-        else {
-            newReliabilityMeasurement = 0.0f;
-        }
-    }
-    else {
-        // Negative values - out of range or failed hardware
-        newReliabilityMeasurement = 0.0f;
-    }
-
-    /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
-    if (dt > 0.5f) {
-        posEstimator.surface.reliability = 0.0f;
-    }
-    else {
-        const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
-        posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
-    }
-}
-#endif
-
-#ifdef USE_OPTICAL_FLOW
-/**
- * Read optical flow topic
- *  Function is called by OPFLOW task as soon as new update is available
- */
-void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
-{
-    posEstimator.flow.lastUpdateTime = currentTimeUs;
-    posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
-    posEstimator.flow.flowRate[X] = opflow.flowRate[X];
-    posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
-    posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
-    posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
-}
-#endif
-
 /**
 * Update IMU topic
 *  Function is called at main loop rate
@ -611,62 +414,97 @@ static void updateIMUTopic(void)
    }
 }

-static float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w)
+float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w)
 {
    return oldEPE + (newEPE - oldEPE) * w * dt;
 }

+static bool navIsAccelerationUsable(void)
+{
+    return true;
+}
+
 static bool navIsHeadingUsable(void)
 {
+    if (sensors(SENSOR_GPS)) {
        // If we have GPS - we need true IMU north (valid heading)
        return isImuHeadingValid();
    }
+    else {
+        // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame
+        return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning;
+    }
+}

-/**
- * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
- *  Function is called at main loop rate
- */
-static void updateEstimatedTopic(timeUs_t currentTimeUs)
+static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 {
-    /* Calculate dT */
-    float dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
-    posEstimator.est.lastUpdateTime = currentTimeUs;
-
-    /* If IMU is not ready we can't estimate anything */
-    if (!isImuReady()) {
-        posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
-        posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
-        return;
-    }
-
-    /* Calculate new EPH and EPV for the case we didn't update postion */
-    float newEPH = posEstimator.est.eph;
-    float newEPV = posEstimator.est.epv;
-
-    if (newEPH <= positionEstimationConfig()->max_eph_epv) {
-        newEPH *= 1.0f + dt;
-    }
-
-    if (newEPV <= positionEstimationConfig()->max_eph_epv) {
-        newEPV *= 1.0f + dt;
-    }
-
    /* Figure out if we have valid position data from our data sources */
-#if defined(NAV_GPS_GLITCH_DETECTION)
-    //isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected;
-#endif
-    const bool isGPSValid = sensors(SENSOR_GPS) &&
-                            posControl.gpsOrigin.valid &&
-                            ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
-                            (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv);
-    const bool isGPSZValid = isGPSValid && (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv);
-    const bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
-#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-    const bool isSurfaceValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS));
-#endif
+    uint32_t newFlags = 0;
+
+    if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid &&
+        ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
+        (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
+        if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
+            newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
+        }
+        else {
+            newFlags |= EST_GPS_XY_VALID;
+        }
+    }
+
+    if (sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS))) {
+        newFlags |= EST_BARO_VALID;
+    }
+
+    if (sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS))) {
+        newFlags |= EST_SURFACE_VALID;
+    }
+
+    if (sensors(SENSOR_OPFLOW) && posEstimator.flow.isValid && ((currentTimeUs - posEstimator.flow.lastUpdateTime) <= MS2US(INAV_FLOW_TIMEOUT_MS))) {
+        newFlags |= EST_FLOW_VALID;
+    }
+
+    if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
+        newFlags |= EST_XY_VALID;
+    }
+
+    if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+        newFlags |= EST_Z_VALID;
+    }
+
+    return newFlags;
+}
+
+static void estimationPredict(estimationContext_t * ctx)
+{
+    /* Prediction step: Z-axis */
+    if ((ctx->newFlags & EST_Z_VALID)) {
+        posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
+        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
+        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
+    }
+
+    /* Prediction step: XY-axis */
+    if ((ctx->newFlags & EST_XY_VALID)) {
+        // Predict based on known velocity
+        posEstimator.est.pos.x += posEstimator.est.vel.x * ctx->dt;
+        posEstimator.est.pos.y += posEstimator.est.vel.y * ctx->dt;
+
+        // If heading is valid, accelNEU is valid as well. Account for acceleration
+        if (navIsHeadingUsable() && navIsAccelerationUsable()) {
+            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
+            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
+            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
+            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
+        }
+    }
+}
+
+static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
+{
+    if (ctx->newFlags & EST_BARO_VALID) {
+        timeUs_t currentTimeUs = micros();

-    /* Do some preparations to data */
-    if (isBaroValid) {
        if (!ARMING_FLAG(ARMED)) {
            posEstimator.state.baroGroundAlt = posEstimator.est.pos.z;
            posEstimator.state.isBaroGroundValid = true;
@ -682,261 +520,189 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
                posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
            }
        }
-    }
-    else {
-        posEstimator.state.isBaroGroundValid = false;
-    }

-#if defined(USE_BARO)
-    /* We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it */
+        // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
        bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
-                                        ((isSurfaceValid && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
-                                         (isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
-#endif
+                                            (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
+                                             ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));

-    /* Validate EPV for GPS and calculate altitude/climb rate correction flags */
-    const bool useGpsZPos = STATE(FIXED_WING) && !sensors(SENSOR_BARO) && isGPSValid && isGPSZValid;
-    const bool useGpsZVel = isGPSValid && isGPSZValid;
+        // Altitude
+        const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
+        ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
+        ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;

-    /* Estimate validity */
-    const bool isEstXYValid = (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv);
-    const bool isEstZValid = (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv);
-
-    /* Prediction step: Z-axis */
-    if (isEstZValid) {
-        inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.z);
+        // If GPS is available - also use GPS climb rate
+        if (ctx->newFlags & EST_GPS_Z_VALID) {
+            // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
+            const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
+            const float gpsRocScaler = bellCurve(gpsRocResidual, 2.5f);
+            ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
        }

-    /* Prediction step: XY-axis */
-    if (isEstXYValid) {
-        if (navIsHeadingUsable()) {
-            inavFilterPredict(X, dt, posEstimator.imu.accelNEU.x);
-            inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.y);
+        ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
+
+        // Accelerometer bias
+        if (!isAirCushionEffectDetected) {
+            ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+        }
+
+        return true;
+    }
+    else if (STATE(FIXED_WING) && (ctx->newFlags & EST_GPS_Z_VALID)) {
+        // If baro is not available - use GPS Z for correction on a plane
+        // Reset current estimate to GPS altitude if estimate not valid
+        if (!(ctx->newFlags & EST_Z_VALID)) {
+            ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
+            ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z;
+            ctx->newEPV = posEstimator.gps.epv;
        }
        else {
-            inavFilterPredict(X, dt, 0.0f);
-            inavFilterPredict(Y, dt, 0.0f);
-        }
+            // Altitude
+            const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
+
+            ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
+            ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
+            ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
+            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
+
+            // Accelerometer bias
+            ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
        }

-    /* Accelerometer bias correction */
-    const bool updateAccBias = (positionEstimationConfig()->w_acc_bias > 0);
-    fpVector3_t accelBiasCorr = { { 0, 0, 0} };
-
-    /* Correction step: Z-axis */
-    if (useGpsZPos || isBaroValid) {
-        float gpsWeightScaler = 1.0f;
-
-        /* Handle Z-axis reset */
-        if (!isEstZValid && useGpsZPos) {
-            posEstimator.est.pos.z = posEstimator.gps.pos.z;
-            posEstimator.est.vel.z = posEstimator.gps.vel.z;
-            newEPV = posEstimator.gps.epv;
-        }
-        else {
-#if defined(USE_BARO)
-            /* Apply BARO correction to altitude */
-            if (isBaroValid) {
-                const float baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
-                inavFilterCorrectPos(Z, dt, baroResidual, positionEstimationConfig()->w_z_baro_p);
-                newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
-
-                /* accelerometer bias correction for baro */
-                if (updateAccBias && !isAirCushionEffectDetected) {
-                    accelBiasCorr.z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
-                }
-            }
-#endif
-
-            /* Apply GPS correction to altitude */
-            if (useGpsZPos) {
-                const float gpsResidualZ = posEstimator.gps.pos.z - posEstimator.est.pos.z;
-                inavFilterCorrectPos(Z, dt, gpsResidualZ, positionEstimationConfig()->w_z_gps_p * gpsWeightScaler);
-                newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidualZ), positionEstimationConfig()->w_z_gps_p);
-
-                if (updateAccBias) {
-                    accelBiasCorr.z -= gpsResidualZ * sq(positionEstimationConfig()->w_z_gps_p);
-                }
+        return true;
    }

-            /* Apply GPS correction to climb rate */
-            if (useGpsZVel) {
-                const float gpsResidualZVel = posEstimator.gps.vel.z - posEstimator.est.vel.z;
-                inavFilterCorrectVel(Z, dt, gpsResidualZVel, positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler));
-            }
-        }
-    }
-    else {
-        inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.z, positionEstimationConfig()->w_z_res_v);
+    return false;
 }

-    /* Correction step: XY-axis */
-    /* GPS */
-    if (isGPSValid) {
+static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
+{
+    if (ctx->newFlags & EST_GPS_XY_VALID) {
        /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
-        if (!isEstXYValid) {
-            posEstimator.est.pos.x = posEstimator.gps.pos.x;
-            posEstimator.est.pos.y = posEstimator.gps.pos.y;
-            posEstimator.est.vel.x = posEstimator.gps.vel.x;
-            posEstimator.est.vel.y = posEstimator.gps.vel.y;
-            newEPH = posEstimator.gps.eph;
+        if (!(ctx->newFlags & EST_XY_VALID)) {
+            ctx->estPosCorr.x += posEstimator.gps.pos.x - posEstimator.est.pos.x;
+            ctx->estPosCorr.y += posEstimator.gps.pos.y - posEstimator.est.pos.y;
+            ctx->estVelCorr.x += posEstimator.gps.vel.x - posEstimator.est.vel.x;
+            ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y;
+            ctx->newEPH = posEstimator.gps.epv;
        }
        else {
-            const float gpsResidualX = posEstimator.gps.pos.x - posEstimator.est.pos.x;
-            const float gpsResidualY = posEstimator.gps.pos.y - posEstimator.est.pos.y;
-            const float gpsResidualXVel = posEstimator.gps.vel.x - posEstimator.est.vel.x;
-            const float gpsResidualYVel = posEstimator.gps.vel.y - posEstimator.est.vel.y;
-            const float gpsResidualXYMagnitude = sqrtf(sq(gpsResidualX) + sq(gpsResidualY));
+            const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x;
+            const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y;
+            const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x;
+            const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y;
+            const float gpsPosResidualMag = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));

-            //const float gpsWeightScaler = scaleRangef(bellCurve(gpsResidualXYMagnitude, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
+            //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
            const float gpsWeightScaler = 1.0f;

            const float w_xy_gps_p = positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler;
            const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler);

-            inavFilterCorrectPos(X, dt, gpsResidualX, w_xy_gps_p);
-            inavFilterCorrectPos(Y, dt, gpsResidualY, w_xy_gps_p);
+            // Coordinates
+            ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt;
+            ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt;

-            inavFilterCorrectVel(X, dt, gpsResidualXVel, w_xy_gps_v);
-            inavFilterCorrectVel(Y, dt, gpsResidualYVel, w_xy_gps_v);
+            // Velocity from coordinates
+            ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt;
+            ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt;
+
+            // Velocity from direct measurement
+            ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt;
+            ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt;
+
+            // Accelerometer bias
+            ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p);
+            ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p);

            /* Adjust EPH */
-            newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsResidualXYMagnitude), positionEstimationConfig()->w_xy_gps_p);
+            ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
        }
+
+        return true;
    }
-    else {
-        inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.x, positionEstimationConfig()->w_xy_res_v);
-        inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.y, positionEstimationConfig()->w_xy_res_v);
+
+    return false;
 }

+/**
+ * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
+ *  Function is called at main loop rate
+ */
+static void updateEstimatedTopic(timeUs_t currentTimeUs)
+{
+    estimationContext_t ctx;
+
+    /* Calculate dT */
+    ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
+    posEstimator.est.lastUpdateTime = currentTimeUs;
+
+    /* If IMU is not ready we can't estimate anything */
+    if (!isImuReady()) {
+        posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
+        posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
+        posEstimator.flags = 0;
+        return;
+    }
+
+    /* Calculate new EPH and EPV for the case we didn't update postion */
+    ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
+    vectorZero(&ctx.estPosCorr);
+    vectorZero(&ctx.estVelCorr);
+    vectorZero(&ctx.accBiasCorr);
+
+    /* AGL estimation - separate process, decouples from Z coordinate */
+    estimationCalculateAGL(&ctx);
+
+    /* Prediction stage: X,Y,Z */
+    estimationPredict(&ctx);
+
+    /* Correction stage: Z */
+    const bool estZCorrectOk =
+        estimationCalculateCorrection_Z(&ctx);
+
+    /* Correction stage: XY: GPS, FLOW */
+    // FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor
+    const bool estXYCorrectOk =
+        estimationCalculateCorrection_XY_GPS(&ctx) ||
+        estimationCalculateCorrection_XY_FLOW(&ctx);
+
+    // If we can't apply correction or accuracy is off the charts - decay velocity to zero
+    if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
+        ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
+        ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
+    }
+
+    if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
+        ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
+    }
+
+    // Apply corrections
+    vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
+    vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
+
    /* Correct accelerometer bias */
-    if (updateAccBias) {
-        const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
+    if (positionEstimationConfig()->w_acc_bias > 0.0f) {
+        const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
        if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
            /* transform error vector from NEU frame to body frame */
-            imuTransformVectorEarthToBody(&accelBiasCorr);
+            imuTransformVectorEarthToBody(&ctx.accBiasCorr);

            /* Correct accel bias */
-            posEstimator.imu.accelBias.x += accelBiasCorr.x * positionEstimationConfig()->w_acc_bias * dt;
-            posEstimator.imu.accelBias.y += accelBiasCorr.y * positionEstimationConfig()->w_acc_bias * dt;
-            posEstimator.imu.accelBias.z += accelBiasCorr.z * positionEstimationConfig()->w_acc_bias * dt;
+            posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
        }
    }

    /* Update uncertainty */
-    posEstimator.est.eph = newEPH;
-    posEstimator.est.epv = newEPV;
+    posEstimator.est.eph = ctx.newEPH;
+    posEstimator.est.epv = ctx.newEPV;

-    /* AGL estimation */
-#if defined(USE_RANGEFINDER) && defined(USE_BARO)
-    if (isSurfaceValid && isBaroValid) {
-        navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
-        bool resetSurfaceEstimate = false;
-        switch (posEstimator.est.aglQual) {
-            case SURFACE_QUAL_LOW:
-                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_HIGH;
-                    resetSurfaceEstimate = true;
-                }
-                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                else {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                break;
-
-            case SURFACE_QUAL_MID:
-                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_HIGH;
-                }
-                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_MID;
-                }
-                else {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                break;
-
-            case SURFACE_QUAL_HIGH:
-                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_HIGH;
-                }
-                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_MID;
-                }
-                else {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                break;
-        }
-
-        posEstimator.est.aglQual = newAglQuality;
-
-        if (resetSurfaceEstimate) {
-            posEstimator.est.aglAlt = posEstimator.surface.alt;
-            if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
-                posEstimator.est.aglVel = posEstimator.est.vel.z;
-                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
-            }
-            else {
-                posEstimator.est.aglVel = 0;
-                posEstimator.est.aglOffset = 0;
-            }
-        }
-
-        // Update estimate
-        posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.z * dt * dt * 0.5f;
-        posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * dt;
-
-        // Apply correction
-        if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
-            // Correct estimate from rangefinder
-            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
-            const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
-
-            posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt;
-            posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt;
-
-            // Update estimate offset
-            if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
-                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
-            }
-        }
-        else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
-            // Correct estimate from altitude fused from rangefinder and global altitude
-            const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
-            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
-            const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
-            const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
-
-            posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * dt;
-            posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * dt;
-        }
-        else {  // SURFACE_QUAL_LOW
-            // In this case rangefinder can't be trusted - simply use global altitude
-            posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
-            posEstimator.est.aglVel = posEstimator.est.vel.z;
-        }
-    }
-    else {
-        posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
-        posEstimator.est.aglVel = posEstimator.est.vel.z;
-        posEstimator.est.aglQual = SURFACE_QUAL_LOW;
-    }
-
-#if defined(NAV_BLACKBOX)
-        DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
-        DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
-        DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
-        DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
-#endif
-
-#else
-    posEstimator.est.aglAlt = posEstimator.est.pos.z;
-    posEstimator.est.aglVel = posEstimator.est.vel.z;
-    posEstimator.est.aglQual = SURFACE_QUAL_LOW;
-#endif
+    // Keep flags for further usage
+    posEstimator.flags = ctx.newFlags;
 }

 /**
@ -954,10 +720,11 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
    if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
        /* Publish position update */
        if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
-            updateActualHorizontalPositionAndVelocity(true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
+            // FIXME!!!!!
+            updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
        }
        else {
-            updateActualHorizontalPositionAndVelocity(false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
+            updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
        }

        /* Publish altitude update and set altitude validity */
@ -1001,6 +768,9 @@ void initializePositionEstimator(void)
    posEstimator.est.aglAlt = 0;
    posEstimator.est.aglVel = 0;

+    posEstimator.est.flowCoordinates[X] = 0;
+    posEstimator.est.flowCoordinates[Y] = 0;
+
    posEstimator.imu.gravityCalibrationComplete = false;

    for (axis = 0; axis < 3; axis++) {
--- a/src/main/navigation/navigation_pos_estimator_agl.c
+++ b/src/main/navigation/navigation_pos_estimator_agl.c
@ -0,0 +1,196 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#if defined(USE_NAV)
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+#include "navigation/navigation_pos_estimator_private.h"
+
+#include "sensors/rangefinder.h"
+#include "sensors/barometer.h"
+
+extern navigationPosEstimator_t posEstimator;
+
+#ifdef USE_RANGEFINDER
+/**
+ * Read surface and update alt/vel topic
+ *  Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
+ */
+void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
+{
+    const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
+    float newReliabilityMeasurement = 0;
+
+    posEstimator.surface.lastUpdateTime = currentTimeUs;
+
+    if (newSurfaceAlt >= 0) {
+        if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
+            newReliabilityMeasurement = 1.0f;
+            posEstimator.surface.alt = newSurfaceAlt;
+        }
+        else {
+            newReliabilityMeasurement = 0.0f;
+        }
+    }
+    else {
+        // Negative values - out of range or failed hardware
+        newReliabilityMeasurement = 0.0f;
+    }
+
+    /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
+    if (dt > 0.5f) {
+        posEstimator.surface.reliability = 0.0f;
+    }
+    else {
+        const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
+        posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
+    }
+}
+#endif
+
+void estimationCalculateAGL(estimationContext_t * ctx)
+{
+#if defined(USE_RANGEFINDER) && defined(USE_BARO)
+    if ((ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_BARO_VALID)) {
+        navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
+        bool resetSurfaceEstimate = false;
+        switch (posEstimator.est.aglQual) {
+            case SURFACE_QUAL_LOW:
+                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_HIGH;
+                    resetSurfaceEstimate = true;
+                }
+                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                else {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                break;
+
+            case SURFACE_QUAL_MID:
+                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_HIGH;
+                }
+                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_MID;
+                }
+                else {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                break;
+
+            case SURFACE_QUAL_HIGH:
+                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_HIGH;
+                }
+                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_MID;
+                }
+                else {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                break;
+        }
+
+        posEstimator.est.aglQual = newAglQuality;
+
+        if (resetSurfaceEstimate) {
+            posEstimator.est.aglAlt = posEstimator.surface.alt;
+            if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+                posEstimator.est.aglVel = posEstimator.est.vel.z;
+                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
+            }
+            else {
+                posEstimator.est.aglVel = 0;
+                posEstimator.est.aglOffset = 0;
+            }
+        }
+
+        // Update estimate
+        posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * ctx->dt;
+        posEstimator.est.aglAlt = posEstimator.imu.accelNEU.z * sq(ctx->dt) * 0.5f;
+        posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * ctx->dt;
+
+        // Apply correction
+        if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
+            // Correct estimate from rangefinder
+            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
+            const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
+
+            posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * ctx->dt;
+            posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * ctx->dt;
+
+            // Update estimate offset
+            if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
+                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
+            }
+        }
+        else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
+            // Correct estimate from altitude fused from rangefinder and global altitude
+            const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
+            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
+            const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
+            const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
+
+            posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * ctx->dt;
+            posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * ctx->dt;
+        }
+        else {  // SURFACE_QUAL_LOW
+            // In this case rangefinder can't be trusted - simply use global altitude
+            posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
+            posEstimator.est.aglVel = posEstimator.est.vel.z;
+        }
+    }
+    else {
+        posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
+        posEstimator.est.aglVel = posEstimator.est.vel.z;
+        posEstimator.est.aglQual = SURFACE_QUAL_LOW;
+    }
+
+    DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
+    DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
+    DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
+    DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
+
+#else
+    UNUSED(ctx);
+    posEstimator.est.aglAlt = posEstimator.est.pos.z;
+    posEstimator.est.aglVel = posEstimator.est.vel.z;
+    posEstimator.est.aglQual = SURFACE_QUAL_LOW;
+#endif
+}
+
+#endif  // NAV
--- a/src/main/navigation/navigation_pos_estimator_flow.c
+++ b/src/main/navigation/navigation_pos_estimator_flow.c
@ -0,0 +1,128 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#if defined(USE_NAV)
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+#include "navigation/navigation_pos_estimator_private.h"
+
+#include "sensors/rangefinder.h"
+#include "sensors/opflow.h"
+
+#include "flight/imu.h"
+
+extern navigationPosEstimator_t posEstimator;
+
+#ifdef USE_OPTICAL_FLOW
+/**
+ * Read optical flow topic
+ *  Function is called by OPFLOW task as soon as new update is available
+ */
+void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
+{
+    posEstimator.flow.lastUpdateTime = currentTimeUs;
+    posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
+    posEstimator.flow.flowRate[X] = opflow.flowRate[X];
+    posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
+    posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
+    posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
+}
+#endif
+
+bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
+{
+#if defined(USE_RANGEFINDER) && defined(USE_OPTICAL_FLOW)
+    if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) {
+        return false;
+    }
+
+    // FIXME: flow may use AGL estimate if available
+    const bool canUseFlow = (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD);
+
+    if (!canUseFlow) {
+        return false;
+    }
+
+    // Calculate linear velocity based on angular velocity and altitude
+    // Technically we should calculate arc length here, but for fast sampling this is accurate enough
+    fpVector3_t flowVel = {
+        .x = - (posEstimator.flow.flowRate[Y] - posEstimator.flow.bodyRate[Y]) * posEstimator.surface.alt,
+        .y =   (posEstimator.flow.flowRate[X] - posEstimator.flow.bodyRate[X]) * posEstimator.surface.alt,
+        .z =    posEstimator.est.vel.z
+    };
+
+    // At this point flowVel will hold linear velocities in earth frame
+    imuTransformVectorBodyToEarth(&flowVel);
+
+    // Calculate velocity correction
+    const float flowVelXInnov = flowVel.x - posEstimator.est.vel.x;
+    const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y;
+
+    ctx->estVelCorr.x = flowVelXInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
+    ctx->estVelCorr.y = flowVelYInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
+
+    // Calculate position correction if possible/allowed
+    if ((ctx->newFlags & EST_GPS_XY_VALID)) {
+        // If GPS is valid - reset flow estimated coordinates to GPS
+        posEstimator.est.flowCoordinates[X] = posEstimator.gps.pos.x;
+        posEstimator.est.flowCoordinates[Y] = posEstimator.gps.pos.y;
+    }
+    else if (positionEstimationConfig()->allow_dead_reckoning) {
+        posEstimator.est.flowCoordinates[X] += flowVel.x * ctx->dt;
+        posEstimator.est.flowCoordinates[Y] += flowVel.y * ctx->dt;
+
+        const float flowResidualX = posEstimator.est.flowCoordinates[X] - posEstimator.est.pos.x;
+        const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y;
+
+        ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
+        ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
+
+        ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
+    }
+
+    DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[Y]));
+    DEBUG_SET(DEBUG_FLOW, 1, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));
+    DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]);
+    DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]);
+
+    return true;
+#else
+    UNUSED(ctx);
+    return false;
+#endif
+}
+
+
+#endif  // NAV
--- a/src/main/navigation/navigation_pos_estimator_private.h
+++ b/src/main/navigation/navigation_pos_estimator_private.h
@ -0,0 +1,175 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "sensors/sensors.h"
+
+#define INAV_GPS_DEFAULT_EPH                200.0f  // 2m GPS HDOP  (gives about 1.6s of dead-reckoning if GPS is temporary lost)
+#define INAV_GPS_DEFAULT_EPV                500.0f  // 5m GPS VDOP
+
+#define INAV_GPS_ACCEPTANCE_EPE             500.0f  // 5m acceptance radius
+
+#define INAV_ACC_BIAS_ACCEPTANCE_VALUE      (GRAVITY_CMSS * 0.25f)   // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway
+
+#define INAV_GPS_GLITCH_RADIUS              250.0f  // 2.5m GPS glitch radius
+#define INAV_GPS_GLITCH_ACCEL               1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
+
+#define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
+#define INAV_PITOT_UPDATE_RATE              10
+
+#define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
+#define INAV_BARO_TIMEOUT_MS                200     // Baro timeout
+#define INAV_SURFACE_TIMEOUT_MS             300     // Surface timeout    (missed 3 readings in a row)
+#define INAV_FLOW_TIMEOUT_MS                200
+
+#define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
+#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
+#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD   (0.33f)
+#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD  (0.75f)
+
+typedef struct {
+    timeUs_t    lastTriggeredTime;
+    timeUs_t    deltaTime;
+} navigationTimer_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+#if defined(NAV_GPS_GLITCH_DETECTION)
+    bool        glitchDetected;
+    bool        glitchRecovery;
+#endif
+    fpVector3_t pos;            // GPS position in NEU coordinate system (cm)
+    fpVector3_t vel;            // GPS velocity (cms)
+    float       eph;
+    float       epv;
+} navPositionEstimatorGPS_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    float       alt;            // Raw barometric altitude (cm)
+    float       epv;
+} navPositionEstimatorBARO_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    float       airspeed;            // airspeed (cm/s)
+} navPositionEstimatorPITOT_t;
+
+typedef enum {
+    SURFACE_QUAL_LOW,   // Surface sensor signal lost long ago - most likely surface distance is incorrect
+    SURFACE_QUAL_MID,   // Surface sensor is not available but we can somewhat trust the estimate
+    SURFACE_QUAL_HIGH   // All good
+} navAGLEstimateQuality_e;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    float       alt;            // Raw altitude measurement (cm)
+    float       reliability;
+} navPositionEstimatorSURFACE_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    bool        isValid;
+    float       quality;
+    float       flowRate[2];
+    float       bodyRate[2];
+} navPositionEstimatorFLOW_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+
+    // 3D position, velocity and confidence
+    fpVector3_t pos;
+    fpVector3_t vel;
+    float       eph;
+    float       epv;
+
+    // AGL
+    navAGLEstimateQuality_e aglQual;
+    float                   aglOffset;  // Offset between surface and pos.Z
+    float                   aglAlt;
+    float                   aglVel;
+
+    // FLOW
+    float                   flowCoordinates[2];
+} navPositionEstimatorESTIMATE_t;
+
+typedef struct {
+    fpVector3_t     accelNEU;
+    fpVector3_t     accelBias;
+    bool            gravityCalibrationComplete;
+} navPosisitonEstimatorIMU_t;
+
+typedef enum {
+    EST_GPS_XY_VALID            = (1 << 0),
+    EST_GPS_Z_VALID             = (1 << 1),
+    EST_BARO_VALID              = (1 << 2),
+    EST_SURFACE_VALID           = (1 << 3),
+    EST_FLOW_VALID              = (1 << 4),
+    EST_XY_VALID                = (1 << 5),
+    EST_Z_VALID                 = (1 << 6),
+} navPositionEstimationFlags_e;
+
+typedef struct {
+    timeUs_t    baroGroundTimeout;
+    float       baroGroundAlt;
+    bool        isBaroGroundValid;
+} navPositionEstimatorSTATE_t;
+
+
+typedef struct {
+    uint32_t                    flags;
+
+    // Data sources
+    navPositionEstimatorGPS_t   gps;
+    navPositionEstimatorBARO_t  baro;
+    navPositionEstimatorSURFACE_t surface;
+    navPositionEstimatorPITOT_t pitot;
+    navPositionEstimatorFLOW_t  flow;
+
+    // IMU data
+    navPosisitonEstimatorIMU_t  imu;
+
+    // Estimate
+    navPositionEstimatorESTIMATE_t  est;
+
+    // Extra state variables
+    navPositionEstimatorSTATE_t state;
+} navigationPosEstimator_t;
+
+typedef struct {
+    float dt;
+    uint32_t newFlags;
+    float newEPV;
+    float newEPH;
+    fpVector3_t estPosCorr;
+    fpVector3_t estVelCorr;
+    fpVector3_t accBiasCorr;
+} estimationContext_t;
+
+extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
+extern void estimationCalculateAGL(estimationContext_t * ctx);
+extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
+
+
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@ -21,7 +21,10 @@

 #if defined(USE_NAV)

+#include "common/axis.h"
+#include "common/maths.h"
 #include "common/filter.h"
+#include "common/time.h"
 #include "fc/runtime_config.h"

 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
@ -35,13 +38,6 @@

 #define INAV_SURFACE_MAX_DISTANCE           40

-#define HZ2US(hz)   (1000000 / (hz))
-#define US2S(us)    ((us) * 1e-6f)
-#define US2MS(us)   ((us) * 1e-3f)
-#define MS2US(ms)   ((ms) * 1000)
-#define MS2S(ms)    ((ms) * 1e-3f)
-#define HZ2S(hz)    US2S(HZ2US(hz))
-
 typedef enum {
    NAV_POS_UPDATE_NONE                 = 0,
    NAV_POS_UPDATE_Z                    = 1 << 1,
@ -80,6 +76,7 @@ typedef struct navigationFlags_s {

    navigationEstimateStatus_e estAltStatus;        // Indicates that we have a working altitude sensor (got at least one valid reading from it)
    navigationEstimateStatus_e estPosStatus;        // Indicates that GPS is working (or not)
+    navigationEstimateStatus_e estVelStatus;        // Indicates that GPS is working (or not)
    navigationEstimateStatus_e estAglStatus;
    navigationEstimateStatus_e estHeadingStatus;    // Indicate valid heading - wither mag or GPS at certain speed on airplane

@ -95,46 +92,12 @@ typedef struct navigationFlags_s {
    bool forcedRTHActivated;
 } navigationFlags_t;

-typedef struct {
-    float kP;
-    float kI;
-    float kD;
-    float kT;   // Tracking gain (anti-windup)
-} pidControllerParam_t;
-
-typedef struct {
-    float kP;
-} pControllerParam_t;
-
 typedef enum {
    PID_DTERM_FROM_ERROR            = 1 << 0,
    PID_ZERO_INTEGRATOR             = 1 << 1,
    PID_SHRINK_INTEGRATOR           = 1 << 2,
 } pidControllerFlags_e;

-typedef struct {
-    bool reset;
-    pidControllerParam_t param;
-    pt1Filter_t dterm_filter_state;  // last derivative for low-pass filter
-    float integrator;       // integrator value
-    float last_input;       // last input for derivative
-} pidController_t;
-
-typedef struct {
-    pControllerParam_t param;
-} pController_t;
-
-typedef struct navigationPIDControllers_s {
-    /* Multicopter PIDs */
-    pController_t   pos[XYZ_AXIS_COUNT];
-    pidController_t vel[XYZ_AXIS_COUNT];
-    pidController_t surface;
-
-    /* Fixed-wing PIDs */
-    pidController_t fw_alt;
-    pidController_t fw_nav;
-} navigationPIDControllers_t;
-
 typedef struct {
    fpVector3_t pos;
    fpVector3_t vel;
@ -181,6 +144,9 @@ typedef enum {
    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,

+    NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D,
+    NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
+    NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ,
    NAV_FSM_EVENT_COUNT,
 } navigationFSMEvent_t;

@ -225,6 +191,14 @@ typedef enum {
    NAV_PERSISTENT_ID_LAUNCH_WAIT                               = 26,
    NAV_PERSISTENT_ID_UNUSED_3                                  = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY
    NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS                        = 28,
+
+    NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE                      = 29,
+    NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS                     = 30,
+    NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING                       = 31,
+
+    NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE                      = 32,
+    NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS                     = 33,
+    NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING                       = 34,
 } navigationPersistentId_e;

 typedef enum {
@ -263,6 +237,13 @@ typedef enum {
    NAV_STATE_LAUNCH_WAIT,
    NAV_STATE_LAUNCH_IN_PROGRESS,

+    NAV_STATE_CRUISE_2D_INITIALIZE,
+    NAV_STATE_CRUISE_2D_IN_PROGRESS,
+    NAV_STATE_CRUISE_2D_ADJUSTING,
+    NAV_STATE_CRUISE_3D_INITIALIZE,
+    NAV_STATE_CRUISE_3D_IN_PROGRESS,
+    NAV_STATE_CRUISE_3D_ADJUSTING,
+
    NAV_STATE_COUNT,
 } navigationFSMState_t;

@ -310,6 +291,13 @@ typedef struct {
    float           minimalDistanceToHome;
 } rthSanityChecker_t;

+typedef struct {
+    fpVector3_t                 targetPos;
+    int32_t                     yaw;
+    int32_t                     previousYaw;
+    timeMs_t                    lastYawAdjustmentTime;
+} navCruise_t;
+
 typedef struct {
    /* Flags and navigation system state */
    navigationFSMState_t        navState;
@ -341,6 +329,9 @@ typedef struct {
    uint32_t                    homeDistance;   // cm
    int32_t                     homeDirection;  // deg*100

+    /* Cruise */
+    navCruise_t                 cruise;
+
    /* Waypoint list */
    navWaypoint_t               waypointList[NAV_MAX_WAYPOINTS];
    bool                        waypointListValid;
@ -385,7 +376,7 @@ bool isApproachingLastWaypoint(void);
 float getActiveWaypointSpeed(void);

 void updateActualHeading(bool headingValid, int32_t newHeading);
-void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY);
+void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);

 bool checkForPositionSensorTimeout(void);
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@ -465,7 +465,7 @@ static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample)
    return quickMedianFilter5_16(rcSamples[chan]);
 }

-void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
+bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 {
    UNUSED(currentTimeUs);

@ -535,6 +535,7 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
    }

    rcSampleIndex++;
+    return true;
 }

 void parseRcChannels(const char *input)
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@ -165,7 +165,7 @@ void rxUpdateRSSISource(void);
 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
 bool rxIsReceivingSignal(void);
 bool rxAreFlightChannelsValid(void);
-void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
+bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);

 void parseRcChannels(const char *input);

--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@ -114,7 +114,7 @@ typedef enum {
 #ifdef USE_RCDEVICE
    TASK_RCDEVICE,
 #endif
-#ifdef VTX_CONTROL
+#ifdef USE_VTX_CONTROL
    TASK_VTXCTRL,
 #endif

--- a/Show more
+++ b/Show more