Merge branch 'master' into dzikuvx-rate-dynamics

# Conflicts:
#	docs/Settings.md
#	src/main/CMakeLists.txt
#	src/main/build/debug.h
#	src/main/fc/settings.yaml
#	src/main/target/common.h

.gitignore

@@ -8,6 +8,7 @@
 .project
 .settings
 .cproject
+.cache/
 __pycache__
 startup_stm32f10x_md_gcc.s
 .vagrant/

AUTHORS

@@ -35,6 +35,7 @@ Gaël James
 Gregor Ottmann
 Google LLC
 Hyon Lim
+Hubert Jozwiak
 James Harrison
 Jan Staal
 Jeremy Waters

CMakeLists.txt

@@ -37,7 +37,7 @@ else()
     include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
 endif()
 
-project(INAV VERSION 3.0.0)
+project(INAV VERSION 3.1.0)
 
 enable_language(ASM)
 

cmake/stm32h7.cmake

@@ -143,9 +143,9 @@ main_sources(STM32H7_SRC
 
     config/config_streamer_stm32h7.c
 
-#    drivers/adc_stm32h7xx.c
+    drivers/adc_stm32h7xx.c
     drivers/bus_i2c_hal.c
-#    drivers/dma_stm32h7xx.c
+    drivers/dma_stm32h7xx.c
     drivers/bus_spi_hal.c
     drivers/memprot.h
     drivers/memprot_hal.c

docs/Board - FrSky Pilot.md

@@ -0,0 +1,13 @@
+# Board - FRSKYPILOT
+
+## Vendor Information / specification
+https://www.frsky-rc.com/product/r9rxsr-pilot/
+
+## Firmware
+
+Two firmware variants are available.
+
+* `inav_x.y.z_FRSKYPILOT.hex` 
+* `inav_x.y.z_FRSKYPILOT_LED.hex` 
+
+The only difference is that the `_LED` variant provides WS2812 LED support on S10. S11 & S12 are used for UART3 only.

docs/Board - Matek F411-WSE.md

@@ -0,0 +1,26 @@
+# Board - [MATEKSYS F411-WSE](http://www.mateksys.com/?portfolio=f411-wse)
+
+![Matek F411 Wing](http://www.mateksys.com/wp-content/uploads/2019/06/F411-WSE_1-1500x600.jpg)
+
+## Specification:
+
+* STM32F411 CPU
+* OSD
+* BMP280 barometer
+* Integrated PDB for 2 motors
+* 2 UART ports
+* 6 servos
+* no SD card slot
+* 3 BECs
+
+## Details
+
+* [Full specification](http://www.mateksys.com/?portfolio=f411-wse)
+* SBUS pad has a built-in inverter and is connected to UART2 RX
+
+## Available TARGETS
+
+* `MATEKF411SE` Stock target. LED control and have SS1 on ST1 pad, SS2 on TX2 pad.
+* `MATEKF411SE_PINIO` Adds USER 2 PINIO support on the LED pad.
+* `MATEKF411SE_FD_SFTSRL1` Adds full duplex SS1 by putting the RX on the LED pad.
+* `MATEKF411SE_SS2_CH6` SS2 moved to Ch6 pad. This keeps UART 2 as a full UART (for example, for use with Crossfire) and SS2 support.

docs/Controls.md

@@ -34,7 +34,7 @@ The stick positions are combined to activate different functions:
 | Trim Acc Forwards             | HIGH     | CENTER  | HIGH   | CENTER |
 | Trim Acc Backwards            | HIGH     | CENTER  | LOW    | CENTER |
 | Save current waypoint mission | LOW      | CENTER  | HIGH   | LOW    |
-| Load current waypoint mission | LOW      | CENTER  | HIGH   | HIGH   |
+| Load/unload current waypoint mission | LOW      | CENTER  | HIGH   | HIGH   |
 | Save setting                  | LOW      | LOW     | LOW    | HIGH   |
 | Enter OSD Menu (CMS)          | CENTER   | LOW     | HIGH   | CENTER |
 

docs/Navigation.md

@@ -25,7 +25,7 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co
 Position hold requires GPS, accelerometer and compass sensors. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
 When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From inav 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
 
-### CLI parameters affecting ALTHOLD mode:
+### CLI parameters affecting POSHOLD mode:
 * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own.
 
 ### Related PIDs

docs/Safehomes.md

@@ -12,14 +12,20 @@ One potential risk when landing is that there might be buildings, trees and othe
 
 ## Safehome
 
-Safehomes are a list of GPS coordinates that identify safe landing points.  When the flight controller is armed, it checks the list of safehomes.  The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be selected.  Otherwise, it reverts to the old behaviour of using your current GPS position as home.  
+Safehomes are a list of GPS coordinates that identify safe landing points.  You can define up to 8 safehomes for different locations you fly at.  When the flight controller is armed, it checks the list of safehomes.  The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position is identified.  The arming home location remains as home.
 
-Be aware that the safehome replaces your arming position as home.  When flying, RTH will return to the safehome and OSD elements such as distance to home and direction to home will refer to the selected safehome.
+When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the home location.  If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return back to arming point.
 
-You can define up to 8 safehomes for different locations you fly at.
+The safehome operating mode is set using ```safehome_usage_mode```.  If ```OFF```, safehomes will not be used.  If ```RTH```, the safehome will replace the arming location when RTH is activated, either manually or because of RX failsafe.  If ```RTH_FS```, the safehome will only be used for RX failsafe.  This option can be changed using the OSD menu.
+
+If you frequently use RTH to return back to the arming point, you may not want the aircraft to fly to the safehome.  Let it do this at least once to confirm safehomes is working as expected.  Afterward, `set safehome_usage_mode = RTH_FS` and the safehome will only be used for failsafe.  
+
+When using mode `RTH_FS`, you should confirm that your radio's failsafe configuration triggers the iNav failsafe mode.  With many receivers, you have the ability to specify what signal to output during failsafe conditions.
 
 When you are choosing safehome locations, ensure that the location is clear of obstructions for a radius more than 50m (`nav_fw_loiter_radius`).  As the plane descends, the circles aren't always symmetrical, as wind direction could result in some wider or tighter turns.  Also, the direction and length of the final landing stage is also unknown.  You want to choose a point that has provides a margin for variation and the final landing.
 
+If your safehome is not visible from your current location, use extra caution.  A visual check of the safehome is recommend prior to flying.  If the safehome is in use, you can use the OSD menu to disable safehome usage prior to your flight.
+
 ## OSD Message when Armed
 
 When the aircraft is armed, the OSD briefly shows `ARMED` and the current GPS position and current date and time.
@@ -36,8 +42,14 @@ If a safehome is selected, an additional message appears:
          CURRENT DATE
          CURRENT TIME
 ```
-The GPS details are those of the selected safehome.
-To draw your attention to "HOME" being replaced, the message flashes and stays visible longer.
+The GPS details are those of the arming location, not the safehome.
+To draw your attention to a safehome being selected, the message flashes and stays visible longer.
+
+If a safehome was found, but ``safehome_usage_mode``` is ```OFF```, the message ```SAFEHOME FOUND; MODE OFF``` will appear.
+
+## OSD Message during RTH
+
+If RTH is in progress to a safehome, the message "DIVERTING TO SAFEHOME" will be displayed.
 
 ## CLI command `safehome` to manage safehomes
 

docs/Settings.md

@@ -1,5 +1,6 @@
 # CLI Variable Reference
 
+<<<<<<< HEAD
 | Variable Name | Default Value | Description |
 | ------------- | ------------- | ----------- |
 | 3d_deadband_high | 1514 | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) |
@@ -538,5 +539,569 @@
 | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. |
 | yaw_lpf_hz | 0 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
 | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
+=======
+| Variable Name | Default Value | Min | Max | Description |
+| ------------- | ------------- | --- | --- | ----------- |
+| 3d_deadband_high | 1514 | PWM_RANGE_MIN | PWM_RANGE_MAX | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) |
+| 3d_deadband_low | 1406 | PWM_RANGE_MIN | PWM_RANGE_MAX | Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead) |
+| 3d_deadband_throttle | 50 | 0 | 200 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. |
+| 3d_neutral | 1460 | PWM_RANGE_MIN | PWM_RANGE_MAX | Neutral (stop) throttle value for 3D mode |
+| acc_event_threshold_high | 0 | 0 | 65535 | Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off. |
+| acc_event_threshold_low | 0 | 0 | 900 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. |
+| acc_event_threshold_neg_x | 0 | 0 | 65535 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. |
+| acc_hardware | AUTO |  |  | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info |
+| acc_lpf_hz | 15 | 0 | 200 | 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. |
+| acc_lpf_type | BIQUAD |  |  | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
+| acc_notch_cutoff | 1 | 1 | 255 |  |
+| acc_notch_hz | 0 | 0 | 255 |  |
+| accgain_x | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
+| accgain_y | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
+| accgain_z | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
+| acczero_x | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. |
+| acczero_y | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. |
+| acczero_z | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. |
+| airmode_throttle_threshold | 1300 | 1000 | 2000 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used |
+| airmode_type | STICK_CENTER |  |  | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. |
+| airspeed_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 |
+| align_acc | 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. |
+| align_board_pitch | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc |
+| align_board_roll | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc |
+| align_board_yaw | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc |
+| 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. |
+| align_mag | 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. |
+| align_mag_pitch | 0 | -1800 | 3600 | Same as align_mag_roll, but for the pitch axis. |
+| align_mag_roll | 0 | -1800 | 3600 | 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_yaw | 0 | -1800 | 3600 | Same as align_mag_roll, but for the yaw axis. |
+| align_opflow | CW0FLIP |  |  | Optical flow module alignment (default CW0_DEG_FLIP) |
+| alt_hold_deadband | 50 | 10 | 250 | Defines the deadband of throttle during alt_hold [r/c points] |
+| antigravity_accelerator | 1 | 1 | 20 |  |
+| antigravity_cutoff_lpf_hz | 15 | 1 | 30 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain |
+| antigravity_gain | 1 | 1 | 20 | Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements |
+| applied_defaults | 0 | 0 | 3 | Internal (configurator) hint. Should not be changed manually |
+| baro_cal_tolerance | 150 | 0 | 1000 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. |
+| baro_hardware | AUTO |  |  | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info |
+| baro_median_filter | ON |  |  | 3-point median filtering for barometer readouts. No reason to change this setting |
+| bat_cells | 0 | 0 | 12 | Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. |
+| 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` |
+| battery_capacity | 0 | 0 | 4294967295 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. |
+| battery_capacity_critical | 0 | 0 | 4294967295 | 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). |
+| battery_capacity_warning | 0 | 0 | 4294967295 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. |
+| blackbox_device | _target default_ |  |  | Selection of where to write blackbox data |
+| blackbox_rate_denom | 1 | 1 | 65535 | Blackbox logging rate denominator. See blackbox_rate_num. |
+| blackbox_rate_num | 1 | 1 | 65535 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations |
+| cpu_underclock | OFF |  |  | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz |
+| cruise_power | 0 | 0 | 4294967295 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit |
+| current_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled |
+| current_meter_offset | _target default_ | -32768 | 32767 | This sets the output offset voltage of the current sensor in millivolts. |
+| current_meter_scale | _target default_ | -10000 | 10000 | 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. |
+| current_meter_type | ADC |  |  | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. |
+| d_boost_factor | 1.25 | 1 | 3 |  |
+| d_boost_gyro_delta_lpf_hz | 80 | 10 | 250 |  |
+| d_boost_max_at_acceleration | 7500 | 1000 | 16000 |  |
+| deadband | 5 | 0 | 32 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. |
+| debug_mode | NONE |  |  | Defines debug values exposed in debug variables (developer / debugging setting) |
+| 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. |
+| display_force_sw_blink | OFF |  |  | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON |
+| dji_esc_temp_source | ESC |  |  | Re-purpose the ESC temperature field for IMU/BARO temperature |
+| dji_use_name_for_messages | ON |  |  | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance |
+| dji_workarounds | 1 | 0 | 255 | Enables workarounds for different versions of MSP protocol used |
+| dshot_beeper_enabled | ON |  |  | Whether using DShot motors as beepers is enabled |
+| dshot_beeper_tone | 1 | 1 | 5 | Sets the DShot beeper tone |
+| dterm_lpf2_hz | 0 | 0 | 500 | Cutoff frequency for stage 2 D-term low pass filter |
+| dterm_lpf2_type | BIQUAD |  |  | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
+| dterm_lpf_hz | 40 | 0 | 500 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value |
+| dterm_lpf_type | BIQUAD |  |  | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
+| dynamic_gyro_notch_enabled | OFF |  |  | Enable/disable dynamic gyro notch also known as Matrix Filter |
+| dynamic_gyro_notch_min_hz | 150 | 30 | 1000 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
+| dynamic_gyro_notch_q | 120 | 1 | 1000 | Q factor for dynamic notches |
+| dynamic_gyro_notch_range | MEDIUM |  |  | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers |
+| eleres_freq | 435 | 415 | 450 |  |
+| eleres_loc_delay | 240 | 30 | 1800 |  |
+| eleres_loc_en | OFF |  |  |  |
+| eleres_loc_power | 7 | 0 | 7 |  |
+| eleres_signature | 0 |  | 4294967295 |  |
+| eleres_telemetry_en | OFF |  |  |  |
+| eleres_telemetry_power | 7 | 0 | 7 |  |
+| esc_sensor_listen_only | OFF |  |  | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case |
+| failsafe_delay | 5 | 0 | 200 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). |
+| failsafe_fw_pitch_angle | 100 | -800 | 800 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb |
+| failsafe_fw_roll_angle | -200 | -800 | 800 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll |
+| failsafe_fw_yaw_rate | -45 | -1000 | 1000 | Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn |
+| failsafe_lights | ON |  |  | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. |
+| failsafe_lights_flash_on_time | 100 | 20 | 65535 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. |
+| failsafe_lights_flash_period | 1000 | 40 | 65535 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. |
+| failsafe_min_distance | 0 | 0 | 65000 | If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken. |
+| failsafe_min_distance_procedure | DROP |  |  | What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). |
+| failsafe_mission | ON |  |  | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode |
+| failsafe_off_delay | 200 | 0 | 200 | Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). |
+| failsafe_procedure | SET-THR |  |  | What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). |
+| failsafe_recovery_delay | 5 | 0 | 200 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). |
+| failsafe_stick_threshold | 50 | 0 | 500 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. |
+| failsafe_throttle | 1000 | PWM_RANGE_MIN | PWM_RANGE_MAX | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). |
+| failsafe_throttle_low_delay | 0 | 0 | 300 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
+| fixed_wing_auto_arm | OFF |  |  | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
+| flaperon_throw_offset | 200 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
+| fpv_mix_degrees | 0 | 0 | 50 |  |
+| frsky_coordinates_format | 0 | 0 | FRSKY_FORMAT_NMEA | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
+| frsky_default_latitude | 0 | -90 | 90 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
+| frsky_default_longitude | 0 | -180 | 180 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
+| frsky_pitch_roll | OFF |  |  | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data |
+| frsky_unit | METRIC |  |  | Not used? [METRIC/IMPERIAL] |
+| frsky_vfas_precision | 0 | FRSKY_VFAS_PRECISION_LOW | FRSKY_VFAS_PRECISION_HIGH | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method |
+| fw_autotune_ff_to_i_tc | 600 | 100 | 5000 | FF to I time (defines time for I to reach the same level of response as FF) [ms] |
+| fw_autotune_ff_to_p_gain | 10 | 0 | 100 | FF to P gain (strength relationship) [%] |
+| fw_autotune_max_rate_deflection | 80 | 50 | 100 | The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. |
+| fw_autotune_min_stick | 50 | 0 | 100 | Minimum stick input [%] to consider overshoot/undershoot detection |
+| fw_autotune_p_to_d_gain | 0 | 0 | 200 | P to D gain (strength relationship) [%] |
+| fw_autotune_rate_adjustment | AUTO |  |  | `AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode. |
+| fw_d_level | 75 | 0 | 200 | Fixed-wing attitude stabilisation HORIZON transition point |
+| fw_d_pitch | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for PITCH |
+| fw_d_roll | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for ROLL |
+| fw_d_yaw | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for YAW |
+| fw_ff_pitch | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for PITCH |
+| fw_ff_roll | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for ROLL |
+| fw_ff_yaw | 60 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for YAW |
+| fw_i_level | 5 | 0 | 200 | Fixed-wing attitude stabilisation low-pass filter cutoff |
+| fw_i_pitch | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for PITCH |
+| fw_i_roll | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for ROLL |
+| fw_i_yaw | 10 | 0 | 200 | Fixed-wing rate stabilisation I-gain for YAW |
+| fw_iterm_limit_stick_position | 0.5 | 0 | 1 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side |
+| fw_iterm_throw_limit | 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. |
+| fw_level_pitch_deadband | 5 | 0 | 20 | Deadband for automatic leveling when AUTOLEVEL mode is used. |
+| fw_level_pitch_gain | 5 | 0 | 20 | I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations |
+| fw_level_pitch_trim | 0 | -10 | 10 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level |
+| fw_loiter_direction | RIGHT |  |  | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. |
+| fw_min_throttle_down_pitch | 0 | 0 | 450 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) |
+| fw_p_level | 20 | 0 | 200 | Fixed-wing attitude stabilisation P-gain |
+| fw_p_pitch | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for PITCH |
+| fw_p_roll | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for ROLL |
+| fw_p_yaw | 6 | 0 | 200 | Fixed-wing rate stabilisation P-gain for YAW |
+| fw_reference_airspeed | 1500 | 300 | 6000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. |
+| fw_tpa_time_constant | 0 | 0 | 5000 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups |
+| fw_turn_assist_pitch_gain | 1 | 0 | 2 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
+| fw_turn_assist_yaw_gain | 1 | 0 | 2 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
+| fw_yaw_iterm_freeze_bank_angle | 0 | 0 | 90 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. |
+| gps_auto_baud | ON |  |  | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports |
+| gps_auto_config | ON |  |  | Enable automatic configuration of UBlox GPS receivers. |
+| gps_dyn_model | AIR_1G |  |  | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. |
+| gps_min_sats | 6 | 5 | 10 | Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. |
+| gps_provider | UBLOX |  |  | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
+| gps_sbas_mode | NONE |  |  | Which SBAS mode to be used |
+| gps_ublox_use_galileo | OFF |  |  | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
+| gyro_abg_alpha | 0 | 0 | 1 | Alpha factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_boost | 0.35 | 0 | 2 | Boost factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_half_life | 0.5 | 0 | 10 | Sample half-life for Gyro Alpha-Beta-Gamma filter |
+| gyro_anti_aliasing_lpf_hz | 250 |  | 255 | Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz |
+| gyro_anti_aliasing_lpf_type | PT1 |  |  | Specifies the type of the software LPF of the gyro signals. |
+| gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
+| gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF |
+| gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF |
+| gyro_hardware_lpf | 256HZ |  |  | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. |
+| gyro_main_lpf_hz | 60 | 0 | 500 | Software based gyro main lowpass filter. Value is cutoff frequency (Hz) |
+| gyro_main_lpf_type | BIQUAD |  |  | Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
+| gyro_notch_cutoff | 1 | 1 | 500 |  |
+| gyro_notch_hz | 0 |  | 500 |  |
+| gyro_to_use | 0 | 0 | 1 |  |
+| gyro_use_dyn_lpf | OFF |  |  | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. |
+| has_flaps | OFF |  |  | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
+| heading_hold_rate_limit | 90 | HEADING_HOLD_RATE_LIMIT_MIN | HEADING_HOLD_RATE_LIMIT_MAX | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
+| hott_alarm_sound_interval | 5 | 0 | 120 | Battery alarm delay in seconds for Hott telemetry |
+| i2c_speed | 400KHZ |  |  | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) |
+| ibus_telemetry_type | 0 | 0 | 255 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. |
+| idle_power | 0 | 0 | 65535 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit |
+| imu2_align_pitch | 0 | -1800 | 3600 | Pitch alignment for Secondary IMU. 1/10 of a degree |
+| imu2_align_roll | 0 | -1800 | 3600 | Roll alignment for Secondary IMU. 1/10 of a degree |
+| imu2_align_yaw | 0 | -1800 | 3600 | Yaw alignment for Secondary IMU. 1/10 of a degree |
+| imu2_gain_acc_x | 0 | -32768 | 32767 | Secondary IMU ACC calibration data |
+| imu2_gain_acc_y | 0 | -32768 | 32767 | Secondary IMU ACC calibration data |
+| imu2_gain_acc_z | 0 | -32768 | 32767 | Secondary IMU ACC calibration data |
+| imu2_gain_mag_x | 0 | -32768 | 32767 | Secondary IMU MAG calibration data |
+| imu2_gain_mag_y | 0 | -32768 | 32767 | Secondary IMU MAG calibration data |
+| imu2_gain_mag_z | 0 | -32768 | 32767 | Secondary IMU MAG calibration data |
+| imu2_hardware | NONE |  |  | Selection of a Secondary IMU hardware type. NONE disables this functionality |
+| imu2_radius_acc | 0 | -32768 | 32767 | Secondary IMU MAG calibration data |
+| imu2_radius_mag | 0 | -32768 | 32767 | Secondary IMU MAG calibration data |
+| imu2_use_for_osd_ahi | OFF |  |  | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon |
+| imu2_use_for_osd_heading | OFF |  |  | If set to ON, Secondary IMU data will be used for Analog OSD heading |
+| imu2_use_for_stabilized | OFF |  |  | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) |
+| imu_acc_ignore_rate | 0 | 0 | 20 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes |
+| imu_acc_ignore_slope | 0 | 0 | 5 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) |
+| imu_dcm_ki | 50 |  | 65535 | Inertial Measurement Unit KI Gain for accelerometer measurements |
+| imu_dcm_ki_mag | 0 |  | 65535 | Inertial Measurement Unit KI Gain for compass measurements |
+| imu_dcm_kp | 2500 |  | 65535 | Inertial Measurement Unit KP Gain for accelerometer measurements |
+| imu_dcm_kp_mag | 10000 |  | 65535 | Inertial Measurement Unit KP Gain for compass measurements |
+| inav_allow_dead_reckoning | OFF |  |  | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation |
+| inav_auto_mag_decl | ON |  |  | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. |
+| inav_baro_epv | 100 | 0 | 9999 | Uncertainty value for barometric sensor [cm] |
+| inav_gravity_cal_tolerance | 5 | 0 | 255 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. |
+| inav_max_eph_epv | 1000 | 0 | 9999 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] |
+| inav_max_surface_altitude | 200 | 0 | 1000 | Max allowed altitude for surface following mode. [cm] |
+| 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 | FIRST_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_use_gps_no_baro | OFF |  |  |  |
+| 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_w_acc_bias | 0.01 | 0 | 1 | Weight for accelerometer drift estimation |
+| inav_w_xy_flow_p | 1.0 | 0 | 100 |  |
+| inav_w_xy_flow_v | 2.0 | 0 | 100 |  |
+| inav_w_xy_gps_p | 1.0 | 0 | 10 | Weight of GPS coordinates in estimated UAV position and speed. |
+| inav_w_xy_gps_v | 2.0 | 0 | 10 | Weight of GPS velocity data in estimated UAV speed |
+| inav_w_xy_res_v | 0.5 | 0 | 10 | Decay coefficient for estimated velocity when GPS reference for position is lost |
+| inav_w_xyz_acc_p | 1.0 | 0 | 1 |  |
+| inav_w_z_baro_p | 0.35 | 0 | 10 | Weight of barometer measurements in estimated altitude and climb rate |
+| inav_w_z_gps_p | 0.2 | 0 | 10 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes |
+| inav_w_z_gps_v | 0.1 | 0 | 10 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero |
+| inav_w_z_res_v | 0.5 | 0 | 10 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost |
+| inav_w_z_surface_p | 3.5 | 0 | 100 |  |
+| inav_w_z_surface_v | 6.1 | 0 | 100 |  |
+| iterm_windup | 50 | 0 | 90 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) |
+| ledstrip_visual_beeper | OFF |  |  |  |
+| limit_attn_filter_cutoff | 1.2 |  | 100 | Throttle attenuation PI control output filter cutoff frequency |
+| limit_burst_current | 0 |  | 4000 | Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable |
+| limit_burst_current_falldown_time | 0 |  | 3000 | Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current` |
+| limit_burst_current_time | 0 |  | 3000 | Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced |
+| limit_burst_power | 0 |  | 40000 | Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable |
+| limit_burst_power_falldown_time | 0 |  | 3000 | Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power` |
+| limit_burst_power_time | 0 |  | 3000 | Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced |
+| limit_cont_current | 0 |  | 4000 | Continous current limit (dA), set to 0 to disable |
+| limit_cont_power | 0 |  | 40000 | Continous power limit (dW), set to 0 to disable |
+| limit_pi_i | 100 |  | 10000 | Throttle attenuation PI control I term |
+| limit_pi_p | 100 |  | 10000 | Throttle attenuation PI control P term |
+| log_level | ERROR |  |  | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. |
+| log_topics | 0 | 0 | 4294967295 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. |
+| looptime | 1000 |  | 9000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. |
+| ltm_update_rate | NORMAL |  |  | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. |
+| mag_calibration_time | 30 | 20 | 120 | Adjust how long time the Calibration of mag will last. |
+| mag_declination | 0 | -18000 | 18000 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. |
+| mag_hardware | AUTO |  |  | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info |
+| mag_to_use | 0 | 0 | 1 | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target |
+| maggain_x | 1024 | -32768 | 32767 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed |
+| maggain_y | 1024 | -32768 | 32767 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed |
+| maggain_z | 1024 | -32768 | 32767 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed |
+| magzero_x | 0 | -32768 | 32767 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. |
+| magzero_y | 0 | -32768 | 32767 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. |
+| magzero_z | 0 | -32768 | 32767 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. |
+| manual_pitch_rate | 100 | 0 | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% |
+| manual_rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] |
+| manual_rc_yaw_expo | 20 | 0 | 100 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] |
+| manual_roll_rate | 100 | 0 | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% |
+| manual_yaw_rate | 100 | 0 | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% |
+| mavlink_ext_status_rate | 2 | 0 | 255 |  |
+| mavlink_extra1_rate | 10 | 0 | 255 |  |
+| mavlink_extra2_rate | 2 | 0 | 255 |  |
+| mavlink_extra3_rate | 1 | 0 | 255 |  |
+| mavlink_pos_rate | 2 | 0 | 255 |  |
+| mavlink_rc_chan_rate | 5 | 0 | 255 |  |
+| mavlink_version | 2 | 1 | 2 | Version of MAVLink to use |
+| max_angle_inclination_pit | 300 | 100 | 900 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° |
+| max_angle_inclination_rll | 300 | 100 | 900 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° |
+| max_check | 1900 | PWM_RANGE_MIN | PWM_RANGE_MAX | 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_throttle | 1850 | PWM_RANGE_MIN | PWM_RANGE_MAX | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. |
+| mc_cd_lpf_hz | 30 | 0 | 200 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky |
+| mc_cd_pitch | 60 | 0 | 200 | Multicopter Control Derivative gain for PITCH |
+| mc_cd_roll | 60 | 0 | 200 | Multicopter Control Derivative gain for ROLL |
+| mc_cd_yaw | 60 | 0 | 200 | Multicopter Control Derivative gain for YAW |
+| mc_d_level | 75 | 0 | 200 | Multicopter attitude stabilisation HORIZON transition point |
+| mc_d_pitch | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for PITCH |
+| mc_d_roll | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for ROLL |
+| mc_d_yaw | 0 | 0 | 200 | Multicopter rate stabilisation D-gain for YAW |
+| mc_i_level | 15 | 0 | 200 | Multicopter attitude stabilisation low-pass filter cutoff |
+| mc_i_pitch | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for PITCH |
+| mc_i_roll | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for ROLL |
+| mc_i_yaw | 45 | 0 | 200 | Multicopter rate stabilisation I-gain for YAW |
+| mc_iterm_relax | RP |  |  |  |
+| mc_iterm_relax_cutoff | 15 | 1 | 100 |  |
+| mc_p_level | 20 | 0 | 200 | Multicopter attitude stabilisation P-gain |
+| mc_p_pitch | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for PITCH |
+| mc_p_roll | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for ROLL |
+| mc_p_yaw | 85 | 0 | 200 | Multicopter rate stabilisation P-gain for YAW |
+| min_check | 1100 | PWM_RANGE_MIN | PWM_RANGE_MAX | 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. |
+| min_command | 1000 | 0 | PWM_RANGE_MAX | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. |
+| mode_range_logic_operator | OR |  |  | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. |
+| model_preview_type | -1 | -1 | 32767 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. |
+| moron_threshold | 32 |  | 128 | 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. |
+| motor_accel_time | 0 | 0 | 1000 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] |
+| motor_decel_time | 0 | 0 | 1000 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] |
+| motor_direction_inverted | OFF |  |  | Use if you need to inverse yaw motor direction. |
+| motor_poles | 14 | 4 | 255 | The number of motor poles. Required to compute motor RPM |
+| motor_pwm_protocol | ONESHOT125 |  |  | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED |
+| motor_pwm_rate | 400 | 50 | 32000 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. |
+| msp_override_channels | 0 | 0 | 65535 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. |
+| name | _empty_ |  |  | Craft name |
+| nav_auto_climb_rate | 500 | 10 | 2000 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] |
+| nav_auto_speed | 300 | 10 | 2000 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] |
+| nav_disarm_on_landing | OFF |  |  | If set to ON, iNav disarms the FC after landing |
+| nav_emerg_landing_speed | 500 | 100 | 2000 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] |
+| 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. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used |
+| 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_bank_angle | 35 | 5 | 80 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll |
+| nav_fw_climb_angle | 20 | 5 | 80 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
+| nav_fw_control_smoothness | 0 | 0 | 9 | How smoothly the autopilot controls the airplane to correct the navigation error |
+| nav_fw_cruise_speed | 0 | 0 | 65535 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s |
+| nav_fw_cruise_thr | 1400 | 1000 | 2000 | 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_yaw_rate | 20 | 0 | 60 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] |
+| nav_fw_dive_angle | 15 | 5 | 80 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
+| nav_fw_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Fixedwing) |
+| nav_fw_land_dive_angle | 2 | -20 | 20 | 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_launch_accel | 1863 | 1000 | 20000 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
+| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
+| nav_fw_launch_detect_time | 40 | 10 | 1000 | Time for which thresholds have to breached to consider launch happened [ms] |
+| nav_fw_launch_end_time | 3000 | 0 | 5000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] |
+| nav_fw_launch_idle_thr | 1000 | 1000 | 2000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) |
+| nav_fw_launch_max_altitude | 0 | 0 | 60000 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. |
+| nav_fw_launch_max_angle | 45 | 5 | 180 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] |
+| nav_fw_launch_min_time | 0 | 0 | 60000 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. |
+| nav_fw_launch_motor_delay | 500 | 0 | 5000 | Delay between detected launch and launch sequence start and throttling up (ms) |
+| nav_fw_launch_spinup_time | 100 | 0 | 1000 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller |
+| nav_fw_launch_thr | 1700 | 1000 | 2000 | Launch throttle - throttle to be set during launch sequence (pwm units) |
+| nav_fw_launch_timeout | 5000 |  | 60000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) |
+| nav_fw_launch_velocity | 300 | 100 | 10000 | Forward velocity threshold for swing-launch detection [cm/s] |
+| nav_fw_loiter_radius | 7500 | 0 | 30000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] |
+| nav_fw_max_thr | 1700 | 1000 | 2000 | Maximum throttle for flying wing in GPS assisted modes |
+| nav_fw_min_thr | 1200 | 1000 | 2000 | Minimum throttle for flying wing in GPS assisted modes |
+| nav_fw_pitch2thr | 10 | 0 | 100 | 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_pitch2thr_smoothing | 6 | 0 | 9 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. |
+| nav_fw_pitch2thr_threshold | 50 | 0 | 900 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
+| nav_fw_pos_hdg_d | 0 | 0 | 255 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
+| nav_fw_pos_hdg_i | 2 | 0 | 255 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
+| nav_fw_pos_hdg_p | 30 | 0 | 255 | P gain of heading PID controller. (Fixedwing, rovers, boats) |
+| nav_fw_pos_hdg_pidsum_limit | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) |
+| nav_fw_pos_xy_d | 8 | 0 | 255 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
+| nav_fw_pos_xy_i | 5 | 0 | 255 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
+| nav_fw_pos_xy_p | 75 | 0 | 255 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH |
+| nav_fw_pos_z_d | 10 | 0 | 255 | D gain of altitude PID controller (Fixedwing) |
+| nav_fw_pos_z_i | 5 | 0 | 255 | I gain of altitude PID controller (Fixedwing) |
+| nav_fw_pos_z_p | 40 | 0 | 255 | P gain of altitude PID controller (Fixedwing) |
+| nav_fw_yaw_deadband | 0 | 0 | 90 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course |
+| nav_land_maxalt_vspd | 200 | 100 | 2000 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] |
+| nav_land_minalt_vspd | 50 | 50 | 500 | Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] |
+| nav_land_slowdown_maxalt | 2000 | 500 | 4000 | Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm] |
+| nav_land_slowdown_minalt | 500 | 50 | 1000 | Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] |
+| nav_manual_climb_rate | 200 | 10 | 2000 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] |
+| nav_manual_speed | 500 | 10 | 2000 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
+| nav_max_altitude | 0 | 0 | 65000 | Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled |
+| nav_max_terrain_follow_alt | 100 |  | 1000 | Max allowed above the ground altitude for terrain following mode |
+| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) |
+| nav_mc_bank_angle | 30 | 15 | 45 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
+| nav_mc_braking_bank_angle | 40 | 15 | 60 | max angle that MR is allowed to bank in BOOST mode |
+| nav_mc_braking_boost_disengage_speed | 100 | 0 | 1000 | BOOST will be disabled when speed goes below this value |
+| nav_mc_braking_boost_factor | 100 | 0 | 200 | acceleration factor for BOOST phase |
+| nav_mc_braking_boost_speed_threshold | 150 | 100 | 1000 | BOOST can be enabled when speed is above this value |
+| nav_mc_braking_boost_timeout | 750 | 0 | 5000 | how long in ms BOOST phase can happen |
+| nav_mc_braking_disengage_speed | 75 | 0 | 1000 | braking is disabled when speed goes below this value |
+| nav_mc_braking_speed_threshold | 100 | 0 | 1000 | min speed in cm/s above which braking can happen |
+| nav_mc_braking_timeout | 2000 | 100 | 5000 | timeout in ms for braking |
+| nav_mc_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Multirotor) |
+| nav_mc_hover_thr | 1500 | 1000 | 2000 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. |
+| nav_mc_pos_deceleration_time | 120 | 0 | 255 | Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting |
+| nav_mc_pos_expo | 10 | 0 | 255 | Expo for PosHold control |
+| nav_mc_pos_xy_p | 65 | 0 | 255 | Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity |
+| nav_mc_pos_z_p | 50 | 0 | 255 | P gain of altitude PID controller (Multirotor) |
+| nav_mc_vel_xy_d | 100 | 0 | 255 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. |
+| nav_mc_vel_xy_dterm_attenuation | 90 | 0 | 100 | Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. |
+| nav_mc_vel_xy_dterm_attenuation_end | 60 | 0 | 100 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum |
+| nav_mc_vel_xy_dterm_attenuation_start | 10 | 0 | 100 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins |
+| nav_mc_vel_xy_dterm_lpf_hz | 2 | 0 | 100 |  |
+| nav_mc_vel_xy_ff | 40 | 0 | 255 |  |
+| nav_mc_vel_xy_i | 15 | 0 | 255 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot |
+| nav_mc_vel_xy_p | 40 | 0 | 255 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations |
+| nav_mc_vel_z_d | 10 | 0 | 255 | D gain of velocity PID controller |
+| nav_mc_vel_z_i | 50 | 0 | 255 | I gain of velocity PID controller |
+| nav_mc_vel_z_p | 100 | 0 | 255 | P gain of velocity PID controller |
+| nav_mc_wp_slowdown | ON |  |  | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. |
+| nav_min_rth_distance | 500 | 0 | 5000 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
+| nav_overrides_motor_stop | ALL_NAV |  |  | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
+| nav_position_timeout | 5 | 0 | 10 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
+| nav_rth_abort_threshold | 50000 |  | 65000 | 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_rth_allow_landing | ALWAYS |  |  | If set to ON drone will land as a last phase of RTH. |
+| nav_rth_alt_control_override | OFF |  |  | If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing) |
+| 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 |  | 65000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) |
+| nav_rth_climb_first | ON |  |  | If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) |
+| nav_rth_climb_ignore_emerg | OFF |  |  | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. |
+| nav_rth_home_altitude | 0 |  | 65000 | 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_tail_first | OFF |  |  | If set to ON drone will return tail-first. Obviously meaningless for airplanes. |
+| nav_use_fw_yaw_control | OFF |  |  | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats |
+| 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_user_control_mode | ATTI |  |  | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. |
+| nav_wp_load_on_boot | OFF |  |  | If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup. |
+| nav_wp_radius | 100 | 10 | 10000 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius |
+| nav_wp_safe_distance | 10000 |  | 65000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. |
+| opflow_hardware | NONE |  |  | Selection of OPFLOW hardware. |
+| opflow_scale | 10.5 | 0 | 10000 |  |
+| osd_ahi_bordered | OFF |  |  | Shows a border/corners around the AHI region (pixel OSD only) |
+| osd_ahi_height | 162 |  | 255 | AHI height in pixels (pixel OSD only) |
+| osd_ahi_max_pitch | 20 | 10 | 90 | Max pitch, in degrees, for OSD artificial horizon |
+| osd_ahi_reverse_roll | OFF |  |  |  |
+| osd_ahi_style | DEFAULT |  |  | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. |
+| osd_ahi_vertical_offset | -18 | -128 | 127 | AHI vertical offset from center (pixel OSD only) |
+| osd_ahi_width | 132 |  | 255 | AHI width in pixels (pixel OSD only) |
+| osd_alt_alarm | 100 | 0 | 10000 | Value above which to make the OSD relative altitude indicator blink (meters) |
+| osd_baro_temp_alarm_max | 600 | -550 | 1250 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) |
+| osd_baro_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) |
+| osd_camera_fov_h | 135 | 60 | 150 | Horizontal field of view for the camera in degres |
+| osd_camera_fov_v | 85 | 30 | 120 | Vertical field of view for the camera in degres |
+| osd_camera_uptilt | 0 | -40 | 80 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal |
+| osd_coordinate_digits | 9 | 8 | 11 |  |
+| osd_crosshairs_style | DEFAULT |  |  | To set the visual type for the crosshair |
+| osd_crsf_lq_format | TYPE1 |  |  | To select LQ format |
+| osd_current_alarm | 0 | 0 | 255 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. |
+| osd_dist_alarm | 1000 | 0 | 50000 | Value above which to make the OSD distance from home indicator blink (meters) |
+| osd_esc_temp_alarm_max | 900 | -550 | 1500 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+| osd_esc_temp_alarm_min | -200 | -550 | 1500 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+| osd_estimations_wind_compensation | ON |  |  | Use wind estimation for remaining flight time/distance estimation |
+| osd_failsafe_switch_layout | OFF |  |  | If enabled the OSD automatically switches to the first layout during failsafe |
+| osd_force_grid | OFF |  |  | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) |
+| osd_gforce_alarm | 5 | 0 | 20 | Value above which the OSD g force indicator will blink (g) |
+| osd_gforce_axis_alarm_max | 5 | -20 | 20 | Value above which the OSD axis g force indicators will blink (g) |
+| osd_gforce_axis_alarm_min | -5 | -20 | 20 | Value under which the OSD axis g force indicators will blink (g) |
+| osd_home_position_arm_screen | ON |  |  | Should home position coordinates be displayed on the arming screen. |
+| osd_horizon_offset | 0 | -2 | 2 | To vertically adjust the whole OSD and AHI and scrolling bars |
+| osd_hud_homepoint | OFF |  |  | To 3D-display the home point location in the hud |
+| osd_hud_homing | OFF |  |  | To display little arrows around the crossair showing where the home point is in the hud |
+| osd_hud_margin_h | 3 | 0 | 4 | Left and right margins for the hud area |
+| osd_hud_margin_v | 3 | 1 | 3 | Top and bottom margins for the hud area |
+| osd_hud_radar_disp | 0 | 0 | 4 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc |
+| osd_hud_radar_nearest | 0 | 0 | 4000 | To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. |
+| osd_hud_radar_range_max | 4000 | 100 | 9990 | In meters, radar aircrafts further away than this will not be displayed in the hud |
+| osd_hud_radar_range_min | 3 | 1 | 30 | In meters, radar aircrafts closer than this will not be displayed in the hud |
+| osd_hud_wp_disp | 0 | 0 | 3 | How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) |
+| osd_imu_temp_alarm_max | 600 | -550 | 1250 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+| osd_imu_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+| osd_left_sidebar_scroll | NONE |  |  |  |
+| osd_left_sidebar_scroll_step | 0 |  | 255 | How many units each sidebar step represents. 0 means the default value for the scroll type. |
+| osd_link_quality_alarm | 70 | 0 | 100 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% |
+| osd_main_voltage_decimals | 1 | 1 | 2 | Number of decimals for the battery voltages displayed in the OSD [1-2]. |
+| osd_neg_alt_alarm | 5 | 0 | 10000 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
+| osd_pan_servo_index | 0 | 0 | 10 | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. |
+| osd_pan_servo_pwm2centideg | 0 | -36 | 36 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. |
+| osd_plus_code_digits | 11 | 10 | 13 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. |
+| osd_plus_code_short | 0 |  |  | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. |
+| osd_right_sidebar_scroll | NONE |  |  |  |
+| osd_right_sidebar_scroll_step | 0 |  | 255 | Same as left_sidebar_scroll_step, but for the right sidebar |
+| osd_row_shiftdown | 0 | 0 | 1 | Number of rows to shift the OSD display (increase if top rows are cut off) |
+| osd_rssi_alarm | 20 | 0 | 100 | Value below which to make the OSD RSSI indicator blink |
+| osd_sidebar_height | 3 | 0 | 5 | Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) |
+| osd_sidebar_horizontal_offset | 0 | -128 | 127 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. |
+| osd_sidebar_scroll_arrows | OFF |  |  |  |
+| osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) |
+| osd_speed_source | GROUND |  |  | Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR |
+| osd_stats_energy_unit | MAH |  |  | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) |
+| osd_stats_min_voltage_unit | BATTERY |  |  | Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. |
+| osd_telemetry | OFF |  |  | To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` |
+| osd_temp_label_align | LEFT |  |  | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` |
+| osd_time_alarm | 10 | 0 | 600 | Value above which to make the OSD flight time indicator blink (minutes) |
+| osd_units | METRIC |  |  | IMPERIAL, METRIC, UK |
+| osd_video_system | AUTO |  |  | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` |
+| pid_type | AUTO |  |  | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` |
+| pidsum_limit | 500 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
+| pidsum_limit_yaw | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
+| pinio_box1 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 |
+| pinio_box2 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 |
+| pinio_box3 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 |
+| pinio_box4 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 |
+| pitch_rate | 20 | 6 | 180 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
+| pitot_hardware | NONE |  |  | Selection of pitot hardware. |
+| pitot_lpf_milli_hz | 350 | 0 | 10000 |  |
+| pitot_scale | 1.0 | 0 | 100 |  |
+| platform_type | MULTIROTOR |  |  | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented |
+| pos_hold_deadband | 10 | 2 | 250 | Stick deadband in [r/c points], applied after r/c deadband and expo |
+| prearm_timeout | 10000 | 0 | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. |
+| rangefinder_hardware | NONE |  |  | Selection of rangefinder hardware. |
+| rangefinder_median_filter | OFF |  |  | 3-point median filtering for rangefinder readouts |
+| rate_accel_limit_roll_pitch | 0 |  | 500000 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. |
+| rate_accel_limit_yaw | 10000 |  | 500000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. |
+| rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) |
+| rc_filter_frequency | 50 | 0 | 100 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values |
+| rc_yaw_expo | 20 | 0 | 100 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) |
+| reboot_character | 82 | 48 | 126 | Special character used to trigger reboot |
+| receiver_type | _target default_ |  |  | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` |
+| report_cell_voltage | OFF |  |  | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON |
+| roll_rate | 20 | 6 | 180 | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
+| rpm_gyro_filter_enabled | OFF |  |  | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV |
+| rpm_gyro_harmonics | 1 | 1 | 3 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine |
+| rpm_gyro_min_hz | 100 | 30 | 200 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` |
+| rpm_gyro_q | 500 | 1 | 3000 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting |
+| rssi_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled |
+| rssi_channel | 0 | 0 | MAX_SUPPORTED_RC_CHANNEL_COUNT | RX channel containing the RSSI signal |
+| rssi_max | 100 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. |
+| rssi_min | 0 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). |
+| rssi_source | AUTO |  |  | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` |
+| rth_energy_margin | 5 | 0 | 100 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation |
+| rx_max_usec | 2115 | PWM_PULSE_MIN | PWM_PULSE_MAX | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. |
+| rx_min_usec | 885 | PWM_PULSE_MIN | PWM_PULSE_MAX | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. |
+| rx_spi_id | 0 | 0 | 0 |  |
+| rx_spi_protocol | _target default_ |  |  |  |
+| rx_spi_rf_channel_count | 0 | 0 | 8 |  |
+| safehome_max_distance | 20000 | 0 | 65000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. |
+| safehome_usage_mode | RTH |  |  | Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`.  See [Safehome documentation](Safehomes.md#Safehome) for more information. |
+| sbus_sync_interval | 3000 | 500 | 10000 |  |
+| sdcard_detect_inverted | _target default_ |  |  | 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. |
+| serialrx_halfduplex | AUTO |  |  | 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). |
+| serialrx_provider | _target default_ |  |  | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |
+| servo_autotrim_rotation_limit | 15 | 1 | 60 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. |
+| servo_center_pulse | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | Servo midpoint |
+| servo_lpf_hz | 20 | 0 | 400 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
+| servo_protocol | PWM |  |  | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) |
+| servo_pwm_rate | 50 | 50 | 498 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. |
+| setpoint_kalman_enabled | OFF |  |  | Enable Kalman filter on the PID controller setpoint |
+| setpoint_kalman_q | 100 | 1 | 16000 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds |
+| setpoint_kalman_sharpness | 100 | 1 | 16000 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets |
+| setpoint_kalman_w | 4 | 1 | 40 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response |
+| sim_ground_station_number | _empty_ |  |  | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. |
+| sim_low_altitude | -32767 | -32768 | 32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. |
+| sim_pin | 0000 |  |  | PIN code for the SIM module |
+| sim_transmit_flags | `SIM_TX_FLAG_FAILSAFE` |  | 63 | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` |
+| sim_transmit_interval | 60 | SIM_MIN_TRANSMIT_INTERVAL | 65535 | Text message transmission interval in seconds for SIM module. Minimum value: 10 |
+| small_angle | 25 | 0 | 180 | If the aircraft tilt angle exceed this value the copter will refuse to arm. |
+| smartport_fuel_unit | MAH |  |  | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] |
+| smartport_master_halfduplex | ON |  |  |  |
+| smartport_master_inverted | OFF |  |  |  |
+| smith_predictor_delay | 0 | 0 | 8 | Expected delay of the gyro signal. In milliseconds |
+| smith_predictor_lpf_hz | 50 | 1 | 500 | Cutoff frequency for the Smith Predictor Low Pass Filter |
+| smith_predictor_strength | 0.5 | 0 | 1 | The strength factor of a Smith Predictor of PID measurement. In percents |
+| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | SPEKTRUM_SAT_BIND_DISABLED | SPEKTRUM_SAT_BIND_MAX | 0 = disabled. Used to bind the spektrum satellite to RX |
+| srxl2_baud_fast | ON |  |  |  |
+| srxl2_unit_id | 1 | 0 | 15 |  |
+| stats | OFF |  |  | General switch of the statistics recording feature (a.k.a. odometer) |
+| stats_total_dist | 0 |  | 2147483647 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. |
+| stats_total_energy | 0 |  | 2147483647 |  |
+| stats_total_time | 0 |  | 2147483647 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. |
+| switch_disarm_delay | 250 | 0 | 1000 | Delay before disarming when requested by switch (ms) [0-1000] |
+| telemetry_halfduplex | ON |  |  | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details |
+| telemetry_inverted | OFF |  |  | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. |
+| telemetry_switch | OFF |  |  | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. |
+| thr_comp_weight | 1 | 0 | 2 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) |
+| thr_expo | 0 | 0 | 100 | Throttle exposition value |
+| thr_mid | 50 | 0 | 100 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. |
+| throttle_idle | 15 | 0 | 30 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. |
+| throttle_scale | 1.0 | 0 | 1 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% |
+| throttle_tilt_comp_str | 0 | 0 | 100 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. |
+| tpa_breakpoint | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | See tpa_rate. |
+| tpa_rate | 0 | 0 | 100 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. |
+| tri_unarmed_servo | ON |  |  | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
+| turtle_mode_power_factor | 55 | 0 | 100 | Turtle mode power factor |
+| 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`. |
+| tz_offset | 0 | -1440 | 1440 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs |
+| vbat_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled |
+| vbat_cell_detect_voltage | 425 | 100 | 500 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. |
+| vbat_max_cell_voltage | 420 | 100 | 500 | Maximum voltage per cell in 0.01V units, default is 4.20V |
+| vbat_meter_type | ADC |  |  | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running |
+| vbat_min_cell_voltage | 330 | 100 | 500 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) |
+| vbat_scale | _target default_ | VBAT_SCALE_MIN | VBAT_SCALE_MAX | 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_warning_cell_voltage | 350 | 100 | 500 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) |
+| vtx_band | 4 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | 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 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. |
+| 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_max_power_override | 0 | 0 | 10000 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities |
+| vtx_pit_mode_chan | 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL |  |
+| vtx_power | 1 | VTX_SETTINGS_MIN_POWER | VTX_SETTINGS_MAX_POWER | VTX RF power level to use. The exact number of mw depends on the VTX hardware. |
+| vtx_smartaudio_early_akk_workaround | ON |  |  | Enable workaround for early AKK SAudio-enabled VTX bug. |
+| yaw_deadband | 5 | 0 | 100 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. |
+| yaw_lpf_hz | 0 | 0 | 200 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
+| yaw_rate | 20 | 2 | 180 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
+>>>>>>> master
 
 > Note: this table is autogenerated. Do not edit it manually.

docs/development/Building in Windows 10 with MSYS2.md

@@ -0,0 +1,80 @@
+# General Info
+
+This is a guide on how to use Windows MSYS2 distribution and building platform to build iNav firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com
+
+Some of those benefits are described here:
+
+https://xpack.github.io/arm-none-eabi-gcc/
+
+## Setting up build environment
+
+Download MSYS2 for your architecture (most likely 64-bit)
+
+https://www.msys2.org/wiki/MSYS2-installation/
+
+Click on 64-bit, scroll all the way down for the latest release
+
+pacman is the package manager which makes it a lot easier to install and maintain all the dependencies
+
+## Installing dependencies
+
+Once MSYS2 is installed, you can open up a new terminal window by running:
+
+"C:\msys64\mingw64.exe"
+
+You can also make a shortcut of this somewhere on your taskbar, desktop, etc, or even setup a shortcut key to open it up every time you need to get a terminal window. If you right click on the window you can customize the font and layout to make it more comfortable to work with. This is very similar to cygwin or any other terminal program you might have used before
+
+This is the best part:
+```
+pacman -S git ruby make cmake gcc mingw-w64-x86_64-libwinpthread-git unzip wget
+```
+
+Now, each release needs a different version of arm toolchain. To setup the xPack ARM toolchain, use the following process:
+
+First, setup your work path, get the release you want to build or master if you want the latest/greatest
+```
+mkdir /c/Workspace
+cd /c/Workspace
+# you can also check out your own fork here which makes contributing easier
+git clone https://github.com/iNavFlight/inav
+cd inav
+# switch to release you want or skip next 2 lines if you want latest
+git fetch origin
+git checkout -b release_2.6.1 origin/release_2.6.1
+```
+Now create the build and xpack directories and get the toolkit version you need for your inav version
+```
+mkdir build
+cd build
+mkdir /c/Workspace/xpack
+cd /c/Workspace/xpack
+cat /c/Workspace/inav/cmake/arm-none-eabi-checks.cmake | grep "set(arm_none_eabi_gcc_version" | cut -d\" -f2
+```
+This will give you the version you need for any given release or master branch. You can get to all the releases here and find the version you need
+
+https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/
+```
+# for version 2.6.1, version needed is 9.2.1
+wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v9.2.1-1.1/xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
+unzip xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
+```
+This is important, put the toolkit first before your path so that it is  picked up ahead of any other versions that may be present on your system
+```
+export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-9.2.1-1.1/bin:$PATH
+cd /c/Workspace/inav/build
+```
+You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake
+```
+# while inside the build directory
+cmake ..
+```
+Once that's done you can compile the firmware for your controller
+```
+make DALRCF405
+```
+To get a list of available targets in iNav, see the target src folder
+https://github.com/tednv/inav/tree/master/src/main/target
+
+The generated hex file will be in /c/Workspace/inav/build folder
+
+At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building iNav 2.1 and 2.2, and now I'm getting back into it so figured I would share my method

src/main/CMakeLists.txt

@@ -105,6 +105,8 @@ main_sources(COMMON_SRC
     drivers/accgyro/accgyro_mpu9250.h
     drivers/accgyro/accgyro_bno055.c
     drivers/accgyro/accgyro_bno055.h
+    drivers/accgyro/accgyro_bno055_serial.c
+    drivers/accgyro/accgyro_bno055_serial.h
 
     drivers/adc.c
     drivers/adc.h
@@ -318,6 +320,8 @@ main_sources(COMMON_SRC
     flight/imu.h
     flight/kalman.c
     flight/kalman.h
+    flight/smith_predictor.c
+    flight/smith_predictor.h
     flight/rate_dynamics.c
     flight/rate_dynamics.h
     flight/mixer.c
@@ -325,6 +329,8 @@ main_sources(COMMON_SRC
     flight/pid.c
     flight/pid.h
     flight/pid_autotune.c
+    flight/power_limits.c
+    flight/power_limits.h
     flight/rth_estimator.c
     flight/rth_estimator.h
     flight/servos.c

src/main/blackbox/blackbox.c

@@ -1723,9 +1723,8 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("deadband", "%d",                        rcControlsConfig()->deadband);
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d",                    rcControlsConfig()->yaw_deadband);
         BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d",                        gyroConfig()->gyro_lpf);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d",                     gyroConfig()->gyro_soft_lpf_hz);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d",                   gyroConfig()->gyro_soft_lpf_type);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d",                    gyroConfig()->gyro_stage2_lowpass_hz);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d",                     gyroConfig()->gyro_main_lpf_hz);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d",                   gyroConfig()->gyro_main_lpf_type);
 #ifdef USE_DYNAMIC_FILTERS
         BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d",           gyroConfig()->dynamicGyroNotchRange);
         BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d",               gyroConfig()->dynamicGyroNotchQ);
@@ -1755,7 +1754,6 @@ static bool blackboxWriteSysinfo(void)
 #endif
         BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d",                    accelerometerConfig()->acc_notch_hz);
         BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d",                accelerometerConfig()->acc_notch_cutoff);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_stage2_lowpass_hz", "%d",          gyroConfig()->gyro_stage2_lowpass_hz);
         BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d",                     pidProfile()->pidSumLimit);
         BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d",                  pidProfile()->pidSumLimitYaw);
         BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d",        pidProfile()->axisAccelerationLimitYaw);

src/main/blackbox/blackbox_io.c

@@ -42,6 +42,7 @@
 #include "msp/msp_serial.h"
 
 #include "sensors/gyro.h"
+#include "fc/config.h"
 
 #define BLACKBOX_SERIAL_PORT_MODE MODE_TX
 
@@ -239,7 +240,7 @@ bool blackboxDeviceOpen(void)
              *                              = floor((looptime_ns * 3) / 500.0)
              *                              = (looptime_ns * 3) / 500
              */
-            blackboxMaxHeaderBytesPerIteration = constrain((gyro.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
+            blackboxMaxHeaderBytesPerIteration = constrain((getLooptime() * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
 
             return blackboxPort != NULL;
         }

src/main/build/debug.h

@@ -80,9 +80,14 @@ typedef enum {
     DEBUG_SPM_VARIO,    // Smartport master variometer
     DEBUG_PCF8574,
     DEBUG_DYNAMIC_GYRO_LPF,
+    DEBUG_AUTOLEVEL,
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,
+    DEBUG_GYRO_ALPHA_BETA_GAMMA,
+    DEBUG_SMITH_PREDICTOR,
+    DEBUG_AUTOTRIM,
+    DEBUG_AUTOTUNE,
     DEBUG_RATE_DYNAMICS,
     DEBUG_COUNT
 } debugType_e;

src/main/cms/cms_menu_imu.c

@@ -402,8 +402,7 @@ static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
 {
     OSD_LABEL_DATA_ENTRY("-- FILTERING  --", profileIndexString),
     OSD_SETTING_ENTRY("HARDWARE LPF", SETTING_GYRO_HARDWARE_LPF),
-    OSD_SETTING_ENTRY("GYRO LPF", SETTING_GYRO_LPF_HZ),
-    OSD_SETTING_ENTRY("GYRO LPF2", SETTING_GYRO_STAGE2_LOWPASS_HZ),
+    OSD_SETTING_ENTRY("GYRO MAIN", SETTING_GYRO_MAIN_LPF_HZ),
     OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ),
     OSD_SETTING_ENTRY("DTERM LPF2", SETTING_DTERM_LPF2_HZ),
 #ifdef USE_DYNAMIC_FILTERS

src/main/cms/cms_menu_navigation.c

@@ -75,12 +75,14 @@ static const CMS_Menu cmsx_menuNavSettings = {
     OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST),
     OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST),
     OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING),
-    OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED),
+    OSD_SETTING_ENTRY("LAND MINALT VSPD", SETTING_NAV_LAND_MINALT_VSPD),
+    OSD_SETTING_ENTRY("LAND MAXALT VSPD", SETTING_NAV_LAND_MAXALT_VSPD),
     OSD_SETTING_ENTRY("LAND SPEED MIN AT", SETTING_NAV_LAND_SLOWDOWN_MINALT),
     OSD_SETTING_ENTRY("LAND SPEED SLOW AT", SETTING_NAV_LAND_SLOWDOWN_MAXALT),
     OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE),
     OSD_SETTING_ENTRY("RTH ABORT THRES", SETTING_NAV_RTH_ABORT_THRESHOLD),
     OSD_SETTING_ENTRY("EMERG LANDING SPEED", SETTING_NAV_EMERG_LANDING_SPEED),
+    OSD_SETTING_ENTRY("SAFEHOME USAGE MODE", SETTING_SAFEHOME_USAGE_MODE),
 
     OSD_BACK_AND_END_ENTRY,
  };

src/main/cms/cms_menu_osd.c

@@ -275,9 +275,11 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("WIND VERT", OSD_WIND_SPEED_VERTICAL),
 
     OSD_ELEMENT_ENTRY("G-FORCE", OSD_GFORCE),
-    OSD_ELEMENT_ENTRY("G-FORCE X", OSD_GFORCE),
-    OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE),
-    OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE),
+    OSD_ELEMENT_ENTRY("G-FORCE X", OSD_GFORCE_X),
+    OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE_Y),
+    OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE_Z),
+
+    OSD_ELEMENT_ENTRY("VTX POWER", OSD_VTX_POWER),
 
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
     OSD_ELEMENT_ENTRY("RC SOURCE", OSD_RC_SOURCE),
@@ -288,8 +290,6 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE),
 #endif
 
-    OSD_ELEMENT_ENTRY("VTX POWER", OSD_VTX_POWER),
-
 #ifdef USE_TEMPERATURE_SENSOR
     OSD_ELEMENT_ENTRY("SENSOR 0 TEMP", OSD_TEMP_SENSOR_0_TEMPERATURE),
     OSD_ELEMENT_ENTRY("SENSOR 1 TEMP", OSD_TEMP_SENSOR_1_TEMPERATURE),
@@ -301,11 +301,29 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE),
 #endif
 
+    OSD_ELEMENT_ENTRY("GVAR 0", OSD_GVAR_0),
+    OSD_ELEMENT_ENTRY("GVAR 1", OSD_GVAR_1),
+    OSD_ELEMENT_ENTRY("GVAR 2", OSD_GVAR_2),
+    OSD_ELEMENT_ENTRY("GVAR 3", OSD_GVAR_3),
+
+    OSD_ELEMENT_ENTRY("TPA", OSD_TPA),
+    OSD_ELEMENT_ENTRY("FW CTRL SMOOTH", OSD_NAV_FW_CONTROL_SMOOTHNESS),
+    OSD_ELEMENT_ENTRY("VERSION", OSD_VERSION),
+    OSD_ELEMENT_ENTRY("RANGEFINDER", OSD_RANGEFINDER),
+
 #ifdef USE_ESC_SENSOR
     OSD_ELEMENT_ENTRY("ESC RPM", OSD_ESC_RPM),
     OSD_ELEMENT_ENTRY("ESC TEMPERATURE", OSD_ESC_TEMPERATURE),
 #endif
 
+#ifdef USE_POWER_LIMITS
+    OSD_ELEMENT_ENTRY("PLIM BURST TIME", OSD_PLIMIT_REMAINING_BURST_TIME),
+    OSD_ELEMENT_ENTRY("PLIM CURR LIMIT", OSD_PLIMIT_ACTIVE_CURRENT_LIMIT),
+#ifdef USE_ADC
+    OSD_ELEMENT_ENTRY("PLIM POWER LIMIT", OSD_PLIMIT_ACTIVE_POWER_LIMIT),
+#endif // USE_ADC
+#endif // USE_POWER_LIMITS
+
     OSD_BACK_AND_END_ENTRY,
 };
 

src/main/common/filter.c

@@ -250,3 +250,69 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     filter->y1 = y1;
     filter->y2 = y2;
 }
+
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
+    // beta, gamma, and eta gains all derived from
+    // http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
+
+    const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
+    filter->xk = 0.0f;
+    filter->vk = 0.0f;
+    filter->ak = 0.0f;
+    filter->jk = 0.0f;
+    filter->a = alpha;
+    filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
+    filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
+    filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
+	filter->dT = dT;
+	filter->dT2 = dT * dT;
+    filter->dT3 = dT * dT * dT;
+    pt1FilterInit(&filter->boostFilter, 100, dT);
+
+    const float boost = boostGain * 100;
+
+    filter->boost = (boost * boost / 10000) * 0.003;
+    filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
+}
+
+FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
+    //xk - current system state (ie: position)
+	//vk - derivative of system state (ie: velocity)
+    //ak - derivative of system velociy (ie: acceleration)
+    //jk - derivative of system acceleration (ie: jerk)
+    float rk;   // residual error
+
+    // give the filter limited history
+    filter->xk *= filter->halfLife;
+    filter->vk *= filter->halfLife;
+    filter->ak *= filter->halfLife;
+    filter->jk *= filter->halfLife;
+
+    // update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
+    filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
+    
+    // update (estimated) velocity (also estimated dterm from measurement)
+    filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
+    filter->ak += filter->dT * filter->jk;
+    
+    // what is our residual error (measured - estimated)
+    rk = input - filter->xk;
+
+    // artificially boost the error to increase the response of the filter
+    rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
+    if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
+        rk = (input - filter->xk) / filter->a;
+    }
+    filter->rk = rk; // for logging
+
+    // update our estimates given the residual error.
+    filter->xk += filter->a * rk;
+    filter->vk += filter->b / filter->dT * rk;
+    filter->ak += filter->g / (2.0f * filter->dT2) * rk;
+    filter->jk += filter->e / (6.0f * filter->dT3) * rk;
+
+	return filter->xk;
+}
+
+#endif

src/main/common/filter.h

@@ -56,6 +56,18 @@ typedef struct firFilter_s {
     uint8_t coeffsLength;
 } firFilter_t;
 
+typedef struct alphaBetaGammaFilter_s {
+    float a, b, g, e;
+    float ak; // derivative of system velociy (ie: acceleration)
+    float vk; // derivative of system state (ie: velocity)
+    float xk; // current system state (ie: position)
+    float jk; // derivative of system acceleration (ie: jerk)
+    float rk; // residual error
+    float dT, dT2, dT3;
+    float halfLife, boost;
+    pt1Filter_t boostFilter;
+} alphaBetaGammaFilter_t;
+
 typedef float (*filterApplyFnPtr)(void *filter, float input);
 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
 
@@ -86,3 +98,6 @@ float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
+
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
+float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);

src/main/common/fp_pid.c

@@ -96,7 +96,10 @@ float navPidApply3(
     pid->output_constrained = outValConstrained;
 
     /* Update I-term */
-    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
+    if (
+        !(pidFlags & PID_ZERO_INTEGRATOR) &&
+        !(pidFlags & PID_FREEZE_INTEGRATOR) 
+    ) {
         const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
 
         if (pidFlags & PID_SHRINK_INTEGRATOR) {
@@ -147,6 +150,10 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
         float Td = _kD / _kP;
         pid->param.kT = 2.0f / (Ti + Td);
     }
+    else if (_kI > 1e-6f) {
+        pid->param.kI = _kI;
+        pid->param.kT = 0.0f;
+    }
     else {
         pid->param.kI = 0.0;
         pid->param.kT = 0.0;

src/main/common/fp_pid.h

@@ -42,6 +42,7 @@ typedef enum {
     PID_ZERO_INTEGRATOR             = 1 << 1,
     PID_SHRINK_INTEGRATOR           = 1 << 2,
     PID_LIMIT_INTEGRATOR            = 1 << 3,
+    PID_FREEZE_INTEGRATOR           = 1 << 4,
 } pidControllerFlags_e;
 
 typedef struct {

src/main/common/maths.c

@@ -142,6 +142,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
     return value;
 }
 
+int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max)
+{
+    if (ABS(value) < deadband) {
+        value = 0;
+    } else if (value > 0) {
+        value = scaleRange(value - deadband, 0, max - deadband, 0, max);
+    } else if (value < 0) {
+        value = scaleRange(value + deadband, min + deadband, 0, min, 0);
+    }
+    return value;
+}
+
 int32_t constrain(int32_t amt, int32_t low, int32_t high)
 {
     if (amt < low)

src/main/common/maths.h

@@ -132,6 +132,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);
+int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max);
 
 int32_t constrain(int32_t amt, int32_t low, int32_t high);
 float constrainf(float amt, float low, float high);

src/main/common/time.h

@@ -24,10 +24,19 @@
 
 #include "config/parameter_group.h"
 
-// time difference, 32 bits always sufficient
+// time difference, signed 32 bits of microseconds overflows at ~35 minutes
+// this is worth leaving as int32_t for performance reasons, use timeDeltaLarge_t for deltas that can be big
 typedef int32_t timeDelta_t;
+#define TIMEDELTA_MAX INT32_MAX
+
+// time difference large, signed 64 bits of microseconds overflows at ~300000 years
+typedef int64_t timeDeltaLarge_t;
+#define TIMEDELTALARGE_MAX INT64_MAX
+
 // millisecond time
-typedef uint32_t timeMs_t ;
+typedef uint32_t timeMs_t;
+#define TIMEMS_MAX UINT32_MAX
+
 // microsecond time
 #ifdef USE_64BIT_TIME
 typedef uint64_t timeUs_t;
@@ -48,6 +57,7 @@ typedef uint32_t timeUs_t;
 #define MS2S(ms)    ((ms) * 1e-3f)
 #define HZ2S(hz)    US2S(HZ2US(hz))
 
+// Use this function only to get small deltas (difference overflows at ~35 minutes)
 static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
 
 typedef enum {

src/main/config/parameter_group_ids.h

@@ -117,7 +117,9 @@
 #define PG_DJI_OSD_CONFIG 1027
 #define PG_PROGRAMMING_PID 1028
 #define PG_SECONDARY_IMU 1029
-#define PG_INAV_END 1029
+#define PG_POWER_LIMITS_CONFIG 1030
+#define PG_OSD_COMMON_CONFIG 1031
+#define PG_INAV_END 1031
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
@@ -132,4 +134,3 @@
 #define PG_ID_INVALID   0
 #define PG_ID_FIRST     PG_CF_START
 #define PG_ID_LAST      PG_INAV_END
-

src/main/drivers/accgyro/accgyro.c

@@ -98,7 +98,7 @@ void gyroIntExtiInit(gyroDev_t *gyro)
     }
 #endif
 
-#if defined (STM32F7)
+#if defined (STM32F7) || defined (STM32H7)
     IOInit(gyro->busDev->irqPin, OWNER_MPU, RESOURCE_EXTI, 0);
 
     EXTIHandlerInit(&gyro->exti, gyroIntExtiHandler);

src/main/drivers/accgyro/accgyro_bno055.c

@@ -32,47 +32,6 @@
 
 #ifdef USE_IMU_BNO055
 
-#define BNO055_ADDR_PWR_MODE 0x3E
-#define BNO055_ADDR_OPR_MODE 0x3D
-#define BNO055_ADDR_CALIB_STAT 0x35
-
-#define BNO055_PWR_MODE_NORMAL  0x00
-#define BNO055_OPR_MODE_CONFIG  0x00
-#define BNO055_OPR_MODE_NDOF    0x0C
-
-#define BNO055_ADDR_EUL_YAW_LSB 0x1A
-#define BNO055_ADDR_EUL_YAW_MSB 0x1B
-#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
-#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
-#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
-#define BNO055_ADDR_EUL_PITCH_MSB 0x1F
-
-#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A 
-#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
-#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
-#define BNO055_ADDR_ACC_RADIUS_LSB 0x67
-
-#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
-#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
-#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
-#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
-#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
-#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61
-
-#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
-#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
-#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
-#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
-#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
-#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B
-
-#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
-#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
-#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
-#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
-#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
-#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55
-
 static busDevice_t *busDev;
 
 static bool deviceDetect(busDevice_t *busDev)
@@ -99,18 +58,45 @@ static void bno055SetMode(uint8_t mode)
     delay(25);
 }
 
+static void bno055SetCalibrationData(bno055CalibrationData_t data) 
+{
+    uint8_t buf[12];
+
+    //Prepare gains
+    //We do not restore gyro offsets, they are quickly calibrated at startup
+    uint8_t bufferBit = 0;
+    for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
+    {
+        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
+        {
+            buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
+            buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
+            bufferBit += 2;
+        }
+    }
+
+    busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
+
+    //Prepare radius
+    buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
+    buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
+    buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
+    buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
+
+    //Write to the device
+    busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
+}
+
 bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration)
 {
     busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0);
     if (busDev == NULL)
     {
-        DEBUG_SET(DEBUG_IMU2, 2, 1);
         return false;
     }
 
     if (!deviceDetect(busDev))
     {
-        DEBUG_SET(DEBUG_IMU2, 2, 2);
         busDeviceDeInit(busDev);
         return false;
     }
@@ -192,33 +178,4 @@ bno055CalibrationData_t bno055GetCalibrationData(void)
     return data;
 }
 
-void bno055SetCalibrationData(bno055CalibrationData_t data) 
-{
-    uint8_t buf[12];
-
-    //Prepare gains
-    //We do not restore gyro offsets, they are quickly calibrated at startup
-    uint8_t bufferBit = 0;
-    for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
-    {
-        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
-        {
-            buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
-            buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
-            bufferBit += 2;
-        }
-    }
-
-    busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
-
-    //Prepare radius
-    buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
-    buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
-    buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
-    buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
-
-    //Write to the device
-    busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
-}
-
 #endif

src/main/drivers/accgyro/accgyro_bno055.h

@@ -25,6 +25,47 @@
 
 #include "common/vector.h"
 
+#define BNO055_ADDR_PWR_MODE 0x3E
+#define BNO055_ADDR_OPR_MODE 0x3D
+#define BNO055_ADDR_CALIB_STAT 0x35
+
+#define BNO055_PWR_MODE_NORMAL  0x00
+#define BNO055_OPR_MODE_CONFIG  0x00
+#define BNO055_OPR_MODE_NDOF    0x0C
+
+#define BNO055_ADDR_EUL_YAW_LSB 0x1A
+#define BNO055_ADDR_EUL_YAW_MSB 0x1B
+#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
+#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
+#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
+#define BNO055_ADDR_EUL_PITCH_MSB 0x1F
+
+#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A 
+#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
+#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
+#define BNO055_ADDR_ACC_RADIUS_LSB 0x67
+
+#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
+#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
+#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
+#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
+#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
+#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61
+
+#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
+#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
+#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
+#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
+#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
+#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B
+
+#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
+#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
+#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
+#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
+#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
+#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55
+
 typedef struct {
     uint8_t sys;
     uint8_t gyr;
@@ -48,4 +89,3 @@ fpVector3_t bno055GetEurlerAngles(void);
 void bno055FetchEulerAngles(int16_t * buffer);
 bno055CalibStat_t bno055GetCalibStat(void);
 bno055CalibrationData_t bno055GetCalibrationData(void);
-void bno055SetCalibrationData(bno055CalibrationData_t data);

src/main/drivers/accgyro/accgyro_bno055_serial.c

@@ -0,0 +1,271 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 "platform.h"
+#ifdef USE_IMU_BNO055
+
+#define BNO055_BAUD_RATE 115200
+#define BNO055_FRAME_MAX_TIME_MS 10
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "io/serial.h"
+#include "drivers/accgyro/accgyro_bno055.h"
+#include "build/debug.h"
+#include "drivers/time.h"
+#include "flight/secondary_imu.h"
+
+static serialPort_t * bno055SerialPort = NULL;
+static uint8_t receiveBuffer[22];
+
+typedef enum {
+    BNO055_RECEIVE_IDLE,
+    BNO055_RECEIVE_HEADER,
+    BNO055_RECEIVE_LENGTH,
+    BNO055_RECEIVE_PAYLOAD,
+    BNO055_RECEIVE_ACK,
+} bno055ReceiveState_e;
+
+typedef enum {
+    BNO055_FRAME_ACK,
+    BNO055_FRAME_DATA,
+} bno055FrameType_e;
+
+typedef enum {
+    BNO055_DATA_TYPE_NONE,
+    BNO055_DATA_TYPE_EULER,
+    BNO055_DATA_TYPE_CALIBRATION_STATS,
+} bno055DataType_e;
+
+static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE;
+static uint8_t bno055FrameType;
+static uint8_t bno055FrameLength;
+static uint8_t bno055FrameIndex;
+static timeMs_t bno055FrameStartAtMs = 0;
+static uint8_t bno055DataType = BNO055_DATA_TYPE_NONE;
+
+
+static void bno055SerialWriteBuffer(const uint8_t reg, const uint8_t *data, const uint8_t count) {
+
+    bno055ProtocolState = BNO055_RECEIVE_IDLE;
+    serialWrite(bno055SerialPort, 0xAA); // Start Byte
+    serialWrite(bno055SerialPort, 0x00); // Write command
+    serialWrite(bno055SerialPort, reg);
+    serialWrite(bno055SerialPort, count);
+    for (uint8_t i = 0; i < count; i++) {
+        serialWrite(bno055SerialPort, data[i]);
+    }
+}
+
+static void bno055SerialWrite(const uint8_t reg, const uint8_t data) {
+    uint8_t buff[1];
+    buff[0] = data;
+
+    bno055SerialWriteBuffer(reg, buff, 1);
+}
+
+static void bno055SerialRead(const uint8_t reg, const uint8_t len) {
+    bno055ProtocolState = BNO055_RECEIVE_IDLE;
+    serialWrite(bno055SerialPort, 0xAA); // Start Byte
+    serialWrite(bno055SerialPort, 0x01); // Read command
+    serialWrite(bno055SerialPort, reg);
+    serialWrite(bno055SerialPort, len);
+}
+
+void bno055SerialDataReceive(uint16_t c, void *data) {
+
+    UNUSED(data);
+
+    const uint8_t incoming = (uint8_t) c;
+
+    //Failsafe for stuck frames
+    if (bno055ProtocolState != BNO055_RECEIVE_IDLE && millis() - bno055FrameStartAtMs > BNO055_FRAME_MAX_TIME_MS) {
+        bno055ProtocolState = BNO055_RECEIVE_IDLE;
+    }
+
+    if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xEE) {
+        bno055FrameStartAtMs = millis();
+        bno055ProtocolState = BNO055_RECEIVE_HEADER;
+        bno055FrameType = BNO055_FRAME_ACK;
+    } else if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xBB) {
+        bno055FrameStartAtMs = millis();
+        bno055ProtocolState = BNO055_RECEIVE_HEADER;
+        bno055FrameType = BNO055_FRAME_DATA;
+    } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_ACK) {
+        receiveBuffer[0] = incoming;
+        bno055ProtocolState = BNO055_RECEIVE_IDLE;
+    } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_DATA) {
+        bno055FrameLength = incoming;
+        bno055FrameIndex = 0;
+        bno055ProtocolState = BNO055_RECEIVE_LENGTH;
+    } else if (bno055ProtocolState == BNO055_RECEIVE_LENGTH) {
+        receiveBuffer[bno055FrameIndex] = incoming;
+        bno055FrameIndex++;
+
+        if (bno055FrameIndex == bno055FrameLength) {
+            bno055ProtocolState = BNO055_RECEIVE_IDLE;
+
+            if (bno055DataType == BNO055_DATA_TYPE_EULER) {
+                secondaryImuState.eulerAngles.raw[0] = ((int16_t)((receiveBuffer[3] << 8) | receiveBuffer[2])) / 1.6f;
+                secondaryImuState.eulerAngles.raw[1] = ((int16_t)((receiveBuffer[5] << 8) | receiveBuffer[4])) / -1.6f; //Pitch has to be reversed to match INAV notation
+                secondaryImuState.eulerAngles.raw[2] = ((int16_t)((receiveBuffer[1] << 8) | receiveBuffer[0])) / 1.6f;
+                secondaryImuProcess();
+            }  else if (bno055DataType == BNO055_DATA_TYPE_CALIBRATION_STATS) {
+                secondaryImuState.calibrationStatus.mag = receiveBuffer[0] & 0b00000011;
+                secondaryImuState.calibrationStatus.acc = (receiveBuffer[0] >> 2) & 0b00000011;
+                secondaryImuState.calibrationStatus.gyr = (receiveBuffer[0] >> 4) & 0b00000011;
+                secondaryImuState.calibrationStatus.sys = (receiveBuffer[0] >> 6) & 0b00000011;
+            }
+
+            bno055DataType = BNO055_DATA_TYPE_NONE;
+        }
+    }
+
+}
+
+static void bno055SerialSetCalibrationData(bno055CalibrationData_t data) 
+{
+    uint8_t buf[12];
+
+    //Prepare gains
+    //We do not restore gyro offsets, they are quickly calibrated at startup
+    uint8_t bufferBit = 0;
+    for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
+    {
+        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
+        {
+            buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
+            buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
+            bufferBit += 2;
+        }
+    }
+
+    bno055SerialWriteBuffer(BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
+    delay(25);
+
+    //Prepare radius
+    buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
+    buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
+    buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
+    buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
+
+    //Write to the device
+    bno055SerialWriteBuffer(BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
+    delay(25);
+}
+
+bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration) {
+    bno055SerialPort = NULL;
+
+    serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_IMU2);
+    if (!portConfig) {
+        return false;
+    }
+
+    bno055SerialPort = openSerialPort(
+        portConfig->identifier,
+        FUNCTION_IMU2,
+        bno055SerialDataReceive,
+        NULL,
+        BNO055_BAUD_RATE,
+        MODE_RXTX, 
+        SERIAL_NOT_INVERTED | SERIAL_UNIDIR | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO
+    );
+    
+    if (!bno055SerialPort) {
+        return false;
+    }
+
+    bno055SerialRead(0x00, 1); // Read ChipID
+    delay(5);
+
+    //Check ident
+    if (bno055FrameType != BNO055_FRAME_DATA || receiveBuffer[0] != 0xA0 || bno055ProtocolState != BNO055_RECEIVE_IDLE) {
+        return false; // Ident does not match, leave
+    }
+
+    bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
+    delay(25);
+
+    if (setCalibration) {
+        bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG);
+        delay(25);
+
+        bno055SerialSetCalibrationData(calibrationData);
+    }
+
+    bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF);
+    delay(25);
+
+    return true;
+}
+
+/*
+ * This function is non-blocking and response will be processed by bno055SerialDataReceive
+ */
+void bno055SerialFetchEulerAngles() {
+    bno055DataType = BNO055_DATA_TYPE_EULER;
+    bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6);
+}
+
+/*
+ * This function is non-blocking and response will be processed by bno055SerialDataReceive
+ */
+void bno055SerialGetCalibStat(void) {
+    bno055DataType = BNO055_DATA_TYPE_CALIBRATION_STATS;
+    bno055SerialRead(BNO055_ADDR_CALIB_STAT, 1);
+}
+
+/*
+ * This function is blocking and should not be used during flight conditions!
+ */
+bno055CalibrationData_t bno055SerialGetCalibrationData(void) {
+
+    bno055CalibrationData_t data;
+
+    bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG);
+    delay(25);
+
+    bno055SerialRead(BNO055_ADDR_ACC_OFFSET_X_LSB, 22);
+    delay(50);
+
+    uint8_t bufferBit = 0;
+    for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
+    {
+        for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
+        {
+            data.offset[sensorIndex][axisIndex] = (int16_t)((receiveBuffer[bufferBit + 1] << 8) | receiveBuffer[bufferBit]);
+            bufferBit += 2;
+        }
+    }
+
+    data.radius[ACC] = (int16_t)((receiveBuffer[19] << 8) | receiveBuffer[18]);
+    data.radius[MAG] = (int16_t)((receiveBuffer[21] << 8) | receiveBuffer[20]);
+
+    bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF);
+    delay(25);
+
+    return data;
+}
+
+#endif

src/main/drivers/accgyro/accgyro_bno055_serial.h

@@ -0,0 +1,33 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "common/vector.h"
+#include "drivers/accgyro/accgyro_bno055.h"
+
+bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration);
+void bno055SerialFetchEulerAngles(void);
+void bno055SerialGetCalibStat(void);
+bno055CalibrationData_t bno055SerialGetCalibrationData(void);

src/main/drivers/adc.c

@@ -55,9 +55,9 @@ static uint8_t activeChannelCount[ADCDEV_COUNT] = {0};
 
 static int adcFunctionMap[ADC_FUNCTION_COUNT];
 adc_config_t adcConfig[ADC_CHN_COUNT];  // index 0 is dummy for ADC_CHN_NONE
-volatile uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES];
+volatile ADC_VALUES_ALIGNMENT(uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]);
 
-uint8_t adcChannelByTag(ioTag_t ioTag)
+uint32_t adcChannelByTag(ioTag_t ioTag)
 {
     for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) {
         if (ioTag == adcTagMap[i].tag)

src/main/drivers/adc_impl.h

@@ -22,19 +22,27 @@
 
 #if defined(STM32F4) || defined(STM32F7)
 #define ADC_TAG_MAP_COUNT 16
+#elif defined(STM32H7)
+#define ADC_TAG_MAP_COUNT 28
 #elif defined(STM32F3)
 #define ADC_TAG_MAP_COUNT 39
 #else
 #define ADC_TAG_MAP_COUNT 10
 #endif
 
+#if defined(STM32H7)
+#define ADC_VALUES_ALIGNMENT(def) __attribute__ ((section(".DMA_RAM"))) def __attribute__ ((aligned (32)))
+#else
+#define ADC_VALUES_ALIGNMENT(def) def 
+#endif
+
 typedef enum ADCDevice {
     ADCINVALID = -1,
     ADCDEV_1   = 0,
 #if defined(STM32F3)
     ADCDEV_2,
     ADCDEV_MAX = ADCDEV_2,
-#elif defined(STM32F4) || defined(STM32F7)
+#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
     ADCDEV_2,
     ADCDEV_3,
     ADCDEV_MAX = ADCDEV_3,
@@ -46,20 +54,20 @@ typedef enum ADCDevice {
 
 typedef struct adcTagMap_s {
     ioTag_t tag;
-    uint8_t channel;
+    uint32_t channel;
 } adcTagMap_t;
 
 typedef struct adcDevice_s {
     ADC_TypeDef* ADCx;
     rccPeriphTag_t rccADC;
     rccPeriphTag_t rccDMA;
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
     DMA_Stream_TypeDef* DMAy_Streamx;
     uint32_t channel;
 #else
     DMA_Channel_TypeDef* DMAy_Channelx;
 #endif
-#if defined(STM32F7)
+#if defined(STM32F7) || defined(STM32H7)
     ADC_HandleTypeDef ADCHandle;
     DMA_HandleTypeDef DmaHandle;
 #endif
@@ -70,7 +78,7 @@ typedef struct adcDevice_s {
 typedef struct adc_config_s {
     ioTag_t tag;
     ADCDevice adcDevice;
-    uint8_t adcChannel;         // ADC1_INxx channel number
+    uint32_t adcChannel;         // ADC1_INxx channel number
     uint8_t dmaIndex;           // index into DMA buffer in case of sparse channels
     bool enabled;
     uint8_t sampleTime;
@@ -78,8 +86,8 @@ typedef struct adc_config_s {
 
 extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT];
 extern adc_config_t adcConfig[ADC_CHN_COUNT];
-extern volatile uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES];
+extern volatile ADC_VALUES_ALIGNMENT(uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]);
 
 void adcHardwareInit(drv_adc_config_t *init);
 ADCDevice adcDeviceByInstance(ADC_TypeDef *instance);
-uint8_t adcChannelByTag(ioTag_t ioTag);
+uint32_t adcChannelByTag(ioTag_t ioTag);

src/main/drivers/adc_stm32h7xx.c

@@ -0,0 +1,230 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "platform.h"
+#include "drivers/time.h"
+
+#include "drivers/io.h"
+#include "io_impl.h"
+#include "rcc.h"
+#include "dma.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro.h"
+
+#include "adc.h"
+#include "adc_impl.h"
+
+
+static adcDevice_t adcHardware[ADCDEV_COUNT] = {
+    {
+        .ADCx = ADC1,
+        .rccADC = RCC_AHB1(ADC12),
+        .rccDMA = RCC_AHB1(DMA2), 
+        .DMAy_Streamx = DMA2_Stream0,
+        .channel = DMA_REQUEST_ADC1,
+        .enabled = false, 
+        .usedChannelCount = 0
+    },
+    /* currently not used
+    { 
+        .ADCx = ADC2,
+        .rccADC = RCC_AHB1(ADC12),
+        .rccDMA = RCC_AHB1(DMA2), 
+        .DMAy_Streamx = DMA2_Stream1,
+        .channel = DMA_REQUEST_ADC2,
+        .enabled = false, 
+        .usedChannelCount = 0
+    }
+    */
+};
+
+adcDevice_t adcDevice[ADCDEV_COUNT];
+
+/* note these could be packed up for saving space */
+const adcTagMap_t adcTagMap[] = {
+    { DEFIO_TAG_E__PC0, ADC_CHANNEL_10 },
+    { DEFIO_TAG_E__PC1, ADC_CHANNEL_11 },
+    { DEFIO_TAG_E__PC2, ADC_CHANNEL_0  },
+    { DEFIO_TAG_E__PC3, ADC_CHANNEL_1  },
+    { DEFIO_TAG_E__PC4, ADC_CHANNEL_4  },
+    { DEFIO_TAG_E__PC5, ADC_CHANNEL_8  },
+    { DEFIO_TAG_E__PB0, ADC_CHANNEL_9  },
+    { DEFIO_TAG_E__PB1, ADC_CHANNEL_5  },
+    { DEFIO_TAG_E__PA0, ADC_CHANNEL_16 },
+    { DEFIO_TAG_E__PA1, ADC_CHANNEL_17 },
+    { DEFIO_TAG_E__PA2, ADC_CHANNEL_14 },
+    { DEFIO_TAG_E__PA3, ADC_CHANNEL_15 },
+    { DEFIO_TAG_E__PA4, ADC_CHANNEL_18 },
+    { DEFIO_TAG_E__PA5, ADC_CHANNEL_19 },
+    { DEFIO_TAG_E__PA6, ADC_CHANNEL_3  },
+    { DEFIO_TAG_E__PA7, ADC_CHANNEL_7  },
+};
+
+// Translate rank number x to ADC_REGULAR_RANK_x (Note that array index is 0-origin)
+static const uint32_t adcRegularRankMap[] = {
+    ADC_REGULAR_RANK_1,
+    ADC_REGULAR_RANK_2,
+    ADC_REGULAR_RANK_3,
+    ADC_REGULAR_RANK_4,
+    ADC_REGULAR_RANK_5,
+    ADC_REGULAR_RANK_6,
+    ADC_REGULAR_RANK_7,
+    ADC_REGULAR_RANK_8,
+    ADC_REGULAR_RANK_9,
+    ADC_REGULAR_RANK_10,
+    ADC_REGULAR_RANK_11,
+    ADC_REGULAR_RANK_12,
+    ADC_REGULAR_RANK_13,
+    ADC_REGULAR_RANK_14,
+    ADC_REGULAR_RANK_15,
+    ADC_REGULAR_RANK_16,
+};
+
+ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
+{
+    if (instance == ADC1)
+        return ADCDEV_1;
+
+    if (instance == ADC2)
+        return ADCDEV_2;
+
+    return ADCINVALID;
+}
+
+static void adcInstanceInit(ADCDevice adcDevice)
+{
+    adcDevice_t * adc = &adcHardware[adcDevice];
+
+    RCC_ClockCmd(adc->rccDMA, ENABLE);
+    RCC_ClockCmd(adc->rccADC, ENABLE);
+
+    adc->ADCHandle.Instance = adc->ADCx;
+
+    adc->ADCHandle.Init.ClockPrescaler        = ADC_CLOCK_SYNC_PCLK_DIV4;
+    adc->ADCHandle.Init.Resolution            = ADC_RESOLUTION_12B;
+    adc->ADCHandle.Init.ContinuousConvMode    = ENABLE;
+    adc->ADCHandle.Init.ExternalTrigConv      = ADC_EXTERNALTRIG_T1_CC1;
+    adc->ADCHandle.Init.ExternalTrigConvEdge  = ADC_EXTERNALTRIGCONVEDGE_NONE;
+    adc->ADCHandle.Init.NbrOfConversion       = adc->usedChannelCount;
+    adc->ADCHandle.Init.ScanConvMode          = adc->usedChannelCount > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
+    adc->ADCHandle.Init.DiscontinuousConvMode = DISABLE;
+    adc->ADCHandle.Init.NbrOfDiscConversion   = 0;
+    adc->ADCHandle.Init.EOCSelection          = DISABLE;
+    adc->ADCHandle.Init.LowPowerAutoWait      = DISABLE;
+
+    // Enable circular DMA.
+    adc->ADCHandle.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_CIRCULAR;
+
+    adc->ADCHandle.Init.Overrun                  = ADC_OVR_DATA_OVERWRITTEN;
+    adc->ADCHandle.Init.OversamplingMode         = DISABLE;
+
+    if (HAL_ADC_Init(&adc->ADCHandle) != HAL_OK) {
+        return;
+    }
+
+    if (HAL_ADCEx_Calibration_Start(&adc->ADCHandle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) != HAL_OK) {
+      return;
+    }
+
+    adc->DmaHandle.Init.Request = adc->channel;
+    adc->DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    adc->DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+    adc->DmaHandle.Init.MemInc = ((adc->usedChannelCount > 1) || (ADC_AVERAGE_N_SAMPLES > 1)) ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;
+    adc->DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+    adc->DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+    adc->DmaHandle.Init.Mode = DMA_CIRCULAR;
+    adc->DmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
+    adc->DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    adc->DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+    adc->DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+    adc->DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+    adc->DmaHandle.Instance = adc->DMAy_Streamx;
+
+    if (HAL_DMA_Init(&adc->DmaHandle) != HAL_OK) {
+        return;
+    }
+
+    __HAL_LINKDMA(&adc->ADCHandle, DMA_Handle, adc->DmaHandle);
+
+    uint8_t rank = 0;
+    for (int i = ADC_CHN_1; i < ADC_CHN_COUNT; i++) {
+        if (!adcConfig[i].enabled || adcConfig[i].adcDevice != adcDevice) {
+            continue;
+        }
+
+        ADC_ChannelConfTypeDef sConfig;
+        sConfig.Channel      = adcConfig[i].adcChannel;
+        sConfig.Rank         = adcRegularRankMap[rank++];
+        sConfig.SamplingTime = adcConfig[i].sampleTime;
+        sConfig.SingleDiff   = ADC_SINGLE_ENDED;
+        sConfig.OffsetNumber = ADC_OFFSET_NONE;
+        sConfig.Offset       = 0;
+
+        if (HAL_ADC_ConfigChannel(&adc->ADCHandle, &sConfig) != HAL_OK) {
+            return;
+        }
+    }
+
+    if (HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t*)&adcValues[adcDevice], adc->usedChannelCount * ADC_AVERAGE_N_SAMPLES) != HAL_OK) {
+        return;
+    }
+}
+
+void adcHardwareInit(drv_adc_config_t *init)
+{
+    UNUSED(init);
+    int configuredAdcChannels = 0;
+
+    for (int i = ADC_CHN_1; i < ADC_CHN_COUNT; i++) {
+        if (!adcConfig[i].tag)
+            continue;
+
+        adcDevice_t * adc = &adcHardware[adcConfig[i].adcDevice];
+
+        IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_CH1 + (i - ADC_CHN_1), 0);
+        IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_NOPULL));
+
+        adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
+        adcConfig[i].dmaIndex = adc->usedChannelCount++;
+        adcConfig[i].sampleTime = ADC_SAMPLETIME_387CYCLES_5;
+        adcConfig[i].enabled = true;
+
+        adc->enabled = true;
+        configuredAdcChannels++;
+    }
+
+    if (configuredAdcChannels == 0)
+        return;
+
+    for (int i = 0; i < ADCDEV_COUNT; i++) {
+        if (adcHardware[i].enabled) {
+            adcInstanceInit(i);
+        }
+    }
+}

src/main/drivers/display_font_metadata.h

@@ -10,12 +10,11 @@ typedef struct displayFontMetadata_s {
     uint16_t charCount;
 } displayFontMetadata_t;
 
-// 'I', 'N', 'A', 'V', 1
+// 'I', 'N', 'A', 'V'
 #define FONT_CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \
     (chr)->data[1] == 'N' && \
     (chr)->data[2] == 'A' && \
-    (chr)->data[3] == 'V' && \
-    (chr)->data[4] == 1)
+    (chr)->data[3] == 'V')
 
 #define FONT_METADATA_CHR_INDEX 255
 // Used for runtime detection of display drivers that might

src/main/drivers/dma_stm32h7xx.c

@@ -0,0 +1,128 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform.h>
+
+#include "common/utils.h"
+#include "drivers/nvic.h"
+#include "drivers/dma.h"
+#include "drivers/rcc.h"
+
+/*
+ * DMA descriptors.
+ */
+static dmaChannelDescriptor_t dmaDescriptors[] = {
+    [0]  = DEFINE_DMA_CHANNEL(1, 0,  0), // DMA1_ST0
+    [1]  = DEFINE_DMA_CHANNEL(1, 1,  6), // DMA1_ST1
+    [2]  = DEFINE_DMA_CHANNEL(1, 2, 16), // DMA1_ST2
+    [3]  = DEFINE_DMA_CHANNEL(1, 3, 22), // DMA1_ST3
+    [4]  = DEFINE_DMA_CHANNEL(1, 4, 32), // DMA1_ST4
+    [5]  = DEFINE_DMA_CHANNEL(1, 5, 38), // DMA1_ST5
+    [6]  = DEFINE_DMA_CHANNEL(1, 6, 48), // DMA1_ST6
+    [7]  = DEFINE_DMA_CHANNEL(1, 7, 54), // DMA1_ST7
+
+    [8]  = DEFINE_DMA_CHANNEL(2, 0,  0), // DMA2_ST0
+    [9]  = DEFINE_DMA_CHANNEL(2, 1,  6), // DMA2_ST1
+    [10] = DEFINE_DMA_CHANNEL(2, 2, 16), // DMA2_ST2
+    [11] = DEFINE_DMA_CHANNEL(2, 3, 22), // DMA2_ST3
+    [12] = DEFINE_DMA_CHANNEL(2, 4, 32), // DMA2_ST4
+    [13] = DEFINE_DMA_CHANNEL(2, 5, 38), // DMA2_ST5
+    [14] = DEFINE_DMA_CHANNEL(2, 6, 48), // DMA2_ST6
+    [15] = DEFINE_DMA_CHANNEL(2, 7, 54), // DMA2_ST7
+};
+
+/*
+ * DMA IRQ Handlers
+ */
+DEFINE_DMA_IRQ_HANDLER(1, 0, 0) // DMA1_ST0
+DEFINE_DMA_IRQ_HANDLER(1, 1, 1) // DMA1_ST1
+DEFINE_DMA_IRQ_HANDLER(1, 2, 2) // DMA1_ST2
+DEFINE_DMA_IRQ_HANDLER(1, 3, 3) // DMA1_ST3
+DEFINE_DMA_IRQ_HANDLER(1, 4, 4) // DMA1_ST4
+DEFINE_DMA_IRQ_HANDLER(1, 5, 5) // DMA1_ST5
+DEFINE_DMA_IRQ_HANDLER(1, 6, 6) // DMA1_ST6
+DEFINE_DMA_IRQ_HANDLER(1, 7, 7) // DMA1_ST7
+DEFINE_DMA_IRQ_HANDLER(2, 0, 8) // DMA2_ST0
+DEFINE_DMA_IRQ_HANDLER(2, 1, 9) // DMA2_ST1
+DEFINE_DMA_IRQ_HANDLER(2, 2, 10) // DMA2_ST2
+DEFINE_DMA_IRQ_HANDLER(2, 3, 11) // DMA2_ST3
+DEFINE_DMA_IRQ_HANDLER(2, 4, 12) // DMA2_ST4
+DEFINE_DMA_IRQ_HANDLER(2, 5, 13) // DMA2_ST5
+DEFINE_DMA_IRQ_HANDLER(2, 6, 14) // DMA2_ST6
+DEFINE_DMA_IRQ_HANDLER(2, 7, 15) // DMA2_ST7
+
+DMA_t dmaGetByTag(dmaTag_t tag)
+{
+    for (unsigned i = 0; i < ARRAYLEN(dmaDescriptors); i++) {
+        if (DMATAG_GET_DMA(dmaDescriptors[i].tag) == DMATAG_GET_DMA(tag) && DMATAG_GET_STREAM(dmaDescriptors[i].tag) == DMATAG_GET_STREAM(tag)) {
+            return (DMA_t)&dmaDescriptors[i];
+        }
+    }
+
+    return (DMA_t) NULL;
+}
+
+void dmaEnableClock(DMA_t dma)
+{
+    if (dma->dma == DMA1) {
+        RCC_ClockCmd(RCC_AHB1(DMA1), ENABLE);
+    }
+    else {
+        RCC_ClockCmd(RCC_AHB1(DMA2), ENABLE);
+    }
+}
+
+resourceOwner_e dmaGetOwner(DMA_t dma)
+{
+    return dma->owner;
+}
+
+void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
+{
+    dmaEnableClock(dma);
+    dma->owner = owner;
+    dma->resourceIndex = resourceIndex;
+}
+
+void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
+{
+    dmaEnableClock(dma);
+
+    dma->irqHandlerCallback = callback;
+    dma->userParam = userParam;
+
+    HAL_NVIC_SetPriority(dma->irqNumber, priority, 0);
+    HAL_NVIC_EnableIRQ(dma->irqNumber);
+}
+
+DMA_t dmaGetByRef(const DMA_Stream_TypeDef* ref)
+{
+    for (unsigned i = 0; i < (sizeof(dmaDescriptors) / sizeof(dmaDescriptors[0])); i++) {
+        if (ref == dmaDescriptors[i].ref) {
+            return &dmaDescriptors[i];
+        }
+    }
+
+    return NULL;
+}

src/main/drivers/exti.c

@@ -24,7 +24,7 @@ extiChannelRec_t extiChannelRecs[16];
 static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 };
 static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS];
 
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
 static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
     EXTI0_IRQn,
     EXTI1_IRQn,
@@ -48,6 +48,15 @@ static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
 # warning "Unknown CPU"
 #endif
 
+// Absorb the difference in IMR and PR assignments to registers
+#if defined(STM32H7)
+#define EXTI_REG_IMR (EXTI_D1->IMR1)
+#define EXTI_REG_PR  (EXTI_D1->PR1)
+#else
+#define EXTI_REG_IMR (EXTI->IMR)
+#define EXTI_REG_PR  (EXTI->PR)
+#endif
+
 
 void EXTIInit(void)
 {
@@ -64,7 +73,7 @@ void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn)
     self->fn = fn;
 }
 
-#if defined(STM32F7)
+#if defined(STM32F7) || defined(STM32H7)
 void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config)
 {
     (void)config;
@@ -156,23 +165,23 @@ void EXTIRelease(IO_t io)
 
 void EXTIEnable(IO_t io, bool enable)
 {
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
     uint32_t extiLine = IO_EXTI_Line(io);
     if (!extiLine)
         return;
     if (enable)
-        EXTI->IMR |= extiLine;
+        EXTI_REG_IMR |= extiLine;
     else
-        EXTI->IMR &= ~extiLine;
+        EXTI_REG_IMR &= ~extiLine;
 #elif defined(STM32F303xC)
     int extiLine = IO_EXTI_Line(io);
     if (extiLine < 0)
         return;
     // assume extiLine < 32 (valid for all EXTI pins)
     if (enable)
-        EXTI->IMR |= 1 << extiLine;
+        EXTI_REG_IMR |= 1 << extiLine;
     else
-        EXTI->IMR &= ~(1 << extiLine);
+        EXTI_REG_IMR &= ~(1 << extiLine);
 #else
 # error "Unsupported target"
 #endif
@@ -180,13 +189,13 @@ void EXTIEnable(IO_t io, bool enable)
 
 void EXTI_IRQHandler(void)
 {
-    uint32_t exti_active = EXTI->IMR & EXTI->PR;
+    uint32_t exti_active = EXTI_REG_IMR & EXTI_REG_PR;
 
     while (exti_active) {
         unsigned idx = 31 - __builtin_clz(exti_active);
         uint32_t mask = 1 << idx;
         extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler);
-        EXTI->PR = mask;  // clear pending mask (by writing 1)
+        EXTI_REG_PR = mask;  // clear pending mask (by writing 1)
         exti_active &= ~mask;
     }
 }
@@ -201,7 +210,7 @@ void EXTI_IRQHandler(void)
 
 _EXTI_IRQ_HANDLER(EXTI0_IRQHandler);
 _EXTI_IRQ_HANDLER(EXTI1_IRQHandler);
-#if defined(STM32F7)
+#if defined(STM32F7) || defined(STM32H7)
 _EXTI_IRQ_HANDLER(EXTI2_IRQHandler);
 #elif defined(STM32F3) || defined(STM32F4)
 _EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler);

src/main/drivers/osd_symbols.h

@@ -31,11 +31,11 @@
 #define SYM_VOLT                  0x06 // 006 V
 #define SYM_MAH                   0x07 // 007 MAH
 
-//                                0x08 // 008 -
-//                                0x09 // 009 -
-//                                0x0A // 010 -
-//                                0x0B // 011 -
-//                                0x0C // 012 -
+#define SYM_AH_KM                 0x08 // 008 Ah/km
+#define SYM_AH_MI                 0x09 // 009 Ah/mi
+#define SYM_MAH_MI_0              0x0A // 010 mAh/mi left
+#define SYM_MAH_MI_1              0x0B // 010 mAh/mi left
+#define SYM_LQ                    0x0C // 012 LQ
 
 #define SYM_TEMP_F                0x0D // 013 °F
 #define SYM_TEMP_C                0x0E // 014 °C
@@ -119,8 +119,9 @@
 #define SYM_RPM                   0x8B // 139 RPM
 #define SYM_WAYPOINT              0x8C // 140 Waypoint
 #define SYM_AZIMUTH               0x8D // 141 Azimuth
-//                                0x8E // 142 -
-//                                0x8F // 143 -
+
+#define SYM_TELEMETRY_0           0x8E // 142 Antenna tracking telemetry
+#define SYM_TELEMETRY_1           0x8F // 143 Antenna tracking telemetry
 
 #define SYM_BATT_FULL             0x90 // 144 Battery full
 #define SYM_BATT_5                0x91 // 145 Battery
@@ -153,8 +154,8 @@
 #define SYM_HEADING               0xA9 // 169 Compass Heading symbol
 #define SYM_ALT                   0xAA // 170 ALT
 #define SYM_WH                    0xAB // 171 WH
-#define SYM_WH_KM_0               0xAC // 172 WH/KM left
-#define SYM_WH_KM_1               0xAD // 173 WH/KM right
+#define SYM_WH_KM                 0xAC // 172 WH/KM
+#define SYM_WH_MI                 0xAD // 173 WH/MI
 #define SYM_WATT                  0xAE // 174 W
 #define SYM_SCALE                 0xAF // 175 Map scale
 #define SYM_MPH                   0xB0 // 176 MPH
@@ -246,6 +247,9 @@
 #define SYM_SNR                   0xEE  // SNR
 #define SYM_MW                    0xED  // mW
 
+#define SYM_KILOWATT              0xEF // 239 kW
+
+
 #else
 
 #define TEMP_SENSOR_SYM_COUNT 0

src/main/drivers/pwm_output.h

@@ -54,5 +54,5 @@ bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
 void pwmWriteBeeper(bool onoffBeep);
 void beeperPwmInit(ioTag_t tag, uint16_t frequency);
 
-void sendDShotCommand(dshotCommands_e directionSpin);
+void sendDShotCommand(dshotCommands_e cmd);
 void initDShotCommands(void);

src/main/drivers/serial_softserial.c

@@ -169,7 +169,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
 
 static void serialTimerConfigureTimebase(TCH_t * tch, uint32_t baud)
 {
-    uint32_t baseClock = SystemCoreClock / timerClockDivisor(tch->timHw->tim);
+    uint32_t baseClock = timerClock(tch->timHw->tim);
     uint32_t clock = baseClock;
     uint32_t timerPeriod;
 

src/main/drivers/serial_uart_hal.c

@@ -71,7 +71,7 @@ static void uartReconfigure(uartPort_t *uartPort)
 
     HAL_UART_DeInit(&uartPort->Handle);
     uartPort->Handle.Init.BaudRate = uartPort->port.baudRate;
-    uartPort->Handle.Init.WordLength = UART_WORDLENGTH_8B;
+    uartPort->Handle.Init.WordLength = (uartPort->port.options & SERIAL_PARITY_EVEN) ? UART_WORDLENGTH_9B : UART_WORDLENGTH_8B;
     uartPort->Handle.Init.StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_STOPBITS_2 : USART_STOPBITS_1;
     uartPort->Handle.Init.Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_PARITY_EVEN : USART_PARITY_NONE;
     uartPort->Handle.Init.HwFlowCtl = UART_HWCONTROL_NONE;

src/main/drivers/serial_uart_stm32h7xx.c

@@ -331,6 +331,10 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
 
     s->Handle.Instance = uart->dev;
 
+    if (uart->rcc) {
+        RCC_ClockCmd(uart->rcc, ENABLE);
+    }
+
     IO_t tx = IOGetByTag(uart->tx);
     IO_t rx = IOGetByTag(uart->rx);
 

src/main/drivers/serial_usb_vcp.c

@@ -216,6 +216,12 @@ void usbVcpInitHardware(void)
 
     /* Start Device Process */
     USBD_Start(&USBD_Device);
+    
+#ifdef STM32H7
+    HAL_PWREx_EnableUSBVoltageDetector();
+    delay(100); // Cold boot failures observed without this, even when USB cable is not connected
+#endif
+
 #else
     Set_System();
     Set_USBClock();

src/main/drivers/system_stm32h7xx.c

@@ -51,10 +51,13 @@ bool isMPUSoftReset(void)
         return false;
 }
 
-#define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1ff09800)
 uint32_t systemBootloaderAddress(void)
 {
-    return SYSMEMBOOT_VECTOR_TABLE[1];
+#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
+    return 0x1ff09800;
+#else
+#error Unknown MCU
+#endif
 }
 
 void systemClockSetup(uint8_t cpuUnderclock)

src/main/drivers/timer.c

@@ -240,7 +240,7 @@ uint32_t timerGetBaseClock(TCH_t * tch)
 
 uint32_t timerGetBaseClockHW(const timerHardware_t * timHw)
 {
-    return SystemCoreClock / timerClockDivisor(timHw->tim);
+    return timerClock(timHw->tim);
 }
 
 bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)

src/main/drivers/timer.h

@@ -157,7 +157,7 @@ typedef enum {
     TYPE_TIMER
 } channelType_t;
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim);
+uint32_t timerClock(TIM_TypeDef *tim);
 uint32_t timerGetBaseClockHW(const timerHardware_t * timHw);
 
 const timerHardware_t * timerGetByUsageFlag(timerUsageFlag_e flag);

src/main/drivers/timer_stm32f30x.c

@@ -48,10 +48,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [16] = { .tim = TIM17, .rcc = RCC_APB2(TIM17), .irq = TIM1_TRG_COM_TIM17_IRQn },
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
+uint32_t timerClock(TIM_TypeDef *tim)
 {
     UNUSED(tim);
-    return 1;
+    return SystemCoreClock;
 }
 
 _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);

src/main/drivers/timer_stm32f4xx.c

@@ -95,16 +95,16 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
 #endif
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
+uint32_t timerClock(TIM_TypeDef *tim)
 {
 #if defined (STM32F411xE)
     UNUSED(tim);
-    return 1;
+    return SystemCoreClock;
 #elif defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F446xx)
     if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
-        return 1;
+        return SystemCoreClock;
     } else {
-        return 2;
+        return SystemCoreClock / 2;
     }
 #else
     #error "No timer clock defined correctly for the MCU"

src/main/drivers/timer_stm32f7xx.c

@@ -48,12 +48,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [13] = { .tim = TIM14, .rcc = RCC_APB1(TIM14), .irq = TIM8_TRG_COM_TIM14_IRQn},
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
-{
-    UNUSED(tim);
-    return 1;
-}
-
 uint32_t timerClock(TIM_TypeDef *tim)
 {
     UNUSED(tim);

src/main/drivers/timer_stm32h7xx.c

@@ -48,16 +48,48 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [13] = { .tim = TIM17, .rcc = RCC_APB2(TIM17),  .irq = TIM17_IRQn},
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
-{
-    UNUSED(tim);
-    return 1;
-}
-
 uint32_t timerClock(TIM_TypeDef *tim)
 {
-    UNUSED(tim);
-    return SystemCoreClock;
+    int timpre;
+    uint32_t pclk;
+    uint32_t ppre;
+
+    // Implement the table:
+    // RM0433 (Rev 6) Table 52.
+    // RM0455 (Rev 3) Table 55.
+    // "Ratio between clock timer and pclk"
+    // (Tables are the same, just D2 or CD difference)
+
+#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
+#define PERIPH_PRESCALER(bus) ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE ## bus) >> RCC_D2CFGR_D2PPRE ## bus ## _Pos)
+#elif defined(STM32H7A3xx) || defined(STM32H7A3xxQ)
+#define PERIPH_PRESCALER(bus) ((RCC->CDCFGR2 & RCC_CDCFGR2_CDPPRE ## bus) >> RCC_CDCFGR2_CDPPRE ## bus ## _Pos)
+#else
+#error Unknown MCU type
+#endif
+
+    if (tim == TIM1 || tim == TIM8 || tim == TIM15 || tim == TIM16 || tim == TIM17) {
+        // Timers on APB2
+        pclk = HAL_RCC_GetPCLK2Freq();
+        ppre = PERIPH_PRESCALER(2);
+    } else {
+        // Timers on APB1
+        pclk = HAL_RCC_GetPCLK1Freq();
+        ppre = PERIPH_PRESCALER(1);
+    }
+
+    timpre = (RCC->CFGR & RCC_CFGR_TIMPRE) ? 1 : 0;
+
+    int index = (timpre << 3) | ppre;
+
+    static uint8_t periphToKernel[16] = { // The mutiplier table
+        1, 1, 1, 1, 2, 2, 2, 2, // TIMPRE = 0
+        1, 1, 1, 1, 2, 4, 4, 4  // TIMPRE = 1
+    };
+
+    return pclk * periphToKernel[index];
+
+#undef PERIPH_PRESCALER
 }
 
 _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
@@ -66,8 +98,6 @@ _TIM_IRQ_HANDLER(TIM3_IRQHandler, 3);
 _TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
 _TIM_IRQ_HANDLER(TIM5_IRQHandler, 5);
 _TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
-//_TIM_IRQ_HANDLER(TIM1_BRK_TIM9_IRQHandler, 9);
-//_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM11_IRQHandler, 11);
 _TIM_IRQ_HANDLER(TIM8_BRK_TIM12_IRQHandler, 12);
 _TIM_IRQ_HANDLER(TIM15_IRQHandler, 15);
 _TIM_IRQ_HANDLER(TIM16_IRQHandler, 16);

src/main/fc/cli.c

@@ -156,7 +156,7 @@ static const char * const featureNames[] = {
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "", "TRANSPONDER", "AIRMODE",
     "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE",
-    "OSD", "FW_LAUNCH", NULL
+    "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
@@ -2501,7 +2501,7 @@ static void cliFeature(char *cmdline)
     }
 }
 
-#ifdef BEEPER
+#if defined(BEEPER) || defined(USE_DSHOT)
 static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault)
 {
     const uint8_t beeperCount = beeperTableEntryCount();
@@ -3234,7 +3234,7 @@ static void cliStatus(char *cmdline)
 #endif
 
     cliPrintf("System load: %d", averageSystemLoadPercent);
-    const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_GYROPID);
+    const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_PID);
     const int pidRate = pidTaskDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)pidTaskDeltaTime));
     const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX)));
     const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
@@ -3484,7 +3484,7 @@ static void printConfig(const char *cmdline, bool doDiff)
         cliPrintHashLine("feature");
         printFeature(dumpMask, &featureConfig_Copy, featureConfig());
 
-#ifdef BEEPER
+#if defined(BEEPER) || defined(USE_DSHOT)
         cliPrintHashLine("beeper");
         printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig());
 #endif
@@ -3655,7 +3655,7 @@ const clicmd_t cmdTable[] = {
 #ifdef USE_CLI_BATCH
     CLI_COMMAND_DEF("batch", "start or end a batch of commands", "start | end", cliBatch),
 #endif
-#ifdef BEEPER
+#if defined(BEEPER) || defined(USE_DSHOT)
     CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
             "\t<+|->[name]", cliBeeper),
 #endif

src/main/fc/config.c

@@ -123,7 +123,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
     .name = SETTING_NAME_DEFAULT
 );
 
-PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1);
+
+PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig,
+                  .beeper_off_flags = 0,
+                  .preferred_beeper_off_flags = 0,
+                  .dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT,
+                  .dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT,
+);
 
 PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0);
 
@@ -166,6 +173,10 @@ __attribute__((weak)) void targetConfiguration(void)
 #endif
 
 uint32_t getLooptime(void) {
+    return gyroConfig()->looptime;
+}
+
+uint32_t getGyroLooptime(void) {
     return gyro.targetLooptime;
 }
 
@@ -287,13 +298,15 @@ void validateAndFixConfig(void)
     }
 #endif
 
-#if !defined(USE_MPU_DATA_READY_SIGNAL)
-    gyroConfigMutable()->gyroSync = false;
-#endif
-
     // Call target-specific validation function
     validateAndFixTargetConfig();
 
+#ifdef USE_MAG
+    if (compassConfig()->mag_align == ALIGN_DEFAULT) {
+        compassConfigMutable()->mag_align = CW270_DEG_FLIP;
+    }
+#endif
+
     if (settingsValidate(NULL)) {
         DISABLE_ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING);
     } else {

src/main/fc/config.h

@@ -29,13 +29,8 @@
 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
 #define MAX_NAME_LENGTH 16
 
-typedef enum {
-    ASYNC_MODE_NONE,
-    ASYNC_MODE_GYRO,
-    ASYNC_MODE_ALL
-} asyncMode_e;
-
-typedef enum {
+#define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz
+ typedef enum {
     FEATURE_THR_VBAT_COMP = 1 << 0,
     FEATURE_VBAT = 1 << 1,
     FEATURE_TX_PROF_SEL = 1 << 2,       // Profile selection by TX stick command
@@ -67,6 +62,7 @@ typedef enum {
     FEATURE_PWM_OUTPUT_ENABLE = 1 << 28,
     FEATURE_OSD = 1 << 29,
     FEATURE_FW_LAUNCH = 1 << 30,
+    FEATURE_FW_AUTOTRIM = 1 << 31,
 } features_e;
 
 typedef struct systemConfig_s {
@@ -88,6 +84,8 @@ PG_DECLARE(systemConfig_t, systemConfig);
 typedef struct beeperConfig_s {
     uint32_t beeper_off_flags;
     uint32_t preferred_beeper_off_flags;
+    bool dshot_beeper_enabled;
+    uint8_t dshot_beeper_tone;
 } beeperConfig_t;
 
 PG_DECLARE(beeperConfig_t, beeperConfig);
@@ -140,3 +138,4 @@ void resetConfigs(void);
 void targetConfiguration(void);
 
 uint32_t getLooptime(void);
+uint32_t getGyroLooptime(void);

src/main/fc/controlrate_profile.h

@@ -22,21 +22,6 @@
 #include "config/parameter_group.h"
 
 #define MAX_CONTROL_RATE_PROFILE_COUNT 3
-/*
-Max and min available values for rates are now stored as absolute
-tenths of degrees-per-second [dsp/10]
-That means, max. rotation rate 180 equals 1800dps
-
-New defaults of 200dps for pitch,roll and yaw are more less
-equivalent of rates 0 from previous versions of iNav, Cleanflight, Baseflight
-and so on.
-*/
-#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX  180
-#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN  6
-#define CONTROL_RATE_CONFIG_YAW_RATE_MAX         180
-#define CONTROL_RATE_CONFIG_YAW_RATE_MIN         2
-
-#define CONTROL_RATE_CONFIG_TPA_MAX              100
 
 typedef struct controlRateConfig_s {
 

src/main/fc/fc_core.c

@@ -45,6 +45,7 @@ FILE_COMPILE_FOR_SPEED
 #include "sensors/boardalignment.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/compass.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
@@ -90,6 +91,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/rate_dynamics.h"
 
 #include "flight/failsafe.h"
+#include "flight/power_limits.h"
 
 #include "config/feature.h"
 #include "common/vector.h"
@@ -103,9 +105,6 @@ enum {
     ALIGN_MAG = 2
 };
 
-#define GYRO_WATCHDOG_DELAY                 100 // Watchdog for boards without interrupt for gyro
-#define GYRO_SYNC_MAX_CONSECUTIVE_FAILURES  100 // After this many consecutive missed interrupts disable gyro sync and fall back to scheduled updates
-
 #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000
 #define EMERGENCY_ARMING_COUNTER_STEP_MS 100
 
@@ -123,6 +122,7 @@ typedef struct emergencyArmingState_s {
 
 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;
+static timeUs_t armTime = 0;
 
 EXTENDED_FASTRAM float dT;
 
@@ -131,14 +131,14 @@ int16_t headFreeModeHold;
 uint8_t motorControlEnable = false;
 
 static bool isRXDataNew;
-static uint32_t gyroSyncFailureCount;
 static disarmReason_t lastDisarmReason = DISARM_NONE;
+timeUs_t lastDisarmTimeUs = 0;
 static emergencyArmingState_t emergencyArming;
 
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;
 
-bool isCalibrating(void)
+bool areSensorsCalibrating(void)
 {
 #ifdef USE_BARO
     if (sensors(SENSOR_BARO) && !baroIsCalibrationComplete()) {
@@ -146,6 +146,12 @@ bool isCalibrating(void)
     }
 #endif
 
+#ifdef USE_MAG
+    if (sensors(SENSOR_MAG) && !compassIsCalibrationComplete()) {
+        return true;
+    }
+#endif
+
 #ifdef USE_PITOT
     if (sensors(SENSOR_PITOT) && !pitotIsCalibrationComplete()) {
         return true;
@@ -174,7 +180,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
     int16_t stickDeflection;
 
     stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
-    stickDeflection = applyDeadband(stickDeflection, deadband);
+    stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500);
 
     return rcLookup(stickDeflection, rate);
 }
@@ -186,7 +192,7 @@ static void updateArmingStatus(void)
     } else {
         /* CHECK: Run-time calibration */
         static bool calibratingFinishedBeep = false;
-        if (isCalibrating()) {
+        if (areSensorsCalibrating()) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_SENSORS_CALIBRATING);
             calibratingFinishedBeep = false;
         }
@@ -302,6 +308,17 @@ static void updateArmingStatus(void)
 	       DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
 	    }
 
+#ifdef USE_DSHOT
+        /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */
+        if (micros() - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) {
+            ENABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
+        } else {
+            DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
+        }
+#else
+        DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
+#endif
+
         if (isModeActivationConditionPresent(BOXPREARM)) {
             if (IS_RC_MODE_ACTIVE(BOXPREARM)) {
                 if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) {
@@ -417,6 +434,7 @@ void disarm(disarmReason_t disarmReason)
 {
     if (ARMING_FLAG(ARMED)) {
         lastDisarmReason = disarmReason;
+        lastDisarmTimeUs = micros();
         DISABLE_ARMING_FLAG(ARMED);
 
 #ifdef USE_BLACKBOX
@@ -425,9 +443,9 @@ void disarm(disarmReason_t disarmReason)
         }
 #endif
 #ifdef USE_DSHOT
-        if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+        if (FLIGHT_MODE(TURTLE_MODE)) {
             sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
-            DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
+            DISABLE_FLIGHT_MODE(TURTLE_MODE);
         }
 #endif
         statsOnDisarm();
@@ -443,6 +461,10 @@ void disarm(disarmReason_t disarmReason)
     }
 }
 
+timeUs_t getLastDisarmTimeUs(void) {
+    return lastDisarmTimeUs;
+}
+
 disarmReason_t getDisarmReason(void)
 {
     return lastDisarmReason;
@@ -498,17 +520,19 @@ void tryArm(void)
 #ifdef USE_DSHOT
     if (
             STATE(MULTIROTOR) &&
-            IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
+            IS_RC_MODE_ACTIVE(BOXTURTLE) &&
             emergencyArmingCanOverrideArmingDisabled() &&
             isMotorProtocolDshot() &&
-            !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
+            !ARMING_FLAG(ARMED) &&
+            !FLIGHT_MODE(TURTLE_MODE)
             ) {
         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
         ENABLE_ARMING_FLAG(ARMED);
-        enableFlightMode(FLIP_OVER_AFTER_CRASH);
+        enableFlightMode(TURTLE_MODE);
         return;
     }
 #endif
+
 #ifdef USE_PROGRAMMING_FRAMEWORK
     if (
         !isArmingDisabled() ||
@@ -709,7 +733,7 @@ void processRx(timeUs_t currentTimeUs)
     // Handle passthrough mode
     if (STATE(FIXED_WING_LEGACY)) {
         if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) ||    // Normal activation of passthrough
-            (!ARMING_FLAG(ARMED) && isCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating
+            (!ARMING_FLAG(ARMED) && areSensorsCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating
             ENABLE_FLIGHT_MODE(MANUAL_MODE);
         } else {
             DISABLE_FLIGHT_MODE(MANUAL_MODE);
@@ -806,36 +830,17 @@ void processRx(timeUs_t currentTimeUs)
 
 // Function for loop trigger
 void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
+    UNUSED(currentTimeUs);
     // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
     // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
     const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
-    timeUs_t gyroUpdateUs = currentTimeUs;
-
-    if (gyroConfig()->gyroSync) {
-        while (true) {
-            gyroUpdateUs = micros();
-            if (gyroSyncCheckUpdate()) {
-                gyroSyncFailureCount = 0;
-                break;
-            }
-            else if ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (timeDelta_t)(getLooptime() + GYRO_WATCHDOG_DELAY)) {
-                gyroSyncFailureCount++;
-                break;
-            }
-        }
-
-        // If we detect gyro sync failure - disable gyro sync
-        if (gyroSyncFailureCount > GYRO_SYNC_MAX_CONSECUTIVE_FAILURES) {
-            gyroConfigMutable()->gyroSync = false;
-        }
-    }
 
     /* Update actual hardware readings */
     gyroUpdate();
 
 #ifdef USE_OPFLOW
     if (sensors(SENSOR_OPFLOW)) {
-        opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs));
+        opflowGyroUpdateCallback(currentDeltaTime);
     }
 #endif
 }
@@ -857,10 +862,15 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
 
     if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
         flightTime += cycleTime;
+        armTime += cycleTime;
         updateAccExtremes();
     }
+    if (!ARMING_FLAG(ARMED)) {
+        armTime = 0;
+    }
+
+    gyroFilter();
 
-    taskGyro(currentTimeUs);
     imuUpdateAccelerometer();
     imuUpdateAttitude(currentTimeUs);
 
@@ -905,6 +915,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         // FIXME: throttle pitch comp for FW
     }
 
+#ifdef USE_POWER_LIMITS
+    powerLimiterApply(&rcCommand[THROTTLE]);
+#endif
+
     // Calculate stabilisation
     pidController(dT);
 
@@ -919,7 +933,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
 
     if (isMixerUsingServos()) {
         servoMixer(dT);
-        processServoAutotrim();
+        processServoAutotrim(dT);
     }
 
     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
@@ -976,6 +990,11 @@ float getFlightTime()
     return (float)(flightTime / 1000) / 1000;
 }
 
+float getArmTime()
+{
+    return (float)(armTime / 1000) / 1000;
+}
+
 void fcReboot(bool bootLoader)
 {
     // stop motor/servo outputs

src/main/fc/fc_core.h

@@ -37,12 +37,14 @@ typedef enum disarmReason_e {
 void handleInflightCalibrationStickPosition(void);
 
 void disarm(disarmReason_t disarmReason);
+timeUs_t getLastDisarmTimeUs(void);
 void tryArm(void);
 disarmReason_t getDisarmReason(void);
 
 void emergencyArmingUpdate(bool armingSwitchIsOn);
 
-bool isCalibrating(void);
+bool areSensorsCalibrating(void);
 float getFlightTime(void);
+float getArmTime(void);
 
 void fcReboot(bool bootLoader);

src/main/fc/fc_init.c

@@ -97,8 +97,9 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/servos.h"
+#include "flight/power_limits.h"
 #include "flight/rpm_filter.h"
+#include "flight/servos.h"
 #include "flight/secondary_imu.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
@@ -709,6 +710,10 @@ void init(void)
     ioPortExpanderInit();
 #endif
 
+#ifdef USE_POWER_LIMITS
+    powerLimiterInit();
+#endif
+
     // Considering that the persistent reset reason is only used during init
     persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
 

src/main/fc/fc_msp.c

@@ -1229,11 +1229,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
         sbufWriteU16(dst, motorConfig()->motorPwmRate);
         sbufWriteU16(dst, servoConfig()->servoPwmRate);
-        sbufWriteU8(dst, gyroConfig()->gyroSync);
+        sbufWriteU8(dst, 0);
         break;
 
     case MSP_FILTER_CONFIG :
-        sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
+        sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz);
         sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
         sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
         sbufWriteU16(dst, gyroConfig()->gyro_notch_hz);
@@ -1247,7 +1247,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
         sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
 
-        sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);
+        sbufWriteU16(dst, 0);    //Was gyroConfig()->gyro_stage2_lowpass_hz
         break;
 
     case MSP_PID_ADVANCED:
@@ -1334,7 +1334,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, navConfig()->general.flags.rth_alt_control_mode);
         sbufWriteU16(dst, navConfig()->general.rth_abort_threshold);
         sbufWriteU16(dst, navConfig()->general.rth_altitude);
-        sbufWriteU16(dst, navConfig()->general.land_descent_rate);
+        sbufWriteU16(dst, navConfig()->general.land_minalt_vspd);
+        sbufWriteU16(dst, navConfig()->general.land_maxalt_vspd);
         sbufWriteU16(dst, navConfig()->general.land_slowdown_minalt);
         sbufWriteU16(dst, navConfig()->general.land_slowdown_maxalt);
         sbufWriteU16(dst, navConfig()->general.emerg_descent_rate);
@@ -1798,14 +1799,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             for (int i = 0; i < 3; i++) {
                 tmp_u8 = sbufReadU8(src);
                 if (i == FD_YAW) {
-                    ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
+                    ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
                 }
                 else {
-                    ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
+                    ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
                 }
             }
             tmp_u8 = sbufReadU8(src);
-            ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, CONTROL_RATE_CONFIG_TPA_MAX);
+            ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX);
             ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src);
             ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src);
             ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src);
@@ -1835,9 +1836,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             for (uint8_t i = 0; i < 3; ++i) {
                 tmp_u8 = sbufReadU8(src);
                 if (i == FD_YAW) {
-                    currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
+                    currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
                 } else {
-                    currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
+                    currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
                 }
             }
 
@@ -1847,9 +1848,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             for (uint8_t i = 0; i < 3; ++i) {
                 tmp_u8 = sbufReadU8(src);
                 if (i == FD_YAW) {
-                    currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
+                    currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
                 } else {
-                    currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
+                    currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
                 }
             }
 
@@ -2163,14 +2164,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             motorConfigMutable()->motorPwmProtocol = sbufReadU8(src);
             motorConfigMutable()->motorPwmRate = sbufReadU16(src);
             servoConfigMutable()->servoPwmRate = sbufReadU16(src);
-            gyroConfigMutable()->gyroSync = sbufReadU8(src);
+            sbufReadU8(src);    //Was gyroSync
         } else
             return MSP_RESULT_ERROR;
         break;
 
     case MSP_SET_FILTER_CONFIG :
         if (dataSize >= 5) {
-            gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
+            gyroConfigMutable()->gyro_main_lpf_hz = sbufReadU8(src);
             pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500);
             pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
             if (dataSize >= 9) {
@@ -2201,7 +2202,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             }
 
             if (dataSize >= 22) {
-                gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500);
+                sbufReadU16(src); //Was gyro_stage2_lowpass_hz
             } else {
                 return MSP_RESULT_ERROR;
             }
@@ -2307,7 +2308,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             navConfigMutable()->general.flags.rth_alt_control_mode = sbufReadU8(src);
             navConfigMutable()->general.rth_abort_threshold = sbufReadU16(src);
             navConfigMutable()->general.rth_altitude = sbufReadU16(src);
-            navConfigMutable()->general.land_descent_rate = sbufReadU16(src);
+            navConfigMutable()->general.land_minalt_vspd = sbufReadU16(src);
+            navConfigMutable()->general.land_maxalt_vspd = sbufReadU16(src);
             navConfigMutable()->general.land_slowdown_minalt = sbufReadU16(src);
             navConfigMutable()->general.land_slowdown_maxalt = sbufReadU16(src);
             navConfigMutable()->general.emerg_descent_rate = sbufReadU16(src);

src/main/fc/fc_msp_box.c

@@ -89,7 +89,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
     { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
     { BOXPREARM, "PREARM", 51 },
-    { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
+    { BOXTURTLE, "TURTLE", 52 },
+    { BOXNAVCRUISE, "NAV CRUISE", 53 },
+    { BOXAUTOLEVEL, "AUTO LEVEL", 54 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -220,6 +222,7 @@ void initActiveBoxIds(void)
             activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
             if (STATE(AIRPLANE)) {
                 activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD;
+                activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
             }
         }
     }
@@ -240,10 +243,17 @@ void initActiveBoxIds(void)
         if (!feature(FEATURE_FW_LAUNCH)) {
            activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
         }
-        activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
+
+        if (!feature(FEATURE_FW_AUTOTRIM)) {
+            activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
+        }
+        
 #if defined(USE_AUTOTUNE_FIXED_WING)
         activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
 #endif
+        if (sensors(SENSOR_BARO)) {
+            activeBoxIds[activeBoxIdCount++] = BOXAUTOLEVEL;
+        }
     }
 
     /*
@@ -312,7 +322,7 @@ void initActiveBoxIds(void)
 
 #ifdef USE_DSHOT
     if(STATE(MULTIROTOR) && isMotorProtocolDshot())
-        activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
+        activeBoxIds[activeBoxIdCount++] = BOXTURTLE;
 #endif
 }
 
@@ -346,6 +356,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     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_COURSE_HOLD_MODE)),     BOXNAVCOURSEHOLD);
+    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) && IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_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);
@@ -373,6 +384,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)),   BOXMSPRCOVERRIDE);
 #endif
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)),       BOXAUTOLEVEL);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {

src/main/fc/fc_tasks.c

@@ -43,14 +43,15 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
+#include "flight/dynamic_lpf.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/wind_estimator.h"
-#include "flight/secondary_imu.h"
+#include "flight/power_limits.h"
 #include "flight/rpm_filter.h"
+#include "flight/secondary_imu.h"
 #include "flight/servos.h"
-#include "flight/dynamic_lpf.h"
+#include "flight/wind_estimator.h"
 
 #include "navigation/navigation.h"
 
@@ -112,18 +113,29 @@ void taskHandleSerial(timeUs_t currentTimeUs)
 void taskUpdateBattery(timeUs_t currentTimeUs)
 {
     static timeUs_t batMonitoringLastServiced = 0;
-    timeUs_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced);
+    timeDelta_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced);
 
-    if (isAmperageConfigured())
+    if (isAmperageConfigured()) {
         currentMeterUpdate(BatMonitoringTimeSinceLastServiced);
+#ifdef USE_POWER_LIMITS
+        currentLimiterUpdate(BatMonitoringTimeSinceLastServiced);
+#endif
+    }
+
 #ifdef USE_ADC
-    if (feature(FEATURE_VBAT))
+    if (feature(FEATURE_VBAT)) {
         batteryUpdate(BatMonitoringTimeSinceLastServiced);
+    }
+
     if (feature(FEATURE_VBAT) && isAmperageConfigured()) {
         powerMeterUpdate(BatMonitoringTimeSinceLastServiced);
         sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced);
+#if defined(USE_POWER_LIMITS) && defined(USE_ADC)
+        powerLimiterUpdate(BatMonitoringTimeSinceLastServiced);
+#endif
     }
 #endif
+
     batMonitoringLastServiced = currentTimeUs;
 }
 
@@ -292,21 +304,25 @@ void taskUpdateOsd(timeUs_t currentTimeUs)
 
 void taskUpdateAux(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
     updatePIDCoefficients();
     dynamicLpfGyroTask();
+    updateFixedWingLevelTrim(currentTimeUs);
 }
 
 void fcTasksInit(void)
 {
     schedulerInit();
 
-    rescheduleTask(TASK_GYROPID, getLooptime());
-    setTaskEnabled(TASK_GYROPID, true);
+    rescheduleTask(TASK_PID, getLooptime());
+    setTaskEnabled(TASK_PID, true);
+
+    rescheduleTask(TASK_GYRO, getGyroLooptime());
+    setTaskEnabled(TASK_GYRO, true);
+
     setTaskEnabled(TASK_AUX, true);
 
     setTaskEnabled(TASK_SERIAL, true);
-#ifdef BEEPER
+#if defined(BEEPER) || defined(USE_DSHOT)
     setTaskEnabled(TASK_BEEPER, true);
 #endif
 #ifdef USE_LIGHTS
@@ -388,12 +404,18 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .desiredPeriod = TASK_PERIOD_HZ(10),              // run every 100 ms, 10Hz
         .staticPriority = TASK_PRIORITY_HIGH,
     },
-    [TASK_GYROPID] = {
-        .taskName = "GYRO/PID",
+    [TASK_PID] = {
+        .taskName = "PID",
         .taskFunc = taskMainPidLoop,
         .desiredPeriod = TASK_PERIOD_US(1000),
         .staticPriority = TASK_PRIORITY_REALTIME,
     },
+    [TASK_GYRO] = {
+        .taskName = "GYRO",
+        .taskFunc = taskGyro,
+        .desiredPeriod = TASK_PERIOD_US(TASK_GYRO_LOOPTIME),
+        .staticPriority = TASK_PRIORITY_REALTIME,
+    },
     [TASK_SERIAL] = {
         .taskName = "SERIAL",
         .taskFunc = taskHandleSerial,
@@ -401,7 +423,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_LOW,
     },
 
-#ifdef BEEPER
+#if defined(BEEPER) || defined(USE_DSHOT)
     [TASK_BEEPER] = {
         .taskName = "BEEPER",
         .taskFunc = beeperUpdate,

src/main/fc/rc_adjustments.c

@@ -39,6 +39,7 @@
 #include "fc/controlrate_profile.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_curves.h"
+#include "fc/settings.h"
 
 #include "navigation/navigation.h"
 
@@ -61,15 +62,6 @@ 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] = {
     {
@@ -368,17 +360,17 @@ static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t
 
 static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
 {
-    applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_EXPO_MIN, RC_ADJUSTMENT_EXPO_MAX);
+    applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_RC_EXPO_MIN, SETTING_RC_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);
+    return applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_CONSTANT_MANUAL_RATE_MIN, SETTING_CONSTANT_MANUAL_RATE_MAX);
 }
 
 static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta)
 {
-    applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
+    applyAdjustmentU16(adjustmentFunction, val, delta, SETTING_CONSTANT_RPYL_PID_MIN, SETTING_CONSTANT_RPYL_PID_MAX);
 }
 
 static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
@@ -406,7 +398,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             break;
         case ADJUSTMENT_PITCH_ROLL_RATE:
         case ADJUSTMENT_PITCH_RATE:
-            applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
+            applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, SETTING_PITCH_RATE_MIN, SETTING_PITCH_RATE_MAX);
             if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
                 schedulePidGainsUpdate();
                 break;
@@ -415,7 +407,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             FALLTHROUGH;
 
         case ADJUSTMENT_ROLL_RATE:
-            applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
+            applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
             schedulePidGainsUpdate();
             break;
         case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
@@ -429,7 +421,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta);
             break;
         case ADJUSTMENT_YAW_RATE:
-            applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
+            applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
             schedulePidGainsUpdate();
             break;
         case ADJUSTMENT_MANUAL_YAW_RATE:
@@ -508,10 +500,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             schedulePidGainsUpdate();
             break;
         case ADJUSTMENT_NAV_FW_CRUISE_THR:
-            applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000);
+            applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
             break;
         case ADJUSTMENT_NAV_FW_PITCH2THR:
-            applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100);
+            applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
             break;
         case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
             updateBoardAlignment(delta, 0);
@@ -586,7 +578,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             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);
+            applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
             break;
 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
         case ADJUSTMENT_VTX_POWER_LEVEL:
@@ -599,13 +591,13 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             break;
 #endif
         case ADJUSTMENT_TPA:
-            applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, CONTROL_RATE_CONFIG_TPA_MAX);
+            applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX);
             break;
         case ADJUSTMENT_TPA_BREAKPOINT:
             applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX);
             break;
         case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
-            applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, 0, 9);
+            applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX);
             break;
         default:
             break;

src/main/fc/rc_modes.c

@@ -212,6 +212,7 @@ void updateUsedModeActivationConditionFlags(void)
     isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
                         isModeActivationConditionPresent(BOXNAVRTH) ||
                         isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
+                        isModeActivationConditionPresent(BOXNAVCRUISE) ||
                         isModeActivationConditionPresent(BOXNAVWP);
 #endif
 }

src/main/fc/rc_modes.h

@@ -69,7 +69,9 @@ typedef enum {
     BOXLOITERDIRCHN  = 40,
     BOXMSPRCOVERRIDE = 41,
     BOXPREARM        = 42,
-    BOXFLIPOVERAFTERCRASH = 43,
+    BOXTURTLE        = 43,
+    BOXNAVCRUISE     = 44,
+    BOXAUTOLEVEL     = 45,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/runtime_config.c

@@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= {
     "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS",
     "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX",
     "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM",
-    "SETTINGFAIL", "PWMOUT", "NOPREARM"
+    "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER"
 };
 #endif
 
@@ -58,7 +58,8 @@ const armingFlag_e armDisableReasonsChecklist[] = {
     ARMING_DISABLED_ROLLPITCH_NOT_CENTERED,
     ARMING_DISABLED_SERVO_AUTOTRIM,
     ARMING_DISABLED_OOM,
-    ARMING_DISABLED_NO_PREARM
+    ARMING_DISABLED_NO_PREARM,
+    ARMING_DISABLED_DSHOT_BEEPER
 };
 
 armingFlag_e isArmingDisabledReason(void)
@@ -154,9 +155,12 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
     if (FLIGHT_MODE(NAV_POSHOLD_MODE))
         return FLM_POSITION_HOLD;
 
-    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
         return FLM_CRUISE;
 
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
+        return FLM_COURSE_HOLD;
+
     if (FLIGHT_MODE(NAV_WP_MODE))
         return FLM_MISSION;
 

src/main/fc/runtime_config.h

@@ -44,6 +44,7 @@ typedef enum {
     ARMING_DISABLED_INVALID_SETTING                 = (1 << 26),
     ARMING_DISABLED_PWM_OUTPUT_ERROR                = (1 << 27),
     ARMING_DISABLED_NO_PREARM                       = (1 << 28),
+    ARMING_DISABLED_DSHOT_BEEPER                    = (1 << 29),
 
     ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | 
                                                        ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
@@ -52,7 +53,7 @@ typedef enum {
                                                        ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
                                                        ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED |
                                                        ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING |
-                                                       ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM),
+                                                       ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER),
 } armingFlag_e;
 
 // Arming blockers that can be overriden by emergency arming.
@@ -97,7 +98,7 @@ typedef enum {
     NAV_COURSE_HOLD_MODE  = (1 << 12),
     FLAPERON              = (1 << 13),
     TURN_ASSISTANT        = (1 << 14),
-    FLIP_OVER_AFTER_CRASH = (1 << 15),
+    TURTLE_MODE           = (1 << 15),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;
@@ -151,6 +152,7 @@ typedef enum {
     FLM_POSITION_HOLD,
     FLM_RTH,
     FLM_MISSION,
+    FLM_COURSE_HOLD,
     FLM_CRUISE,
     FLM_LAUNCH,
     FLM_FAILSAFE,

src/main/fc/settings.yaml

@@ -10,7 +10,7 @@ tables:
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"]
     enum: rangefinderType_e
   - name: secondary_imu_hardware
-    values: ["NONE", "BNO055"]
+    values: ["NONE", "BNO055", "BNO055_SERIAL"]
     enum: secondaryImuType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"]
@@ -69,9 +69,15 @@ tables:
   - name: osd_stats_energy_unit
     values: ["MAH", "WH"]
     enum: osd_stats_energy_unit_e
+  - name: osd_stats_min_voltage_unit
+    values: ["BATTERY", "CELL"]
+    enum: osd_stats_min_voltage_unit_e
   - name: osd_video_system
     values: ["AUTO", "PAL", "NTSC"]
     enum: videoSystem_e
+  - name: osd_telemetry
+    values: ["OFF", "ON","TEST"]
+    enum: osd_telemetry_e
   - name: osd_alignment
     values: ["LEFT", "RIGHT"]
     enum: osd_alignment_e
@@ -87,8 +93,8 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", 
-      "RATE_DYNAMICS"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "FW_D", "IMU2", "ALTITUDE",
+      "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -98,7 +104,7 @@ tables:
     values: ["DEFAULT", "AIRCRAFT", "TYPE3", "TYPE4", "TYPE5", "TYPE6", "TYPE7"]
     enum: osd_crosshairs_style_e
   - name: osd_sidebar_scroll
-    values: ["NONE", "ALTITUDE", "GROUND_SPEED", "HOME_DISTANCE"]
+    values: ["NONE", "ALTITUDE", "SPEED", "HOME_DISTANCE"]
     enum: osd_sidebar_scroll_e
   - name: nav_rth_allow_landing
     values: ["NEVER", "ALWAYS", "FS_ONLY"]
@@ -150,24 +156,40 @@ tables:
     values: ["AUTO", "ON", "OFF"]
   - name: osd_crsf_lq_format
     enum: osd_crsf_lq_format_e
-    values: ["TYPE1", "TYPE2"]
+    values: ["TYPE1", "TYPE2", "TYPE3"]
   - name: off_on
     values: ["OFF", "ON"]
   - name: djiOsdTempSource
     values: ["ESC", "IMU", "BARO"]
     enum: djiOsdTempSource_e
-  - name: djiOsdSpeedSource
+  - name: osdSpeedSource
     values: ["GROUND", "3D", "AIR"]
-    enum: djiOsdSpeedSource_e
+    enum: osdSpeedSource_e
   - name: nav_overrides_motor_stop
     enum: navOverridesMotorStop_e
     values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"]
   - name: osd_plus_code_short
     values: ["0", "2", "4", "6"]
+  - name: autotune_rate_adjustment
+    enum: autotune_rate_adjustment_e
+    values: ["FIXED", "LIMIT", "AUTO"]
+  - name: safehome_usage_mode
+    values: ["OFF", "RTH", "RTH_FS"]
+    enum: safehomeUsageMode_e
   - name: nav_rth_climb_first
     enum: navRTHClimbFirst_e
     values: ["OFF", "ON", "ON_FW_SPIRAL"]
 
+constants:
+  RPYL_PID_MIN: 0
+  RPYL_PID_MAX: 200
+
+  MANUAL_RATE_MIN: 0
+  MANUAL_RATE_MAX: 100
+
+  ROLL_PITCH_RATE_MIN: 6
+  ROLL_PITCH_RATE_MAX: 180
+
 groups:
   - name: PG_GYRO_CONFIG
     type: gyroConfig_t
@@ -177,11 +199,6 @@ groups:
         description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible."
         default_value: 1000
         max: 9000
-      - name: gyro_sync
-        description: "This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf"
-        default_value: ON
-        field: gyroSync
-        type: bool
       - name: align_gyro
         description: "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."
         default_value: "DEFAULT"
@@ -193,15 +210,15 @@ groups:
         default_value: "256HZ"
         field: gyro_lpf
         table: gyro_lpf
-      - name: gyro_lpf_hz
-        description: "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."
-        default_value: 60
-        field: gyro_soft_lpf_hz
-        max: 200
-      - name: gyro_lpf_type
-        description: "Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds."
-        default_value: "BIQUAD"
-        field: gyro_soft_lpf_type
+      - name: gyro_anti_aliasing_lpf_hz
+        description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
+        default_value: 250
+        field: gyro_anti_aliasing_lpf_hz
+        max: 255
+      - name: gyro_anti_aliasing_lpf_type
+        description: "Specifies the type of the software LPF of the gyro signals."
+        default_value: "PT1"
+        field: gyro_anti_aliasing_lpf_type
         table: filter_type
       - name: moron_threshold
         description: "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."
@@ -217,16 +234,16 @@ groups:
         min: 1
         max: 500
         default_value: 1
-      - name: gyro_stage2_lowpass_hz
-        description: "Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz)"
-        default_value: 0
-        field: gyro_stage2_lowpass_hz
+      - name: gyro_main_lpf_hz
+        description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
+        default_value: 60
+        field: gyro_main_lpf_hz
         min: 0
         max: 500
-      - name: gyro_stage2_lowpass_type
-        description: "Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation."
+      - name: gyro_main_lpf_type
+        description: "Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation."
         default_value: BIQUAD
-        field: gyro_stage2_lowpass_type
+        field: gyro_main_lpf_type
         table: filter_type
       - name: gyro_use_dyn_lpf
         description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
@@ -282,6 +299,27 @@ groups:
         min: 0
         max: 1
         default_value: 0
+      - name: gyro_abg_alpha
+        description: "Alpha factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0
+        field: alphaBetaGammaAlpha
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 1
+      - name: gyro_abg_boost
+        description: "Boost factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.35
+        field: alphaBetaGammaBoost
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 2
+      - name: gyro_abg_half_life
+        description: "Sample half-life for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.5
+        field: alphaBetaGammaHalfLife
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 10
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t
@@ -837,10 +875,10 @@ groups:
         min: 4
         max: 255
         default_value: 14
-      - name: flip_over_after_crash_power_factor
-        field: flipOverAfterPowerFactor
-        default_value: 65
-        description: "flip over after crash power factor"
+      - name: turtle_mode_power_factor
+        field: turtleModePowerFactor
+        default_value: 55
+        description: "Turtle mode power factor"
         condition: "USE_DSHOT"
         min: 0
         max: 100
@@ -1180,6 +1218,11 @@ groups:
         description: "On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF."
         default_value: ON
         type: bool
+      - name: servo_autotrim_rotation_limit
+        description: "Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`."
+        default_value: 15
+        min: 1
+        max: 60
 
   - name: PG_CONTROL_RATE_PROFILES
     type: controlRateConfig_t
@@ -1203,7 +1246,7 @@ groups:
         default_value: 0
         field: throttle.dynPID
         min: 0
-        max: CONTROL_RATE_CONFIG_TPA_MAX
+        max: 100
       - name: tpa_breakpoint
         description: "See tpa_rate."
         default_value: 1500
@@ -1234,20 +1277,20 @@ groups:
         description: "Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure."
         default_value: 20
         field: stabilized.rates[FD_ROLL]
-        min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
-        max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
+        min: ROLL_PITCH_RATE_MIN
+        max: ROLL_PITCH_RATE_MAX
       - name: pitch_rate
         description: "Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure."
         default_value: 20
         field: stabilized.rates[FD_PITCH]
-        min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
-        max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
+        min: ROLL_PITCH_RATE_MIN
+        max: ROLL_PITCH_RATE_MAX
       - name: yaw_rate
         description: "Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure."
         default_value: 20
         field: stabilized.rates[FD_YAW]
-        min: CONTROL_RATE_CONFIG_YAW_RATE_MIN
-        max: CONTROL_RATE_CONFIG_YAW_RATE_MAX
+        min: 2
+        max: 180
       - name: manual_rc_expo
         description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]"
         default_value: 70
@@ -1264,20 +1307,20 @@ groups:
         description: "Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]%"
         default_value: 100
         field: manual.rates[FD_ROLL]
-        min: 0
-        max: 100
+        min: MANUAL_RATE_MIN
+        max: MANUAL_RATE_MAX
       - name: manual_pitch_rate
         description: "Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]%"
         default_value: 100
         field: manual.rates[FD_PITCH]
-        min: 0
-        max: 100
+        min: MANUAL_RATE_MIN
+        max: MANUAL_RATE_MAX
       - name: manual_yaw_rate
         description: "Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]%"
         default_value: 100
         field: manual.rates[FD_YAW]
-        min: 0
-        max: 100
+        min: MANUAL_RATE_MIN
+        max: MANUAL_RATE_MAX
       - name: fpv_mix_degrees
         field: misc.fpvCamAngleDegrees
         min: 0
@@ -1540,182 +1583,182 @@ groups:
         description: "Multicopter rate stabilisation P-gain for PITCH"
         default_value: 40
         field: bank_mc.pid[PID_PITCH].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_i_pitch
         description: "Multicopter rate stabilisation I-gain for PITCH"
         default_value: 30
         field: bank_mc.pid[PID_PITCH].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_d_pitch
         description: "Multicopter rate stabilisation D-gain for PITCH"
         default_value: 23
         field: bank_mc.pid[PID_PITCH].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_cd_pitch
         description: "Multicopter Control Derivative gain for PITCH"
         default_value: 60
         field: bank_mc.pid[PID_PITCH].FF
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_p_roll
         description: "Multicopter rate stabilisation P-gain for ROLL"
         default_value: 40
         field: bank_mc.pid[PID_ROLL].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_i_roll
         description: "Multicopter rate stabilisation I-gain for ROLL"
         default_value: 30
         field: bank_mc.pid[PID_ROLL].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_d_roll
         description: "Multicopter rate stabilisation D-gain for ROLL"
         default_value: 23
         field: bank_mc.pid[PID_ROLL].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_cd_roll
         description: "Multicopter Control Derivative gain for ROLL"
         default_value: 60
         field: bank_mc.pid[PID_ROLL].FF
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_p_yaw
         description: "Multicopter rate stabilisation P-gain for YAW"
         default_value: 85
         field: bank_mc.pid[PID_YAW].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_i_yaw
         description: "Multicopter rate stabilisation I-gain for YAW"
         default_value: 45
         field: bank_mc.pid[PID_YAW].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_d_yaw
         description: "Multicopter rate stabilisation D-gain for YAW"
         default_value: 0
         field: bank_mc.pid[PID_YAW].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_cd_yaw
         description: "Multicopter Control Derivative gain for YAW"
         default_value: 60
         field: bank_mc.pid[PID_YAW].FF
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_p_level
         description: "Multicopter attitude stabilisation P-gain"
         default_value: 20
         field: bank_mc.pid[PID_LEVEL].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_i_level
         description: "Multicopter attitude stabilisation low-pass filter cutoff"
         default_value: 15
         field: bank_mc.pid[PID_LEVEL].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: mc_d_level
         description: "Multicopter attitude stabilisation HORIZON transition point"
         default_value: 75
         field: bank_mc.pid[PID_LEVEL].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_p_pitch
         description: "Fixed-wing rate stabilisation P-gain for PITCH"
         default_value: 5
         field: bank_fw.pid[PID_PITCH].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_i_pitch
         description: "Fixed-wing rate stabilisation I-gain for PITCH"
         default_value: 7
         field: bank_fw.pid[PID_PITCH].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_d_pitch
         description: "Fixed wing rate stabilisation D-gain for PITCH"
         default_value: 0
         field: bank_fw.pid[PID_PITCH].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_ff_pitch
         description: "Fixed-wing rate stabilisation FF-gain for PITCH"
         default_value: 50
         field: bank_fw.pid[PID_PITCH].FF
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_p_roll
         description: "Fixed-wing rate stabilisation P-gain for ROLL"
         default_value: 5
         field: bank_fw.pid[PID_ROLL].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_i_roll
         description: "Fixed-wing rate stabilisation I-gain for ROLL"
         default_value: 7
         field: bank_fw.pid[PID_ROLL].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_d_roll
         description: "Fixed wing rate stabilisation D-gain for ROLL"
         default_value: 0
         field: bank_fw.pid[PID_ROLL].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_ff_roll
         description: "Fixed-wing rate stabilisation FF-gain for ROLL"
         default_value: 50
         field: bank_fw.pid[PID_ROLL].FF
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_p_yaw
         description: "Fixed-wing rate stabilisation P-gain for YAW"
         default_value: 6
         field: bank_fw.pid[PID_YAW].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_i_yaw
         description: "Fixed-wing rate stabilisation I-gain for YAW"
         default_value: 10
         field: bank_fw.pid[PID_YAW].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_d_yaw
         description: "Fixed wing rate stabilisation D-gain for YAW"
         default_value: 0
         field: bank_fw.pid[PID_YAW].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_ff_yaw
         description: "Fixed-wing rate stabilisation FF-gain for YAW"
         default_value: 60
         field: bank_fw.pid[PID_YAW].FF
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_p_level
         description: "Fixed-wing attitude stabilisation P-gain"
         default_value: 20
         field: bank_fw.pid[PID_LEVEL].P
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_i_level
         description: "Fixed-wing attitude stabilisation low-pass filter cutoff"
         default_value: 5
         field: bank_fw.pid[PID_LEVEL].I
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: fw_d_level
         description: "Fixed-wing attitude stabilisation HORIZON transition point"
         default_value: 75
         field: bank_fw.pid[PID_LEVEL].D
-        min: 0
-        max: 200
+        min: RPYL_PID_MIN
+        max: RPYL_PID_MAX
       - name: max_angle_inclination_rll
         description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°"
         default_value: 300
@@ -1767,10 +1810,10 @@ groups:
         table: direction
       - name: fw_reference_airspeed
         description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present."
-        default_value: 1000
+        default_value: 1500
         field: fixedWingReferenceAirspeed
-        min: 1
-        max: 5000
+        min: 300
+        max: 6000
       - name: fw_turn_assist_yaw_gain
         description: "Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter"
         default_value: 1
@@ -2090,27 +2133,48 @@ groups:
         field: fixedWingLevelTrim
         min: -10
         max: 10
+      - name: smith_predictor_strength
+        description: "The strength factor of a Smith Predictor of PID measurement. In percents"
+        default_value: 0.5
+        field: smithPredictorStrength
+        condition: USE_SMITH_PREDICTOR
+        min: 0
+        max: 1
+      - name: smith_predictor_delay
+        description: "Expected delay of the gyro signal. In milliseconds"
+        default_value: 0
+        field: smithPredictorDelay
+        condition: USE_SMITH_PREDICTOR
+        min: 0
+        max: 8
+      - name: smith_predictor_lpf_hz
+        description: "Cutoff frequency for the Smith Predictor Low Pass Filter"
+        default_value: 50
+        field: smithPredictorFilterHz
+        condition: USE_SMITH_PREDICTOR
+        min: 1
+        max: 500
+      - name: fw_level_pitch_gain
+        description: "I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations"
+        default_value: 5
+        field: fixedWingLevelTrimGain
+        min: 0
+        max: 20
+      - name: fw_level_pitch_deadband
+        description: "Deadband for automatic leveling when AUTOLEVEL mode is used."
+        default_value: 5
+        field: fixedWingLevelTrimDeadband
+        min: 0
+        max: 20
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
     condition: USE_AUTOTUNE_FIXED_WING
     members:
-      - name: fw_autotune_overshoot_time
-        description: "Time [ms] to detect sustained overshoot"
-        default_value: 100
-        field: fw_overshoot_time
-        min: 50
-        max: 500
-      - name: fw_autotune_undershoot_time
-        description: "Time [ms] to detect sustained undershoot"
-        default_value: 200
-        field: fw_undershoot_time
-        min: 50
-        max: 500
-      - name: fw_autotune_threshold
-        description: "Threshold [%] of max rate to consider overshoot/undershoot detection"
+      - name: fw_autotune_min_stick
+        description: "Minimum stick input [%] to consider overshoot/undershoot detection"
         default_value: 50
-        field: fw_max_rate_threshold
+        field: fw_min_stick
         min: 0
         max: 100
       - name: fw_autotune_ff_to_p_gain
@@ -2119,12 +2183,30 @@ groups:
         field: fw_ff_to_p_gain
         min: 0
         max: 100
+      - name: fw_autotune_p_to_d_gain
+        description: "P to D gain (strength relationship) [%]"
+        default_value: 0
+        field: fw_p_to_d_gain
+        min: 0
+        max: 200
       - name: fw_autotune_ff_to_i_tc
         description: "FF to I time (defines time for I to reach the same level of response as FF) [ms]"
         default_value: 600
         field: fw_ff_to_i_time_constant
         min: 100
         max: 5000
+      - name: fw_autotune_rate_adjustment
+        description: "`AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode."
+        default_value: "AUTO"
+        field: fw_rate_adjustment
+        table: autotune_rate_adjustment
+        type: uint8_t
+      - name: fw_autotune_max_rate_deflection
+        description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`."
+        default_value: 80
+        field: fw_max_rate_deflection
+        min: 50
+        max: 100
 
   - name: PG_POSITION_ESTIMATION_CONFIG
     type: positionEstimationConfig_t
@@ -2288,6 +2370,11 @@ groups:
         field: general.pos_failure_timeout
         min: 0
         max: 10
+      - name: nav_wp_load_on_boot
+        description: "If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup."
+        default_value: OFF
+        field: general.waypoint_load_on_boot
+        type: bool
       - name: nav_wp_radius
         description: "Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius"
         default_value: 100
@@ -2323,20 +2410,26 @@ groups:
         field: general.max_manual_climb_rate
         min: 10
         max: 2000
-      - name: nav_landing_speed
-        description: "Vertical descent velocity during the RTH landing phase. [cm/s]"
+      - name: nav_land_minalt_vspd
+        description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]"
+        default_value: 50
+        field: general.land_minalt_vspd
+        min: 50
+        max: 500
+      - name: nav_land_maxalt_vspd
+        description: "Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s]"
         default_value: 200
-        field: general.land_descent_rate
+        field: general.land_maxalt_vspd
         min: 100
         max: 2000
       - name: nav_land_slowdown_minalt
-        description: "Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm]"
+        description: "Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm]"
         default_value: 500
         field: general.land_slowdown_minalt
         min: 50
         max: 1000
       - name: nav_land_slowdown_maxalt
-        description: "Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm]"
+        description: "Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm]"
         default_value: 2000
         field: general.land_slowdown_maxalt
         min: 500
@@ -2383,6 +2476,11 @@ groups:
         default_value: "AT_LEAST"
         field: general.flags.rth_alt_control_mode
         table: nav_rth_alt_mode
+      - name: nav_rth_alt_control_override
+        description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing)"
+        default_value: OFF
+        field: general.flags.rth_alt_control_override
+        type: bool
       - name: nav_rth_abort_threshold
         description: "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]"
         default_value: 50000
@@ -2394,6 +2492,12 @@ groups:
         description: "Max allowed above the ground altitude for terrain following mode"
         max: 1000
         default_value: 100
+      - name: nav_max_altitude
+        field: general.max_altitude
+        description: "Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled"
+        default_value: 0
+        max: 65000
+        min: 0
       - name: nav_rth_altitude
         description: "Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters)"
         default_value: 1000
@@ -2410,6 +2514,11 @@ groups:
         field: general.safehome_max_distance
         min: 0
         max: 65000
+      - name: safehome_usage_mode
+        description: "Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`.  See [Safehome documentation](Safehomes.md#Safehome) for more information."
+        default_value: "RTH"
+        field: general.flags.safehome_usage_mode
+        table: safehome_usage_mode
       - name: nav_mc_bank_angle
         description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
         default_value: 30
@@ -2656,7 +2765,7 @@ groups:
         min: 0
         max: 60000
       - name: nav_fw_launch_climb_angle
-        description: "Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
+        description: "Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
         default_value: 18
         field: fw.launch_climb_angle
         min: 5
@@ -2916,6 +3025,12 @@ groups:
     headers: ["io/osd.h", "drivers/osd.h"]
     condition: USE_OSD
     members:
+      - name: osd_telemetry
+        description: "To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST`"
+        table: osd_telemetry
+        field: telemetry
+        type: uint8_t
+        default_value: "OFF"
       - name: osd_video_system
         description: "Video system used. Possible values are `AUTO`, `PAL` and `NTSC`"
         default_value: "AUTO"
@@ -2940,6 +3055,12 @@ groups:
         field: stats_energy_unit
         table: osd_stats_energy_unit
         type: uint8_t
+      - name: osd_stats_min_voltage_unit
+        description: "Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats."
+        default_value: "BATTERY"
+        field: stats_min_voltage_unit
+        table: osd_stats_min_voltage_unit
+        type: uint8_t
 
       - name: osd_rssi_alarm
         description: "Value below which to make the OSD RSSI indicator blink"
@@ -3262,6 +3383,13 @@ groups:
         default_value: 0
         description: Same as left_sidebar_scroll_step, but for the right sidebar
 
+      - name: osd_sidebar_height
+        field: sidebar_height
+        min: 0
+        max: 5
+        default_value: 3
+        description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
+
       - name: osd_home_position_arm_screen
         type: bool
         default_value: ON
@@ -3281,6 +3409,17 @@ groups:
         min: -36
         max: 36
 
+  - name: PG_OSD_COMMON_CONFIG
+    type: osdCommonConfig_t
+    headers: ["io/osd_common.h"]
+    condition: USE_OSD || USE_DJI_HD_OSD
+    members:
+      - name: osd_speed_source
+        description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR"
+        default_value: "GROUND"
+        field: speedSource
+        table: osdSpeedSource
+        type: uint8_t
 
   - name: PG_SYSTEM_CONFIG
     type: systemConfig_t
@@ -3530,9 +3669,85 @@ groups:
           field: esc_temperature_source
           table: djiOsdTempSource
           type: uint8_t
-        - name: dji_speed_source
-          description: "Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR"
-          default_value: "GROUND"
-          field: speedSource
-          table: djiOsdSpeedSource
-          type: uint8_t
+
+  - name: PG_BEEPER_CONFIG
+    type: beeperConfig_t
+    headers: [ "fc/config.h" ]
+    members:
+      - name: dshot_beeper_enabled
+        description: "Whether using DShot motors as beepers is enabled"
+        default_value: ON
+        field: dshot_beeper_enabled
+        type: bool
+      - name: dshot_beeper_tone
+        description: "Sets the DShot beeper tone"
+        min: 1
+        max: 5
+        default_value: 1
+        field: dshot_beeper_tone
+        type: uint8_t
+
+  - name: PG_POWER_LIMITS_CONFIG
+    type: powerLimitsConfig_t
+    headers: ["flight/power_limits.h"]
+    condition: USE_POWER_LIMITS
+    members:
+      - name: limit_cont_current
+        description: "Continous current limit (dA), set to 0 to disable"
+        default_value: 0
+        field: continuousCurrent
+        max: 4000
+      - name: limit_burst_current
+        description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
+        default_value: 0
+        field: burstCurrent
+        max: 4000
+      - name: limit_burst_current_time
+        description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
+        default_value: 0
+        field: burstCurrentTime
+        max: 3000
+      - name: limit_burst_current_falldown_time
+        description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
+        default_value: 0
+        field: burstCurrentFalldownTime
+        max: 3000
+      - name: limit_cont_power
+        condition: USE_ADC
+        description: "Continous power limit (dW), set to 0 to disable"
+        default_value: 0
+        field: continuousPower
+        max: 40000
+      - name: limit_burst_power
+        condition: USE_ADC
+        description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
+        default_value: 0
+        field: burstPower
+        max: 40000
+      - name: limit_burst_power_time
+        condition: USE_ADC
+        description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
+        default_value: 0
+        field: burstPowerTime
+        max: 3000
+      - name: limit_burst_power_falldown_time
+        condition: USE_ADC
+        description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
+        default_value: 0
+        field: burstPowerFalldownTime
+        max: 3000
+      - name: limit_pi_p
+        description: "Throttle attenuation PI control P term"
+        default_value: 100
+        field: piP
+        max: 10000
+      - name: limit_pi_i
+        description: "Throttle attenuation PI control I term"
+        default_value: 100
+        field: piI
+        max: 10000
+      - name: limit_attn_filter_cutoff
+        description: "Throttle attenuation PI control output filter cutoff frequency"
+        default_value: 1.2
+        field: attnFilterCutoff
+        max: 100

src/main/flight/failsafe.c

@@ -47,6 +47,7 @@
 #include "flight/pid.h"
 
 #include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
 
 #include "rx/rx.h"
 
@@ -362,12 +363,15 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
 
     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
     // GPS must also be working, and home position set
-    if ((failsafeConfig()->failsafe_min_distance > 0) &&
-        ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) &&
-        sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
+    if (failsafeConfig()->failsafe_min_distance > 0 &&
+            sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 
-        // Use the alternate, minimum distance failsafe procedure instead
-        return failsafeConfig()->failsafe_min_distance_procedure;
+        // get the distance to the original arming point
+        uint32_t distance = calculateDistanceToDestination(&original_rth_home);
+        if (distance < failsafeConfig()->failsafe_min_distance) {
+            // Use the alternate, minimum distance failsafe procedure instead
+            return failsafeConfig()->failsafe_min_distance_procedure;
+        }
     }
 
     return failsafeConfig()->failsafe_procedure;

src/main/flight/mixer.c

@@ -73,6 +73,8 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0;
 static EXTENDED_FASTRAM int throttleRangeMax = 0;
 static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
 
+int motorZeroCommand = 0;
+
 PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
 
 PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
@@ -114,13 +116,12 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
     .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT,            // Most brushless motors that we use are 14 poles
 #ifdef USE_DSHOT
-    .flipOverAfterPowerFactor = SETTING_FLIP_OVER_AFTER_CRASH_POWER_FACTOR_DEFAULT,
+    .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
 #endif
 );
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 
-#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f
 #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
 
 typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
@@ -273,7 +274,6 @@ void mixerInit(void)
 
 void mixerResetDisarmedMotors(void)
 {
-    int motorZeroCommand;
 
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
         motorZeroCommand = reversibleMotorsConfig()->neutral;
@@ -327,10 +327,10 @@ static uint16_t handleOutputScaling(
     }
     return value;
 }
-static void applyFlipOverAfterCrashModeToMotors(void) {
+static void applyTurtleModeToMotors(void) {
 
     if (ARMING_FLAG(ARMED)) {
-        const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
+        const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f;
         const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
         const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
         const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
@@ -393,13 +393,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) {
 
             motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
 
-            float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle;
-
-            // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
-            motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (
-                    motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND);
-
-            motor[i] = motorOutput;
+            motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, motorConfig()->maxthrottle);
         }
     } else {
         // Disarmed mode
@@ -531,8 +525,8 @@ static int getReversibleMotorsThrottleDeadband(void)
 void FAST_CODE mixTable(const float dT)
 {
 #ifdef USE_DSHOT
-    if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
-        applyFlipOverAfterCrashModeToMotors();
+    if (FLIGHT_MODE(TURTLE_MODE)) {
+        applyTurtleModeToMotors();
         return;
     }
 #endif
@@ -720,3 +714,18 @@ void loadPrimaryMotorMixer(void) {
         currentMixer[i] = *primaryMotorMixer(i);
     }
 }
+
+bool areMotorsRunning(void)
+{
+    if (ARMING_FLAG(ARMED)) {
+        return true;
+    } else {
+        for (int i = 0; i < motorCount; i++) {
+            if (motor_disarmed[i] != motorZeroCommand) {
+                return true;
+            }
+        }
+    }
+
+    return false;
+}

src/main/flight/mixer.h

@@ -25,11 +25,6 @@
 #define MAX_SUPPORTED_MOTORS 12
 #endif
 
-#define YAW_JUMP_PREVENTION_LIMIT_LOW 80
-#define YAW_JUMP_PREVENTION_LIMIT_HIGH 500
-
-#define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450
-
 // Digital protocol has fixed values
 #define DSHOT_DISARM_COMMAND      0
 #define DSHOT_MIN_THROTTLE       48
@@ -92,7 +87,7 @@ typedef struct motorConfig_s {
     float throttleIdle;                     // Throttle IDLE value based on min_command, max_throttle, in percent
     float throttleScale;                    // Scaling factor for throttle.
     uint8_t motorPoleCount;                 // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
-    uint8_t flipOverAfterPowerFactor;       // Power factor from 0 to 100% of flip over after crash
+    uint8_t turtleModePowerFactor;          // Power factor from 0 to 100% of flip over after crash
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);
@@ -126,8 +121,11 @@ void mixerUpdateStateFlags(void);
 void mixerResetDisarmedMotors(void);
 void mixTable(const float dT);
 void writeMotors(void);
-void processServoAutotrim(void);
+void processServoAutotrim(const float dT);
+void processServoAutotrimMode(void);
+void processContinuousServoAutotrim(const float dT);
 void stopMotors(void);
 void stopPwmAllMotors(void);
 
 void loadPrimaryMotorMixer(void);
+bool areMotorsRunning(void);

src/main/flight/pid.c

@@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/rpm_filter.h"
 #include "flight/secondary_imu.h"
 #include "flight/kalman.h"
+#include "flight/smith_predictor.h"
 
 #include "io/gps.h"
 
@@ -109,6 +110,8 @@ typedef struct {
     bool itermFreezeActive;
 
     biquadFilter_t rateTargetFilter;
+
+    smithPredictor_t smithPredictor;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -157,9 +160,16 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
-static EXTENDED_FASTRAM float fixedWingLevelTrim;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1);
+#define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
+#define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f
+#define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER
+#define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE
+
+static EXTENDED_FASTRAM float fixedWingLevelTrim;
+static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
+
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -297,6 +307,14 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
 
         .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
+        .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
+        .fixedWingLevelTrimDeadband = SETTING_FW_LEVEL_PITCH_DEADBAND_DEFAULT,
+
+#ifdef USE_SMITH_PREDICTOR
+        .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
+        .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
+        .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT,
+#endif
 );
 
 bool pidInitFilters(void)
@@ -349,6 +367,30 @@ bool pidInitFilters(void)
         }
     }
 
+#ifdef USE_SMITH_PREDICTOR
+    smithPredictorInit(
+        &pidState[FD_ROLL].smithPredictor, 
+        pidProfile()->smithPredictorDelay, 
+        pidProfile()->smithPredictorStrength, 
+        pidProfile()->smithPredictorFilterHz, 
+        getLooptime()
+    );
+    smithPredictorInit(
+        &pidState[FD_PITCH].smithPredictor, 
+        pidProfile()->smithPredictorDelay, 
+        pidProfile()->smithPredictorStrength, 
+        pidProfile()->smithPredictorFilterHz, 
+        getLooptime()
+    );
+    smithPredictorInit(
+        &pidState[FD_YAW].smithPredictor, 
+        pidProfile()->smithPredictorDelay, 
+        pidProfile()->smithPredictorStrength, 
+        pidProfile()->smithPredictorFilterHz, 
+        getLooptime()
+    );
+#endif
+
     pidFiltersConfigured = true;
 
     return true;
@@ -371,6 +413,22 @@ void pidResetErrorAccumulators(void)
     }
 }
 
+void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) 
+{
+    pidState[axis].errorGyroIf -= delta;
+    pidState[axis].errorGyroIfLimit -= delta;
+}
+
+float getTotalRateTarget(void)
+{
+    return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
+}
+
+float getAxisIterm(uint8_t axis) 
+{
+    return pidState[axis].errorGyroIf;
+}
+
 static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
 {
     stick = constrain(stick, -500, 500);
@@ -406,7 +464,7 @@ static float calculateFixedWingTPAFactor(uint16_t throttle)
 
     // tpa_rate is amount of curve TPA applied to PIDs
     // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
-    if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) {
+    if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) {
         if (throttle > getThrottleIdleValue()) {
             // Calculate TPA according to throttle
             tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f);
@@ -548,8 +606,28 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
     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(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
+    if ((axis == FD_PITCH) && STATE(AIRPLANE) && 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);        
+    }
+
+    //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
+    if (axis == FD_PITCH && STATE(AIRPLANE)) {
+        DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10);
+        DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
+        DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
+
+        /* 
+         * fixedWingLevelTrim has opposite sign to rcCommand.
+         * Positive rcCommand means nose should point downwards
+         * Negative rcCommand mean nose should point upwards
+         * This is counter intuitive and a natural way suggests that + should mean UP
+         * This is why fixedWingLevelTrim has opposite sign to rcCommand
+         * Positive fixedWingLevelTrim means nose should point upwards
+         * Negative fixedWingLevelTrim means nose should point downwards
+         */
+        angleTarget -= fixedWingLevelTrim;   
+        DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
+    }
 
         //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
     if (axis == FD_PITCH && STATE(AIRPLANE)) {
@@ -717,14 +795,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
     }
 
+    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
+
 #ifdef USE_AUTOTUNE_FIXED_WING
     if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
-        autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
+        autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit));
     }
 #endif
 
-    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
-
 #ifdef USE_BLACKBOX
     axisPID_P[axis] = newPTerm;
     axisPID_I[axis] = pidState->errorGyroIf;
@@ -1052,6 +1130,13 @@ void FAST_CODE pidController(float dT)
             pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget);
         }
 #endif
+
+#ifdef USE_SMITH_PREDICTOR
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis, pidState[axis].gyroRate);
+        pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate);
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis + 3, pidState[axis].gyroRate);
+#endif
+
         DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate);
     }
 
@@ -1194,6 +1279,16 @@ void pidInit(void)
 #endif
 
     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
+
+    navPidInit(
+        &fixedWingLevelTrimController,
+        0.0f,
+        (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
+        0.0f,
+        0.0f,
+        2.0f
+    );
+
 }
 
 const pidBank_t * pidBank(void) { 
@@ -1202,3 +1297,61 @@ const pidBank_t * pidBank(void) {
 pidBank_t * pidBankMutable(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
+{
+    if (!STATE(AIRPLANE)) {
+        return;
+    }
+
+    static timeUs_t previousUpdateTimeUs;
+    static bool previousArmingState;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+
+    /*
+     * On every ARM reset the controller
+     */
+    if (ARMING_FLAG(ARMED) && !previousArmingState) {
+        navPidReset(&fixedWingLevelTrimController);
+    } 
+
+    /*
+     * On disarm update the default value
+     */
+    if (!ARMING_FLAG(ARMED) && previousArmingState) {
+        pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
+    } 
+
+    /*
+     * Prepare flags for the PID controller
+     */
+    pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
+
+    //Iterm should freeze when pitch stick is deflected
+    if (
+        !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
+        rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) ||
+        rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband) ||
+        (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) ||
+        navigationIsControllingAltitude()
+    ) {
+        flags |= PID_FREEZE_INTEGRATOR;
+    }
+
+    const float output = navPidApply3(
+        &fixedWingLevelTrimController,
+        0,  //Setpoint is always 0 as we try to keep level flight
+        getEstimatedActualVelocity(Z),
+        dT,
+        -FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        flags,
+        1.0f,
+        1.0f
+    );
+
+    DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
+    fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
+
+    previousArmingState = !!ARMING_FLAG(ARMED);
+}

src/main/flight/pid.h

@@ -19,12 +19,13 @@
 
 #include "config/parameter_group.h"
 #include "fc/runtime_config.h"
+#include "common/time.h"
 
 #define GYRO_SATURATION_LIMIT       1800        // 1800dps
 #define PID_SUM_LIMIT_MIN           100
 #define PID_SUM_LIMIT_MAX           1000
 #define PID_SUM_LIMIT_DEFAULT       500
-#define PID_SUM_LIMIT_YAW_DEFAULT   350
+#define PID_SUM_LIMIT_YAW_DEFAULT   400
 
 #define HEADING_HOLD_RATE_LIMIT_MIN 10
 #define HEADING_HOLD_RATE_LIMIT_MAX 250
@@ -54,6 +55,8 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
 
 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
 
+#define FIXED_WING_LEVEL_TRIM_DEADBAND_DEFAULT 5
+
 #define TASK_AUX_RATE_HZ   100 //In Hz
 
 typedef enum {
@@ -161,16 +164,31 @@ typedef struct pidProfile_s {
 #endif
 
     float fixedWingLevelTrim;
+    float fixedWingLevelTrimGain;
+    float fixedWingLevelTrimDeadband;
+#ifdef USE_SMITH_PREDICTOR
+    float smithPredictorStrength;
+    float smithPredictorDelay;
+    uint16_t smithPredictorFilterHz;
+#endif
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {
-    uint16_t    fw_overshoot_time;          // Time [ms] to detect sustained overshoot
-    uint16_t    fw_undershoot_time;         // Time [ms] to detect sustained undershoot
-    uint8_t     fw_max_rate_threshold;      // Threshold [%] of max rate to consider autotune detection
+    uint16_t    fw_detect_time;             // Time [ms] to detect sustained undershoot or overshoot
+    uint8_t     fw_min_stick;               // Minimum stick input required to update rates and gains
     uint8_t     fw_ff_to_p_gain;            // FF to P gain (strength relationship) [%]
+    uint8_t     fw_p_to_d_gain;             // P to D gain (strength relationship) [%]
     uint16_t    fw_ff_to_i_time_constant;   // FF to I time (defines time for I to reach the same level of response as FF) [ms]
+    uint8_t     fw_rate_adjustment;         // Adjust rate settings during autotune?
+    uint8_t     fw_max_rate_deflection;     // Percentage of max mixer output used for calculating the rates
 } pidAutotuneConfig_t;
 
+typedef enum {
+    FIXED,
+    LIMIT,
+    AUTO,
+} fw_autotune_rate_adjustment_e;
+
 PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
 PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig);
 
@@ -183,6 +201,9 @@ extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
 void pidInit(void);
 bool pidInitFilters(void);
 void pidResetErrorAccumulators(void);
+void pidReduceErrorAccumulators(int8_t delta, uint8_t axis);
+float getAxisIterm(uint8_t axis);
+float getTotalRateTarget(void);
 void pidResetTPAFilter(void);
 
 struct controlRateConfig_s;
@@ -210,3 +231,5 @@ void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs);

src/main/flight/pid_autotune.c

@@ -25,6 +25,8 @@
 #include "blackbox/blackbox.h"
 #include "blackbox/blackbox_fielddefs.h"
 
+#include "build/debug.h"
+
 #include "common/axis.h"
 #include "common/maths.h"
 #include "common/utils.h"
@@ -44,36 +46,45 @@
 
 #include "flight/pid.h"
 
-#define AUTOTUNE_FIXED_WING_INTEGRATOR_TC       600
-#define AUTOTUNE_FIXED_WING_DECREASE_STEP       8           // 8%
-#define AUTOTUNE_FIXED_WING_INCREASE_STEP       5           // 5%
 #define AUTOTUNE_FIXED_WING_MIN_FF              10
 #define AUTOTUNE_FIXED_WING_MAX_FF              200
+#define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40
+#define AUTOTUNE_FIXED_WING_MIN_YAW_RATE        10
+#define AUTOTUNE_FIXED_WING_MAX_RATE            720
+#define AUTOTUNE_FIXED_WING_CONVERGENCE_RATE    10
+#define AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL     20      // ms
+#define AUTOTUNE_FIXED_WING_SAMPLES             1000    // Use averagea over the last 20 seconds
+#define AUTOTUNE_FIXED_WING_MIN_SAMPLES         250     // Start updating tune after 5 seconds
 
-PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2);
 
 PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig,
-    .fw_overshoot_time = SETTING_FW_AUTOTUNE_OVERSHOOT_TIME_DEFAULT,
-    .fw_undershoot_time = SETTING_FW_AUTOTUNE_UNDERSHOOT_TIME_DEFAULT,
-    .fw_max_rate_threshold = SETTING_FW_AUTOTUNE_THRESHOLD_DEFAULT,
+    .fw_min_stick = SETTING_FW_AUTOTUNE_MIN_STICK_DEFAULT,
     .fw_ff_to_p_gain = SETTING_FW_AUTOTUNE_FF_TO_P_GAIN_DEFAULT,
     .fw_ff_to_i_time_constant = SETTING_FW_AUTOTUNE_FF_TO_I_TC_DEFAULT,
+    .fw_p_to_d_gain = SETTING_FW_AUTOTUNE_P_TO_D_GAIN_DEFAULT,
+    .fw_rate_adjustment = SETTING_FW_AUTOTUNE_RATE_ADJUSTMENT_DEFAULT,
+    .fw_max_rate_deflection = SETTING_FW_AUTOTUNE_MAX_RATE_DEFLECTION_DEFAULT,
 );
 
 typedef enum {
     DEMAND_TOO_LOW,
     DEMAND_UNDERSHOOT,
     DEMAND_OVERSHOOT,
+    TUNE_UPDATED,
 } pidAutotuneState_e;
 
 typedef struct {
-    pidAutotuneState_e  state;
-    timeMs_t            stateEnterTime;
-
-    bool    pidSaturated;
     float   gainP;
     float   gainI;
+    float   gainD;
     float   gainFF;
+    float   rate;
+    float   initialRate;
+    float   absDesiredRateAccum;
+    float   absReachedRateAccum;
+    float   absPidOutputAccum;
+    uint32_t updateCount;
 } pidAutotuneData_t;
 
 #define AUTOTUNE_SAVE_PERIOD        5000        // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago
@@ -89,8 +100,9 @@ void autotuneUpdateGains(pidAutotuneData_t * data)
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP);
         pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI);
-        pidBankMutable()->pid[axis].D = 0;
+        pidBankMutable()->pid[axis].D = lrintf(data[axis].gainD);
         pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF);
+        ((controlRateConfig_t *)currentControlRateProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f);
     }
     schedulePidGainsUpdate();
 }
@@ -114,10 +126,14 @@ void autotuneStart(void)
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         tuneCurrent[axis].gainP = pidBank()->pid[axis].P;
         tuneCurrent[axis].gainI = pidBank()->pid[axis].I;
+        tuneCurrent[axis].gainD = pidBank()->pid[axis].D;
         tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF;
-        tuneCurrent[axis].pidSaturated = false;
-        tuneCurrent[axis].stateEnterTime = millis();
-        tuneCurrent[axis].state = DEMAND_TOO_LOW;
+        tuneCurrent[axis].rate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
+        tuneCurrent[axis].initialRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
+        tuneCurrent[axis].absDesiredRateAccum = 0;
+        tuneCurrent[axis].absReachedRateAccum = 0;
+        tuneCurrent[axis].absPidOutputAccum = 0;
+        tuneCurrent[axis].updateCount = 0;
     }
 
     memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
@@ -161,73 +177,87 @@ static void blackboxLogAutotuneEvent(adjustmentFunction_e adjustmentFunction, in
 
 #if defined(USE_AUTOTUNE_FIXED_WING)
 
-void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput)
+void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRate, float reachedRate, float pidOutput)
 {
+    float maxRateSetting = tuneCurrent[axis].rate;
+    float gainFF = tuneCurrent[axis].gainFF;
+    float maxDesiredRate = maxRateSetting;
+
+    const float pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit;
+    const float absDesiredRate = fabsf(desiredRate);
+    const float absReachedRate = fabsf(reachedRate);
+    const float absPidOutput = fabsf(pidOutput);
+    const bool correctDirection = (desiredRate>0) == (reachedRate>0);
+    float rateFullStick;
+
+    bool gainsUpdated = false;
+    bool ratesUpdated = false;
+
     const timeMs_t currentTimeMs = millis();
-    const float absDesiredRateDps = fabsf(desiredRateDps);
-    float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
-    pidAutotuneState_e newState;
+    static timeMs_t previousSampleTimeMs = 0;
+    const timeDelta_t timeSincePreviousSample = currentTimeMs - previousSampleTimeMs;
 
-    // Use different max desired rate in ANGLE for pitch and roll
-    // Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet.
-    if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) {
+    // Use different max rate in ANLGE mode
+    if (FLIGHT_MODE(ANGLE_MODE)) {
         float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
-        maxDesiredRate = MIN(maxDesiredRate, maxDesiredRateInAngleMode);
+        maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode);
     }
 
-    if (fabsf(pidOutput) >= pidProfile()->pidSumLimit) {
-        // If we have saturated the pid output by P+FF don't increase the gain
-        tuneCurrent[axis].pidSaturated = true;
-    }
+    const float stickInput = absDesiredRate / maxDesiredRate;
 
-    if (absDesiredRateDps < (pidAutotuneConfig()->fw_max_rate_threshold / 100.0f) * maxDesiredRate) {
-        // We can make decisions only when we are demanding at least 50% of max configured rate
-        newState = DEMAND_TOO_LOW;
-    }
-    else if (fabsf(reachedRateDps) > absDesiredRateDps) {
-        newState = DEMAND_OVERSHOOT;
-    }
-    else {
-        newState = DEMAND_UNDERSHOOT;
-    }
+    if ((stickInput > (pidAutotuneConfig()->fw_min_stick / 100.0f)) && correctDirection && (timeSincePreviousSample >= AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL)) {
+        // Record values every 20ms and calculate moving average over samples
+        tuneCurrent[axis].updateCount++;
+        tuneCurrent[axis].absDesiredRateAccum += (absDesiredRate - tuneCurrent[axis].absDesiredRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
+        tuneCurrent[axis].absReachedRateAccum += (absReachedRate - tuneCurrent[axis].absReachedRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
+        tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);;
 
-    if (newState != tuneCurrent[axis].state) {
-        const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime;
-        bool gainsUpdated = false;
+        if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) {
+            if (pidAutotuneConfig()->fw_rate_adjustment != FIXED  && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode
                 
-        switch (tuneCurrent[axis].state) {
-            case DEMAND_TOO_LOW:
-                break;
-            case DEMAND_OVERSHOOT:
-                if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) {
-                    tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
-                    if (tuneCurrent[axis].gainFF < AUTOTUNE_FIXED_WING_MIN_FF) {
-                        tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MIN_FF;
-                    }
-                    gainsUpdated = true;
+                // Target 80% control surface deflection to leave some room for P and I to work
+                float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit;
+
+                // Theoretically achievable rate with target deflection
+                rateFullStick = pidSumTarget / tuneCurrent[axis].absPidOutputAccum * tuneCurrent[axis].absReachedRateAccum;
+
+                // Rate update
+                if (rateFullStick > (maxRateSetting + 10.0f)) {
+                    maxRateSetting += 10.0f;
+                } else if (rateFullStick < (maxRateSetting - 10.0f)) {
+                    maxRateSetting -= 10.0f;
                 }
-                break;
-            case DEMAND_UNDERSHOOT:
-                if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) {
-                    tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
-                    if (tuneCurrent[axis].gainFF > AUTOTUNE_FIXED_WING_MAX_FF) {
-                        tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MAX_FF;
-                    }
-                    gainsUpdated = true;
-                }
-                break;
+
+                // Constrain to safe values
+                uint16_t minRate = (axis == FD_YAW) ? AUTOTUNE_FIXED_WING_MIN_YAW_RATE : AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE;
+                uint16_t maxRate = (pidAutotuneConfig()->fw_rate_adjustment == AUTO) ? AUTOTUNE_FIXED_WING_MAX_RATE : MAX(tuneCurrent[axis].initialRate, minRate);
+                tuneCurrent[axis].rate = constrainf(maxRateSetting, minRate, maxRate);
+                ratesUpdated = true;
+            }
+
+            // Update FF towards value needed to achieve current rate target
+            gainFF += (tuneCurrent[axis].absPidOutputAccum / tuneCurrent[axis].absReachedRateAccum * FP_PID_RATE_FF_MULTIPLIER - gainFF) * (AUTOTUNE_FIXED_WING_CONVERGENCE_RATE / 100.0f);
+            tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF);
+            gainsUpdated = true;
         }
 
-        if (gainsUpdated) {
-            // Set P-gain to 10% of FF gain (quite agressive - FIXME)
-            tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
+        // Reset timer
+        previousSampleTimeMs = currentTimeMs;
+    }
 
-            // Set integrator gain to reach the same response as FF gain in 0.667 second
-            tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
-            tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
-            autotuneUpdateGains(tuneCurrent);
+    if (gainsUpdated) {
+        // Set P-gain to 10% of FF gain (quite agressive - FIXME)
+        tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
 
-            switch (axis) {
+        // Set D-gain relative to P-gain (0 for now)
+        tuneCurrent[axis].gainD = tuneCurrent[axis].gainP * (pidAutotuneConfig()->fw_p_to_d_gain / 100.0f);
+
+        // Set integrator gain to reach the same response as FF gain in 0.667 second
+        tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
+        tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
+        autotuneUpdateGains(tuneCurrent);
+
+        switch (axis) {
             case FD_ROLL:
                 blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
                 break;
@@ -239,13 +269,30 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
             case FD_YAW:
                 blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
                 break;
-            }
+        }
+        gainsUpdated = false;
+    }
+
+    if (ratesUpdated) {
+        autotuneUpdateGains(tuneCurrent);
+
+        switch (axis) {
+            case FD_ROLL:
+                blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_RATE, tuneCurrent[axis].rate);
+                break;
+
+            case FD_PITCH:
+                blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_RATE, tuneCurrent[axis].rate);
+                break;
+
+            case FD_YAW:
+                blackboxLogAutotuneEvent(ADJUSTMENT_YAW_RATE, tuneCurrent[axis].rate);
+                break;
         }
 
-        // Change state and reset saturation flag
-        tuneCurrent[axis].state = newState;
-        tuneCurrent[axis].stateEnterTime = currentTimeMs;
-        tuneCurrent[axis].pidSaturated = false;
+        // Debug
+        DEBUG_SET(DEBUG_AUTOTUNE, axis, rateFullStick);
+        ratesUpdated = false;
     }
 }
 #endif

src/main/flight/power_limits.c

@@ -0,0 +1,292 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * iNav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with iNav.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "platform.h"
+FILE_COMPILE_FOR_SPEED
+
+#if defined(USE_POWER_LIMITS)
+
+#include "flight/power_limits.h"
+
+#include "build/debug.h"
+
+#include "common/filter.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/time.h"
+
+#include "fc/settings.h"
+
+#include "rx/rx.h"
+
+#include "sensors/battery.h"
+
+#define LIMITING_THR_FILTER_TCONST 50
+
+PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 0);
+
+PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig,
+    .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT,                            // dA
+    .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT,                                // dA
+    .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT,                       // dS
+    .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT,      // dS
+#ifdef USE_ADC
+    .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT,                                // dW
+    .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT,                                    // dW
+    .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT,                           // dS
+    .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT,          // dS
+#endif
+    .piP = SETTING_LIMIT_PI_P_DEFAULT,
+    .piI = SETTING_LIMIT_PI_I_DEFAULT,
+    .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT,                       // Hz
+);
+
+static float burstCurrentReserve;               // cA.µs
+static float burstCurrentReserveMax;            // cA.µs
+static float burstCurrentReserveFalldown;       // cA.µs
+static int32_t activeCurrentLimit;              // cA
+static float currentThrAttnIntegrator = 0;
+static pt1Filter_t currentThrAttnFilter;
+static pt1Filter_t currentThrLimitingBaseFilter;
+static bool wasLimitingCurrent = false;
+
+#ifdef USE_ADC
+static float burstPowerReserve;                 // cW.µs
+static float burstPowerReserveMax;              // cW.µs
+static float burstPowerReserveFalldown;         // cW.µs
+static int32_t activePowerLimit;                // cW
+static float powerThrAttnIntegrator = 0;
+static pt1Filter_t powerThrAttnFilter;
+static pt1Filter_t powerThrLimitingBaseFilter;
+static bool wasLimitingPower = false;
+#endif
+
+void powerLimiterInit(void) {
+    if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) {
+        powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent;
+    }
+
+    activeCurrentLimit = powerLimitsConfig()->burstCurrent;
+
+    uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
+    burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6;
+    burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6;
+
+    pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
+    pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+
+#ifdef USE_ADC
+    if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) {
+        powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower;
+    }
+
+    activePowerLimit = powerLimitsConfig()->burstPower;
+
+    uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
+    burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6;
+    burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6;
+
+    pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
+    pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+#endif
+}
+
+static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, uint32_t burstLimit, float *burstReserve, float burstReserveFalldown, float burstReserveMax, timeDelta_t timeDelta) {
+    int32_t continuousDiff = value - continuousLimit * 10;
+    float spentReserveChunk = continuousDiff * timeDelta;
+    *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax);
+
+    if (powerLimitsConfig()->burstCurrentFalldownTime) {
+        return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10;
+    }
+
+    return (*burstReserve ? burstLimit : continuousLimit) * 10;
+}
+
+void currentLimiterUpdate(timeDelta_t timeDelta) {
+    activeCurrentLimit = calculateActiveLimit(getAmperage(),
+                            powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent,
+                            &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax,
+                            timeDelta);
+}
+
+#ifdef USE_ADC
+void powerLimiterUpdate(timeDelta_t timeDelta) {
+    activePowerLimit = calculateActiveLimit(getPower(),
+                            powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower,
+                            &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax,
+                            timeDelta);
+}
+#endif
+
+void powerLimiterApply(int16_t *throttleCommand) {
+
+#ifdef USE_ADC
+    if (!activeCurrentLimit && !activePowerLimit) {
+        return;
+    }
+#else
+    if (!activeCurrentLimit) {
+        return;
+    }
+#endif
+
+    static timeUs_t lastCallTimestamp = 0;
+    timeUs_t currentTimeUs = micros();
+    timeDelta_t callTimeDelta = cmpTimeUs(currentTimeUs, lastCallTimestamp);
+
+    int16_t throttleBase;
+    int16_t currentThrottleCommand;
+#ifdef USE_ADC
+    int16_t powerThrottleCommand;
+#endif
+
+    int16_t current = getAmperageSample();
+#ifdef USE_ADC
+    uint16_t voltage = getVBatSample();
+    int32_t power = (int32_t)voltage * current / 100;
+#endif
+
+    // Current limiting
+    int32_t overCurrent = current - activeCurrentLimit;
+
+    if (lastCallTimestamp) {
+        currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
+    }
+
+    float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3;
+
+    uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6));
+
+    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
+    uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn);
+
+    if (activeCurrentLimit && currentThrAttned < *throttleCommand) {
+        if (!wasLimitingCurrent && getAmperage() >= activeCurrentLimit) {
+            pt1FilterReset(&currentThrLimitingBaseFilter, *throttleCommand);
+            wasLimitingCurrent = true;
+        }
+
+        currentThrottleCommand = currentThrAttned;
+    } else {
+        wasLimitingCurrent = false;
+        currentThrAttnIntegrator = 0;
+        pt1FilterReset(&currentThrAttnFilter, 0);
+
+        currentThrottleCommand = *throttleCommand;
+    }
+
+#ifdef USE_ADC
+    // Power limiting
+    int32_t overPower = power - activePowerLimit;
+
+    if (lastCallTimestamp) {
+        powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
+    }
+
+    float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1;
+
+    uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6));
+
+    throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
+    uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn);
+
+    if (activePowerLimit && powerThrAttned < *throttleCommand) {
+        if (!wasLimitingPower && getPower() >= activePowerLimit) {
+            pt1FilterReset(&powerThrLimitingBaseFilter, *throttleCommand);
+            wasLimitingPower = true;
+        }
+
+        powerThrottleCommand = powerThrAttned;
+    } else {
+        wasLimitingPower = false;
+        powerThrAttnIntegrator = 0;
+        pt1FilterReset(&powerThrAttnFilter, 0);
+
+        powerThrottleCommand = *throttleCommand;
+    }
+
+    *throttleCommand = MIN(currentThrottleCommand, powerThrottleCommand);
+#else
+    *throttleCommand = currentThrottleCommand;
+#endif
+
+    lastCallTimestamp = currentTimeUs;
+}
+
+bool powerLimiterIsLimiting(void) {
+#ifdef USE_ADC
+    return wasLimitingPower || wasLimitingCurrent;
+#else
+    return wasLimitingCurrent;
+#endif
+}
+
+bool powerLimiterIsLimitingCurrent(void) {
+    return wasLimitingCurrent;
+}
+
+#ifdef USE_ADC
+bool powerLimiterIsLimitingPower(void) {
+    return wasLimitingPower;
+}
+#endif
+
+// returns seconds
+float powerLimiterGetRemainingBurstTime(void) {
+    uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
+    float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
+
+#ifdef USE_ADC
+    uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
+    float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
+
+    if (!powerLimitsConfig()->continuousCurrent) {
+        return remainingPowerBurstTime;
+    }
+
+    if (!powerLimitsConfig()->continuousPower) {
+        return remainingCurrentBurstTime;
+    }
+
+    return MIN(remainingCurrentBurstTime, remainingPowerBurstTime);
+#else
+    return remainingCurrentBurstTime;
+#endif
+}
+
+// returns cA
+uint16_t powerLimiterGetActiveCurrentLimit(void) {
+    return activeCurrentLimit;
+}
+
+#ifdef USE_ADC
+// returns cW
+uint16_t powerLimiterGetActivePowerLimit(void) {
+    return activePowerLimit;
+}
+#endif
+
+#endif

src/main/flight/power_limits.h

@@ -0,0 +1,66 @@
+/*
+ * This file is part of iNav
+ *
+ * iNav 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.
+ *
+ * iNav 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <common/time.h>
+
+#include "config/parameter_group.h"
+
+#if defined(USE_POWER_LIMITS)
+
+typedef struct {
+    uint16_t continuousCurrent;         // dA
+    uint16_t burstCurrent;              // dA
+    uint16_t burstCurrentTime;          // ds
+    uint16_t burstCurrentFalldownTime;  // ds
+
+#ifdef USE_ADC
+    uint16_t continuousPower;           // dW
+    uint16_t burstPower;                // dW
+    uint16_t burstPowerTime;            // ds
+    uint16_t burstPowerFalldownTime;    // ds
+#endif
+
+    uint16_t piP;
+    uint16_t piI;
+
+    float attnFilterCutoff;             // Hz
+} powerLimitsConfig_t;
+
+PG_DECLARE(powerLimitsConfig_t, powerLimitsConfig);
+
+void powerLimiterInit(void);
+void currentLimiterUpdate(timeDelta_t timeDelta);
+void powerLimiterUpdate(timeDelta_t timeDelta);
+void powerLimiterApply(int16_t *throttleCommand);
+bool powerLimiterIsLimiting(void);
+bool powerLimiterIsLimitingCurrent(void);
+float powerLimiterGetRemainingBurstTime(void);      // returns seconds
+uint16_t powerLimiterGetActiveCurrentLimit(void);   // returns cA
+#ifdef USE_ADC
+uint16_t powerLimiterGetActivePowerLimit(void);     // returns cW
+bool powerLimiterIsLimitingPower(void);
+#endif
+
+#endif

src/main/flight/secondary_imu.c

@@ -29,10 +29,13 @@
 #include "common/utils.h"
 #include "common/axis.h"
 
+#include "build/debug.h"
+#include "scheduler/scheduler.h"
 #include "config/parameter_group_ids.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro/accgyro_bno055.h"
+#include "drivers/accgyro/accgyro_bno055_serial.h"
 
 #include "fc/settings.h"
 
@@ -94,6 +97,17 @@ void secondaryImuInit(void)
 
     if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
         secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
+
+        if (secondaryImuState.active) {
+            rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(10));
+        }
+
+    } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
+        secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
+
+        if (secondaryImuState.active) {
+            rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50));
+        }
     }
 
     if (!secondaryImuState.active) {
@@ -102,19 +116,7 @@ void secondaryImuInit(void)
 
 }
 
-void taskSecondaryImu(timeUs_t currentTimeUs)
-{
-    if (!secondaryImuState.active)
-    {
-        return;
-    }
-    /*
-     * Secondary IMU works in decidegrees
-     */
-    UNUSED(currentTimeUs);
-
-    bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
-
+void secondaryImuProcess(void) {
     //Apply mag declination
     secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
 
@@ -141,17 +143,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
     secondaryImuState.eulerAngles.values.pitch = rotated.y;
     secondaryImuState.eulerAngles.values.yaw = rotated.z;
 
-    /*
-     * Every 10 cycles fetch current calibration state
-     */
-    static uint8_t tick = 0;
-    tick++;
-    if (tick == 10)
-    {
-        secondaryImuState.calibrationStatus = bno055GetCalibStat();
-        tick = 0;
-    }
-
     DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll);
     DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch);
     DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw);
@@ -162,8 +153,56 @@ void taskSecondaryImu(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination);
 }
 
+void taskSecondaryImu(timeUs_t currentTimeUs)
+{
+    static uint8_t tick = 0;
+    tick++;
+
+    if (!secondaryImuState.active)
+    {
+        return;
+    }
+    /*
+     * Secondary IMU works in decidegrees
+     */
+    UNUSED(currentTimeUs);
+
+    if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
+        bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
+        secondaryImuProcess();
+
+        /*
+        * Every 2 seconds fetch current calibration state
+        */
+        if (tick == 20)
+        {
+            secondaryImuState.calibrationStatus = bno055GetCalibStat();
+            tick = 0;
+        }
+    } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
+        /*
+         * Every 2 seconds fetch current calibration state
+         */
+        if (tick == 100)
+        {
+            bno055SerialGetCalibStat();
+            tick = 0;
+        } else {
+            bno055SerialFetchEulerAngles();
+        }
+    }
+}
+
 void secondaryImuFetchCalibration(void) {
-    bno055CalibrationData_t calibrationData = bno055GetCalibrationData();
+    bno055CalibrationData_t calibrationData;
+
+    if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
+        calibrationData = bno055GetCalibrationData();
+    } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) {
+        calibrationData = bno055SerialGetCalibrationData();
+    } else {
+        return;
+    }
 
     secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X];
     secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y];

src/main/flight/secondary_imu.h

@@ -28,10 +28,12 @@
 #include "common/time.h"
 #include "sensors/sensors.h"
 #include "drivers/accgyro/accgyro_bno055.h"
+#include "drivers/accgyro/accgyro_bno055_serial.h"
 
 typedef enum {
-    SECONDARY_IMU_NONE      = 0,
-    SECONDARY_IMU_BNO055    = 1,
+    SECONDARY_IMU_NONE          = 0,
+    SECONDARY_IMU_BNO055        = 1,
+    SECONDARY_IMU_BNO055_SERIAL = 2,
 } secondaryImuType_e;
 
 typedef struct secondaryImuConfig_s {
@@ -60,6 +62,7 @@ extern secondaryImuState_t secondaryImuState;
 
 PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
 
+void secondaryImuProcess(void);
 void secondaryImuInit(void);
 void taskSecondaryImu(timeUs_t currentTimeUs);
 void secondaryImuFetchCalibration(void);

src/main/flight/servos.c

@@ -40,6 +40,7 @@
 #include "drivers/time.h"
 
 #include "fc/config.h"
+#include "fc/fc_core.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -51,11 +52,13 @@
 #include "flight/pid.h"
 #include "flight/servos.h"
 
+#include "io/gps.h"
+
 #include "rx/rx.h"
 
 #include "sensors/gyro.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 2);
 
 PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
     .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT,
@@ -63,7 +66,8 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
     .servo_lowpass_freq = SETTING_SERVO_LPF_HZ_DEFAULT,         // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions
     .servo_protocol = SETTING_SERVO_PROTOCOL_DEFAULT,
     .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT,
-    .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT
+    .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT,
+    .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT
 );
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1);
@@ -83,7 +87,7 @@ void pgResetFn_customServoMixers(servoMixer_t *instance)
     }
 }
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2);
+PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 3);
 
 void pgResetFn_servoParams(servoParam_t *instance)
 {
@@ -113,6 +117,9 @@ static bool servoFilterIsSet;
 static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS];
 static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES];
 
+STATIC_FASTRAM pt1Filter_t rotRateFilter;
+STATIC_FASTRAM pt1Filter_t targetRateFilter;
+
 int16_t getFlaperonDirection(uint8_t servoPin)
 {
     if (servoPin == SERVO_FLAPPERON_2) {
@@ -196,7 +203,7 @@ static void filterServos(void)
         // Initialize servo lowpass filter (servos are calculated at looptime rate)
         if (!servoFilterIsSet) {
             for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-                biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, gyro.targetLooptime);
+                biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, getLooptime());
                 biquadFilterReset(&servoFilter[i], servo[i]);
             }
             servoFilterIsSet = true;
@@ -385,27 +392,31 @@ typedef enum {
     AUTOTRIM_DONE,
 } servoAutotrimState_e;
 
-void processServoAutotrim(void)
+void processServoAutotrimMode(void)
 {
     static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
     static timeMs_t trimStartedAt;
 
     static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS];
     static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS];
-    static int32_t servoMiddleAccumCount;
+    static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS];
 
     if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
         switch (trimState) {
             case AUTOTRIM_IDLE:
                 if (ARMING_FLAG(ARMED)) {
-                    // We are activating servo trim - backup current middles and prepare to average the data
-                    for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                        servoMiddleBackup[servoIndex] = servoParams(servoIndex)->middle;
-                        servoMiddleAccum[servoIndex] = 0;
+                    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                        for (int i = 0; i < servoRuleCount; i++) {
+                            const uint8_t target = currentServoMixer[i].targetChannel;
+                            const uint8_t source = currentServoMixer[i].inputSource;
+                            if (source == axis) {
+                                servoMiddleBackup[target] = servoParams(target)->middle;
+                                servoMiddleAccum[target] = 0;
+                                servoMiddleAccumCount[target] = 0;
+                            }
+                        }
                     }
-
                     trimStartedAt = millis();
-                    servoMiddleAccumCount = 0;
                     trimState = AUTOTRIM_COLLECTING;
                 }
                 else {
@@ -415,15 +426,26 @@ void processServoAutotrim(void)
 
             case AUTOTRIM_COLLECTING:
                 if (ARMING_FLAG(ARMED)) {
-                    servoMiddleAccumCount++;
-
-                    for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                        servoMiddleAccum[servoIndex] += servo[servoIndex];
+                    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                        for (int i = 0; i < servoRuleCount; i++) {
+                            const uint8_t target = currentServoMixer[i].targetChannel;
+                            const uint8_t source = currentServoMixer[i].inputSource;
+                            if (source == axis) {
+                                servoMiddleAccum[target] += servo[target];
+                                servoMiddleAccumCount[target]++;
+                            }
+                        }
                     }
 
                     if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) {
-                        for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                            servoParamsMutable(servoIndex)->middle = servoMiddleAccum[servoIndex] / servoMiddleAccumCount;
+                        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                            for (int i = 0; i < servoRuleCount; i++) {
+                                const uint8_t target = currentServoMixer[i].targetChannel;
+                                const uint8_t source = currentServoMixer[i].inputSource;
+                                if (source == axis) {
+                                    servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount[target];
+                                }
+                            }
                         }
                         trimState = AUTOTRIM_SAVE_PENDING;
                         pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors
@@ -449,8 +471,14 @@ void processServoAutotrim(void)
     else {
         // We are deactivating servo trim - restore servo midpoints
         if (trimState == AUTOTRIM_SAVE_PENDING) {
-            for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                servoParamsMutable(servoIndex)->middle = servoMiddleBackup[servoIndex];
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                for (int i = 0; i < servoRuleCount; i++) {
+                    const uint8_t target = currentServoMixer[i].targetChannel;
+                    const uint8_t source = currentServoMixer[i].inputSource;
+                    if (source == axis) {
+                        servoParamsMutable(target)->middle = servoMiddleBackup[target];
+                    }
+                }
             }
         }
 
@@ -458,6 +486,90 @@ void processServoAutotrim(void)
     }
 }
 
+#define SERVO_AUTOTRIM_FILTER_CUTOFF    1       // LPF cutoff frequency
+#define SERVO_AUTOTRIM_CENTER_MIN       1300
+#define SERVO_AUTOTRIM_CENTER_MAX       1700
+#define SERVO_AUTOTRIM_UPDATE_SIZE      5
+
+void processContinuousServoAutotrim(const float dT)
+{
+    static timeMs_t lastUpdateTimeMs;
+    static servoAutotrimState_e trimState = AUTOTRIM_IDLE;    
+    static uint32_t servoMiddleUpdateCount;
+
+    const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
+    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+    const float targetRateMagnitude = getTotalRateTarget();
+    const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+
+    if (ARMING_FLAG(ARMED)) {
+        trimState = AUTOTRIM_COLLECTING;
+        if ((millis() - lastUpdateTimeMs) > 500) {
+            const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
+            const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
+            const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f;
+            if (
+                planeIsFlyingStraight && 
+                noRotationCommanded && 
+                planeIsFlyingLevel &&
+                !FLIGHT_MODE(MANUAL_MODE) && 
+                isGPSHeadingValid() // TODO: proper flying detection
+            ) { 
+                // Plane is flying straight and level: trim servos
+                for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                    // For each stabilized axis, add 5 units of I-term to all associated servo midpoints
+                    const float axisIterm = getAxisIterm(axis);
+                    if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) {
+                        const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE;
+                        for (int i = 0; i < servoRuleCount; i++) {
+#ifdef USE_PROGRAMMING_FRAMEWORK
+                            if (!logicConditionGetValue(currentServoMixer[i].conditionId)) {
+                                continue;
+                            }
+#endif
+                            const uint8_t target = currentServoMixer[i].targetChannel;
+                            const uint8_t source = currentServoMixer[i].inputSource;
+                            if (source == axis) {
+                                // Convert axis I-term to servo PWM and add to midpoint
+                                const float mixerRate = currentServoMixer[i].rate / 100.0f;
+                                const float servoRate = servoParams(target)->rate / 100.0f;
+                                servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate;
+                                servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX);
+                                }
+                        }
+                        pidReduceErrorAccumulators(ItermUpdate, axis);
+                    }
+                }
+                servoMiddleUpdateCount++;
+            }
+            // Reset timer
+            lastUpdateTimeMs = millis();
+        }
+    } else if (trimState == AUTOTRIM_COLLECTING) {
+        // We have disarmed, save midpoints to EEPROM
+        writeEEPROM();
+        trimState = AUTOTRIM_IDLE;
+    }
+
+    // Debug
+    DEBUG_SET(DEBUG_AUTOTRIM, 0, servoParams(2)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 2, servoParams(3)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount);
+    DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered));
+    DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]);
+    DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);    
+}
+
+void processServoAutotrim(const float dT) {
+    if (feature(FEATURE_FW_AUTOTRIM)) {
+        processContinuousServoAutotrim(dT);
+    } else {
+        processServoAutotrimMode();
+    }
+}
+
 bool isServoOutputEnabled(void)
 {
     return servoOutputEnabled;

src/main/flight/servos.h

@@ -138,6 +138,8 @@ typedef struct servoConfig_s {
     uint16_t flaperon_throw_offset;
     uint8_t servo_protocol;                 // See servoProtocolType_e
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
+    uint8_t servo_autotrim_rotation_limit;  // Max rotation for servo midpoints to be updated
+    uint8_t servo_autotrim_iterm_threshold; // How much of the Iterm is carried over to the servo midpoints on each update
 } servoConfig_t;
 
 PG_DECLARE(servoConfig_t, servoConfig);

src/main/flight/smith_predictor.c

@@ -0,0 +1,70 @@
+/*
+ * 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/.
+ * 
+ * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight
+ *  
+ */
+
+#include "platform.h"
+#ifdef USE_SMITH_PREDICTOR
+
+#include <stdbool.h>
+#include "common/axis.h"
+#include "common/utils.h"
+#include "flight/smith_predictor.h"
+#include "build/debug.h"
+
+FUNCTION_COMPILE_FOR_SPEED
+float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) {
+    UNUSED(axis);
+    if (predictor->enabled) {
+        predictor->data[predictor->idx] = sample;
+
+        predictor->idx++;
+        if (predictor->idx > predictor->samples) {
+            predictor->idx = 0;
+        }
+
+        // filter the delayed data to help reduce the overall noise this prediction adds
+        float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]);
+        float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed);
+
+        sample += delayCompensatedSample;
+    }
+    return sample;
+}
+
+FUNCTION_COMPILE_FOR_SIZE
+void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) {
+    if (delay > 0.1) {
+        predictor->enabled = true;
+        predictor->samples = (delay * 1000) / looptime;
+        predictor->idx = 0;
+        predictor->smithPredictorStrength = strength;
+        pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f);
+    } else {
+        predictor->enabled = false;
+    }
+}
+
+#endif

src/main/flight/smith_predictor.h

@@ -0,0 +1,45 @@
+/*
+ * 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/.
+ * 
+ * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight
+ * 
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "common/filter.h"
+
+#define MAX_SMITH_SAMPLES 64
+
+typedef struct smithPredictor_s {
+    bool enabled;
+    uint8_t samples;
+    uint8_t idx;
+    float data[MAX_SMITH_SAMPLES + 1];
+    pt1Filter_t smithPredictorFilter;
+    float smithPredictorStrength;
+} smithPredictor_t;
+
+float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample);
+void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime);

src/main/io/beeper.c

@@ -45,6 +45,13 @@
 
 #include "config/feature.h"
 
+#ifdef USE_DSHOT
+#include "drivers/pwm_output.h"
+#include "fc/fc_core.h"
+#include "flight/mixer.h"
+static timeUs_t lastDshotBeeperCommandTimeUs;
+#endif
+
 #include "io/beeper.h"
 
 #define MAX_MULTI_BEEPS 20   //size limit for 'beep_multiBeeps[]'
@@ -331,6 +338,15 @@ void beeperUpdate(timeUs_t currentTimeUs)
     }
 
     if (!beeperIsOn) {
+#ifdef USE_DSHOT
+        if (isMotorProtocolDshot() && !areMotorsRunning() && beeperConfig()->dshot_beeper_enabled
+            && currentTimeUs - lastDshotBeeperCommandTimeUs > getDShotBeaconGuardDelayUs())
+        {
+            lastDshotBeeperCommandTimeUs = currentTimeUs;
+            sendDShotCommand(beeperConfig()->dshot_beeper_tone);
+        }
+#endif
+
         beeperIsOn = 1;
         if (currentBeeperEntry->sequence[beeperPos] != 0) {
             if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1))))
@@ -407,3 +423,25 @@ int beeperTableEntryCount(void)
 {
     return (int)BEEPER_TABLE_ENTRY_COUNT;
 }
+
+#ifdef USE_DSHOT
+timeUs_t getDShotBeaconGuardDelayUs(void) {
+    // Based on Digital_Cmd_Spec.txt - all delays have 100ms added to ensure that the minimum time has passed.
+    switch (beeperConfig()->dshot_beeper_tone) {
+        case 1:
+        case 2:
+            return 260000 + 100000;
+        case 3:
+        case 4:
+            return 280000 + 100000;
+        case 5:
+        default:
+            return 1020000 + 100000;
+    }
+}
+
+timeUs_t getLastDshotBeeperCommandTimeUs(void)
+{
+    return lastDshotBeeperCommandTimeUs;
+}
+#endif

src/main/io/beeper.h

@@ -60,3 +60,7 @@ uint32_t getArmingBeepTimeMicros(void);
 beeperMode_e beeperModeForTableIndex(int idx);
 const char *beeperNameForTableIndex(int idx);
 int beeperTableEntryCount(void);
+#ifdef USE_DSHOT
+timeUs_t getDShotBeaconGuardDelayUs(void);
+timeUs_t getLastDshotBeeperCommandTimeUs(void);
+#endif

src/main/io/ledstrip.c

@@ -935,7 +935,7 @@ void ledStripUpdate(timeUs_t currentTimeUs)
     for (timId_e timId = 0; timId < timTimerCount; timId++) {
         // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
         // max delay is limited to 5s
-        int32_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]);
+        timeDelta_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]);
         if (delta < 0 && delta > -LED_STRIP_MS(5000))
             continue;  // not ready yet
         timActive |= 1 << timId;

src/main/io/osd.c

@@ -85,10 +85,11 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/power_limits.h"
 #include "flight/rth_estimator.h"
-#include "flight/wind_estimator.h"
 #include "flight/secondary_imu.h"
 #include "flight/servos.h"
+#include "flight/wind_estimator.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -144,7 +145,7 @@ FILE_COMPILE_FOR_SPEED
 #define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2)
 #define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s))
 
-#define OSD_MIN_FONT_VERSION 1
+#define OSD_MIN_FONT_VERSION 2
 
 static unsigned currentLayout = 0;
 static int layoutOverride = -1;
@@ -156,8 +157,8 @@ static float GForce, GForceAxis[XYZ_AXIS_COUNT];
 typedef struct statistic_s {
     uint16_t max_speed;
     uint16_t min_voltage; // /100
-    int16_t max_current; // /100
-    int16_t max_power; // /100
+    int16_t max_current;
+    int32_t max_power;
     int16_t min_rssi;
     int16_t min_lq; // for CRSF
     int16_t min_rssi_dbm; // for CRSF
@@ -193,7 +194,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 2);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
 
 static int digitCount(int32_t value)
@@ -534,18 +535,24 @@ static uint16_t osdConvertRSSI(void)
     return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
 }
 
-static uint16_t osdGetLQ(void)
+static uint16_t osdGetCrsfLQ(void)
 {
     int16_t statsLQ = rxLinkStatistics.uplinkLQ;
     int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
-    if (rxLinkStatistics.rfMode == 2) {
-        return scaledLQ;
-    } else {
-        return statsLQ;
+    int16_t displayedLQ;
+    switch (osdConfig()->crsf_lq_format) {
+        case OSD_CRSF_LQ_TYPE1:
+        case OSD_CRSF_LQ_TYPE2:
+            displayedLQ = statsLQ;
+            break;
+        case OSD_CRSF_LQ_TYPE3:
+            displayedLQ = rxLinkStatistics.rfMode >= 2 ? scaledLQ : statsLQ;
+            break;
     }
+    return displayedLQ;
 }
 
-static uint16_t osdGetdBm(void)
+static int16_t osdGetCrsfdBm(void)
 {
     return rxLinkStatistics.uplinkRSSI;
 }
@@ -737,6 +744,8 @@ static const char * osdArmingDisabledReasonMessage(void)
             return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR);
         case ARMING_DISABLED_NO_PREARM:
             return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM);
+        case ARMING_DISABLED_DSHOT_BEEPER:
+            return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER);
             // Cases without message
         case ARMING_DISABLED_CMS_MENU:
             FALLTHROUGH;
@@ -802,6 +811,15 @@ static const char * osdFailsafeInfoMessage(void)
     }
     return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
 }
+#if defined(USE_SAFE_HOME)
+static const char * divertingToSafehomeMessage(void)
+{
+	if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) {
+	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
+	}
+	return NULL;
+}
+#endif
 
 static const char * navigationStateMessage(void)
 {
@@ -837,6 +855,11 @@ static const char * navigationStateMessage(void)
             return OSD_MESSAGE_STR(OSD_MSG_LANDING);
         case MW_NAV_STATE_HOVER_ABOVE_HOME:
             if (STATE(FIXED_WING_LEGACY)) {
+#if defined(USE_SAFE_HOME)
+                if (safehome_applied) {
+                    return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME);
+                }
+#endif
                 return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME);
             }
             return OSD_MESSAGE_STR(OSD_MSG_HOVERING);
@@ -899,9 +922,15 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t
     if (autoThr && navigationIsControllingThrottle()) {
         buff[0] = SYM_AUTO_THR0;
         buff[1] = SYM_AUTO_THR1;
-        if (isFixedWingAutoThrottleManuallyIncreased())
+        if (isFixedWingAutoThrottleManuallyIncreased()) {
             TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
+        }
     }
+#ifdef USE_POWER_LIMITS
+    if (powerLimiterIsLimiting()) {
+        TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
+    }
+#endif
     tfp_sprintf(buff + 2, "%3d", getThrottlePercent());
 }
 
@@ -1167,6 +1196,67 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale)
     osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale);
 }
 
+static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum)
+{
+    uint8_t tmp;
+    tmp = data ^ (uint8_t)(crcAccum & 0xff);
+    tmp ^= (tmp << 4);
+    crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4);
+    return crcAccum;
+}
+
+
+static void osdDisplayTelemetry(void)
+{
+    uint32_t          trk_data;
+    uint16_t          trk_crc = 0;
+    char              trk_buffer[31];
+    static int16_t    trk_elevation = 127;
+    static uint16_t   trk_bearing   = 0;
+
+    if (ARMING_FLAG(ARMED)) {
+      if (STATE(GPS_FIX)){
+        if (GPS_distanceToHome > 5) {
+          trk_bearing = GPS_directionToHome;
+          trk_bearing += 360 + 180;
+          trk_bearing %= 360;
+          int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude());
+          float at = atan2(alt, GPS_distanceToHome);
+          trk_elevation = (float)at * 57.2957795; // 57.2957795 = 1 rad
+          trk_elevation += 37; // because elevation in telemetry should be from -37 to 90
+          if (trk_elevation < 0) {
+            trk_elevation = 0;
+          }
+        }
+      }
+    }
+    else{
+      trk_elevation = 127;
+      trk_bearing   = 0;
+    }
+
+    trk_data = 0;                                                // bit  0    - packet type 0 = bearing/elevation, 1 = 2 byte data packet
+    trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7  - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127.
+    trk_data = trk_data | (uint32_t)trk_bearing << 8;            // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360
+    trk_crc = crc_accumulate(0xFF & trk_data, trk_crc);          // CRC First Byte  bits 0-7
+    trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc);       // CRC Second Byte bits 8-15
+    trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc);         // CRC Third Byte  bits  16-17
+    trk_data = trk_data | (uint32_t)trk_crc << 17;               // bits 18-29 CRC & 0x3FFFF
+
+    for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) {               // Prepare screen buffer and write data line.
+      if (trk_data & (uint32_t)1 << t_ctr){
+        trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0;
+      }
+      else{
+        trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1;
+      }
+    }
+    trk_buffer[30] = 0;
+    displayWrite(osdDisplayPort, 0, 0, trk_buffer);
+    if (osdConfig()->telemetry>1){
+      displayWrite(osdDisplayPort, 0, 3, trk_buffer);               // Test display because normal telemetry line is not visible
+    }
+}
 #endif
 
 static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
@@ -1628,7 +1718,7 @@ static bool osdDrawSingleElement(uint8_t item)
             /*static int32_t updatedTimeSeconds = 0;*/
             timeUs_t currentTimeUs = micros();
             static int32_t timeSeconds = -1;
-            if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+            if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) {
 #ifdef USE_WIND_ESTIMATOR
                 timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
 #else
@@ -1659,7 +1749,7 @@ static bool osdDrawSingleElement(uint8_t item)
         static timeUs_t updatedTimestamp = 0;
         timeUs_t currentTimeUs = micros();
         static int32_t distanceMeters = -1;
-        if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+        if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) {
 #ifdef USE_WIND_ESTIMATOR
             distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
 #else
@@ -1693,8 +1783,10 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "!FS!";
             else if (FLIGHT_MODE(MANUAL_MODE))
                 p = "MANU";
+            else if (FLIGHT_MODE(TURTLE_MODE))
+                p = "TURT";
             else if (FLIGHT_MODE(NAV_RTH_MODE))
-                p = "RTH ";
+                p = isWaypointMissionRTHActive() ? "WRTH" : "RTH ";
             else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
                 p = "HOLD";
             else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
@@ -1713,8 +1805,6 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
-            else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH))
-                p = "TURT";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
             return true;
@@ -1759,38 +1849,33 @@ static bool osdDrawSingleElement(uint8_t item)
 #if defined(USE_SERIALRX_CRSF)
     case OSD_CRSF_RSSI_DBM:
         {
-            if (rxLinkStatistics.activeAnt == 0) {
-              buff[0] = SYM_RSSI;
-              tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM);
-              if (!failsafeIsReceivingRxData()){
-                  TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-              }
+            int16_t rssi = rxLinkStatistics.uplinkRSSI;
+            buff[0] = (rxLinkStatistics.activeAnt == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna
+            if (rssi <= -100) {
+                tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM);
             } else {
-              buff[0] = SYM_2RSS;
-              tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM);
-              if (!failsafeIsReceivingRxData()){
-                  TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-              }
+                tfp_sprintf(buff + 1, "%3d%c%c", rssi, SYM_DBM, ' ');
+            }
+            if (!failsafeIsReceivingRxData()){
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
             break;
         }
     case OSD_CRSF_LQ:
         {
-            buff[0] = SYM_BLANK;
+            buff[0] = SYM_LQ;
             int16_t statsLQ = rxLinkStatistics.uplinkLQ;
             int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
-            if (rxLinkStatistics.rfMode == 2) {
-                if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
-                    tfp_sprintf(buff, "%5d%s", scaledLQ, "%");
-                } else {
-                    tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
-                }
-            } else {
-                if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
-                    tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%");
-                } else {
-                    tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
-                }
+            switch (osdConfig()->crsf_lq_format) {
+                case OSD_CRSF_LQ_TYPE1:
+                    tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
+                    break;
+                case OSD_CRSF_LQ_TYPE2:
+                    tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ);
+                    break;
+                case OSD_CRSF_LQ_TYPE3:
+                    tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ);
+                    break;
             }
             if (!failsafeIsReceivingRxData()){
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@@ -1802,10 +1887,14 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_CRSF_SNR_DB:
         {
+            static pt1Filter_t snrFilterState;
+            static timeMs_t snrUpdated = 0;
+            int8_t snrFiltered = pt1FilterApply4(&snrFilterState, rxLinkStatistics.uplinkSNR, 0.5f, MS2S(millis() - snrUpdated));
+            snrUpdated = millis();
+
             const char* showsnr = "-20";
             const char* hidesnr = "     ";
-            int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR;
-            if (osdSNR_Alarm > osdConfig()->snr_alarm) {
+            if (snrFiltered > osdConfig()->snr_alarm) {
                 if (cmsInMenu) {
                     buff[0] = SYM_SNR;
                     tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB);
@@ -1813,9 +1902,13 @@ static bool osdDrawSingleElement(uint8_t item)
                     buff[0] = SYM_BLANK;
                     tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK);
                 }
-            } else if (osdSNR_Alarm <= osdConfig()->snr_alarm) {
+            } else if (snrFiltered <= osdConfig()->snr_alarm) {
                 buff[0] = SYM_SNR;
-                tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB);
+                if (snrFiltered <= -10) {
+                    tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB);
+                } else {
+                    tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' ');
+                }
             }
             break;
         }
@@ -1881,14 +1974,18 @@ static bool osdDrawSingleElement(uint8_t item)
 
                 for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
                     j = posControl.activeWaypointIndex + i;
-                    if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {
+                    if (j > posControl.waypointCount - 1) { // limit to max WP index for mission
+                        break;
+                    }
+                    if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) {
                         wp2.lat = posControl.waypointList[j].lat;
                         wp2.lon = posControl.waypointList[j].lon;
                         wp2.alt = posControl.waypointList[j].alt;
                         fpVector3_t poi;
-                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
+                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3));
+                        int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude();
                         while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
-                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
+                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i);
                     }
                 }
             }
@@ -2158,8 +2255,8 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_POWER:
         {
-            osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3);
-            buff[3] = SYM_WATT;
+            bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3);
+            buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
             buff[4] = '\0';
 
             uint8_t current_alarm = osdConfig()->current_alarm;
@@ -2186,7 +2283,7 @@ static bool osdDrawSingleElement(uint8_t item)
             dateTime_t dateTime;
             rtcGetDateTimeLocal(&dateTime);
             buff[0] = SYM_CLOCK;
-            tfp_sprintf(buff + 1, "%02u:%02u", dateTime.hours, dateTime.minutes);
+            tfp_sprintf(buff + 1, "%02u:%02u:%02u", dateTime.hours, dateTime.minutes, dateTime.seconds);
             break;
         }
 
@@ -2241,26 +2338,52 @@ static bool osdDrawSingleElement(uint8_t item)
             static pt1Filter_t eFilterState;
             static timeUs_t efficiencyUpdated = 0;
             int32_t value = 0;
+            bool moreThanAh = false;
             timeUs_t currentTimeUs = micros();
             timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
             if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                     value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
-                        1, efficiencyTimeDelta * 1e-6f);
+                        1, US2S(efficiencyTimeDelta));
 
                     efficiencyUpdated = currentTimeUs;
                 } else {
                     value = eFilterState.state;
                 }
             }
-            if (value > 0 && gpsSol.groundSpeed > 100) {
-                tfp_sprintf(buff, "%3d", (int)value);
-            } else {
-                buff[0] = buff[1] = buff[2] = '-';
+            bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100);
+            switch (osdConfig()->units) {
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_IMPERIAL:
+                    moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, 3);
+                    if (!moreThanAh) {
+                        tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1);
+                    } else {
+                        tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI);
+                    }
+                    if (!efficiencyValid) {
+                        buff[0] = buff[1] = buff[2] = '-';
+                        buff[3] = SYM_MAH_MI_0;
+                        buff[4] = SYM_MAH_MI_1;
+                        buff[5] = '\0';
+                    }
+                    break;
+                case OSD_UNIT_METRIC:
+                    moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, 3);
+                    if (!moreThanAh) {
+                        tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1);
+                    } else {
+                        tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM);
+                    }
+                    if (!efficiencyValid) {
+                        buff[0] = buff[1] = buff[2] = '-';
+                        buff[3] = SYM_MAH_KM_0;
+                        buff[4] = SYM_MAH_KM_1;
+                        buff[5] = '\0';
+                    }
+                    break;
             }
-            buff[3] = SYM_MAH_KM_0;
-            buff[4] = SYM_MAH_KM_1;
-            buff[5] = '\0';
             break;
         }
 
@@ -2276,21 +2399,30 @@ static bool osdDrawSingleElement(uint8_t item)
             if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                     value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
-                        1, efficiencyTimeDelta * 1e-6f);
+                        1, US2S(efficiencyTimeDelta));
 
                     efficiencyUpdated = currentTimeUs;
                 } else {
                     value = eFilterState.state;
                 }
             }
-            if (value > 0 && gpsSol.groundSpeed > 100) {
-                osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3);
-            } else {
+            bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100);
+            switch (osdConfig()->units) {
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_IMPERIAL:
+                    osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3);
+                    buff[3] = SYM_WH_MI;
+                    break;
+                case OSD_UNIT_METRIC:
+                    osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3);
+                    buff[3] = SYM_WH_KM;
+                    break;
+            }
+            buff[4] = '\0';
+            if (!efficiencyValid) {
                 buff[0] = buff[1] = buff[2] = '-';
             }
-            buff[3] = SYM_WH_KM_0;
-            buff[4] = SYM_WH_KM_1;
-            buff[5] = '\0';
             break;
         }
 
@@ -2584,9 +2716,47 @@ static bool osdDrawSingleElement(uint8_t item)
 
             return true;
         }
+
     case OSD_NAV_FW_CONTROL_SMOOTHNESS:
         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS);
         return true;
+
+#ifdef USE_POWER_LIMITS
+    case OSD_PLIMIT_REMAINING_BURST_TIME:
+        osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3);
+        buff[3] = 'S';
+        buff[4] = '\0';
+        break;
+
+    case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT:
+        if (powerLimitsConfig()->continuousCurrent) {
+            osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3);
+            buff[3] = SYM_AMP;
+            buff[4] = '\0';
+
+            if (powerLimiterIsLimitingCurrent()) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
+        }
+        break;
+
+#ifdef USE_ADC
+    case OSD_PLIMIT_ACTIVE_POWER_LIMIT:
+        {
+            if (powerLimitsConfig()->continuousPower) {
+                bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3);
+                buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
+                buff[4] = '\0';
+
+                if (powerLimiterIsLimitingPower()) {
+                    TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+                }
+            }
+            break;
+        }
+#endif // USE_ADC
+#endif // USE_POWER_LIMITS
+
     default:
         return false;
     }
@@ -2661,6 +2831,12 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
         }
     }
 
+#ifndef USE_POWER_LIMITS
+    if (elementIndex == OSD_NAV_FW_CONTROL_SMOOTHNESS) {
+        elementIndex = OSD_ITEM_COUNT;
+    }
+#endif
+
     if (elementIndex == OSD_ITEM_COUNT) {
         elementIndex = 0;
     }
@@ -2676,8 +2852,11 @@ void osdDrawNextElement(void)
         elementIndex = osdIncElementIndex(elementIndex);
     } while(!osdDrawSingleElement(elementIndex) && index != elementIndex);
 
-    // Draw artificial horizon last
+    // Draw artificial horizon + tracking telemtry last
     osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
+    if (osdConfig()->telemetry>0){
+      osdDisplayTelemetry();
+    }
 }
 
 PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
@@ -2732,6 +2911,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT,
     .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT,
     .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT,
+    .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT,
     .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
     .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
     .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
@@ -2758,7 +2938,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
 
     .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT,
 
-    .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT
+    .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT,
+    .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT
 );
 
 void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
@@ -2902,6 +3083,12 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4);
 #endif
 
+#ifdef USE_POWER_LIMITS
+    osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4);
+    osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_CURRENT_LIMIT] = OSD_POS(3, 5);
+    osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_POWER_LIMIT] = OSD_POS(3, 6);
+#endif
+
     // Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
     osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
 
@@ -3004,12 +3191,17 @@ static void osdCompleteAsyncInitialization(void)
             displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:");
             if (statsConfig()->stats_total_dist) {
                 uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km
-                osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3);
-            } else
-                strcpy(string_buffer, "---");
-            string_buffer[3] = SYM_WH_KM_0;
-            string_buffer[4] = SYM_WH_KM_1;
-            string_buffer[5] = '\0';
+                if (osdConfig()->units == OSD_UNIT_IMPERIAL) {
+                    osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3);
+                    string_buffer[3] = SYM_WH_MI;
+                } else {
+                    osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3);
+                    string_buffer[3] = SYM_WH_KM;
+                }
+            } else {
+                string_buffer[0] = string_buffer[1] = string_buffer[2] = '-';
+            }
+            string_buffer[4] = '\0';
             displayWrite(osdDisplayPort, STATS_VALUE_X_POS-3, y,  string_buffer);
         }
 #endif // USE_ADC
@@ -3052,7 +3244,7 @@ static void osdResetStats(void)
 
 static void osdUpdateStats(void)
 {
-    int16_t value;
+    int32_t value;
 
     if (feature(FEATURE_GPS)) {
         value = osdGet3DSpeed();
@@ -3067,11 +3259,11 @@ static void osdUpdateStats(void)
     if (stats.min_voltage > value)
         stats.min_voltage = value;
 
-    value = abs(getAmperage() / 100);
+    value = abs(getAmperage());
     if (stats.max_current < value)
         stats.max_current = value;
 
-    value = abs(getPower() / 100);
+    value = labs(getPower());
     if (stats.max_power < value)
         stats.max_power = value;
 
@@ -3079,11 +3271,11 @@ static void osdUpdateStats(void)
     if (stats.min_rssi > value)
         stats.min_rssi = value;
 
-    value = osdGetLQ();
+    value = osdGetCrsfLQ();
     if (stats.min_lq > value)
         stats.min_lq = value;
 
-    value = osdGetdBm();
+    value = osdGetCrsfdBm();
     if (stats.min_rssi_dbm > value)
         stats.min_rssi_dbm = value;
 
@@ -3102,7 +3294,7 @@ static void osdShowStatsPage1(void)
     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
     displayClearScreen(osdDisplayPort);
 
-    displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---      1/2");
+    displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---      1/2 ->");
 
     if (feature(FEATURE_GPS)) {
         displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED        :");
@@ -3123,22 +3315,24 @@ static void osdShowStatsPage1(void)
     osdFormatAltitudeStr(buff, stats.max_altitude);
     displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
-#if defined(USE_SERIALRX_CRSF)
-    displayWrite(osdDisplayPort, statNameX, top, "MIN LQ           :");
-    itoa(stats.min_lq, buff, 10);
-    strcat(buff, "%");
-    displayWrite(osdDisplayPort, statValuesX, top++, buff);
+    switch (rxConfig()->serialrx_provider) {
+        case SERIALRX_CRSF:
+            displayWrite(osdDisplayPort, statNameX, top, "MIN LQ           :");
+            itoa(stats.min_lq, buff, 10);
+            strcat(buff, "%");
+            displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
-    displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI         :");
-    itoa(stats.min_rssi_dbm, buff, 10);
-    tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
-    displayWrite(osdDisplayPort, statValuesX, top++, buff);
-#else
-    displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI         :");
-    itoa(stats.min_rssi, buff, 10);
-    strcat(buff, "%");
-    displayWrite(osdDisplayPort, statValuesX, top++, buff);
-#endif
+            displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI         :");
+            itoa(stats.min_rssi_dbm, buff, 10);
+            tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
+            displayWrite(osdDisplayPort, statValuesX, top++, buff);
+            break;
+        default:
+            displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI         :");
+            itoa(stats.min_rssi, buff, 10);
+            strcat(buff, "%");
+            displayWrite(osdDisplayPort, statValuesX, top++, buff);
+        }
 
     displayWrite(osdDisplayPort, statNameX, top, "FLY TIME         :");
     uint16_t flySeconds = getFlightTime();
@@ -3165,59 +3359,93 @@ static void osdShowStatsPage2(void)
     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
     displayClearScreen(osdDisplayPort);
 
-    displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---      2/2");
+    displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---   <- 2/2");
 
-    displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
-    osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
+    if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) {
+        displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
+        osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
+    } else {
+        displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :");
+        osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3);
+    }
     tfp_sprintf(buff, "%s%c", buff, SYM_VOLT);
     displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
     if (feature(FEATURE_CURRENT_METER)) {
         displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT      :");
-        itoa(stats.max_current, buff, 10);
+        osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3);
         tfp_sprintf(buff, "%s%c", buff, SYM_AMP);
         displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
         displayWrite(osdDisplayPort, statNameX, top, "MAX POWER        :");
-        itoa(stats.max_power, buff, 10);
-        tfp_sprintf(buff, "%s%c", buff, SYM_WATT);
+        bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3);
+        buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
+        buff[4] = '\0';
         displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
+        displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY    :");
         if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
-            displayWrite(osdDisplayPort, statNameX, top, "USED MAH         :");
             tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH);
         } else {
-            displayWrite(osdDisplayPort, statNameX, top, "USED WH          :");
             osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3);
             tfp_sprintf(buff, "%s%c", buff, SYM_WH);
         }
         displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
         int32_t totalDistance = getTotalTravelDistance();
+        bool moreThanAh = false;
+        bool efficiencyValid = totalDistance >= 10000;
         if (feature(FEATURE_GPS)) {
             displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY   :");
-            if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH)
-                tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance),
-                    SYM_MAH_KM_0, SYM_MAH_KM_1);
-            else {
-                osdFormatCentiNumber(buff, getMWhDrawn() * 10000 / totalDistance, 0, 2, 0, 3);
-                buff[3] = SYM_WH_KM_0;
-                buff[4] = SYM_WH_KM_1;
-                buff[5] = '\0';
-            }
-            // If traveled distance is less than 100 meters efficiency numbers are useless and unreliable so display --- instead
-            if (totalDistance < 10000) {
-                buff[0] = buff[1] = buff[2] = '-';
-                if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH){
-                    buff[3] = SYM_MAH_KM_0;
-                    buff[4] = SYM_MAH_KM_1;
-                    buff[5] = '\0';
-                } else {
-                    buff[3] = SYM_WH_KM_0;
-                    buff[4] = SYM_WH_KM_1;
-                    buff[5] = '\0';
-                }
+            switch (osdConfig()->units) {
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_IMPERIAL:
+                    if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
+                        moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, 3);
+                        if (!moreThanAh) {
+                            tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1);
+                        } else {
+                            tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI);
+                        }
+                        if (!efficiencyValid) {
+                            buff[0] = buff[1] = buff[2] = '-';
+                            buff[3] = SYM_MAH_MI_0;
+                            buff[4] = SYM_MAH_MI_1;
+                            buff[5] = '\0';
+                        }
+                    } else {
+                        osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, 3);
+                        tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI);
+                        if (!efficiencyValid) {
+                            buff[0] = buff[1] = buff[2] = '-';
+                        }
+                    }
+                    break;
+                case OSD_UNIT_METRIC:
+                    if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
+                        moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3);
+                        if (!moreThanAh) {
+                            tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1);
+                        } else {
+                            tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM);
+                        }
+                        if (!efficiencyValid) {
+                            buff[0] = buff[1] = buff[2] = '-';
+                            buff[3] = SYM_MAH_KM_0;
+                            buff[4] = SYM_MAH_KM_1;
+                            buff[5] = '\0';
+                        }
+                    } else {
+                        osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, 3);
+                        tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM);
+                        if (!efficiencyValid) {
+                            buff[0] = buff[1] = buff[2] = '-';
+                        }
+                    }
+                    break;
             }
+            osdLeftAlignString(buff);
             displayWrite(osdDisplayPort, statValuesX, top++, buff);
         }
     }
@@ -3278,13 +3506,17 @@ static void osdShowArmed(void)
             }
             y += 4;
 #if defined (USE_SAFE_HOME)
-            if (isSafeHomeInUse()) {
-                textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
-                char buf2[12]; // format the distance first
-                osdFormatDistanceStr(buf2, safehome_distance);
-                tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_used);
-                // write this message above the ARMED message to make it obvious
-                displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
+            if (safehome_distance) { // safehome found during arming
+                if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
+                    strcpy(buf, "SAFEHOME FOUND; MODE OFF");
+				} else {
+					char buf2[12]; // format the distance first
+					osdFormatDistanceStr(buf2, safehome_distance);
+					tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index);
+				}
+				textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
+				// write this message above the ARMED message to make it obvious
+				displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
             }
 #endif
         } else {
@@ -3310,7 +3542,7 @@ static void osdShowArmed(void)
 
 static void osdFilterData(timeUs_t currentTimeUs) {
     static timeUs_t lastRefresh = 0;
-    float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6;
+    float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
 
     GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
     for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
@@ -3353,7 +3585,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
             statsPagesCheck = 0;
 #if defined(USE_SAFE_HOME)
-            if (isSafeHomeInUse())
+            if (safehome_distance)
                 delay *= 3;
 #endif
             osdSetNextRefreshIn(delay);
@@ -3555,6 +3787,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 const char *failsafePhaseMessage = osdFailsafePhaseMessage();
                 const char *failsafeInfoMessage = osdFailsafeInfoMessage();
                 const char *navStateFSMessage = navigationStateMessage();
+
                 if (failsafePhaseMessage) {
                     messages[messageCount++] = failsafePhaseMessage;
                 }
@@ -3564,6 +3797,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 if (navStateFSMessage) {
                     messages[messageCount++] = navStateFSMessage;
                 }
+#if defined(USE_SAFE_HOME)
+                const char *safehomeMessage = divertingToSafehomeMessage();
+				if (safehomeMessage) {
+					messages[messageCount++] = safehomeMessage;
+				}
+#endif
                 if (messageCount > 0) {
                     message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
                     if (message == failsafeInfoMessage) {
@@ -3580,9 +3819,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 }
             } else {
                 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
+                    if (isWaypointMissionRTHActive()) {
+                        // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
+                    }
+
                     if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
                         // Countdown display for remaining Waypoints
-                        tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount);
+                        char buf[6];
+                        osdFormatDistanceSymbol(buf, posControl.wpDistance, 0);
+                        tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", posControl.activeWaypointIndex + 1, posControl.waypointCount, buf);
                         messages[messageCount++] = messageBuf;
                     } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
                         // WP hold time countdown in seconds
@@ -3598,6 +3844,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = navStateMessage;
                         }
                     }
+#if defined(USE_SAFE_HOME)
+					const char *safehomeMessage = divertingToSafehomeMessage();
+					if (safehomeMessage) {
+						messages[messageCount++] = safehomeMessage;
+					}
+#endif
                 } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
                         const char *launchStateMessage = fixedWingLaunchStateMessage();

src/main/io/osd.h

@@ -77,6 +77,7 @@
 #define OSD_MSG_CLI_ACTIVE          "CLI IS ACTIVE"
 #define OSD_MSG_PWM_INIT_ERROR      "PWM INIT ERROR"
 #define OSD_MSG_NO_PREARM           "NO PREARM"
+#define OSD_MSG_DSHOT_BEEPER        "MOTOR BEEPER ACTIVE"
 #define OSD_MSG_RTH_FS              "(RTH)"
 #define OSD_MSG_EMERG_LANDING_FS    "(EMERGENCY LANDING)"
 #define OSD_MSG_MOVE_EXIT_FS        "!MOVE STICKS TO EXIT FS!"
@@ -86,6 +87,7 @@
 #define OSD_MSG_HOLDING_WAYPOINT    "HOLDING WAYPOINT"
 #define OSD_MSG_TO_WP               "TO WP"
 #define OSD_MSG_PREPARE_NEXT_WP     "PREPARING FOR NEXT WAYPOINT"
+#define OSD_MSG_WP_RTH_CANCEL       "CANCEL WP TO EXIT RTH"
 #define OSD_MSG_EMERG_LANDING       "EMERGENCY LANDING"
 #define OSD_MSG_LANDING             "LANDING"
 #define OSD_MSG_LOITERING_HOME      "LOITERING AROUND HOME"
@@ -99,6 +101,11 @@
 #define OSD_MSG_HEADFREE            "(HEADFREE)"
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"
 
+#if defined(USE_SAFE_HOME)
+#define OSD_MSG_DIVERT_SAFEHOME     "DIVERTING TO SAFEHOME"
+#define OSD_MSG_LOITERING_SAFEHOME  "LOITERING AROUND SAFEHOME"
+#endif
+
 typedef enum {
     OSD_RSSI_VALUE,
     OSD_MAIN_BATT_VOLTAGE,
@@ -221,6 +228,9 @@ typedef enum {
     OSD_NAV_FW_CONTROL_SMOOTHNESS,
     OSD_VERSION,
     OSD_RANGEFINDER,
+    OSD_PLIMIT_REMAINING_BURST_TIME,
+    OSD_PLIMIT_ACTIVE_CURRENT_LIMIT,
+    OSD_PLIMIT_ACTIVE_POWER_LIMIT,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -237,6 +247,11 @@ typedef enum {
     OSD_STATS_ENERGY_UNIT_WH,
 } osd_stats_energy_unit_e;
 
+typedef enum {
+    OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY,
+    OSD_STATS_MIN_VOLTAGE_UNIT_CELL,
+} osd_stats_min_voltage_unit_e;
+
 typedef enum {
     OSD_CROSSHAIRS_STYLE_DEFAULT,
     OSD_CROSSHAIRS_STYLE_AIRCRAFT,
@@ -250,7 +265,7 @@ typedef enum {
 typedef enum {
     OSD_SIDEBAR_SCROLL_NONE,
     OSD_SIDEBAR_SCROLL_ALTITUDE,
-    OSD_SIDEBAR_SCROLL_GROUND_SPEED,
+    OSD_SIDEBAR_SCROLL_SPEED,
     OSD_SIDEBAR_SCROLL_HOME_DISTANCE,
 
     OSD_SIDEBAR_SCROLL_MAX = OSD_SIDEBAR_SCROLL_HOME_DISTANCE,
@@ -269,6 +284,7 @@ typedef enum {
 typedef enum {
     OSD_CRSF_LQ_TYPE1,
     OSD_CRSF_LQ_TYPE2,
+    OSD_CRSF_LQ_TYPE3
 } osd_crsf_lq_format_e;
 
 typedef struct osdLayoutsConfig_s {
@@ -333,6 +349,7 @@ typedef struct osdConfig_s {
 
     uint8_t units; // from osd_unit_e
     uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
+    uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e
 
 #ifdef USE_WIND_ESTIMATOR
     bool    estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
@@ -356,6 +373,8 @@ typedef struct osdConfig_s {
     uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
     int8_t pan_servo_pwm2centideg;      // Centidegrees of servo rotation per us pwm
     uint8_t crsf_lq_format;
+    uint8_t sidebar_height;             // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
+    uint8_t telemetry; 				    // use telemetry on displayed pixel line 0
 
 } osdConfig_t;
 

src/main/io/osd_canvas.c

@@ -510,20 +510,23 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll)
                     return osdGetAltitude();
             }
             break;
-        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+        case OSD_SIDEBAR_SCROLL_SPEED:
+            {
 #if defined(USE_GPS)
-            switch ((osd_unit_e)osdConfig()->units) {
-                case OSD_UNIT_UK:
-                    FALLTHROUGH;
-                case OSD_UNIT_IMPERIAL:
-                    // cms/s to (mi/h) * 100
-                    return gpsSol.groundSpeed * 224 / 100;
-                case OSD_UNIT_METRIC:
-                    // cm/s to (km/h) * 100
-                    return gpsSol.groundSpeed * 36 / 10;
-            }
+                int speed = osdGetSpeedFromSelectedSource();
+                switch ((osd_unit_e)osdConfig()->units) {
+                    case OSD_UNIT_UK:
+                        FALLTHROUGH;
+                    case OSD_UNIT_IMPERIAL:
+                        // cms/s to (mi/h) * 100
+                        return speed * 224 / 100;
+                    case OSD_UNIT_METRIC:
+                        // cm/s to (km/h) * 100
+                        return speed * 36 / 10;
+                }
 #endif
-            break;
+                break;
+            }
         case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
 #if defined(USE_GPS)
             switch ((osd_unit_e)osdConfig()->units) {
@@ -547,7 +550,7 @@ static uint8_t osdCanvasSidebarGetOptions(int *width, osd_sidebar_scroll_e scrol
             break;
         case OSD_SIDEBAR_SCROLL_ALTITUDE:
             FALLTHROUGH;
-        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+        case OSD_SIDEBAR_SCROLL_SPEED:
             FALLTHROUGH;
         case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
             *width = OSD_CHAR_WIDTH * 5; // 4 numbers + unit
@@ -588,7 +591,7 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os
                     break;
             }
             break;
-        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+        case OSD_SIDEBAR_SCROLL_SPEED:
             switch ((osd_unit_e)osdConfig()->units) {
                 case OSD_UNIT_UK:
                     FALLTHROUGH;

src/main/io/osd_common.c

@@ -28,15 +28,50 @@
 
 #include "common/utils.h"
 
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "drivers/display.h"
 #include "drivers/display_canvas.h"
 #include "drivers/osd.h"
 
+#include "fc/settings.h"
+
 #include "io/osd_canvas.h"
 #include "io/osd_common.h"
 #include "io/osd_grid.h"
 
 #include "navigation/navigation.h"
+#include "sensors/pitotmeter.h"
+
+
+#if defined(USE_OSD) || defined(USE_DJI_HD_OSD)
+
+PG_REGISTER_WITH_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig, PG_OSD_COMMON_CONFIG, 0);
+
+PG_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig,
+    .speedSource = SETTING_OSD_SPEED_SOURCE_DEFAULT
+);
+
+int osdGetSpeedFromSelectedSource(void) {
+    int speed = 0;
+    switch (osdCommonConfig()->speedSource) {
+        case OSD_SPEED_SOURCE_GROUND:
+            speed = gpsSol.groundSpeed;
+            break;
+        case OSD_SPEED_SOURCE_3D:
+            speed = osdGet3DSpeed();
+            break;
+        case OSD_SPEED_SOURCE_AIR:
+            #ifdef USE_PITOT
+            speed = pitot.airSpeed;
+            #endif
+            break;
+    }
+    return speed;
+}
+
+#endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD)
 
 #if defined(USE_OSD)
 

src/main/io/osd_common.h

@@ -29,6 +29,26 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "config/parameter_group.h"
+
+#if defined(USE_OSD) || defined(USE_DJI_HD_OSD)
+
+typedef enum {
+    OSD_SPEED_SOURCE_GROUND    = 0,
+    OSD_SPEED_SOURCE_3D        = 1,
+    OSD_SPEED_SOURCE_AIR       = 2
+} osdSpeedSource_e;
+
+typedef struct {
+    osdSpeedSource_e speedSource;
+} osdCommonConfig_t;
+
+PG_DECLARE(osdCommonConfig_t, osdCommonConfig);
+
+int osdGetSpeedFromSelectedSource(void);
+
+#endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD)
+
 #define OSD_VARIO_CM_S_PER_ARROW 50 // 1 arrow = 50cm/s
 #define OSD_VARIO_HEIGHT_ROWS 5
 
@@ -86,4 +106,3 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas);
 #ifdef USE_GPS
 int16_t osdGet3DSpeed(void);
 #endif
-

src/main/io/osd_dji_hd.c

@@ -125,7 +125,6 @@ PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
     .use_name_for_messages  = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT,
     .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT,
     .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT,
-    .speedSource = SETTING_DJI_SPEED_SOURCE_DEFAULT,
 );
 
 // External dependency on looptime
@@ -505,6 +504,8 @@ static const char * osdArmingDisabledReasonMessage(void)
             return OSD_MESSAGE_STR("PWM ERR");
         case ARMING_DISABLED_NO_PREARM:
             return OSD_MESSAGE_STR("NO PREARM");
+        case ARMING_DISABLED_DSHOT_BEEPER:
+            return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
             // Cases without message
         case ARMING_DISABLED_CMS_MENU:
             FALLTHROUGH;
@@ -722,7 +723,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
     if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
         if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
             value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
-                1, efficiencyTimeDelta * 1e-6f);
+                1, US2S(efficiencyTimeDelta));
 
             efficiencyUpdated = currentTimeUs;
         }
@@ -1002,21 +1003,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             sbufWriteU32(dst, gpsSol.llh.lat);
             sbufWriteU32(dst, gpsSol.llh.lon);
             sbufWriteU16(dst, gpsSol.llh.alt / 100);
-
-            int reportedSpeed = 0;
-            if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_GROUND) {
-                reportedSpeed = gpsSol.groundSpeed;
-            } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_3D) {
-                reportedSpeed = osdGet3DSpeed();
-            } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_AIR) {
-            #ifdef USE_PITOT
-                reportedSpeed = pitot.airSpeed;
-            #else
-                reportedSpeed = 0;
-            #endif
-            }
-
-            sbufWriteU16(dst, reportedSpeed);
+            sbufWriteU16(dst, osdGetSpeedFromSelectedSource());
             sbufWriteU16(dst, gpsSol.groundCourse);
             break;
 
@@ -1186,7 +1173,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             break;
 
         case DJI_MSP_FILTER_CONFIG:
-            sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);           // BF: gyroConfig()->gyro_lowpass_hz
+            sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz);           // BF: gyroConfig()->gyro_lowpass_hz
             sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);              // BF: currentPidProfile->dterm_lowpass_hz
             sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);                // BF: currentPidProfile->yaw_lowpass_hz
             sbufWriteU16(dst, gyroConfig()->gyro_notch_hz);             // BF: gyroConfig()->gyro_soft_notch_hz_1
@@ -1198,8 +1185,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             sbufWriteU8(dst, 0);                                        // BF: currentPidProfile->dterm_filter_type
             sbufWriteU8(dst, gyroConfig()->gyro_lpf);                   // BF: gyroConfig()->gyro_hardware_lpf);
             sbufWriteU8(dst, 0);                                        // BF: DEPRECATED: gyro_32khz_hardware_lpf
-            sbufWriteU16(dst, gyroConfig()->gyro_soft_lpf_hz);          // BF: gyroConfig()->gyro_lowpass_hz);
-            sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);    // BF: gyroConfig()->gyro_lowpass2_hz);
+            sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz);          // BF: gyroConfig()->gyro_lowpass_hz);
+            sbufWriteU16(dst, 0);                                       // BF: gyroConfig()->gyro_lowpass2_hz);
             sbufWriteU8(dst, 0);                                        // BF: gyroConfig()->gyro_lowpass_type);
             sbufWriteU8(dst, 0);                                        // BF: gyroConfig()->gyro_lowpass2_type);
             sbufWriteU16(dst, 0);                                       // BF: currentPidProfile->dterm_lowpass2_hz);

src/main/io/osd_dji_hd.h

@@ -68,12 +68,6 @@ enum djiOsdTempSource_e {
     DJI_OSD_TEMP_BARO   = 2
 };
 
-enum djiOsdSpeedSource_e {
-    DJI_OSD_SPEED_GROUND    = 0,
-    DJI_OSD_SPEED_3D        = 1,
-    DJI_OSD_SPEED_AIR       = 2
-};
-
 enum djiOsdProtoWorkarounds_e {
     DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA    = 1 << 0,
 };
@@ -82,7 +76,6 @@ typedef struct djiOsdConfig_s {
     uint8_t use_name_for_messages;
     uint8_t esc_temperature_source;
     uint8_t proto_workarounds;
-    uint8_t speedSource;
 } djiOsdConfig_t;
 
 PG_DECLARE(djiOsdConfig_t, djiOsdConfig);

src/main/io/osd_grid.c

@@ -237,7 +237,7 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
             offset = osdGetAltitude();
             steps = offset / 20;
             break;
-        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+        case OSD_SIDEBAR_SCROLL_SPEED:
 #if defined(USE_GPS)
             offset = gpsSol.groundSpeed;
 #endif
@@ -295,7 +295,7 @@ void osdGridDrawSidebars(displayPort_t *display)
     uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
 
     const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS;
-    const int hudheight = OSD_AH_SIDEBAR_HEIGHT_POS;
+    const int hudheight = osdConfig()->sidebar_height;
 
     // Arrows
     if (osdConfig()->sidebar_scroll_arrows) {
@@ -315,11 +315,12 @@ void osdGridDrawSidebars(displayPort_t *display)
     // Draw AH sides
     int leftX = MAX(elemPosX - hudwidth - osdConfig()->sidebar_horizontal_offset, 0);
     int rightX = MIN(elemPosX + hudwidth + osdConfig()->sidebar_horizontal_offset, display->cols - 1);
-    for (int y = -hudheight; y <= hudheight; y++) {
-        displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
-        displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
+    if (osdConfig()->sidebar_height) {
+        for (int y = -hudheight; y <= hudheight; y++) {
+            displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
+            displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
+        }
     }
-
     // AH level indicators
     displayWriteChar(display, leftX + 1, elemPosY, SYM_AH_RIGHT);
     displayWriteChar(display, rightX - 1, elemPosY, SYM_AH_LEFT);

src/main/io/serial.c

@@ -543,6 +543,8 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
         if (serialRxBytesWaiting(left)) {
             LED0_ON;
             uint8_t c = serialRead(left);
+            // Make sure there is space in the tx buffer
+            while (!serialTxBytesFree(right));
             serialWrite(right, c);
             leftC(c);
             LED0_OFF;
@@ -550,6 +552,8 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
          if (serialRxBytesWaiting(right)) {
              LED0_ON;
              uint8_t c = serialRead(right);
+             // Make sure there is space in the tx buffer
+             while (!serialTxBytesFree(left));
              serialWrite(left, c);
              rightC(c);
              LED0_OFF;

src/main/io/serial.h

@@ -44,7 +44,7 @@ typedef enum {
     FUNCTION_RCDEVICE                   = (1 << 10), // 1024
     FUNCTION_VTX_SMARTAUDIO             = (1 << 11), // 2048
     FUNCTION_VTX_TRAMP                  = (1 << 12), // 4096
-    FUNCTION_UNUSED_1                   = (1 << 13), // 8192: former UAV_INTERCONNECT
+    FUNCTION_UNUSED_1                   = (1 << 13), // 8192: former\ UAV_INTERCONNECT
     FUNCTION_OPTICAL_FLOW               = (1 << 14), // 16384
     FUNCTION_LOG                        = (1 << 15), // 32768
     FUNCTION_RANGEFINDER                = (1 << 16), // 65536
@@ -55,6 +55,7 @@ typedef enum {
     FUNCTION_DJI_HD_OSD                 = (1 << 21), // 2097152
     FUNCTION_SERVO_SERIAL               = (1 << 22), // 4194304
     FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
+    FUNCTION_IMU2                     = (1 << 24), // 16777216
 } serialPortFunction_e;
 
 typedef enum {

src/main/io/serial_4way.c

@@ -427,11 +427,11 @@ void esc4wayProcess(serialPort_t *mspPort)
     port = mspPort;
 
     // Start here  with UART Main loop
-    #ifdef BEEPER
+#if defined(BEEPER) || defined(USE_DSHOT)
     // fix for buzzer often starts beeping continuously when the ESCs are read
     // switch beeper silent here
     beeperSilence();
-    #endif
+#endif
     bool isExitScheduled = false;
 
     while (1) {

src/main/io/smartport_master.c

@@ -550,7 +550,7 @@ void smartportMasterHandle(timeUs_t currentTimeUs)
         return;
     }
 
-    if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) {
+    if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > MS2US(SMARTPORT_POLLING_INTERVAL))) {
         if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one
             smartportMasterForwardNextPayload();
         } else {

src/main/navigation/navigation.c

@@ -74,10 +74,14 @@ gpsLocation_t GPS_home;
 uint32_t      GPS_distanceToHome;        // distance to home point in meters
 int16_t       GPS_directionToHome;       // direction to home point in degrees
 
+fpVector3_t   original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
+
 radar_pois_t radar_pois[RADAR_MAX_POIS];
 #if defined(USE_SAFE_HOME)
-int8_t safehome_used;                     // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
-uint32_t safehome_distance;               // distance to the selected safehome
+int8_t safehome_index = -1;               // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+uint32_t safehome_distance = 0;           // distance to the nearest safehome
+fpVector3_t nearestSafeHome;              // The nearestSafeHome found during arming
+bool safehome_applied = false;            // whether the safehome has been applied to home.
 
 PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
 
@@ -92,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -107,20 +111,24 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
             .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT,
             .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT,
             .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT,
+            .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
             .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
+            .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
         },
 
         // General navigation parameters
         .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT,                  // 5 sec
         .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT,                             // 2m diameter
         .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT,               // centimeters - first waypoint should be closer than this
+        .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT,               // load waypoints automatically during boot
         .max_auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT,                             // 3 m/s = 10.8 km/h
         .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT,                   // 5 m/s
         .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
         .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
-        .land_descent_rate = SETTING_NAV_LANDING_SPEED_DEFAULT,                       // centimeters/s
         .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT,             // altitude in centimeters
         .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT,             // altitude in meters
+        .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT,                     // centimeters/s
+        .land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT,                     // centimeters/s
         .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT,                // centimeters/s
         .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT,                     // centimeters, if closer than this land immediately
         .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT,                             // altitude in centimeters
@@ -128,7 +136,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .rth_abort_threshold = SETTING_NAV_RTH_ABORT_THRESHOLD_DEFAULT,               // centimeters - 500m should be safe for all aircraft
         .max_terrain_follow_altitude = SETTING_NAV_MAX_TERRAIN_FOLLOW_ALT_DEFAULT,    // max altitude in centimeters in terrain following mode
         .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT,               // Max distance that a safehome is from the arming point
-        },
+        .max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT,
+    },
 
     // MC-specific
     .mc = {
@@ -229,11 +238,15 @@ 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);
 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
-static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
+static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
 static navigationFSMEvent_t nextForNonGeoStates(void);
+static bool isWaypointMissionValid(void);
 
 void initializeRTHSanityChecker(const fpVector3_t * pos);
 bool validateRTHSanityChecker(void);
+void updateHomePosition(void);
+
+static bool rthAltControlStickOverrideCheck(unsigned axis);
 
 /*************************************************************************************************/
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
@@ -263,7 +276,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
-static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState);
@@ -655,11 +667,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .mwState = MW_NAV_STATE_PROCESS_NEXT,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_WAYPOINT_PRE_ACTION,   // re-process the state (for JUMP)
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_IN_PROGRESS,
-            [NAV_FSM_EVENT_ERROR]                          = NAV_STATE_IDLE,
-            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
-            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED]    = NAV_STATE_WAYPOINT_FINISHED,
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_PRE_ACTION,   // re-process the state (for JUMP)
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_IN_PROGRESS,
+            [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
         }
     },
 
@@ -693,19 +706,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .mwState = MW_NAV_STATE_PROCESS_NEXT,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_WAYPOINT_REACHED,   // re-process state
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_NEXT,
-            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME]   = NAV_STATE_WAYPOINT_HOLD_TIME,
-            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED]    = NAV_STATE_WAYPOINT_FINISHED,
-            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND]    = NAV_STATE_WAYPOINT_RTH_LAND,
-            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME,
-            [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_RTH]                  = NAV_STATE_RTH_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_REACHED,   // re-process state
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_NEXT,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
+            [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_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]       = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]            = NAV_STATE_CRUISE_INITIALIZE,
         }
     },
 
@@ -784,27 +796,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         }
     },
 
-    [NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME] = {
-        .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME,
-        .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME,
-        .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
-        .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
-        .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
-        .mwError = MW_NAV_ERROR_NONE,
-        .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME,   // re-process state
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_WAYPOINT_FINISHED,
-            [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_RTH]                  = NAV_STATE_RTH_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
-        }
-    },
-
     /** EMERGENCY LANDING ************************************************/
     [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
@@ -1191,6 +1182,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
 {
     UNUSED(previousState);
 
+    rthAltControlStickOverrideCheck(PITCH);
+
     if ((posControl.flags.estHeadingStatus == EST_NONE)) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
@@ -1206,7 +1199,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
         const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
 
         // If we reached desired initial RTH altitude or we don't want to climb first
-        if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF)) {
+        if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) {
 
             // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
             if (STATE(FIXED_WING_LEGACY)) {
@@ -1270,6 +1263,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
 {
     UNUSED(previousState);
 
+    rthAltControlStickOverrideCheck(PITCH);
+
     if ((posControl.flags.estHeadingStatus == EST_NONE)) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
@@ -1317,30 +1312,21 @@ 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()) {
-
-        // Wait until target heading is reached (with 15 deg margin for error)
-        if (STATE(FIXED_WING_LEGACY)) {
+        // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
+        if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
             resetLandingDetector();
             updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
-            return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
+            return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
+        }
+        else if (!validateRTHSanityChecker()) {
+            // Continue to check for RTH sanity during pre-landing hover
+            return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
         }
         else {
-            if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
-                resetLandingDetector();
-                updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
-                return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
-            }
-            else if (!validateRTHSanityChecker()) {
-                // Continue to check for RTH sanity during pre-landing hover
-                return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
-            }
-            else {
-                fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
-                setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
-                return NAV_FSM_EVENT_NONE;
-            }
+            fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
+            setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+            return NAV_FSM_EVENT_NONE;
         }
-
     } else {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
@@ -1400,22 +1386,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
         if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
             // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
             // Do not allow descent velocity slower than -30cm/s so the landing detector works.
-            descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
+            descentVelLimited = navConfig()->general.land_minalt_vspd;
         }
         else {
-            fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
 
             // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
-            float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt)
-                / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f;  // Yield 1.0 at 2000 alt and 0.25 at 500 alt
+            float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
+                                                    navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
+                                                    navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
 
-            descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
+            descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
 
-            // Do not allow descent velocity slower than -50cm/s so the landing detector works.
-            descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
         }
 
-        updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
+        updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
 
         return NAV_FSM_EVENT_NONE;
     }
@@ -1439,7 +1423,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
     UNUSED(previousState);
 
     if (STATE(ALTITUDE_CONTROL)) {
-        updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL);  // FIXME
+        updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL);  // FIXME
     }
 
     // Prevent I-terms growing when already landed
@@ -1467,6 +1451,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
 */
         setupJumpCounters();
         posControl.activeWaypointIndex = 0;
+        wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
         return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
     }
 }
@@ -1519,7 +1504,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
             if (STATE(MULTIROTOR)) {
                 wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
                 mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
-                                           &posControl.waypointList[posControl.activeWaypointIndex]);
+                                           &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
             }
             return nextForNonGeoStates();
 
@@ -1536,10 +1521,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
             return nextForNonGeoStates();
 
         case NAV_WP_ACTION_RTH:
-            posControl.rthState.rthInitialDistance = posControl.homeDistance;
-            initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
-            calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
-            return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
+            return NAV_FSM_EVENT_SWITCH_TO_RTH;
     };
 
     UNREACHABLE();
@@ -1585,21 +1567,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
             case NAV_WP_ACTION_JUMP:
             case NAV_WP_ACTION_SET_HEAD:
             case NAV_WP_ACTION_SET_POI:
-                 UNREACHABLE();
-
             case NAV_WP_ACTION_RTH:
-                if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
-                    return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
-                }
-                else {
-                    if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY))
-                        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
-                    else
-                        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
-                    setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z);
-                    return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
-                }
-                break;
+                UNREACHABLE();
         }
     }
     /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
@@ -1624,15 +1593,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
         case NAV_WP_ACTION_JUMP:
         case NAV_WP_ACTION_SET_HEAD:
         case NAV_WP_ACTION_SET_POI:
-            UNREACHABLE();
-
         case NAV_WP_ACTION_RTH:
-            if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
-                return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
-            }
-            else {
-                return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME;
-            }
+            UNREACHABLE();
 
         case NAV_WP_ACTION_LAND:
             return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@@ -1712,14 +1674,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
     }
 }
 
-static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
-{
-    UNUSED(previousState);
-
-    const navigationFSMEvent_t hoverEvent = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(previousState);
-    return hoverEvent;
-}
-
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
 {
     // TODO:
@@ -1923,6 +1877,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
             break;
 
         case RTH_HOME_FINAL_LAND:
+            // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
+            if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
+                posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100;   // 100 -> m to cm
+                if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
+                    posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt;  // correct to relative if absolute SL altitude datum used
+                }
+            }
             break;
     }
 
@@ -2343,26 +2304,42 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 
 #if defined(USE_SAFE_HOME)
 
-/*******************************************************
- * Is a safehome being used instead of the arming point?
- *******************************************************/
-bool isSafeHomeInUse(void)
+void checkSafeHomeState(bool shouldBeEnabled)
 {
-    return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
+	if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
+		shouldBeEnabled = false;
+	} else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
+		// if safehomes are only used with failsafe and we're trying to enable safehome
+		// then enable the safehome only with failsafe
+		shouldBeEnabled = posControl.flags.forcedRTHActivated;
+	}
+    // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
+	if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
+		return;
+	}
+    if (shouldBeEnabled) {
+		// set home to safehome
+        setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+		safehome_applied = true;
+	} else {
+		// set home to original arming point
+        setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+		safehome_applied = false;
+	}
+	// if we've changed the home position, update the distance and direction
+    updateHomePosition();
 }
 
 /***********************************************************
  *  See if there are any safehomes near where we are arming.
- *  If so, use it instead of the arming point for home.
- *  Select the nearest safehome
+ *  If so, save the nearest one in case we need it later for RTH.
  **********************************************************/
-bool foundNearbySafeHome(void)
+bool findNearestSafeHome(void)
 {
-    safehome_used = -1;
+    safehome_index = -1;
     uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
     uint32_t distance_to_current;
     fpVector3_t currentSafeHome;
-    fpVector3_t nearestSafeHome;
     gpsLocation_t shLLH;
     shLLH.alt = 0;
     for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
@@ -2375,20 +2352,19 @@ bool foundNearbySafeHome(void)
         distance_to_current = calculateDistanceToDestination(&currentSafeHome);
         if (distance_to_current < nearest_safehome_distance) {
              // this safehome is the nearest so far - keep track of it.
-             safehome_used = i;
+             safehome_index = i;
              nearest_safehome_distance = distance_to_current;
              nearestSafeHome.x = currentSafeHome.x;
              nearestSafeHome.y = currentSafeHome.y;
              nearestSafeHome.z = currentSafeHome.z;
         }
     }
-    if (safehome_used >= 0) {
+    if (safehome_index >= 0) {
 		safehome_distance = nearest_safehome_distance;
-        setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-        return true;
+    } else {
+        safehome_distance = 0;
     }
-    safehome_distance = 0;
-    return false;
+    return safehome_distance > 0;
 }
 #endif
 
@@ -2414,9 +2390,13 @@ void updateHomePosition(void)
             }
             if (setHome) {
 #if defined(USE_SAFE_HOME)
-                if (!foundNearbySafeHome())
+                findNearestSafeHome();
 #endif
-                    setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+                setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+                // save the current location in case it is replaced by a safehome or HOME_RESET
+                original_rth_home.x = posControl.rthState.homePosition.pos.x;
+                original_rth_home.y = posControl.rthState.homePosition.pos.y;
+                original_rth_home.z = posControl.rthState.homePosition.pos.z;
             }
         }
     }
@@ -2445,6 +2425,39 @@ void updateHomePosition(void)
     }
 }
 
+/* -----------------------------------------------------------
+ * Override RTH preset altitude and Climb First option
+ * using Pitch/Roll stick held for > 1 seconds
+ * Climb First override limited to Fixed Wing only
+ *-----------------------------------------------------------*/
+static bool rthAltControlStickOverrideCheck(unsigned axis)
+{
+    if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR))) {
+        return false;
+    }
+    static timeMs_t rthOverrideStickHoldStartTime[2];
+
+    if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
+        timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis];
+
+        if (!rthOverrideStickHoldStartTime[axis]) {
+            rthOverrideStickHoldStartTime[axis] = millis();
+        } else if (ABS(1500 - holdTime) < 500) {    // 1s delay to activate, activation duration limited to 1 sec
+            if (axis == PITCH) {           // PITCH down to override preset altitude reset to current altitude
+                posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
+                posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
+                return true;
+            } else if (axis == ROLL) {     // ROLL right to override climb first
+                return true;
+            }
+        }
+    } else {
+        rthOverrideStickHoldStartTime[axis] = 0;
+    }
+
+    return false;
+}
+
 /*-----------------------------------------------------------
  * Update flight statistics
  *-----------------------------------------------------------*/
@@ -2565,6 +2578,18 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
         posControl.desiredState.pos.z = altitudeToUse;
     }
     else {
+
+        /* 
+         * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
+         * In other words, when altitude is reached, allow it only to shrink
+         */
+        if (navConfig()->general.max_altitude > 0 && 
+            altitudeToUse >= navConfig()->general.max_altitude &&
+            desiredClimbRate > 0
+        ) {
+            desiredClimbRate = 0;
+        }
+
         if (STATE(FIXED_WING_LEGACY)) {
             // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
             float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
@@ -2692,8 +2717,8 @@ static bool adjustPositionFromRCInput(void)
     }
     else {
 
-        const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
-        const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
+        const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500);
+        const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
 
         retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
     }
@@ -2844,6 +2869,12 @@ bool loadNonVolatileWaypointList(void)
     if (ARMING_FLAG(ARMED))
         return false;
 
+    // if waypoints are already loaded, just unload them.
+    if (posControl.waypointCount > 0) {
+        resetWaypointList();
+        return false;
+    }
+
     resetWaypointList();
 
     for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
@@ -2886,7 +2917,7 @@ void resetSafeHomes(void)
 }
 #endif
 
-static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint)
+static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
 {
     gpsLocation_t wpLLH;
 
@@ -2894,7 +2925,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
     wpLLH.lon = waypoint->lon;
     wpLLH.alt = waypoint->alt;
 
-    geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
+    geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
 }
 
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
@@ -2908,10 +2939,15 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
     setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 }
 
+geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
+{
+    return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
+}
+
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
 {
     fpVector3_t localPos;
-    mapWaypointToLocalPosition(&localPos, waypoint);
+    mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
     calculateAndSetActiveWaypointToLocalPosition(&localPos);
 }
 
@@ -3125,6 +3161,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         const bool canActivatePosHold    = canActivatePosHoldMode();
         const bool canActivateNavigation = canActivateNavigationModes();
         const bool isExecutingRTH        = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
+        checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
 
         // Keep canActivateWaypoint flag at FALSE if there is no mission loaded
         // Also block WP mission if we are executing RTH
@@ -3156,15 +3193,16 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             }
         }
 
-        // RTH/Failsafe_RTH can override MANUAL
+        // Failsafe_RTH (can override MANUAL)
         if (posControl.flags.forcedRTHActivated) {
             // If we request forced RTH - attempt to activate it no matter what
             // This might switch to emergency landing controller if GPS is unavailable
             return NAV_FSM_EVENT_SWITCH_TO_RTH;
         }
 
-        // Pilot-triggered RTH, also fall-back for WP if there is no mission loaded
-        if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint)) {
+        // Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded
+        // Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
+        if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !IS_RC_MODE_ACTIVE(BOXMANUAL))) {
             // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
             // If don't keep this, loss of any of the canActivatePosHold && canActivateNavigation && canActivateAltHold
             // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
@@ -3195,7 +3233,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
                 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
         }
 
-        // PH has priority over CRUISE
+        // CRUISE has priority over COURSE_HOLD and AH
+        if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
+            if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
+                return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
+        }
+
+        // PH has priority over COURSE_HOLD
         // CRUISE has priority on AH
         if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
 
@@ -3285,8 +3329,8 @@ bool navigationTerrainFollowingEnabled(void)
 
 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
 {
-    const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD));
-    const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD));
+    const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || 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(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
 
     if (usedBypass) {
         *usedBypass = false;
@@ -3316,7 +3360,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
     // Don't allow arming if first waypoint is farther than configured safe distance
     if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
         fpVector3_t startingWaypointPos;
-        mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
+        mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
 
         const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
 
@@ -3539,6 +3583,11 @@ void navigationInit(void)
     } else {
         DISABLE_STATE(FW_HEADING_USE_YAW);
     }
+
+#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
+    if (navConfig()->general.waypoint_load_on_boot)
+        loadNonVolatileWaypointList();
+#endif
 }
 
 /*-----------------------------------------------------------
@@ -3561,6 +3610,7 @@ void activateForcedRTH(void)
 {
     abortFixedWingLaunch();
     posControl.flags.forcedRTHActivated = true;
+    checkSafeHomeState(true);
     navProcessFSMEvents(selectNavEventFromBoxModeInput());
 }
 
@@ -3569,6 +3619,7 @@ void abortForcedRTH(void)
     // Disable failsafe RTH and make sure we back out of navigation mode to IDLE
     // If any navigation mode was active prior to RTH it will be re-enabled with next RX update
     posControl.flags.forcedRTHActivated = false;
+    checkSafeHomeState(false);
     navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
 }
 
@@ -3588,6 +3639,11 @@ rthState_e getStateOfForcedRTH(void)
     }
 }
 
+bool isWaypointMissionRTHActive(void)
+{
+    return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated);
+}
+
 bool navigationIsExecutingAnEmergencyLanding(void)
 {
     return navGetCurrentStateFlags() & NAV_CTL_EMERG;
@@ -3605,6 +3661,11 @@ bool navigationIsControllingThrottle(void)
     return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER);
 }
 
+bool navigationIsControllingAltitude(void) {
+    navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
+    return (stateFlags & NAV_CTL_ALT);
+}
+
 bool navigationIsFlyingAutonomousMode(void)
 {
     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
@@ -3613,9 +3674,12 @@ bool navigationIsFlyingAutonomousMode(void)
 
 bool navigationRTHAllowsLanding(void)
 {
-    if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)
-        return true;
+    // WP mission RTH landing setting
+    if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
+        return posControl.waypointList[posControl.waypointCount - 1].p1 > 0;
+    }
 
+    // normal RTH landing setting
     navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
     return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
         (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));

src/main/navigation/navigation.h

@@ -35,6 +35,7 @@
 extern gpsLocation_t        GPS_home;
 extern uint32_t             GPS_distanceToHome;        // distance to home point in meters
 extern int16_t              GPS_directionToHome;       // direction to home point in degrees
+extern fpVector3_t          original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
 
 extern bool autoThrottleManuallyIncreased;
 
@@ -50,14 +51,20 @@ typedef struct {
     int32_t lon;
 } navSafeHome_t;
 
+typedef enum {
+    SAFEHOME_USAGE_OFF = 0,    // Don't use safehomes
+    SAFEHOME_USAGE_RTH = 1,    // Default - use safehome for RTH
+    SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only
+} safehomeUsageMode_e;
+
 PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
 
-extern int8_t safehome_used;                     // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
-extern uint32_t safehome_distance;               // distance to the selected safehome
+extern int8_t safehome_index;                    // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+extern uint32_t safehome_distance;               // distance to the nearest safehome
+extern bool safehome_applied;                    // whether the safehome has been applied to home.
 
 void resetSafeHomes(void);                       // remove all safehomes
-bool isSafeHomeInUse(void);                      // Are we using a safehome instead of the arming point?
-bool foundNearbySafeHome(void);                  // Did we find a safehome nearby?
+bool findNearestSafeHome(void);                  // Find nearest safehome
 
 #endif // defined(USE_SAFE_HOME)
 
@@ -189,17 +196,21 @@ typedef struct navConfig_s {
             uint8_t disarm_on_landing;          //
             uint8_t rth_allow_landing;          // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e.
             uint8_t rth_climb_ignore_emerg;     // Option to ignore GPS loss on initial climb stage of RTH
+            uint8_t rth_alt_control_override;   // Override RTH Altitude and Climb First settings using Pitch and Roll stick
             uint8_t nav_overrides_motor_stop;   // Autonomous modes override motor_stop setting and user command to stop motor
+            uint8_t safehome_usage_mode;        // Controls when safehomes are used
         } flags;
 
         uint8_t  pos_failure_timeout;           // Time to wait before switching to emergency landing (0 - disable)
         uint16_t waypoint_radius;               // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
         uint16_t waypoint_safe_distance;        // Waypoint mission sanity check distance
+        bool     waypoint_load_on_boot;         // load waypoints automatically during boot
         uint16_t max_auto_speed;                // autonomous navigation speed cm/sec
         uint16_t max_auto_climb_rate;           // max vertical speed limitation cm/sec
         uint16_t max_manual_speed;              // manual velocity control max horizontal speed
         uint16_t max_manual_climb_rate;         // manual velocity control max vertical speed
-        uint16_t land_descent_rate;             // normal RTH landing descent rate
+        uint16_t land_minalt_vspd;              // Final RTH landing descent rate under minalt
+        uint16_t land_maxalt_vspd;              // RTH landing descent rate target at maxalt
         uint16_t land_slowdown_minalt;          // Altitude to stop lowering descent rate during RTH descend
         uint16_t land_slowdown_maxalt;          // Altitude to start lowering descent rate during RTH descend
         uint16_t emerg_descent_rate;            // emergency landing descent rate
@@ -209,6 +220,7 @@ typedef struct navConfig_s {
         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
         uint16_t safehome_max_distance;         // Max distance that a safehome is from the arming point
+        uint16_t max_altitude;                  // Max altitude when in AltHold mode (not Surface Following)
     } general;
 
     struct {
@@ -471,6 +483,11 @@ typedef enum {
     GEO_ORIGIN_RESET_ALTITUDE
 } geoOriginResetMode_e;
 
+typedef enum {
+    NAV_WP_TAKEOFF_DATUM,
+    NAV_WP_MSL_DATUM
+} geoAltitudeDatumFlag_e;
+
 // geoSetOrigin stores the location provided in llh as a GPS origin in the
 // provided origin parameter. resetMode indicates wether all origin coordinates
 // should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving
@@ -491,6 +508,8 @@ bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh
 // the provided origin is valid and the conversion could be performed.
 bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
 float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
+// Select absolute or relative altitude based on WP mission flag setting
+geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
 
 /* Distance/bearing calculation */
 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
@@ -506,10 +525,12 @@ bool navigationIsControllingThrottle(void);
 bool isFixedWingAutoThrottleManuallyIncreased(void);
 bool navigationIsFlyingAutonomousMode(void);
 bool navigationIsExecutingAnEmergencyLanding(void);
+bool navigationIsControllingAltitude(void);
 /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
  * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
  */
 bool navigationRTHAllowsLanding(void);
+bool isWaypointMissionRTHActive(void);
 
 bool isNavLaunchEnabled(void);
 bool isFixedWingLaunchDetected(void);

src/main/navigation/navigation_fixedwing.c

@@ -106,7 +106,7 @@ void resetFixedWingAltitudeController(void)
 
 bool adjustFixedWingAltitudeFromRCInput(void)
 {
-    int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband);
+    int16_t rcAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband, -500, 500);
 
     if (rcAdjustment) {
         // set velocity proportional to stick movement
@@ -171,11 +171,11 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
 
     if ((posControl.flags.estAltStatus >= EST_USABLE)) {
         if (posControl.flags.verticalPositionDataNew) {
-            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
             previousTimePositionUpdate = currentTimeUs;
 
-            // Check if last correction was too log ago - ignore this update
-            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+            // Check if last correction was not too long ago
+            if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                 updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
             }
             else {
@@ -281,7 +281,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 
     // Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
     if (posControl.flags.isAdjustingPosition) {
-        int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
+        int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
 
         if (rcRollAdjustment) {
             float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod;
@@ -295,7 +295,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 
 bool adjustFixedWingPositionFromRCInput(void)
 {
-    int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
+    int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
     return (rcRollAdjustment);
 }
 
@@ -401,10 +401,10 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         // If we have new position - update velocity and acceleration controllers
         if (posControl.flags.horizontalPositionDataNew) {
-            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
             previousTimePositionUpdate = currentTimeUs;
 
-            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+            if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                 // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
                 // Account for pilot's roll input (move position target left/right at max of max_manual_speed)
                 // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
@@ -440,10 +440,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         // If we have new position - update velocity and acceleration controllers
         if (posControl.flags.horizontalPositionDataNew) {
-            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
             previousTimePositionUpdate = currentTimeUs;
 
-            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+            if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                 float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
 
                 // If we are in the deadband of 50cm/s - don't update speed boost
@@ -473,7 +473,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
 int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
 {
     static timeUs_t previousTimePitchToThrCorr = 0;
-    const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
+    const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
     previousTimePitchToThrCorr = currentTimeUs;
 
     static pt1Filter_t pitchToThrFilterState;
@@ -650,7 +650,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
         }
 
         if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition)
-            rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
+            rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
 
         //if (navStateFlags & NAV_CTL_YAW)
         if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS))