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

.gitignore

@@ -32,3 +32,5 @@ README.pdf
 # local changes only
 make/local.mk
 launch.json
+.vscode/tasks.json
+.vscode/c_cpp_properties.json

docs/Cli.md

@@ -114,6 +114,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0
 | `status` | Show status. Error codes can be looked up [here](https://github.com/iNavFlight/inav/wiki/%22Something%22-is-disabled----Reasons) |
 | `tasks` | Show task stats |
 | `temp_sensor` | List or configure temperature sensor(s). See [temperature sensors documentation](Temperature-sensors.md) for more information. |
+|  `timer_output_mode`  | Override automatic timer /  pwm function allocation. [Additional Information](#timer_outout_mode)|
 | `version` | Show version |
 | `wp` | List or configure waypoints. See the [navigation documentation](Navigation.md#cli-command-wp-to-manage-waypoints). |
 
@@ -170,6 +171,66 @@ serial 0 -4
 
 `serial` can also be used without any argument to print the current configuration of all the serial ports.
 
+### `timer_output_mode`
+
+Since INAV 7, the firmware can dynamically allocate servo and motor outputs. This removes the need for bespoke targets for special cases (e.g. `MATEKF405` and `MATEKF405_SERVOS6`).
+
+#### Syntax
+
+```
+timer_output_mode [timer [function]]
+```
+where:
+* Without parameters, lists the current timers and modes
+* With just a `timer` lists the mode for that timer
+* With both `timer` and `function`, sets the function for that timers
+
+Note:
+
+* `timer` identifies the timer **index** (from 0); thus is one less than the corresponding `TIMn` definition in a target's `target.c`.
+* The function is one of `AUTO` (the default), `MOTORS` or `SERVOS`.
+
+Motors are allocated first, hence having a servo before a motor may require use of `timer_output_mode`.
+
+#### Example
+
+The original `MATEKF405` target defined a multi-rotor (MR) servo on output S1. The later `MATEKF405_SERVOS6` target defined (for MR) S1 as a motor and S6 as a servo. This was more logical, but annoying for anyone who had a legacy `MATEKF405` tricopter with the servo on S1.
+
+#### Solution
+
+There is now a single `MATEKF405` target. The `target.c` sets the relevant  outputs as:
+
+```
+DEF_TIM(TIM3, CH1, PC6,  TIM_USE_OUTPUT_AUTO, 0, 0), // S1
+DEF_TIM(TIM8, CH2, PC7,  TIM_USE_OUTPUT_AUTO, 0, 1), // S2  UP(2,1)
+DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 1), // S3  UP(2,1)
+DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0), // S4  UP(2,1)
+DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED, 0, 0), // S5  UP(1,7)
+DEF_TIM(TIM1, CH1, PA8,  TIM_USE_OUTPUT_AUTO, 0, 0), // S6  UP(2,5)
+DEF_TIM(TIM4, CH3, PB8,  TIM_USE_OUTPUT_AUTO, 0, 0), // S7  D(1,7)!S5 UP(2,6)
+```
+
+Using the "motors first" allocation, the servo would end up on S6, which in the legacy "tricopter servo on S1" case is not desired.
+
+Forcing the S1 output (`TIM3`) to servo is achieved by:
+
+```
+timer_output_mode 2 SERVOS
+```
+
+with resulting `resource` output:
+
+```
+C06: SERVO4 OUT
+C07: MOTOR1 OUT
+C08: MOTOR2 OUT
+C09: MOTOR3 OUT
+```
+
+Note that the `timer` **index** in the `timer_output_mode` line is one less than the mnemonic in `target.c`, `timer` of 2 for `TIM3`.
+
+Note that the usual caveat that one should not share a timer with both a motor and a servo still apply.
+
 ## Flash chip management
 
 For targets that have a flash data chip, typically used for blackbox logs, the following additional comamnds are provided.

docs/ESC and servo outputs.md

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

docs/Programming Framework.md

@@ -1,17 +1,28 @@
 # INAV Programming Framework
 
-INAV Programming Framework (abbr. IPF) is a mechanism that allows to evaluate cenrtain flight parameters (RC channels, switches, altitude, distance, timers, other logic conditions) and use the value of evaluated expression in different places of INAV. Currently, the result of LCs can be used in:
+INAV Programming Framework (IPF) is a mechanism that allows you to to create 
+custom functionality in INAV. You can choose for certain actions to be done,
+based on custom conditions you select.
+
+Logic conditions can be based on things such as RC channel values, switches, altitude, 
+distance, timers, etc. The conditions you create  can also make use of other conditions
+you've entered previously.
+The results can be used in:
 
 * [Servo mixer](Mixer.md) to activate/deactivate certain servo mix rulers
 * To activate/deactivate system overrides
 
-INAV Programming Framework coinsists of:
+INAV Programming Framework consists of:
 
-* Logic Conditions - each Logic Condition can be understood as a single command, a single line of code
+* Logic Conditions - each Logic Condition can be understood as a single command, a single line of code. Each logic condition consists of:
-* Global Variables - variables that can store values from and for LogiC Conditions and servo mixer
+	* an operator (action), such as "plus" or "set vtx power"
+	* one or two operands (nouns), which the action acts upon. Operands are often numbers, such as a channel value or the distance to home.
+	* "activator" condition - optional. This condition is only active when another condition is true
+* Global Variables - variables that can store values from and for Logic Conditions and servo mixer
 * Programming PID - general purpose, user configurable PID controllers
 
-IPF can be edited using INAV Configurator user interface, or via CLI
+IPF can be edited using INAV Configurator user interface, or via CLI. To use COnfigurator, click the tab labeled
+"Programming". The various options shown in Configurator are described below.
 
 ## Logic Conditions
 
@@ -46,16 +57,16 @@ IPF can be edited using INAV Configurator user interface, or via CLI
 | 10            | NAND                          | `false` if `Operand A` and `Operand B` are both `true`|
 | 11            | NOR                           | `true` if `Operand A` and `Operand B` are both `false` |
 | 12            | NOT                           | The boolean opposite to `Operand A` |         
-| 13            | STICKY                        | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|         
+| 13            | Sticky                        | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|         
-| 14            | ADD                           | Add `Operand A` to `Operand B` and returns the result |
+| 14            | Basic: Add                           | Add `Operand A` to `Operand B` and returns the result |
-| 15            | SUB                           | Substract `Operand B` from `Operand A` and returns the result |
+| 15            | Basic: Subtract                           | Substract `Operand B` from `Operand A` and returns the result |
-| 16            | MUL                           | Multiply `Operand A` by `Operand B` and returns the result |
+| 16            | Basic: Multiply                           | Multiply `Operand A` by `Operand B` and returns the result |
-| 17            | DIV                           | Divide `Operand A` by `Operand B` and returns the result |
+| 17            | Basic: Divide                           | Divide `Operand A` by `Operand B` and returns the result |
-| 18            | GVAR SET                      | Store value from `Operand B` into the Global Variable addressed by
+| 18            | Set GVAR                      | Store value from `Operand B` into the Global Variable addressed by
 `Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` |
-| 19            | GVAR INC                      | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
+| 19            | Increase GVAR                      | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
-| 20            | GVAR DEC                      | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
+| 20            | Decrease GVAR                      | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
-| 21            | IO PORT SET                   | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
+| 21            | Set IO Port                   | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
 | 22            | OVERRIDE_ARMING_SAFETY        | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. |
 | 23            | OVERRIDE_THROTTLE_SCALE       | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. |
 | 24            | SWAP_ROLL_YAW                 | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes |
@@ -67,18 +78,18 @@ IPF can be edited using INAV Configurator user interface, or via CLI
 | 30            | SET_VTX_BAND                  | Sets VTX band. Accepted values are `1-5` |
 | 31            | SET_VTX_CHANNEL               | Sets VTX channel. Accepted values are `1-8` |
 | 32            | SET_OSD_LAYOUT                | Sets OSD layout. Accepted values are `0-3` |
-| 33            | SIN                           | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
+| 33            | Trigonometry: Sine                           | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
-| 34            | COS                           | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
+| 34            | Trigonometry: Cosine                           | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
-| 35            | TAN                           | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
+| 35            | Trigonometry: Tangent                           | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
 | 36            | MAP_INPUT                     | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
 | 37            | MAP_OUTPUT                    | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
 | 38            | RC_CHANNEL_OVERRIDE           | Overrides channel set by `Operand A` to value of `Operand B` |
 | 39            | SET_HEADING_TARGET            | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
-| 40            | MOD                           | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
+| 40            | Modulo                           | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
 | 41            | LOITER_RADIUS_OVERRIDE        | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
 | 42            | SET_PROFILE                   | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
-| 43            | MIN                           | Finds the lowest value of `Operand A` and `Operand B` |
+| 43            | Use Lowest Value                           | Finds the lowest value of `Operand A` and `Operand B` |
-| 44            | MAX                           | Finds the highest value of `Operand A` and `Operand B` |
+| 44            | Use Highest Value                           | Finds the highest value of `Operand A` and `Operand B` |
 | 45			| FLIGHT_AXIS_ANGLE_OVERRIDE	| Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
 | 46			| FLIGHT_AXIS_RATE_OVERRIDE	    | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
 | 47            | EDGE                          | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
@@ -158,7 +169,7 @@ The flight mode operands return `true` when the mode is active. These are modes
 | 7             | HORIZON           | `true` when you are in the **Horizon** flight mode. |
 | 8             | AIR               | `true` when you the **Airmode** flight mode modifier is active. |
 | 9             | USER1             | `true` when the **USER 1** mode is active. |
-| 10            | USER2             | `true` when the **USER 21** mode is active. |
+| 10            | USER2             | `true` when the **USER 2** mode is active. |
 | 11            | COURSE_HOLD       | `true` when you are in the **Course Hold** flight mode. |
 | 12            | USER3             | `true` when the **USER 3** mode is active. |
 | 13            | USER4             | `true` when the **USER 4** mode is active. |
@@ -310,3 +321,14 @@ Steps:
 2. Scale range [0:1000] to [0:3]
 3. Increase range by `1` to have the range of [1:4]
 4. Assign LC#2 to VTX power function
+
+## Common issues / questions about IPF
+
+One common mistake involves setting RC channel values. To override (set) the 
+value of a specific RC channel, choose "Override RC value", then for operand A
+choose *value* and enter the channel number. Choosing "get RC value" is a common mistake,
+which does something other than what you probably want.
+
+![screenshot of override an RC channel with a value](./assets/images/ipf_set_get_rc_channel.png)
+
+

docs/Settings.md

@@ -2662,6 +2662,16 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
 
 ---
 
+### nav_cruise_yaw_rate
+
+Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 20 | 0 | 120 |
+
+---
+
 ### nav_disarm_on_landing
 
 If set to ON, INAV disarms the FC after landing
@@ -2752,16 +2762,6 @@ Cruise throttle in GPS assisted modes, this includes RTH. Should be set high eno
 
 ---
 
-### nav_fw_cruise_yaw_rate
-
-Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 20 | 0 | 60 |
-
----
-
 ### nav_fw_dive_angle
 
 Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit
@@ -3282,6 +3282,16 @@ Max allowed above the ground altitude for terrain following mode
 
 ---
 
+### nav_mc_althold_throttle
+
+If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| STICK |  |  |
+
+---
+
 ### nav_mc_bank_angle
 
 Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude
@@ -3752,16 +3762,6 @@ Enables or Disables the use of the heading PID controller on fixed wing. Heading
 
 ---
 
-### nav_use_midthr_for_althold
-
-If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude
-
-| Default | Min | Max |
-| --- | --- | --- |
-| OFF | OFF | ON |
-
----
-
 ### nav_user_control_mode
 
 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.
@@ -4732,16 +4732,6 @@ Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF'
 
 ---
 
-### output_mode
-
-Output function assignment mode. AUTO assigns outputs according to the default mapping, SERVOS assigns all outputs to servos, MOTORS assigns all outputs to motors
-
-| Default | Min | Max |
-| --- | --- | --- |
-| AUTO |  |  |
-
----
-
 ### pid_type
 
 Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID`
@@ -4838,7 +4828,7 @@ Selection of pitot hardware.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| NONE |  |  |
+| VIRTUAL |  |  |
 
 ---
 

docs/development/Building in Windows 10 or 11 with Linux Subsystem.md

@@ -26,7 +26,7 @@ Install Git, Make, gcc and Ruby
 -  `sudo apt-get install git make cmake ruby`
 
 Install python and python-yaml to allow updates to settings.md
--  `sudo apt-get install python3 python-yaml`
+-  `sudo apt-get install python3`
 
 ### CMAKE and Ubuntu 18_04
 

docs/development/serial_printf_debugging.md

@@ -76,9 +76,11 @@ If the CLI `log_topics` is non-zero, then all topics matching the mask will be d
 
 A set of macros `LOG_ERROR()` (log error) through `LOG_DEBUG()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
 
+Note that the `topic` is specified without the `LOG_TOPIC_` prefix:
+
 ```
 //  LOG_DEBUG(topic, fmt, ...)
-LOG_DEBUG(LOG_TOPIC_SYSTEM, "This is %s topic debug message, value %d", "system", 42);
+LOG_DEBUG(SYSTEM, "This is %s topic debug message, value %d", "system", 42);
 ```
 
 It is also possible to dump a hex representation of arbitrary  data, using functions named variously `LOG_BUFFER_` (`ERROR`) and `LOG_BUF_` (anything else, alas) e.g.:
@@ -89,7 +91,7 @@ It is also possible to dump a hex representation of arbitrary  data, using funct
 
 struct {...} tstruct;
 ...
-LOG_BUF_DEBUG(LOG_TOPIC_TEMPERATURE, &tstruct, sizeof(tstruct));
+LOG_BUF_DEBUG(TEMPERATURE, &tstruct, sizeof(tstruct));
 ```
 
 ## Output Support
@@ -104,7 +106,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa
 For example, with the final lines of `src/main/fc/fc_init.c` set to:
 
 ```
- LOG_ERROR(LOG_TOPIC_SYSTEM, "Init is complete");
+ LOG_ERROR(SYSTEM, "Init is complete");
  systemState |= SYSTEM_STATE_READY;
 ```
 

src/main/CMakeLists.txt

@@ -208,6 +208,8 @@ main_sources(COMMON_SRC
     drivers/pitotmeter/pitotmeter_adc.h
     drivers/pitotmeter/pitotmeter_ms4525.c
     drivers/pitotmeter/pitotmeter_ms4525.h
+    drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+    drivers/pitotmeter/pitotmeter_dlvr_l10d.h	
     drivers/pitotmeter/pitotmeter_msp.c
     drivers/pitotmeter/pitotmeter_msp.h
     drivers/pitotmeter/pitotmeter_virtual.c
@@ -285,6 +287,8 @@ main_sources(COMMON_SRC
     fc/firmware_update.h
     fc/firmware_update_common.c
     fc/firmware_update_common.h
+    fc/multifunction.c
+    fc/multifunction.h
     fc/rc_smoothing.c
     fc/rc_smoothing.h
     fc/rc_adjustments.c

src/main/blackbox/blackbox.c

@@ -366,6 +366,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"navTgtPos",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navTgtPos",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navTgtPos",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
+    {"navTgtHdg", -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navSurf",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navAcc",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
     {"navAcc",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
@@ -517,7 +518,7 @@ typedef struct blackboxMainState_s {
     int16_t navTargetVel[XYZ_AXIS_COUNT];
     int32_t navTargetPos[XYZ_AXIS_COUNT];
     int16_t navHeading;
-    int16_t navTargetHeading;
+    uint16_t navTargetHeading;
     int16_t navSurface;
 } blackboxMainState_t;
 
@@ -970,6 +971,7 @@ static void writeIntraframe(void)
             blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
         }
 
+        blackboxWriteSignedVB(blackboxCurrent->navTargetHeading);
         blackboxWriteSignedVB(blackboxCurrent->navSurface);
     }
 
@@ -1226,6 +1228,7 @@ static void writeInterframe(void)
             blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
         }
 
+        blackboxWriteSignedVB(blackboxCurrent->navTargetHeading - blackboxLast->navTargetHeading);
         blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
     }
 
@@ -1298,6 +1301,16 @@ static void writeSlowFrame(void)
 static void loadSlowState(blackboxSlowState_t *slow)
 {
     memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
+    // Also log Nav auto selected flight modes rather than just those selected by boxmode
+    if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
+        slow->flightModeFlags |= (1 << BOXANGLE);
+    }
+    if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
+        slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
+    }
+    if (navigationRequiresTurnAssistance()) {
+        slow->flightModeFlags |= (1 << BOXTURNASSIST);
+    }
     slow->stateFlags = stateFlags;
     slow->failsafePhase = failsafePhase();
     slow->rxSignalReceived = rxIsReceivingSignal();
@@ -1661,6 +1674,7 @@ static void loadMainState(timeUs_t currentTimeUs)
         blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
         blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
     }
+    blackboxCurrent->navTargetHeading = navDesiredHeading;
     blackboxCurrent->navSurface = navActualSurface;
 }
 

src/main/cms/cms_menu_navigation.c

@@ -48,7 +48,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
     OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
     OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
     OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
-    OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD),
+    OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE),
     OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),
     OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING),
 

src/main/common/calibration.c

@@ -30,6 +30,7 @@
 #include "drivers/time.h"
 #include "common/calibration.h"
 
+
 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
 {
     // Reset parameters and state
@@ -75,9 +76,11 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
     // Check if calibration is complete
     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
         const float stddev = devStandardDeviation(&s->val.stdDev);
+
         if (stddev > s->params.stdDevThreshold) {
             if (!s->params.allowFailure) {
-                // If deviation is too big - restart calibration
+                // If deviation is too big && no failure allowed - restart calibration
+                // TODO :: some safeguard should exist which will not allow to keep on calibrating for ever
                 s->params.startTimeMs = millis();
                 s->params.sampleCount = 0;
                 s->val.accumulatedValue = 0;

src/main/config/parameter_group_ids.h

@@ -119,7 +119,8 @@
 #define PG_UNUSED_1 1029
 #define PG_POWER_LIMITS_CONFIG 1030
 #define PG_OSD_COMMON_CONFIG 1031
-#define PG_INAV_END 1031
+#define PG_TIMER_OVERRIDE_CONFIG 1032
+#define PG_INAV_END 1032
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/bus.h

@@ -138,6 +138,7 @@ typedef enum {
 
     /* Other hardware */
     DEVHW_MS4525,       // Pitot meter
+    DEVHW_DLVR,         // Pitot meter
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_W25N01G,      // SPI 128MB flash
     DEVHW_UG2864,       // I2C OLED display

src/main/drivers/osd_symbols.h

@@ -174,6 +174,9 @@
 #define SYM_FLIGHT_MINS_REMAINING   0xDA  // 218 Flight time (mins) remaining
 #define SYM_FLIGHT_HOURS_REMAINING  0xDB  // 219 Flight time (hours) remaining
 #define SYM_GROUND_COURSE           0xDC  // 220 Ground course
+#define SYM_ALERT                   0xDD  // 221 General alert symbol
+
+#define SYM_TERRAIN_FOLLOWING       0xFB  // 251 Terrain following (also Alt adjust)
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo

src/main/drivers/pitotmeter/pitotmeter.h

@@ -26,6 +26,7 @@ typedef void (*pitotCalculateFuncPtr)(struct pitotDev_s * pitot, float *pressure
 typedef struct pitotDev_s {
     busDevice_t * busDev;
     uint16_t delay;
+    float calibThreshold;
     pitotOpFuncPtr start;
     pitotOpFuncPtr get;
     pitotCalculateFuncPtr calculate;

src/main/drivers/pitotmeter/pitotmeter_adc.c

@@ -67,6 +67,7 @@ static void adcPitotCalculate(pitotDev_t *pitot, float *pressure, float *tempera
 bool adcPitotDetect(pitotDev_t *pitot)
 {
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;   // TODO :: should be tested !!!
     pitot->start = adcPitotStart;
     pitot->get = adcPitotRead;
     pitot->calculate = adcPitotCalculate;

src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c

@@ -0,0 +1,165 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+#include <build/debug.h>
+
+#include "common/log.h"
+#include "common/utils.h"
+#include "common/maths.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/time.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+
+#define DLVR_L10D_ADDR                 0x28     // this var is not used !!!
+
+// //---------------------------------------------------
+// // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
+// #define ISA_GAS_CONSTANT 287.26f
+// #define ISA_LAPSE_RATE 0.0065f
+
+// // Standard Sea Level values
+// // Ref: https://en.wikipedia.org/wiki/Standard_sea_level
+// #define SSL_AIR_DENSITY         1.225f // kg/m^3
+// #define SSL_AIR_PRESSURE 101325.01576f // Pascal
+// #define SSL_AIR_TEMPERATURE    288.15f // K
+// //---------------------------------------------------
+
+#define INCH_OF_H2O_TO_PASCAL   248.84f
+
+#define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
+
+#define RANGE_INCH_H2O      10
+#define DLVR_OFFSET         8192.0f
+#define DLVR_SCALE          16384.0f
+// NOTE :: DLVR_OFFSET_CORR can be used for offset correction. Now firmware relies on zero calibration
+#define DLVR_OFFSET_CORR    0.0f   //-9.0f
+
+
+typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
+    bool     dataValid;
+    uint32_t dlvr_ut;
+    uint32_t dlvr_up;
+} dlvrCtx_t;
+
+STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
+
+static bool dlvr_start(pitotDev_t * pitot)
+{
+    UNUSED(pitot);
+    return true;
+}
+
+static bool dlvr_read(pitotDev_t * pitot)
+{
+    uint8_t rxbuf1[4];
+
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    ctx->dataValid = false;
+
+    if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) {
+        return false;
+    }
+
+    // status = 00 -> ok, new data
+	// status = 01 -> reserved
+    // status = 10 -> ok, data stale
+    // status = 11 -> error
+    const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
+
+    if (status) {
+        // anything other then 00 in the status bits is an error
+        LOG_DEBUG( PITOT, "DLVR: Bad status read. status = %u", (unsigned int)(status) );
+        return false;
+    }
+
+    int16_t dP_raw1, dT_raw1;
+
+    dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
+    dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
+
+    // Data valid, update ut/up values
+    ctx->dataValid = true;
+    ctx->dlvr_up = dP_raw1;
+    ctx->dlvr_ut = dT_raw1;
+
+    return true;
+}
+
+static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperature)
+{
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+
+  
+    // pressure in inchH2O
+    float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE); 
+
+    // temperature in deg C
+    float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
+
+    // result must fit inside the max pressure range
+    if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
+        LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %f", (double)(dP_inchH2O) );
+        return;
+    }
+
+    if (pressure) {
+        *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);   // Pa
+    }
+
+    if (temperature) {
+        *temperature = C_TO_KELVIN( T_C); // K
+    }
+}
+
+bool dlvrDetect(pitotDev_t * pitot)
+{
+    uint8_t rxbuf[4];
+    bool ack = false;
+
+    pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DLVR, 0, OWNER_AIRSPEED);
+    if (pitot->busDev == NULL) {
+        return false;
+    }
+
+    // Read twice to fix:
+    // Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a
+    // communication error for the next communication, even if the next start condition is correct and the clock pulse is applied.
+    // An additional start condition must be sent, which results in restoration of proper communication.
+    ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
+    ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
+    if (!ack) {
+        return false;
+    }
+
+    // Initialize busDev data
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    ctx->dataValid = false;
+    ctx->dlvr_ut = 0;
+    ctx->dlvr_up = 0;
+
+    // Initialize pitotDev object
+    pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;   // low noise sensor
+    pitot->start = dlvr_start;
+    pitot->get = dlvr_read;
+    pitot->calculate = dlvr_calculate;
+    return true;
+}

src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h

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

src/main/drivers/pitotmeter/pitotmeter_fake.c

@@ -75,6 +75,7 @@ bool fakePitotDetect(pitotDev_t *pitot)
     fakeTemperature = 273;      // 0C
 
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = fakePitotStart;
     pitot->get = fakePitotRead;
     pitot->calculate = fakePitotCalculate;

src/main/drivers/pitotmeter/pitotmeter_ms4525.c

@@ -74,6 +74,13 @@ static bool ms4525_read(pitotDev_t * pitot)
     dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
     dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
 
+    // reject any values that are the absolute minimum or maximums these
+    // can happen due to gnd lifts or communication errors on the bus
+    if (dP_raw1 == 0x3FFF || dP_raw1 == 0 || dT_raw1 == 0x7FF || dT_raw1 == 0 ||
+        dP_raw2 == 0x3FFF || dP_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) {
+        return false;
+    }
+
     // reject any double reads where the value has shifted in the upper more than 0xFF
     if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
         return false;
@@ -83,6 +90,7 @@ static bool ms4525_read(pitotDev_t * pitot)
     ctx->dataValid = true;
     ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2;
     ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2;
+
     return true;
 }
 
@@ -136,6 +144,7 @@ bool ms4525Detect(pitotDev_t * pitot)
 
     // Initialize pitotDev object
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00005f;   // noisy sensor
     pitot->start = ms4525_start;
     pitot->get = ms4525_read;
     pitot->calculate = ms4525_calculate;

src/main/drivers/pitotmeter/pitotmeter_msp.c

@@ -87,6 +87,7 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
     mspPitotTemperature = 27315;    // 0 deg/c
 
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = mspPitotStart;
     pitot->get = mspPitotRead;
     pitot->calculate = mspPitotCalculate;
@@ -95,3 +96,4 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
 }
 
 #endif
+    

src/main/drivers/pitotmeter/pitotmeter_virtual.c

@@ -88,6 +88,7 @@ static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *tem
 bool virtualPitotDetect(pitotDev_t *pitot)
 {
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = virtualPitotStart;
     pitot->get = virtualPitotRead;
     pitot->calculate = virtualPitotCalculate;

src/main/drivers/pwm_esc_detect.c

@@ -38,7 +38,7 @@ uint8_t hardwareMotorType = MOTOR_UNKNOWN;
 void detectBrushedESC(void)
 {
     for (int i = 0; i < timerHardwareCount; i++) {
-        if (timerHardware[i].usageFlags & TIM_USE_MC_MOTOR) {
+        if (timerHardware[i].usageFlags & TIM_USE_MOTOR) {
             IO_t MotorDetectPin = IOGetByTag(timerHardware[i].tag);
             IOInit(MotorDetectPin, OWNER_SYSTEM, RESOURCE_INPUT, 0);
             IOConfigGPIO(MotorDetectPin, IOCFG_IPU);

src/main/drivers/pwm_mapping.c

@@ -211,46 +211,111 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
 }
 
 static void timerHardwareOverride(timerHardware_t * timer) {
-    if (mixerConfig()->outputMode == OUTPUT_MODE_SERVOS) {
+    switch (timerOverrides(timer2id(timer->tim))->outputMode) {
-        
+        case OUTPUT_MODE_MOTORS:
-        //Motors are rewritten as servos
+            if (TIM_IS_SERVO(timer->usageFlags)) {
-        if (timer->usageFlags & TIM_USE_MC_MOTOR) {
+                timer->usageFlags &= ~TIM_USE_SERVO;
-            timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_MOTOR;
+                timer->usageFlags |= TIM_USE_MOTOR;
-            timer->usageFlags = timer->usageFlags | TIM_USE_MC_SERVO;
+            }
+            break;
+        case OUTPUT_MODE_SERVOS:
+            if (TIM_IS_MOTOR(timer->usageFlags)) {
+                timer->usageFlags &= ~TIM_USE_MOTOR;
+                timer->usageFlags |= TIM_USE_SERVO;
+            }
+            break;
+    }
+}
+
+bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim)
+{
+    for (int i = 0; i < timOutputs->maxTimMotorCount; ++i) {
+        if (timOutputs->timMotors[i]->tim == tim) {
+            return true;
         }
-        if (timer->usageFlags & TIM_USE_FW_MOTOR) {
-            timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_MOTOR;
-            timer->usageFlags = timer->usageFlags | TIM_USE_FW_SERVO;
     }
 
-    } else if (mixerConfig()->outputMode == OUTPUT_MODE_MOTORS) {
+    return false;
+}
 
-        // Servos are rewritten as motors
+bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim)
-        if (timer->usageFlags & TIM_USE_MC_SERVO) {
+{
-            timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_SERVO;
+    for (int i = 0; i < timOutputs->maxTimServoCount; ++i) {
-            timer->usageFlags = timer->usageFlags | TIM_USE_MC_MOTOR;
+        if (timOutputs->timServos[i]->tim == tim) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) {
+    uint8_t changed = 0;
+    for (int idx = 0; idx < timerHardwareCount; idx++) {
+        timerHardware_t *timHw = &timerHardware[idx];
+        if (timHw->tim == tim && timHw->usageFlags != usageFlags) {
+            timHw->usageFlags = usageFlags;
+            changed++;
+        }
+    }
+
+    return changed;
+}
+
+void pwmEnsureEnoughtMotors(uint8_t motorCount)
+{
+    uint8_t motorOnlyOutputs = 0;
+
+    for (int idx = 0; idx < timerHardwareCount; idx++) {
+        timerHardware_t *timHw = &timerHardware[idx];
+
+        timerHardwareOverride(timHw);
+
+        if (checkPwmTimerConflicts(timHw)) {
+            continue;
+        }
+
+        if (TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
+            motorOnlyOutputs++;
+            motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
+        }
+    }
+
+    for (int idx = 0; idx < timerHardwareCount; idx++) {
+        timerHardware_t *timHw = &timerHardware[idx];
+
+        if (checkPwmTimerConflicts(timHw)) {
+            continue;
+        }
+
+        if (TIM_IS_MOTOR(timHw->usageFlags) && !TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
+            if (motorOnlyOutputs < motorCount) {
+                timHw->usageFlags &= ~TIM_USE_SERVO;
+                timHw->usageFlags |= TIM_USE_MOTOR;
+                motorOnlyOutputs++;
+                motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
+            } else {
+                timHw->usageFlags &= ~TIM_USE_MOTOR;
+                pwmClaimTimer(timHw->tim, timHw->usageFlags);
             }
-        if (timer->usageFlags & TIM_USE_FW_SERVO) {
-            timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_SERVO;
-            timer->usageFlags = timer->usageFlags | TIM_USE_FW_MOTOR;
         }
     }
 }
 
 void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos)
 {
+    UNUSED(isMixerUsingServos);
     timOutputs->maxTimMotorCount = 0;
     timOutputs->maxTimServoCount = 0;
 
     uint8_t motorCount = getMotorCount();
     uint8_t motorIdx = 0;
 
+    pwmEnsureEnoughtMotors(motorCount);
+
     for (int idx = 0; idx < timerHardwareCount; idx++) {
-
         timerHardware_t *timHw = &timerHardware[idx];
 
-        timerHardwareOverride(timHw);
-
         int type = MAP_TO_NONE;
 
         // Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC)
@@ -259,39 +324,29 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
             continue;
         }
 
-        // Determine if timer belongs to motor/servo
+        // Make sure first motorCount motor outputs get assigned to motor
-        if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER) {
+        if (TIM_IS_MOTOR(timHw->usageFlags) && (motorIdx < motorCount)) {
-            // Multicopter
+            timHw->usageFlags &= ~TIM_USE_SERVO;
-
+            pwmClaimTimer(timHw->tim, timHw->usageFlags);
-            // Make sure first motorCount outputs get assigned to motor
-            if ((timHw->usageFlags & TIM_USE_MC_MOTOR) && (motorIdx < motorCount)) {
-                timHw->usageFlags = timHw->usageFlags & ~TIM_USE_MC_SERVO;
             motorIdx += 1;
         }
 
-            // We enable mapping to servos if mixer is actually using them
+        if (TIM_IS_SERVO(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) {
-            if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO) {
             type = MAP_TO_SERVO_OUTPUT;
-            }
+        } else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
-            else if (timHw->usageFlags & TIM_USE_MC_MOTOR) {
             type = MAP_TO_MOTOR_OUTPUT;
         }
-        } else {
-            // Fixed wing or HELI (one/two motors and a lot of servos
-            if (timHw->usageFlags & TIM_USE_FW_SERVO) {
-                type = MAP_TO_SERVO_OUTPUT;
-            }
-            else if (timHw->usageFlags & TIM_USE_FW_MOTOR) {
-                type = MAP_TO_MOTOR_OUTPUT;
-            }
-        }
 
         switch(type) {
             case MAP_TO_MOTOR_OUTPUT:
+                timHw->usageFlags &= TIM_USE_MOTOR;
                 timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw;
+                pwmClaimTimer(timHw->tim, timHw->usageFlags);
                 break;
             case MAP_TO_SERVO_OUTPUT:
+                timHw->usageFlags &= TIM_USE_SERVO;
                 timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw;
+                pwmClaimTimer(timHw->tim, timHw->usageFlags);
                 break;
             default:
                 break;

src/main/drivers/pwm_output.c

@@ -42,6 +42,9 @@
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
+#include "drivers/timer_impl.h"
+#include "drivers/timer.h"
+
 #define MULTISHOT_5US_PW    (MULTISHOT_TIMER_HZ * 5 / 1000000.0f)
 #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f)
 
@@ -56,7 +59,9 @@
 #define DSHOT_MOTOR_BITLENGTH   20
 
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
+#define MAX_DMA_TIMERS          8
 
+#define DSHOT_COMMAND_DELAY_US 1000
 #define DSHOT_COMMAND_INTERVAL_US 10000
 #define DSHOT_COMMAND_QUEUE_LENGTH 8
 #define DHSOT_COMMAND_QUEUE_SIZE   DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
@@ -64,6 +69,10 @@
 
 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
 
+#ifdef USE_DSHOT_DMAR
+    timerDMASafeType_t dmaBurstBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
+#endif
+
 typedef struct {
     TCH_t * tch;
     bool configured;
@@ -77,6 +86,9 @@ typedef struct {
 #ifdef USE_DSHOT
     // DSHOT parameters
     timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
+#ifdef USE_DSHOT_DMAR
+    timerDMASafeType_t *dmaBurstBuffer;
+#endif
 #endif
 } pwmOutputPort_t;
 
@@ -249,6 +261,22 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
     }
 }
 
+#ifdef USE_DSHOT_DMAR
+burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS];
+uint8_t burstDmaTimersCount = 0;
+
+static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer)
+{
+    for (int i = 0; i < burstDmaTimersCount; i++) {
+        if (burstDmaTimers[i].timer == timer) {
+            return i;
+        }
+    }
+    burstDmaTimers[burstDmaTimersCount++].timer = timer;
+    return burstDmaTimersCount - 1;
+}
+#endif
+
 static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput)
 {
     // Try allocating new port
@@ -259,15 +287,43 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware,
     }
 
     // Configure timer DMA
+#ifdef USE_DSHOT_DMAR
+    //uint8_t timerIndex = lookupTimerIndex(timerHardware->tim);
+    uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim);
+    if (burstDmaTimerIndex >= MAX_DMA_TIMERS) {
+        return NULL;
+    }
+
+    port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0];
+    burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
+    burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer;
+
+    if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
+        port->configured = true;
+    }
+#else
     if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
         // Only mark as DSHOT channel if DMA was set successfully
         ZERO_FARRAY(port->dmaBuffer);
         port->configured = true;
     }
+#endif
 
     return port;
 }
 
+#ifdef USE_DSHOT_DMAR
+static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet)
+{
+    int i;
+    for (i = 0; i < 16; i++) {
+        dmaBuffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0;  // MSB first
+        packet <<= 1;
+    }
+    dmaBuffer[i++ * stride] = 0;
+    dmaBuffer[i++ * stride] = 0;
+}
+#else
 static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
 {
     for (int i = 0; i < 16; i++) {
@@ -275,6 +331,7 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
         packet <<= 1;
     }
 }
+#endif
 
 static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry)
 {
@@ -378,6 +435,7 @@ static void executeDShotCommands(void){
         return;
         }  
     }
+    delayMicroseconds(DSHOT_COMMAND_DELAY_US);
     for (uint8_t i = 0; i < getMotorCount(); i++) {
          motors[i].requestTelemetry = true;
          motors[i].value = currentExecutingCommand.cmd;
@@ -408,6 +466,20 @@ void pwmCompleteMotorUpdate(void) {
 
         executeDShotCommands();
 
+#ifdef USE_DSHOT_DMAR
+        for (int index = 0; index < motorCount; index++) {
+            if (motors[index].pwmPort && motors[index].pwmPort->configured) {
+                uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry);
+                loadDmaBufferDshotStride(&motors[index].pwmPort->dmaBurstBuffer[motors[index].pwmPort->tch->timHw->channelIndex], 4, packet);
+                motors[index].requestTelemetry = false;
+            }
+        }
+
+        for (int burstDmaTimerIndex = 0; burstDmaTimerIndex < burstDmaTimersCount; burstDmaTimerIndex++) {
+            burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
+            pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4);
+        }
+#else
         // Generate DMA buffers
         for (int index = 0; index < motorCount; index++) {
             if (motors[index].pwmPort && motors[index].pwmPort->configured) {
@@ -424,6 +496,7 @@ void pwmCompleteMotorUpdate(void) {
                 timerPWMStartDMA(motors[index].pwmPort->tch);
             }
         }
+#endif
     }
 #endif
 }

src/main/drivers/timer.c

@@ -38,8 +38,18 @@
 
 timHardwareContext_t * timerCtx[HARDWARE_TIMER_DEFINITION_COUNT];
 
+
+uint8_t timer2id(const HAL_Timer_t *tim)
+{
+    for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
+        if (timerDefinitions[i].tim == tim) return i;
+    }
+
+    return (uint8_t)-1;
+}
+
 #if defined(AT32F43x)
-uint8_t lookupTimerIndex(const tmr_type *tim)
+uint8_t lookupTimerIndex(const HAL_Timer_t *tim)
 {
     int i;
 
@@ -54,7 +64,7 @@ uint8_t lookupTimerIndex(const tmr_type *tim)
 }
 #else
 // return index of timer in timer table. Lowest timer has index 0
-uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
+uint8_t lookupTimerIndex(const HAL_Timer_t *tim)
 {
     int i;
 
@@ -287,3 +297,15 @@ bool timerPWMDMAInProgress(TCH_t * tch)
 {
     return tch->dmaState != TCH_DMA_IDLE;
 }
+
+#ifdef USE_DSHOT_DMAR
+bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
+{
+    return impl_timerPWMConfigDMABurst(burstDmaTimer, tch, dmaBuffer, dmaBufferElementSize, dmaBufferElementCount);
+}
+
+void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
+{
+    impl_pwmBurstDMAStart(burstDmaTimer, BurstLength);
+}
+#endif

src/main/drivers/timer.h

@@ -25,6 +25,8 @@
 #include "drivers/rcc_types.h"
 #include "drivers/timer_def.h"
 
+#include "platform.h"
+
 #define CC_CHANNELS_PER_TIMER       4   // TIM_Channel_1..4
 
 typedef uint16_t captureCompare_t;        // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t)
@@ -63,8 +65,9 @@ typedef uint32_t timCNT_t;
 #endif
 //  tmr_type instead in AT32 
 #if defined(AT32F43x)
+typedef tmr_type HAL_Timer_t;
 typedef struct timerDef_s {
-    tmr_type   * tim;
+    HAL_Timer_t   * tim;
     rccPeriphTag_t  rcc;
     uint8_t         irq;
     uint8_t         secondIrq;
@@ -82,8 +85,9 @@ typedef struct timerHardware_s {
     uint32_t dmaMuxid; //DMAMUX ID
 } timerHardware_t;
 #else
+typedef TIM_TypeDef HAL_Timer_t;
 typedef struct timerDef_s {
-    TIM_TypeDef   * tim;
+    HAL_Timer_t * tim;
     rccPeriphTag_t  rcc;
     uint8_t         irq;
     uint8_t         secondIrq;
@@ -106,16 +110,30 @@ typedef enum {
     TIM_USE_ANY             = 0,
     TIM_USE_PPM             = (1 << 0),
     TIM_USE_PWM             = (1 << 1),
-    TIM_USE_MC_MOTOR        = (1 << 2),     // Multicopter motor output
+    TIM_USE_MOTOR           = (1 << 2),     // Motor output
-    TIM_USE_MC_SERVO        = (1 << 3),     // Multicopter servo output (i.e. TRI)
+    TIM_USE_SERVO           = (1 << 3),     // Servo output
     TIM_USE_MC_CHNFW        = (1 << 4),     // Deprecated and not used after removal of CHANNEL_FORWARDING feature
-    TIM_USE_FW_MOTOR        = (1 << 5),
+    //TIM_USE_FW_MOTOR        = (1 << 5),   // We no longer differentiate mc from fw on pwm allocation
-    TIM_USE_FW_SERVO        = (1 << 6),
+    //TIM_USE_FW_SERVO        = (1 << 6),
     TIM_USE_LED             = (1 << 24),
     TIM_USE_BEEPER          = (1 << 25),
 } timerUsageFlag_e;
 
 
+// Compability
+#define TIM_USE_MC_MOTOR    TIM_USE_MOTOR
+#define TIM_USE_MC_SERVO    TIM_USE_SERVO
+#define TIM_USE_FW_MOTOR    TIM_USE_MOTOR
+#define TIM_USE_FW_SERVO    TIM_USE_SERVO
+
+#define TIM_USE_OUTPUT_AUTO (TIM_USE_MOTOR | TIM_USE_SERVO)
+
+#define TIM_IS_MOTOR(flags) ((flags) & TIM_USE_MOTOR)
+#define TIM_IS_SERVO(flags) ((flags) & TIM_USE_SERVO)
+
+#define TIM_IS_MOTOR_ONLY(flags) (TIM_IS_MOTOR(flags) && !TIM_IS_SERVO(flags))
+#define TIM_IS_SERVO_ONLY(flags) (!TIM_IS_MOTOR(flags) && TIM_IS_SERVO(flags))
+
 enum {
     TIMER_OUTPUT_NONE = 0x00,
     TIMER_OUTPUT_INVERTED = 0x02,
@@ -158,6 +176,10 @@ typedef struct timHardwareContext_s {
     TIM_HandleTypeDef * timHandle;
 #endif
     TCH_t               ch[CC_CHANNELS_PER_TIMER];
+#ifdef USE_DSHOT_DMAR
+    DMA_t dmaBurstRef;
+    uint16_t DMASource;
+#endif
 } timHardwareContext_t;
 
 // Per MCU timer definitions
@@ -168,6 +190,20 @@ extern const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT];
 extern timerHardware_t timerHardware[];
 extern const int timerHardwareCount;
 
+#ifdef USE_DSHOT_DMAR
+typedef struct {
+    TIM_TypeDef *timer;
+#ifdef USE_HAL_DRIVER
+    DMA_TypeDef *dma;
+    uint32_t streamLL;
+#else
+    DMA_Stream_TypeDef *dmaBurstStream;
+#endif
+    timerDMASafeType_t *dmaBurstBuffer;
+    uint16_t burstRequestSource;
+} burstDmaTimer_t;
+#endif
+
 typedef enum {
     TYPE_FREE,
     TYPE_PWMINPUT,
@@ -229,3 +265,10 @@ void timerPWMStopDMA(TCH_t * tch);
 bool timerPWMDMAInProgress(TCH_t * tch);
 
 volatile timCCR_t *timerCCR(TCH_t * tch);
+
+uint8_t timer2id(const HAL_Timer_t *tim);
+
+#ifdef USE_DSHOT_DMAR
+bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
+void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength);
+#endif

src/main/drivers/timer_impl.h

@@ -84,3 +84,8 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount);
 void impl_timerPWMStartDMA(TCH_t * tch);
 void impl_timerPWMStopDMA(TCH_t * tch);
+
+#ifdef USE_DSHOT_DMAR
+bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength);
+#endif

src/main/drivers/timer_impl_hal.c

@@ -408,6 +408,109 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     return true;
 }
 
+#ifdef USE_DSHOT_DMAR
+bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
+{
+    tch->dma = dmaGetByTag(tch->timHw->dmaTag);
+    tch->dmaBuffer = dmaBuffer;
+    if (tch->dma == NULL) {
+        return false;
+    }
+
+    // If DMA is already in use - abort
+    if (dmaGetOwner(tch->dma) != OWNER_FREE) {
+        return false;
+    }
+
+    if (!tch->timCtx->dmaBurstRef) {
+        // We assume that timer channels are already initialized by calls to:
+        //  timerConfigBase
+        //  timerPWMConfigChannel
+        const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
+
+        LL_DMA_DeInit(tch->dma->dma, streamLL);
+
+        LL_DMA_InitTypeDef init;
+        LL_DMA_StructInit(&init);
+
+#if defined(STM32H7) || defined(STM32G4)
+        // For H7 the DMA periphRequest is encoded in the DMA tag
+        init.PeriphRequest = DMATAG_GET_CHANNEL(tch->timHw->dmaTag);
+#else
+        init.Channel = lookupDMALLChannelTable[DMATAG_GET_CHANNEL(tch->timHw->dmaTag)];
+#endif
+
+        init.PeriphOrM2MSrcAddress = (uint32_t)&tch->timHw->tim->DMAR;
+        init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
+
+        switch (dmaBufferElementSize) {
+            case 1:
+                init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
+                init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_BYTE;
+                break;
+            case 2:
+                init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD;
+                init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_HALFWORD;
+                break;
+            case 4:
+                init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD;
+                init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD;
+                break;
+            default:
+                // Programmer error
+                while(1) {
+
+                }
+        }
+
+        init.MemoryOrM2MDstAddress = (uint32_t)dmaBuffer;
+        init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
+        init.NbData = dmaBufferElementCount;
+        init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
+        init.Mode = LL_DMA_MODE_NORMAL;
+        init.Priority = LL_DMA_PRIORITY_HIGH;
+        init.FIFOMode = LL_DMA_FIFOMODE_ENABLE;
+        init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL;
+        init.MemBurst = LL_DMA_MBURST_SINGLE;
+        init.PeriphBurst = LL_DMA_PBURST_SINGLE;
+
+        dmaInit(tch->dma, OWNER_TIMER, 0);
+        dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
+
+        LL_DMA_Init(tch->dma->dma, streamLL, &init);
+
+        tch->timCtx->dmaBurstRef = tch->dma;
+        burstDmaTimer->burstRequestSource = lookupDMASourceTable[tch->timHw->channelIndex];
+        burstDmaTimer->streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
+        burstDmaTimer->dma = tch->dma->dma;
+
+        tch->dmaState = TCH_DMA_READY;
+    }
+
+    // Start PWM generation
+    if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
+        HAL_TIMEx_PWMN_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]);
+    }
+    else {
+        HAL_TIM_PWM_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]);
+    }
+
+    return true;
+}
+
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
+{
+    LL_DMA_SetDataLength(burstDmaTimer->dma, burstDmaTimer->streamLL, BurstLength);
+    LL_DMA_EnableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL);
+    LL_DMA_EnableStream(burstDmaTimer->dma, burstDmaTimer->streamLL);
+    /* configure the DMA Burst Mode */
+    LL_TIM_ConfigDMABurst(burstDmaTimer->timer, LL_TIM_DMABURST_BASEADDR_CCR1, LL_TIM_DMABURST_LENGTH_4TRANSFERS);
+    /* Enable the TIM DMA Request */
+    //LL_TIM_EnableDMAReq_UPDATE(burstDmaTimer->timer);
+    LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
+}
+#endif
+
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
 {
     const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];

src/main/drivers/timer_impl_stdperiph.c

@@ -357,6 +357,104 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     return true;
 }
 
+#ifdef USE_DSHOT_DMAR
+bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
+{
+    DMA_InitTypeDef DMA_InitStructure;
+    TIM_TypeDef * timer = tch->timHw->tim;
+    
+    if (!tch->timCtx->dmaBurstRef) {
+        tch->dma = dmaGetByTag(tch->timHw->dmaTag);
+        if (tch->dma == NULL) {
+            return false;
+        }
+
+        // If DMA is already in use - abort
+        if (tch->dma->owner != OWNER_FREE) {
+            return false;
+        }
+    }
+
+    // We assume that timer channels are already initialized by calls to:
+    //  timerConfigBase
+    //  timerPWMConfigChannel
+
+    TIM_CtrlPWMOutputs(timer, ENABLE);
+    TIM_ARRPreloadConfig(timer, ENABLE);
+
+    TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
+    TIM_Cmd(timer, ENABLE);
+
+    if (!tch->timCtx->dmaBurstRef) {
+        dmaInit(tch->dma, OWNER_TIMER, 0);
+        dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
+
+        DMA_DeInit(tch->dma->ref);
+        DMA_Cmd(tch->dma->ref, DISABLE);
+
+        DMA_StructInit(&DMA_InitStructure);
+
+        DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&tch->timHw->tim->DMAR;
+        DMA_InitStructure.DMA_BufferSize = dmaBufferElementCount;
+        DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+        DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+        DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+
+        switch (dmaBufferElementSize) {
+            case 1:
+                DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+                DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+                break;
+            case 2:
+                DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
+                DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
+                break;
+            case 4:
+                DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
+                DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
+                break;
+            default:
+                // Programmer error
+                while(1) {
+
+                }
+        }
+
+#ifdef STM32F4
+        DMA_InitStructure.DMA_Channel = dmaGetChannelByTag(tch->timHw->dmaTag);
+        DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
+        DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+        DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+#else // F3
+        DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dmaBuffer;
+        DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
+        DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+        DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+#endif
+
+        DMA_Init(tch->dma->ref, &DMA_InitStructure);
+        DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE);
+
+        tch->timCtx->dmaBurstRef = tch->dma;
+        tch->timCtx->DMASource = lookupDMASourceTable[tch->timHw->channelIndex];
+        burstDmaTimer->dmaBurstStream = tch->timCtx->dmaBurstRef->ref;
+        burstDmaTimer->burstRequestSource = tch->timCtx->DMASource;
+        
+        tch->dmaState = TCH_DMA_READY;
+    }
+
+    return true;
+}
+
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
+{
+    DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, BurstLength);
+    DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE);
+    TIM_DMAConfig(burstDmaTimer->timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers);
+    TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
+}
+#endif
+
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
 {
     // Make sure we terminate any DMA transaction currently in progress

src/main/fc/cli.c

@@ -156,6 +156,13 @@ static const char * const featureNames[] = {
     "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL
 };
 
+static const char * outputModeNames[] = {
+    "AUTO",
+    "MOTORS",
+    "SERVOS",
+    NULL
+};
+
 #ifdef USE_BLACKBOX
 static const char * const blackboxIncludeFlagNames[] = {
     "NAV_ACC",
@@ -2514,6 +2521,82 @@ static void cliOsdLayout(char *cmdline)
 
 #endif
 
+static void printTimerOutputModes(dumpFlags_e dumpFlags, const timerOverride_t* to, const timerOverride_t* defaultTimerOverride, int timer)
+{
+    const char *format = "timer_output_mode %d %s";
+
+    for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
+        if (timer < 0 || timer == i) {
+            outputMode_e mode = to[i].outputMode;
+            bool equalsDefault = false;
+            if(defaultTimerOverride) {
+                outputMode_e defaultMode = defaultTimerOverride[i].outputMode;
+                equalsDefault = mode == defaultMode;
+                cliDefaultPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[defaultMode]);
+            }
+            cliDumpPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[mode]);
+        }
+    }
+}
+
+static void cliTimerOutputMode(char *cmdline)
+{
+    char * saveptr;
+
+    int timer = -1;
+    uint8_t mode;
+    char *tok = strtok_r(cmdline, " ", &saveptr);
+
+    int ii;
+
+    for (ii = 0; tok != NULL; ii++, tok = strtok_r(NULL, " ", &saveptr)) {
+        switch (ii) {
+            case 0:
+                timer = fastA2I(tok);
+                if (timer < 0 || timer >= HARDWARE_TIMER_DEFINITION_COUNT) {
+                    cliShowParseError();
+                    return;
+                }
+                break;
+            case 1:
+                if(!sl_strcasecmp("AUTO", tok)) {
+                    mode =  OUTPUT_MODE_AUTO;
+                } else if(!sl_strcasecmp("MOTORS", tok)) {
+                    mode = OUTPUT_MODE_MOTORS;
+                } else if(!sl_strcasecmp("SERVOS", tok)) {
+                    mode = OUTPUT_MODE_SERVOS;
+                } else {
+                    cliShowParseError();
+                    return;
+                }
+                break;
+           default:
+                cliShowParseError();
+                return;
+        }
+    }
+
+    switch (ii) {
+        case 0:
+            FALLTHROUGH;
+        case 1:
+            // No args, or just timer. If any of them not provided,
+            // it will be the -1 that we used during initialization, so printOsdLayout()
+            // won't use them for filtering.
+            printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer);
+            break;
+        case 2:
+            timerOverridesMutable(timer)->outputMode = mode;
+            printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer);
+            break;
+        default:
+            // Unhandled
+            cliShowParseError();
+            return;
+    }
+
+}
+
 static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault)
 {
     uint32_t mask = featureConfig->enabledFeatures;
@@ -3652,6 +3735,9 @@ static void printConfig(const char *cmdline, bool doDiff)
         cliPrintHashLine("resources");
         //printResource(dumpMask, &defaultConfig);
 
+        cliPrintHashLine("Timer overrides");
+        printTimerOutputModes(dumpMask, timerOverrides_CopyArray, timerOverrides(0), -1);
+
         cliPrintHashLine("Mixer: motor mixer");
         cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n");
 
@@ -3968,6 +4054,7 @@ const clicmd_t cmdTable[] = {
 #ifdef USE_OSD
     CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[<layout> [<item> [<col> <row> [<visible>]]]]", cliOsdLayout),
 #endif
+    CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.",  "[<timer> [<AUTO|MOTORS|SERVOS>]]", cliTimerOutputMode),
 };
 
 static void cliHelp(char *cmdline)

src/main/fc/fc_core.c

@@ -54,6 +54,7 @@
 #include "fc/cli.h"
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
+#include "fc/multifunction.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_smoothing.h"
 #include "fc/rc_controls.h"
@@ -377,7 +378,7 @@ static bool emergencyArmingCanOverrideArmingDisabled(void)
 
 static bool emergencyArmingIsEnabled(void)
 {
-    return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM)) && emergencyArmingCanOverrideArmingDisabled();
+    return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM), false) && emergencyArmingCanOverrideArmingDisabled();
 }
 
 static void processPilotAndFailSafeActions(float dT)
@@ -469,7 +470,7 @@ disarmReason_t getDisarmReason(void)
     return lastDisarmReason;
 }
 
-bool emergencyArmingUpdate(bool armingSwitchIsOn)
+bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
 {
     if (ARMING_FLAG(ARMED)) {
         return false;
@@ -498,6 +499,10 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn)
         toggle = true;
     }
 
+    if (forceArm) {
+        counter = EMERGENCY_ARMING_MIN_ARM_COUNT;
+    }
+
     return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
 }
 
@@ -532,9 +537,12 @@ void tryArm(void)
     }
 
 #ifdef USE_DSHOT
-    if (STATE(MULTIROTOR) && IS_RC_MODE_ACTIVE(BOXTURTLE) && !FLIGHT_MODE(TURTLE_MODE) &&
+#ifdef USE_MULTI_FUNCTIONS
-        emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()
+    const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE);
-        ) {
+#else
+    const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE);
+#endif
+    if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
         ENABLE_ARMING_FLAG(ARMED);
         enableFlightMode(TURTLE_MODE);
@@ -854,13 +862,25 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
 #endif
 }
 
-static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength)
+static void applyThrottleTiltCompensation(void)
 {
-    if (throttleTiltCompensationStrength) {
+    if (STATE(MULTIROTOR)) {
+        int16_t thrTiltCompStrength = 0;
+
+        if (navigationRequiresThrottleTiltCompensation()) {
+            thrTiltCompStrength = 100;
+        }
+        else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+            thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
+        }
+
+        if (thrTiltCompStrength) {
+            const int throttleIdleValue = getThrottleIdleValue();
             float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f);  // max tilt about 50 deg
-        return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f);
+            tiltCompFactor = 1.0f + (tiltCompFactor - 1.0f) * (thrTiltCompStrength / 100.f);
-    } else {
+
-        return 1.0f;
+            rcCommand[THROTTLE] = setDesiredThrottle(throttleIdleValue + (rcCommand[THROTTLE] - throttleIdleValue) * tiltCompFactor, false);
+        }
     }
 }
 
@@ -912,26 +932,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     applyWaypointNavigationAndAltitudeHold();
 
     // Apply throttle tilt compensation
-    if (!STATE(FIXED_WING_LEGACY)) {
+    applyThrottleTiltCompensation();
-        int16_t thrTiltCompStrength = 0;
-
-        if (navigationRequiresThrottleTiltCompensation()) {
-            thrTiltCompStrength = 100;
-        }
-        else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
-            thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
-        }
-
-        if (thrTiltCompStrength) {
-            rcCommand[THROTTLE] = constrain(getThrottleIdleValue()
-                                            + (rcCommand[THROTTLE] - getThrottleIdleValue()) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
-                                            getThrottleIdleValue(),
-                                            motorConfig()->maxthrottle);
-        }
-    }
-    else {
-        // FIXME: throttle pitch comp for FW
-    }
 
 #ifdef USE_POWER_LIMITS
     powerLimiterApply(&rcCommand[THROTTLE]);

src/main/fc/fc_core.h

@@ -42,7 +42,7 @@ timeUs_t getLastDisarmTimeUs(void);
 void tryArm(void);
 disarmReason_t getDisarmReason(void);
 
-bool emergencyArmingUpdate(bool armingSwitchIsOn);
+bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm);
 
 bool areSensorsCalibrating(void);
 float getFlightTime(void);

src/main/fc/fc_msp.c

@@ -1289,7 +1289,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, navConfig()->general.max_manual_speed);
         sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
         sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
-        sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold);
+        sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
         sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
         break;
 
@@ -1516,6 +1516,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
         break;
 
+    case MSP2_INAV_OUTPUT_MAPPING_EXT:
+        for (uint8_t i = 0; i < timerHardwareCount; ++i)
+            if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
+                #if defined(SITL_BUILD)
+                sbufWriteU8(dst, i);
+                #else
+                sbufWriteU8(dst, timer2id(timerHardware[i].tim));
+                #endif
+                sbufWriteU8(dst, timerHardware[i].usageFlags);
+            }
+        break;
+    
     case MSP2_INAV_MC_BRAKING:
 #ifdef USE_MR_BRAKING_MODE
         sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
@@ -2255,7 +2267,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
             navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
             navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
-            navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
+            navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
             currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
         } else
             return MSP_RESULT_ERROR;
@@ -3661,6 +3673,42 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         *ret = MSP_RESULT_ACK;
         break;
 #endif
+#ifndef SITL_BUILD
+    case MSP2_INAV_TIMER_OUTPUT_MODE:
+        if (dataSize == 0) {
+            for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
+                sbufWriteU8(dst, i);
+                sbufWriteU8(dst, timerOverrides(i)->outputMode);
+            }
+            *ret = MSP_RESULT_ACK;
+        } else if(dataSize == 1) {
+            uint8_t timer = sbufReadU8(src);
+            if(timer < HARDWARE_TIMER_DEFINITION_COUNT) {
+                sbufWriteU8(dst, timer);
+                sbufWriteU8(dst, timerOverrides(timer)->outputMode);
+                *ret = MSP_RESULT_ACK;
+            } else {
+                *ret = MSP_RESULT_ERROR;
+            }
+        } else {
+            *ret = MSP_RESULT_ERROR;
+        }
+        break;
+    case MSP2_INAV_SET_TIMER_OUTPUT_MODE:
+        if(dataSize == 2) {
+            uint8_t timer = sbufReadU8(src);
+            uint8_t outputMode = sbufReadU8(src);
+            if(timer < HARDWARE_TIMER_DEFINITION_COUNT) {
+                timerOverridesMutable(timer)->outputMode = outputMode;
+                *ret = MSP_RESULT_ACK;
+            } else {
+                *ret = MSP_RESULT_ERROR;
+            }
+        } else {
+            *ret = MSP_RESULT_ERROR;
+        }
+        break;
+#endif 
     
     default:
         // Not handled

src/main/fc/fc_msp_box.c

@@ -98,6 +98,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXSOARING,          .boxName = "SOARING",           .permanentId = 56 },
     { .boxId = BOXCHANGEMISSION,    .boxName = "MISSION CHANGE",    .permanentId = 59 },
     { .boxId = BOXBEEPERMUTE,       .boxName = "BEEPER MUTE",       .permanentId = 60 },
+    { .boxId = BOXMULTIFUNCTION,    .boxName = "MULTI FUNCTION",    .permanentId = 61 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -179,6 +180,9 @@ void initActiveBoxIds(void)
     RESET_BOX_ID_COUNT;
     ADD_ACTIVE_BOX(BOXARM);
     ADD_ACTIVE_BOX(BOXPREARM);
+#ifdef USE_MULTI_FUNCTIONS
+    ADD_ACTIVE_BOX(BOXMULTIFUNCTION);
+#endif
 
     if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
         ADD_ACTIVE_BOX(BOXANGLE);
@@ -227,6 +231,8 @@ void initActiveBoxIds(void)
         if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) {
             ADD_ACTIVE_BOX(BOXNAVRTH);
             ADD_ACTIVE_BOX(BOXNAVWP);
+            ADD_ACTIVE_BOX(BOXNAVCRUISE);
+            ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
             ADD_ACTIVE_BOX(BOXHOMERESET);
             ADD_ACTIVE_BOX(BOXGCSNAV);
             ADD_ACTIVE_BOX(BOXPLANWPMISSION);
@@ -236,8 +242,6 @@ void initActiveBoxIds(void)
         }
 
         if (STATE(AIRPLANE)) {
-            ADD_ACTIVE_BOX(BOXNAVCRUISE);
-            ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
             ADD_ACTIVE_BOX(BOXSOARING);
         }
     }
@@ -412,7 +416,9 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #ifdef USE_MULTI_MISSION
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)),   BOXCHANGEMISSION);
 #endif
-
+#ifdef USE_MULTI_FUNCTIONS
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)),   BOXMULTIFUNCTION);
+#endif
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
         if (activeBoxes[activeBoxIds[i]]) {

src/main/fc/fc_tasks.c

@@ -503,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_PITOT] = {
         .taskName = "PITOT",
         .taskFunc = taskUpdatePitot,
-        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .desiredPeriod = TASK_PERIOD_MS(20),
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif

src/main/fc/multifunction.c

@@ -0,0 +1,132 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+#include "build/debug.h"
+#include "drivers/time.h"
+
+#ifdef USE_MULTI_FUNCTIONS
+
+#include "fc/fc_core.h"
+#include "fc/multifunction.h"
+#include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
+
+#include "io/osd.h"
+#include "navigation/navigation.h"
+
+multi_function_e selectedItem = MULTI_FUNC_NONE;
+uint8_t multiFunctionFlags;
+bool nextItemIsAvailable = false;
+
+static void multiFunctionApply(multi_function_e selectedItem)
+{
+    switch (selectedItem) {
+    case MULTI_FUNC_NONE:
+        break;
+    case MULTI_FUNC_1:  // redisplay current warnings
+        osdResetWarningFlags();
+        break;
+    case MULTI_FUNC_2:  // control manual emergency landing
+        checkManualEmergencyLandingControl(ARMING_FLAG(ARMED));
+        break;
+    case MULTI_FUNC_3:  // toggle Safehome suspend
+#if defined(USE_SAFE_HOME)
+        if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
+            MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES);
+        }
+#endif
+        break;
+    case MULTI_FUNC_4:  // toggle RTH Trackback suspend
+        if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
+            MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK);
+        }
+        break;
+    case MULTI_FUNC_5:
+#ifdef USE_DSHOT
+        if (STATE(MULTIROTOR)) {    // toggle Turtle mode
+            MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE);
+        }
+#endif
+        break;
+    case MULTI_FUNC_6:  // emergency ARM
+        if (!ARMING_FLAG(ARMED)) {
+            emergencyArmingUpdate(true, true);
+        }
+    case MULTI_FUNC_END:
+        break;
+    }
+}
+
+bool isNextMultifunctionItemAvailable(void)
+{
+    return nextItemIsAvailable;
+}
+
+void setMultifunctionSelection(multi_function_e item)
+{
+    selectedItem = item == MULTI_FUNC_END ? MULTI_FUNC_1 : item;
+    nextItemIsAvailable = false;
+}
+
+multi_function_e multiFunctionSelection(void)
+{
+    static timeMs_t startTimer;
+    static timeMs_t selectTimer;
+    static bool toggle = true;
+    const timeMs_t currentTime = millis();
+
+    if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) {
+        if (selectTimer) {
+            if (currentTime - selectTimer > 3000) {     // 3s selection duration to activate selected function
+                multiFunctionApply(selectedItem);
+                selectTimer = 0;
+                selectedItem = MULTI_FUNC_NONE;
+                nextItemIsAvailable = false;
+            }
+        } else if (toggle) {
+            if (selectedItem == MULTI_FUNC_NONE) {
+                selectedItem++;
+            } else {
+                selectTimer = currentTime;
+                nextItemIsAvailable = true;
+            }
+        }
+        startTimer = currentTime;
+        toggle = false;
+    } else if (startTimer) {
+        if (!toggle && selectTimer) {
+            setMultifunctionSelection(++selectedItem);
+        }
+        if (currentTime - startTimer > 4000) {      // 4s reset delay after mode deselected
+            startTimer = 0;
+            selectedItem = MULTI_FUNC_NONE;
+        }
+        selectTimer = 0;
+        toggle = true;
+    }
+
+    return selectedItem;
+}
+#endif

src/main/fc/multifunction.h

@@ -0,0 +1,55 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#ifdef USE_MULTI_FUNCTIONS
+
+extern uint8_t multiFunctionFlags;
+
+#define MULTI_FUNC_FLAG_DISABLE(mask) (multiFunctionFlags &= ~(mask))
+#define MULTI_FUNC_FLAG_ENABLE(mask) (multiFunctionFlags |= (mask))
+#define MULTI_FUNC_FLAG(mask) (multiFunctionFlags & (mask))
+
+typedef enum {
+    MF_SUSPEND_SAFEHOMES    = (1 << 0),
+    MF_SUSPEND_TRACKBACK    = (1 << 1),
+    MF_TURTLE_MODE          = (1 << 2),
+} multiFunctionFlags_e;
+
+typedef enum {
+    MULTI_FUNC_NONE,
+    MULTI_FUNC_1,
+    MULTI_FUNC_2,
+    MULTI_FUNC_3,
+    MULTI_FUNC_4,
+    MULTI_FUNC_5,
+    MULTI_FUNC_6,
+    MULTI_FUNC_END,
+} multi_function_e;
+
+multi_function_e multiFunctionSelection(void);
+bool isNextMultifunctionItemAvailable(void);
+void setMultifunctionSelection(multi_function_e item);
+#endif

src/main/fc/rc_controls.c

@@ -225,7 +225,7 @@ void processRcStickPositions(bool isThrottleLow)
             rcDisarmTimeMs = currentTimeMs;
             tryArm();
         } else {
-            emergencyArmingUpdate(armingSwitchIsActive);
+            emergencyArmingUpdate(armingSwitchIsActive, false);
             // Disarming via ARM BOX
             // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
             // and can't afford to risk disarming in the air

src/main/fc/rc_modes.h

@@ -78,6 +78,7 @@ typedef enum {
     BOXUSER4         = 49,
     BOXCHANGEMISSION = 50,
     BOXBEEPERMUTE    = 51,
+    BOXMULTIFUNCTION = 52,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/settings.yaml

@@ -19,7 +19,7 @@ tables:
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"]
     enum: baroSensor_e
   - name: pitot_hardware
-    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
+    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"]
     enum: pitotSensor_e
   - name: receiver_type
     values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
@@ -112,9 +112,6 @@ tables:
     enum: smartportFuelUnit_e
   - name: platform_type
     values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"]
-  - name: output_mode
-    values: ["AUTO", "MOTORS", "SERVOS"]
-    enum: outputMode_e
   - name: tz_automatic_dst
     values: ["OFF", "EU", "USA"]
     enum: tz_automatic_dst_e
@@ -193,6 +190,9 @@ tables:
   - name: gps_auto_baud_max
     values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600']
     enum: gpsBaudRate_e
+  - name: nav_mc_althold_throttle
+    values: ["STICK", "MID_STICK", "HOVER"]
+    enum: navMcAltHoldThrottle_e    
 
 constants:
   RPYL_PID_MIN: 0
@@ -585,7 +585,7 @@ groups:
     members:
       - name: pitot_hardware
         description: "Selection of pitot hardware."
-        default_value: "NONE"
+        default_value: "VIRTUAL"
         table: pitot_hardware
       - name: pitot_lpf_milli_hz
         min: 0
@@ -1159,11 +1159,6 @@ groups:
         field: appliedMixerPreset
         min: -1
         max: INT16_MAX
-      - name: output_mode
-        description: "Output function assignment mode. AUTO assigns outputs according to the default mapping, SERVOS assigns all outputs to servos, MOTORS assigns all outputs to motors"
-        default_value: "AUTO"
-        field: outputMode
-        table: output_mode
 
   - name: PG_REVERSIBLE_MOTORS_CONFIG
     type: reversibleMotorsConfig_t
@@ -2320,11 +2315,11 @@ groups:
         default_value: OFF
         field: general.flags.landing_bump_detection
         type: bool
-      - name: nav_use_midthr_for_althold
+      - name: nav_mc_althold_throttle
-        description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude"
+        description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively."
-        default_value: OFF
+        default_value: "STICK"
-        field: general.flags.use_thr_mid_for_althold
+        field: mc.althold_throttle_type
-        type: bool
+        table: nav_mc_althold_throttle
       - name: nav_extra_arming_safety
         description: "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"
         default_value: "ALLOW_BYPASS"
@@ -2581,6 +2576,12 @@ groups:
         default_value: ON
         field: general.flags.mission_planner_reset
         type: bool
+      - name: nav_cruise_yaw_rate
+        description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]"
+        default_value: 20
+        field: general.cruise_yaw_rate
+        min: 0
+        max: 120
       - 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
@@ -2816,12 +2817,6 @@ groups:
         field: fw.launch_abort_deadband
         min: 2
         max: 250
-      - name: nav_fw_cruise_yaw_rate
-        description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]"
-        default_value: 20
-        field: fw.cruise_yaw_rate
-        min: 0
-        max: 60
       - name: nav_fw_allow_manual_thr_increase
         description: "Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing"
         default_value: OFF

src/main/flight/failsafe.c

@@ -146,6 +146,10 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
  */
 void failsafeReset(void)
 {
+    if (failsafeState.active) {  // can't reset if still active
+        return;
+    }
+
     failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
     failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
     failsafeState.validRxDataReceivedAt = 0;
@@ -157,6 +161,7 @@ void failsafeReset(void)
     failsafeState.phase = FAILSAFE_IDLE;
     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
     failsafeState.activeProcedure = failsafeConfig()->failsafe_procedure;
+    failsafeState.controlling = false;
 
     failsafeState.lastGoodRcCommand[ROLL] = 0;
     failsafeState.lastGoodRcCommand[PITCH] = 0;
@@ -211,14 +216,6 @@ bool failsafeRequiresAngleMode(void)
            failsafeProcedureLogic[failsafeState.activeProcedure].forceAngleMode;
 }
 
-bool failsafeRequiresMotorStop(void)
-{
-    return failsafeState.active &&
-           failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
-           posControl.flags.estAltStatus < EST_USABLE &&
-           currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
-}
-
 void failsafeStartMonitoring(void)
 {
     failsafeState.monitoring = true;
@@ -353,11 +350,9 @@ 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 &&
+    if (failsafeConfig()->failsafe_min_distance > 0 && sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
-            sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
-
         // get the distance to the original arming point
-        uint32_t distance = calculateDistanceToDestination(&original_rth_home);
+        uint32_t distance = calculateDistanceToDestination(&posControl.rthState.originalHomePosition);
         if (distance < failsafeConfig()->failsafe_min_distance) {
             // Use the alternate, minimum distance failsafe procedure instead
             return failsafeConfig()->failsafe_min_distance_procedure;

src/main/flight/failsafe.h

@@ -170,7 +170,6 @@ void failsafeOnRxResume(void);
 bool failsafeMayRequireNavigationMode(void);
 void failsafeApplyControlInput(void);
 bool failsafeRequiresAngleMode(void);
-bool failsafeRequiresMotorStop(void);
 bool failsafeShouldApplyControlInput(void);
 bool failsafeBypassNavigation(void);
 void failsafeUpdateRcCommandValues(void);

src/main/flight/mixer.c

@@ -28,6 +28,7 @@
 #include "common/maths.h"
 #include "common/utils.h"
 
+#include "config/config_reset.h"
 #include "config/feature.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -88,10 +89,9 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
     .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
     .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
     .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
-    .outputMode = SETTING_OUTPUT_MODE_DEFAULT,
 );
 
-PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9);
+PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 10);
 
 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
@@ -103,8 +103,17 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 
+PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0);
+
 #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
 
+void pgResetFn_timerOverrides(timerOverride_t *instance)
+{
+    for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
+        RESET_CONFIG(timerOverride_t, &instance[i], .outputMode = OUTPUT_MODE_AUTO);
+    }
+}
+
 int getThrottleIdleValue(void)
 {
     if (!throttleIdleValue) {
@@ -208,6 +217,7 @@ void mixerInit(void)
 
 void mixerResetDisarmedMotors(void)
 {
+    getThrottleIdleValue();
 
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
         motorZeroCommand = reversibleMotorsConfig()->neutral;
@@ -215,7 +225,7 @@ void mixerResetDisarmedMotors(void)
         throttleRangeMax = motorConfig()->maxthrottle;
     } else {
         motorZeroCommand = motorConfig()->mincommand;
-        throttleRangeMin = getThrottleIdleValue();
+        throttleRangeMin = throttleIdleValue;
         throttleRangeMax = motorConfig()->maxthrottle;
     }
 
@@ -224,7 +234,7 @@ void mixerResetDisarmedMotors(void)
     if (feature(FEATURE_MOTOR_STOP)) {
         motorValueWhenStopped = motorZeroCommand;
     } else {
-        motorValueWhenStopped = getThrottleIdleValue();
+        motorValueWhenStopped = throttleIdleValue;
     }
 
     // set disarmed motor values
@@ -469,6 +479,19 @@ void FAST_CODE mixTable(void)
         return;
     }
 #endif
+#ifdef USE_DEV_TOOLS
+    bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode;
+#else
+    bool isDisarmed = !ARMING_FLAG(ARMED);
+#endif
+    bool motorStopIsActive = getMotorStatus() != MOTOR_RUNNING && !isDisarmed;
+    if (isDisarmed || motorStopIsActive) {
+        for (int i = 0; i < motorCount; i++) {
+            motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped;
+        }
+        mixerThrottleCommand = motor[0];
+        return;
+    }
 
     int16_t input[3];   // RPY, range [-500:+500]
     // Allow direct stick input to motors in passthrough mode on airplanes
@@ -551,11 +574,11 @@ void FAST_CODE mixTable(void)
         throttleRangeMax = motorConfig()->maxthrottle;
 
         // Throttle scaling to limit max throttle when battery is full
-    #ifdef USE_PROGRAMMING_FRAMEWORK
+#ifdef USE_PROGRAMMING_FRAMEWORK
         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
-    #else
+#else
         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
-    #endif
+#endif
         // Throttle compensation based on battery voltage
         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
             mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
@@ -564,7 +587,6 @@ void FAST_CODE mixTable(void)
 
     throttleMin = throttleRangeMin;
     throttleMax = throttleRangeMax;
-
     throttleRange = throttleMax - throttleMin;
 
     #define THROTTLE_CLIPPING_FACTOR    0.33f
@@ -584,8 +606,6 @@ void FAST_CODE mixTable(void)
 
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
-    if (ARMING_FLAG(ARMED)) {
-        const motorStatus_e currentMotorStatus = getMotorStatus();
     for (int i = 0; i < motorCount; i++) {
         motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
 
@@ -594,44 +614,35 @@ void FAST_CODE mixTable(void)
         } else {
             motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
         }
-
-            // Motor stop handling
-            if (currentMotorStatus != MOTOR_RUNNING) {
-                motor[i] = motorValueWhenStopped;
-            }
-#ifdef USE_DEV_TOOLS
-            if (systemConfig()->groundTestMode) {
-                motor[i] = motorZeroCommand;
-            }
-#endif
-        }
-    } else {
-        for (int i = 0; i < motorCount; i++) {
-            motor[i] = motor_disarmed[i];
-        }
     }
 }
 
 int16_t getThrottlePercent(bool useScaled)
 {
-    int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
+    int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX);
-    const int idleThrottle = getThrottleIdleValue();
 
     if (useScaled) {
-       thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle);
+       thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue);
     } else {
         thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
     }
     return thr;
 }
 
+uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop)
+{
+    const uint16_t throttleIdleValue = getThrottleIdleValue();
+
+    if (allowMotorStop && throttle < throttleIdleValue) {
+        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
+        return throttle;
+    }
+    return constrain(throttle, throttleIdleValue, motorConfig()->maxthrottle);
+}
+
 motorStatus_e getMotorStatus(void)
 {
-    if (failsafeRequiresMotorStop()) {
+    if (STATE(NAV_MOTOR_STOP_OR_IDLE)) {
-        return MOTOR_STOPPED_AUTO;
-    }
-
-    if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) {
         return MOTOR_STOPPED_AUTO;
     }
 

src/main/flight/mixer.h

@@ -19,6 +19,8 @@
 
 #include "config/parameter_group.h"
 
+#include "drivers/timer.h"
+
 #if defined(TARGET_MOTOR_COUNT)
 #define MAX_SUPPORTED_MOTORS TARGET_MOTOR_COUNT
 #else
@@ -64,12 +66,17 @@ typedef struct motorMixer_s {
 
 PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer);
 
+typedef struct timerOverride_s {
+    uint8_t outputMode;
+} timerOverride_t;
+
+PG_DECLARE_ARRAY(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides);
+
 typedef struct mixerConfig_s {
     int8_t motorDirectionInverted;
     uint8_t platformType;
     bool hasFlaps;
     int16_t appliedMixerPreset;
-    uint8_t outputMode;
 } mixerConfig_t;
 
 PG_DECLARE(mixerConfig_t, mixerConfig);
@@ -112,6 +119,7 @@ extern int mixerThrottleCommand;
 
 int getThrottleIdleValue(void);
 int16_t getThrottlePercent(bool);
+uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop);
 uint8_t getMotorCount(void);
 float getMotorMixRange(void);
 bool mixerIsOutputSaturated(void);

src/main/flight/pid.c

@@ -886,8 +886,6 @@ static uint8_t getHeadingHoldState(void)
     }
     else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
         return HEADING_HOLD_ENABLED;
-    } else {
-        return HEADING_HOLD_UPDATE_HEADING;
     }
 
     return HEADING_HOLD_UPDATE_HEADING;
@@ -900,17 +898,14 @@ float pidHeadingHold(float dT)
 {
     float headingHoldRate;
 
+    /* Convert absolute error into relative to current heading */
     int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget;
 
-    /*
+    /* Convert absolute error into relative to current heading */
-     * Convert absolute error into relative to current heading
+    if (error > 180) {
-     */
-    if (error <= -180) {
-        error += 360;
-    }
-
-    if (error >= +180) {
         error -= 360;
+    } else if (error < -180) {
+        error += 360;
     }
 
     /*
@@ -1127,7 +1122,7 @@ void FAST_CODE pidController(float dT)
         }
     }
 
-    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
+    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
         float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
         float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
         pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);

src/main/io/ledstrip.h

@@ -159,12 +159,12 @@ typedef struct ledStripConfig_s {
 PG_DECLARE(ledStripConfig_t, ledStripConfig);
 
 #define DEFINE_LED(ledConfigPtr, x, y, col, dir, func, ol, params) { \
-  ledConfigPtr->led_position = CALCULATE_LED_XY(x, y); \
+  (ledConfigPtr)->led_position = CALCULATE_LED_XY(x, y); \
-  ledConfigPtr->led_color = (col); \
+  (ledConfigPtr)->led_color = (col); \
-  ledConfigPtr->led_direction = (dir); \
+  (ledConfigPtr)->led_direction = (dir); \
-  ledConfigPtr->led_function = (func); \
+  (ledConfigPtr)->led_function = (func); \
-  ledConfigPtr->led_overlay = (ol); \
+  (ledConfigPtr)->led_overlay = (ol); \
-  ledConfigPtr->led_params = (params); }
+  (ledConfigPtr)->led_params = (params); }
 
 static inline uint8_t ledGetXY(const ledConfig_t *lcfg)         { return (lcfg->led_position); }
 static inline uint8_t ledGetX(const ledConfig_t *lcfg)          { return ((lcfg->led_position >> (LED_X_BIT_OFFSET)) & LED_XY_MASK); }

src/main/io/osd.c

@@ -76,6 +76,7 @@
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
 #include "fc/fc_tasks.h"
+#include "fc/multifunction.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
@@ -99,6 +100,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
+#include "sensors/compass.h"
 #include "sensors/diagnostics.h"
 #include "sensors/sensors.h"
 #include "sensors/pitotmeter.h"
@@ -186,6 +188,9 @@ static bool fullRedraw = false;
 
 static uint8_t armState;
 
+static textAttributes_t osdGetMultiFunctionMessage(char *buff);
+static uint8_t osdWarningsFlags = 0;
+
 typedef struct osdMapData_s {
     uint32_t scale;
     char referenceSymbol;
@@ -826,14 +831,15 @@ static const char * osdArmingDisabledReasonMessage(void)
             // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
             if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) {
                 if (failsafeIsReceivingRxData()) {
-                    // If we're not using sticks, it means the ARM switch
+                    // reminder to disarm to exit FAILSAFE_RX_LOSS_MONITORING once timeout period ends
-                    // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
+                    if (IS_RC_MODE_ACTIVE(BOXARM)) {
-                    // yet
                         return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF);
                     }
+                } else {
                     // Not receiving RX data
                     return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
                 }
+            }
             return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS);
         case ARMING_DISABLED_NOT_LEVEL:
             return OSD_MESSAGE_STR(OSD_MSG_AIRCRAFT_UNLEVEL);
@@ -986,7 +992,7 @@ static const char * osdFailsafeInfoMessage(void)
 #if defined(USE_SAFE_HOME)
 static const char * divertingToSafehomeMessage(void)
 {
-	if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) {
+    if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
         return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
     }
     return NULL;
@@ -1032,7 +1038,7 @@ static const char * navigationStateMessage(void)
         case MW_NAV_STATE_HOVER_ABOVE_HOME:
             if (STATE(FIXED_WING_LEGACY)) {
 #if defined(USE_SAFE_HOME)
-                if (safehome_applied) {
+                if (posControl.safehomeState.isApplied) {
                     return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME);
                 }
 #endif
@@ -1111,11 +1117,7 @@ bool osdUsingScaledThrottle(void)
  **/
 static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr)
 {
-    if (useScaled) {
-        buff[0] = SYM_SCALE;
-    } else {
     buff[0] = SYM_BLANK;
-    }
     buff[1] = SYM_THR;
     if (navigationIsControllingThrottle()) {
         buff[0] = SYM_AUTO_THR0;
@@ -1130,7 +1132,14 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes
         TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
     }
 #endif
-    tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled));
+    int8_t throttlePercent = getThrottlePercent(useScaled);
+    if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) {
+        const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM";
+        buff[0] = SYM_THR;
+        strcpy(buff + 1, message);
+        return;
+    }
+    tfp_sprintf(buff + 2, "%3d", throttlePercent);
 }
 
 /**
@@ -1787,8 +1796,10 @@ static bool osdDrawSingleElement(uint8_t item)
         buff[1] = SYM_SAT_R;
         tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
         if (!STATE(GPS_FIX)) {
-            if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
+            hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
-                strcpy(buff + 2, "X!");
+            if (sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY) {
+                buff[2] = SYM_ALERT;
+                buff[3] = '\0';
             }
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         }
@@ -2022,8 +2033,24 @@ static bool osdDrawSingleElement(uint8_t item)
 
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
+            displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
+
+            if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
+                /* Indicate MR altitude adjustment active with constant symbol at first blank position.
+                 * Alternate symbol on/off with 600ms cycle if first position not blank (to maintain visibility of -ve sign) */
+                int8_t blankPos;
+                for (blankPos = 2; blankPos >= 0; blankPos--) {
+                    if (buff[blankPos] == SYM_BLANK) {
                         break;
                     }
+                }
+                if (blankPos >= 0 || OSD_ALTERNATING_CHOICES(600, 2) == 0) {
+                    blankPos = blankPos < 0 ? 0 : blankPos;
+                    displayWriteChar(osdDisplayPort, elemPosX + blankPos, elemPosY, SYM_TERRAIN_FOLLOWING);
+                }
+            }
+            return true;
+        }
 
     case OSD_ALTITUDE_MSL:
         {
@@ -3458,6 +3485,21 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 #endif // USE_ADC
 #endif // USE_POWER_LIMITS
+    case OSD_MULTI_FUNCTION:
+        {
+            // message shown infrequently so only write when needed
+            static bool clearMultiFunction = true;
+            elemAttr = osdGetMultiFunctionMessage(buff);
+            if (buff[0] == 0) {
+                if (clearMultiFunction) {
+                    displayWrite(osdDisplayPort, elemPosX, elemPosY, "          ");
+                    clearMultiFunction = false;
+                }
+                return true;
+            }
+            clearMultiFunction = true;
+            break;
+        }
 
     default:
         return false;
@@ -3835,6 +3877,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3);
     osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4);
 
+    osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4);
+
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7);
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8);
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9);
@@ -4402,13 +4446,13 @@ static void osdShowArmed(void)
             }
             y += 4;
 #if defined (USE_SAFE_HOME)
-            if (safehome_distance) { // safehome found during arming
+            if (posControl.safehomeState.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);
+                    osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
-					tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index);
+                    tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
                 }
                 textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
                 // write this message above the ARMED message to make it obvious
@@ -4536,7 +4580,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             osdShowArmed();
             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
 #if defined(USE_SAFE_HOME)
-            if (safehome_distance)
+            if (posControl.safehomeState.distance)
                 delay *= 3;
 #endif
             osdSetNextRefreshIn(delay);
@@ -4865,6 +4909,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         // by OSD_FLYMODE.
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
                     }
+                    if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
+                        if (posControl.cruise.multicopterSpeed >= 50.0f) {
+                            char buf[6];
+                            osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false);
+                            tfp_sprintf(messageBuf, "(SPD %s)", buf);
+                        } else {
+                            strcpy(messageBuf, "(HOLD)");
+                        }
+                        messages[messageCount++] = messageBuf;
+                    }
                     if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
                     }
@@ -4935,12 +4989,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             }
         }
 
-#ifdef USE_DEV_TOOLS
-        if (systemConfig()->groundTestMode) {
-            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE);
-        }
-#endif
-
         if (messageCount > 0) {
             message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)];
             if (message == failsafeInfoMessage) {
@@ -4963,4 +5011,191 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
     return elemAttr;
 }
 
+void osdResetWarningFlags(void)
+{
+    osdWarningsFlags = 0;
+}
+
+static bool osdCheckWarning(bool condition, uint8_t warningFlag, uint8_t *warningsCount)
+{
+#define WARNING_REDISPLAY_DURATION 5000;    // milliseconds
+
+    const timeMs_t currentTimeMs = millis();
+    static timeMs_t warningDisplayStartTime = 0;
+    static timeMs_t redisplayStartTimeMs = 0;
+    static uint16_t osdWarningTimerDuration;
+    static uint8_t newWarningFlags;
+
+    if (condition) {    // condition required to trigger warning
+        if (!(osdWarningsFlags & warningFlag)) {
+            osdWarningsFlags |= warningFlag;
+            newWarningFlags |= warningFlag;
+            redisplayStartTimeMs = 0;
+        }
+#ifdef USE_DEV_TOOLS
+        if (systemConfig()->groundTestMode) {
+            return true;
+        }
+#endif
+        /* Warnings displayed in full for set time before shrinking down to alert symbol with warning count only.
+         * All current warnings then redisplayed for 5s on 30s rolling cycle.
+         * New warnings dislayed individually for 10s */
+        if (currentTimeMs > redisplayStartTimeMs) {
+            warningDisplayStartTime = currentTimeMs;
+            osdWarningTimerDuration = newWarningFlags ? 10000 : WARNING_REDISPLAY_DURATION;
+            redisplayStartTimeMs = currentTimeMs + osdWarningTimerDuration + 30000;
+        }
+
+        if (currentTimeMs - warningDisplayStartTime < osdWarningTimerDuration) {
+            return (newWarningFlags & warningFlag) || osdWarningTimerDuration == WARNING_REDISPLAY_DURATION;
+        } else {
+            newWarningFlags = 0;
+        }
+        *warningsCount += 1;
+    } else if (osdWarningsFlags & warningFlag) {
+        osdWarningsFlags &= ~warningFlag;
+    }
+
+    return false;
+}
+
+static textAttributes_t osdGetMultiFunctionMessage(char *buff)
+{
+    /* Message length limit 10 char max */
+
+    textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
+    static uint8_t warningsCount;
+    const char *message = NULL;
+
+#ifdef USE_MULTI_FUNCTIONS
+    /* --- FUNCTIONS --- */
+    multi_function_e selectedFunction = multiFunctionSelection();
+
+    if (selectedFunction) {
+        multi_function_e activeFunction = selectedFunction;
+
+        switch (selectedFunction) {
+        case MULTI_FUNC_NONE:
+        case MULTI_FUNC_1:
+            message = warningsCount ? "WARNINGS !" : "0 WARNINGS";
+            break;
+        case MULTI_FUNC_2:
+            message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
+            break;
+        case MULTI_FUNC_3:
+#if defined(USE_SAFE_HOME)
+            if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
+                message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
+                break;
+            }
+#endif
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_4:
+            if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
+                message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
+                break;
+            }
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_5:
+#ifdef USE_DSHOT
+            if (STATE(MULTIROTOR)) {
+                message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE";
+                break;
+            }
+#endif
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_6:
+            message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
+            break;
+        case MULTI_FUNC_END:
+            break;
+        }
+
+        if (activeFunction != selectedFunction) {
+            setMultifunctionSelection(activeFunction);
+        }
+
+        strcpy(buff, message);
+
+        if (isNextMultifunctionItemAvailable()) {
+            // provides feedback indicating when a new selection command has been received by flight controller
+            buff[9] = '>';
+        }
+
+        return elemAttr;
+    }
+#endif  // MULTIFUNCTION - functions only, warnings always defined
+
+    /* --- WARNINGS --- */
+    const char *messages[7];
+    uint8_t messageCount = 0;
+    bool warningCondition = false;
+    warningsCount = 0;
+    uint8_t warningFlagID = 1;
+
+    // Low Battery
+    const batteryState_e batteryState = getBatteryState();
+    warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING;
+    if (osdCheckWarning(warningCondition, warningFlagID, &warningsCount)) {
+        messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !";
+    }
+
+#if defined(USE_GPS)
+    // GPS Fix and Failure
+    if (feature(FEATURE_GPS)) {
+        if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1, &warningsCount)) {
+            bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
+            messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
+        }
+    }
+
+    // RTH sanity (warning if RTH heads 200m further away from home than closest point)
+    warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
+                       (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
+    if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
+        messages[messageCount++] = "RTH SANITY";
+    }
+
+    // Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
+    if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1, &warningsCount)) {
+        messages[messageCount++] = "ALT SANITY";
+    }
+#endif
+
+#if defined(USE_MAG)
+    // Magnetometer failure
+    if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
+        hardwareSensorStatus_e magStatus = getHwCompassStatus();
+        if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1, &warningsCount)) {
+            messages[messageCount++] = "MAG FAILED";
+        }
+    }
+#endif
+    // Vibration levels   TODO - needs better vibration measurement to be useful
+    // const float vibrationLevel = accGetVibrationLevel();
+    // warningCondition = vibrationLevel > 1.5f;
+    // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
+        // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
+    // }
+
+#ifdef USE_DEV_TOOLS
+    if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) {
+        messages[messageCount++] = "GRD TEST !";
+    }
+#endif
+
+    if (messageCount) {
+        message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];    // display each warning on 1s cycle
+        strcpy(buff, message);
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    } else if (warningsCount) {
+        buff[0] = SYM_ALERT;
+        tfp_sprintf(buff + 1, "%u        ", warningsCount);
+    }
+
+    return elemAttr;
+}
 #endif // OSD

src/main/io/osd.h

@@ -273,6 +273,7 @@ typedef enum {
     OSD_CROSS_TRACK_ERROR,
     OSD_PILOT_NAME,
     OSD_PAN_SERVO_CENTRED,
+    OSD_MULTI_FUNCTION,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -488,6 +489,8 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max);
 // Returns a heading angle in degrees normalized to [0, 360).
 int osdGetHeadingAngle(int angle);
 
+void osdResetWarningFlags(void);
+
 int16_t osdGetPanServoOffset(void);
 
 /**

src/main/msp/msp_protocol_v2_inav.h

@@ -31,6 +31,9 @@
 #define MSP2_INAV_OUTPUT_MAPPING                0x200A
 #define MSP2_INAV_MC_BRAKING                    0x200B
 #define MSP2_INAV_SET_MC_BRAKING                0x200C
+#define MSP2_INAV_OUTPUT_MAPPING_EXT            0x200D
+#define MSP2_INAV_TIMER_OUTPUT_MODE             0x200E
+#define MSP2_INAV_SET_TIMER_OUTPUT_MODE         0x200F
 
 #define MSP2_INAV_MIXER                         0x2010
 #define MSP2_INAV_SET_MIXER                     0x2011

src/main/navigation/navigation.c

@@ -36,6 +36,7 @@
 
 #include "fc/fc_core.h"
 #include "fc/config.h"
+#include "fc/multifunction.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -82,17 +83,10 @@ 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_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);
-
 #endif
 
 // waypoint 254, 255 are special waypoints
@@ -102,13 +96,12 @@ 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, 2);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
 
         .flags = {
-            .use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT,
             .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT,
             .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT,
             .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT,
@@ -160,6 +153,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT,                 // Changes sensitivity of landing detection
         .auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT,                             // 2000 ms - time delay to disarm when auto disarm after landing enabled
         .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
+        .cruise_yaw_rate  = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT,                                // 20dps
     },
 
     // MC-specific
@@ -180,6 +174,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT,        // posDecelerationTime * 100
         .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT,                         // posResponseExpo * 100
         .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
+        .althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT,           // STICK
     },
 
     // Fixed wing
@@ -214,7 +209,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
         .launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT,  // 100 us
 
-        .cruise_yaw_rate  = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT,             // 20dps
         .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
         .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
         .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
@@ -237,10 +231,10 @@ EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 int16_t navCurrentState;
 int16_t navActualVelocity[3];
 int16_t navDesiredVelocity[3];
-int16_t navActualHeading;
-int16_t navDesiredHeading;
 int32_t navTargetPosition[3];
 int32_t navLatestActualPosition[3];
+int16_t navActualHeading;
+uint16_t navDesiredHeading;
 int16_t navActualSurface;
 uint16_t navFlags;
 uint16_t navEPH;
@@ -430,7 +424,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_COURSE_HOLD_MODE,
         .mwState = MW_NAV_STATE_NONE,
         .mwError = MW_NAV_ERROR_NONE,
@@ -489,7 +483,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
         .mwState = MW_NAV_STATE_NONE,
         .mwError = MW_NAV_ERROR_NONE,
@@ -956,7 +950,7 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
     return navFSM[state].stateFlags;
 }
 
-static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
+flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
 {
     return navFSM[state].mapToFlightModes;
 }
@@ -1063,7 +1057,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
 {
     UNUSED(previousState);
 
-    if (!STATE(FIXED_WING_LEGACY)) {    // Only on FW for now
+    if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) {  // course hold not possible on MR without yaw control
         return NAV_FSM_EVENT_ERROR;
     }
 
@@ -1075,7 +1069,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
 
     resetPositionController();
 
+    if (STATE(AIRPLANE)) {
         posControl.cruise.course = posControl.actualState.cog;  // Store the course to follow
+    } else {    // Multicopter
+        posControl.cruise.course = posControl.actualState.yaw;
+        posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed);
+        posControl.desiredState.pos = posControl.actualState.abs.pos;
+    }
     posControl.cruise.previousCourse = posControl.cruise.course;
     posControl.cruise.lastCourseAdjustmentTime = 0;
 
@@ -1096,15 +1096,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
     DEBUG_SET(DEBUG_CRUISE, 0, 2);
     DEBUG_SET(DEBUG_CRUISE, 2, 0);
 
-    if (posControl.flags.isAdjustingPosition) {
+    if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) {  // inhibit for MR, pitch/roll only adjusts speed using pitch
         return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
     }
 
-    // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
+    const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband;
-    if (posControl.flags.isAdjustingHeading) {
+
+    // User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR
+    // We record the desired course and change the desired target in the meanwhile
+    if (posControl.flags.isAdjustingHeading || mcRollStickHeadingAdjustmentActive) {
+        int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate);
+        int16_t headingAdjustCommand = rcCommand[YAW];
+        if (mcRollStickHeadingAdjustmentActive && ABS(rcCommand[ROLL]) > ABS(headingAdjustCommand)) {
+            headingAdjustCommand = -rcCommand[ROLL];
+        }
+
         timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
         if (timeDifference > 100) timeDifference = 0;   // if adjustment was called long time ago, reset the time difference.
-        float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
+        float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate);
         float centidegsPerIteration = rateTarget * MS2S(timeDifference);
         posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
         DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
@@ -1137,7 +1146,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
 {
-    if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
+    if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) {  // course hold not possible on MR without yaw control
+        return NAV_FSM_EVENT_ERROR;
+    }
 
     navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
 
@@ -1313,8 +1324,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
 
     if (posControl.flags.estPosStatus >= EST_USABLE) {
         const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
+#ifdef USE_MULTI_FUNCTIONS
+        const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
+#else
+        const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
+#endif
         const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
-                                     (rthAltControlStickOverrideCheck(ROLL) && !posControl.flags.forcedRTHActivated);
+                                     (overrideTrackback && !posControl.flags.forcedRTHActivated);
 
         if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
             posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
@@ -1792,8 +1808,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
 {
     UNUSED(previousState);
 
-    disarm(DISARM_NAVIGATION);
-
     return NAV_FSM_EVENT_NONE;
 }
 
@@ -2048,7 +2062,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
 /*-----------------------------------------------------------
  * Processes an update to Z-position and velocity
  *-----------------------------------------------------------*/
-void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
+void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError)
 {
     posControl.actualState.abs.pos.z = newAltitude;
     posControl.actualState.abs.vel.z = newVelocity;
@@ -2071,11 +2085,14 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
         posControl.flags.estAltStatus = EST_TRUSTED;
         posControl.flags.verticalPositionDataNew = true;
         posControl.lastValidAltitudeTimeMs = millis();
+        /* flag set if mismatch between relative GPS and estimated altitude exceeds 20m */
+        posControl.flags.gpsCfEstimatedAltitudeMismatch = fabsf(gpsCfEstimatedAltitudeError) > 2000;
     }
     else {
         posControl.flags.estAltStatus = EST_NONE;
         posControl.flags.estAglStatus = EST_NONE;
         posControl.flags.verticalPositionDataNew = false;
+        posControl.flags.gpsCfEstimatedAltitudeMismatch = false;
     }
 
     if (ARMING_FLAG(ARMED)) {
@@ -2440,12 +2457,13 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 }
 
 #if defined(USE_SAFE_HOME)
-
 void checkSafeHomeState(bool shouldBeEnabled)
 {
-    const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
+    bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || posControl.flags.rthTrackbackActive ||
-                                       posControl.flags.rthTrackbackActive ||
+                                 (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
-                                       (!safehome_applied && posControl.homeDistance < navConfig()->general.min_rth_distance);
+#ifdef USE_MULTI_FUNCTIONS
+    safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated);
+#endif
 
     if (safehomeNotApplicable) {
         shouldBeEnabled = false;
@@ -2455,17 +2473,17 @@ void checkSafeHomeState(bool shouldBeEnabled)
         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)) {
+	if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) {
 		return;
 	}
     if (shouldBeEnabled) {
 		// set home to safehome
-        setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+        setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-		safehome_applied = true;
+		posControl.safehomeState.isApplied = 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());
+        setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-		safehome_applied = false;
+		posControl.safehomeState.isApplied = false;
 	}
 	// if we've changed the home position, update the distance and direction
     updateHomePosition();
@@ -2477,7 +2495,7 @@ void checkSafeHomeState(bool shouldBeEnabled)
  **********************************************************/
 bool findNearestSafeHome(void)
 {
-    safehome_index = -1;
+    posControl.safehomeState.index = -1;
     uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
     uint32_t distance_to_current;
     fpVector3_t currentSafeHome;
@@ -2493,19 +2511,17 @@ bool findNearestSafeHome(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_index = i;
+             posControl.safehomeState.index = i;
              nearest_safehome_distance = distance_to_current;
-             nearestSafeHome.x = currentSafeHome.x;
+             posControl.safehomeState.nearestSafeHome = currentSafeHome;
-             nearestSafeHome.y = currentSafeHome.y;
-             nearestSafeHome.z = currentSafeHome.z;
         }
     }
-    if (safehome_index >= 0) {
+    if (posControl.safehomeState.index >= 0) {
-		safehome_distance = nearest_safehome_distance;
+		posControl.safehomeState.distance = nearest_safehome_distance;
     } else {
-        safehome_distance = 0;
+        posControl.safehomeState.distance = 0;
     }
-    return safehome_distance > 0;
+    return posControl.safehomeState.distance > 0;
 }
 #endif
 
@@ -2535,9 +2551,7 @@ void updateHomePosition(void)
 #endif
                 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
                 // save the current location in case it is replaced by a safehome or HOME_RESET
-                original_rth_home.x = posControl.rthState.homePosition.pos.x;
+                posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
-                original_rth_home.y = posControl.rthState.homePosition.pos.y;
-                original_rth_home.z = posControl.rthState.homePosition.pos.z;
             }
         }
     }
@@ -2827,7 +2841,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
         }
     } else if (STATE(LANDING_DETECTED)) {
         pidResetErrorAccumulators();
-        if (navConfig()->general.flags.disarm_on_landing) {
+        if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
             disarm(DISARM_LANDING);
         } else if (!navigationInAutomaticThrottleMode()) {
@@ -3447,13 +3461,15 @@ bool isNavHoldPositionActive(void)
             navigationIsExecutingAnEmergencyLanding();
 }
 
-float getActiveWaypointSpeed(void)
+float getActiveSpeed(void)
 {
-    if (posControl.flags.isAdjustingPosition) {
+    /* Currently only applicable for multicopter */
-        // In manual control mode use different cap for speed
+
+    // Speed limit for modes where speed manually controlled
+    if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
         return navConfig()->general.max_manual_speed;
     }
-    else {
+
     uint16_t waypointSpeed = navConfig()->general.auto_speed;
 
     if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
@@ -3473,7 +3489,6 @@ float getActiveWaypointSpeed(void)
     }
 
     return waypointSpeed;
-    }
 }
 
 bool isWaypointNavTrackingActive(void)
@@ -3492,29 +3507,21 @@ static void processNavigationRCAdjustments(void)
 {
     /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
-    if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
-        posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput();
-    }
-    else {
-        posControl.flags.isAdjustingAltitude = false;
-    }
 
-    if (navStateFlags & NAV_RC_POS) {
+    if (FLIGHT_MODE(FAILSAFE_MODE)) {
-        posControl.flags.isAdjustingPosition = adjustPositionFromRCInput() && !FLIGHT_MODE(FAILSAFE_MODE);
+        if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
-        if (STATE(MULTIROTOR) && FLIGHT_MODE(FAILSAFE_MODE)) {
             resetMulticopterBrakingMode();
         }
-    }
+        posControl.flags.isAdjustingAltitude = false;
-    else {
         posControl.flags.isAdjustingPosition = false;
+        posControl.flags.isAdjustingHeading = false;
+
+        return;
     }
 
-    if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
+    posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput();
-        posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput();
+    posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput();
-    }
+    posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput();
-    else {
-        posControl.flags.isAdjustingHeading = false;
-    }
 }
 
 /*-----------------------------------------------------------
@@ -3584,6 +3591,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
     navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
     navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
     navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
+
+    navDesiredHeading = wrap_36000(posControl.desiredState.yaw);
 }
 
 /*-----------------------------------------------------------
@@ -3620,7 +3629,7 @@ static bool isWaypointMissionValid(void)
     return posControl.waypointListValid && (posControl.waypointCount > 0);
 }
 
-static void checkManualEmergencyLandingControl(void)
+void checkManualEmergencyLandingControl(bool forcedActivation)
 {
     static timeMs_t timeout = 0;
     static int8_t counter = 0;
@@ -3645,7 +3654,7 @@ static void checkManualEmergencyLandingControl(void)
     }
 
     // Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
-    if (counter >= 5) {
+    if (counter >= 5 || forcedActivation) {
         counter = 0;
         posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
 
@@ -3682,7 +3691,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
 
         /* Emergency landing controlled manually by rapid switching of Poshold mode.
          * Landing toggled ON or OFF for each Poshold activation sequence */
-        checkManualEmergencyLandingControl();
+        checkManualEmergencyLandingControl(false);
 
         /* Emergency landing triggered by failsafe Landing or manually initiated */
         if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
@@ -3854,9 +3863,8 @@ bool navigationRequiresTurnAssistance(void)
         // For airplanes turn assistant is always required when controlling position
         return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
     }
-    else {
+
     return false;
-    }
 }
 
 /**
@@ -3870,17 +3878,16 @@ int8_t navigationGetHeadingControlState(void)
     }
 
     // For multirotors it depends on navigation system mode
+    // Course hold requires Auto Control to update heading hold target whilst RC adjustment active
     if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
-        if (posControl.flags.isAdjustingHeading) {
+        if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
             return NAV_HEADING_CONTROL_MANUAL;
         }
-        else {
+
         return NAV_HEADING_CONTROL_AUTO;
     }
-    }
+
-    else {
     return NAV_HEADING_CONTROL_NONE;
-    }
 }
 
 bool navigationTerrainFollowingEnabled(void)
@@ -4205,6 +4212,7 @@ void navigationInit(void)
     posControl.wpPlannerActiveWPIndex = 0;
     posControl.flags.wpMissionPlannerActive = false;
     posControl.startWpIndex = 0;
+    posControl.safehomeState.isApplied = false;
 #ifdef USE_MULTI_MISSION
     posControl.multiMissionCount = 0;
 #endif
@@ -4417,3 +4425,8 @@ bool isAdjustingHeading(void) {
 int32_t getCruiseHeadingAdjustment(void) {
     return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
 }
+
+int32_t navigationGetHeadingError(void)
+{
+    return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
+}

src/main/navigation/navigation.h

@@ -35,7 +35,6 @@
 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;
 
@@ -59,13 +58,8 @@ typedef enum {
 
 PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
 
-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 findNearestSafeHome(void);      // Find nearest safehome
-
 #endif // defined(USE_SAFE_HOME)
 
 #ifndef NAV_MAX_WAYPOINTS
@@ -175,6 +169,12 @@ typedef enum {
     WP_TURN_SMOOTHING_CUT,
 } wpFwTurnSmoothing_e;
 
+typedef enum {
+    MC_ALT_HOLD_STICK,
+    MC_ALT_HOLD_MID,
+    MC_ALT_HOLD_HOVER,
+} navMcAltHoldThrottle_e;
+
 typedef struct positionEstimationConfig_s {
     uint8_t automatic_mag_declination;
     uint8_t reset_altitude_type; // from nav_reset_type_e
@@ -217,7 +217,6 @@ typedef struct navConfig_s {
 
     struct {
         struct {
-            uint8_t use_thr_mid_for_althold;        // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate
             uint8_t extra_arming_safety;            // from navExtraArmingSafety_e
             uint8_t user_control_mode;              // NAV_GPS_ATTI or NAV_GPS_CRUISE
             uint8_t rth_alt_control_mode;           // Controls the logic for choosing the RTH altitude
@@ -268,6 +267,7 @@ typedef struct navConfig_s {
         uint8_t  land_detect_sensitivity;           // Sensitivity of landing detector
         uint16_t auto_disarm_delay;                 // safety time delay for landing detector
         uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
+        uint8_t  cruise_yaw_rate;                   // Max yaw rate (dps) when CRUISE MODE is enabled
     } general;
 
     struct {
@@ -287,6 +287,7 @@ typedef struct navConfig_s {
         uint8_t posDecelerationTime;            // Brake time parameter
         uint8_t posResponseExpo;                // Position controller expo (taret vel expo for MC)
         bool slowDownForTurning;                // Slow down during WP missions when changing heading on next waypoint
+        uint8_t althold_throttle_type;          // throttle zero datum type for alt hold
     } mc;
 
     struct {
@@ -315,7 +316,6 @@ typedef struct navConfig_s {
         uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
         bool     launch_manual_throttle;     // Allows launch with manual throttle control
         uint8_t  launch_abort_deadband;      // roll/pitch stick movement deadband for launch abort
-        uint8_t  cruise_yaw_rate;            // Max yaw rate (dps) when CRUISE MODE is enabled
         bool     allow_manual_thr_increase;
         bool     useFwNavYawControl;
         uint8_t  yawControlDeadband;
@@ -624,6 +624,7 @@ bool isAdjustingHeading(void);
 float getEstimatedAglPosition(void);
 bool isEstimatedAglTrusted(void);
 
+void checkManualEmergencyLandingControl(bool forcedActivation);
 float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
 
 /* Returns the heading recorded when home position was acquired.
@@ -640,6 +641,7 @@ extern int16_t navActualVelocity[3];
 extern int16_t navDesiredVelocity[3];
 extern int32_t navTargetPosition[3];
 extern int32_t navLatestActualPosition[3];
+extern uint16_t navDesiredHeading;
 extern int16_t navActualSurface;
 extern uint16_t navFlags;
 extern uint16_t navEPH;

src/main/navigation/navigation_fixedwing.c

@@ -71,7 +71,6 @@ static bool isRollAdjustmentValid = false;
 static bool isYawAdjustmentValid = false;
 static float throttleSpeedAdjustment = 0;
 static bool isAutoThrottleManuallyIncreased = false;
-static int32_t navHeadingError;
 static float navCrossTrackError;
 static int8_t loiterDirYaw = 1;
 bool needToCalculateCircularLoiter;
@@ -445,7 +444,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
      * Calculate NAV heading error
      * Units are centidegrees
      */
-    navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
+    int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
 
     // Forced turn direction
     // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
@@ -647,7 +646,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
             isAutoThrottleManuallyIncreased = false;
         }
 
-        rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle);
+        rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
     }
 
 #ifdef NAV_FIXED_WING_LANDING
@@ -662,7 +661,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
            (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) {
 
             // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
-            rcCommand[THROTTLE] = getThrottleIdleValue();
             ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 
             // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
@@ -762,7 +760,7 @@ bool isFixedWingLandingDetected(void)
  *-----------------------------------------------------------*/
 void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
 {
-    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
+    rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
         // target min descent rate 10m above takeoff altitude
@@ -851,11 +849,6 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
     }
 }
 
-int32_t navigationGetHeadingError(void)
-{
-    return navHeadingError;
-}
-
 float navigationGetCrossTrackError(void)
 {
     return navCrossTrackError;

src/main/navigation/navigation_fw_launch.c

@@ -234,19 +234,12 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi
 
 /* Wing control Helpers */
 
-static bool isThrottleIdleEnabled(void)
-{
-    return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue();
-}
-
 static void applyThrottleIdleLogic(bool forceMixerIdle)
 {
-    if (isThrottleIdleEnabled() && !forceMixerIdle) {
+    if (forceMixerIdle) {
-        rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle;
+        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);   // If MOTOR_STOP is enabled mixer will keep motor stopped, otherwise motor will run at idle
-    }
+    } else {
-    else {
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_idle_throttle, true);
-        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);           // If MOTOR_STOP is enabled mixer will keep motor stopped
-        rcCommand[THROTTLE] = getThrottleIdleValue();   // If MOTOR_STOP is disabled, motors will spin given throttle value
     }
 }
 
@@ -290,7 +283,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
     UNUSED(currentTimeUs);
 
     if (!throttleStickIsLow()) {
-        if (isThrottleIdleEnabled()) {
+        if (currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue()) {
             return FW_LAUNCH_EVENT_SUCCESS;
         }
         else {
@@ -404,7 +397,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
 
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
-    const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
+    const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
 
     if (elapsedTimeMs > motorSpinUpMs) {
         rcCommand[THROTTLE] = launchThrottle;
@@ -438,7 +431,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
         }
     } else {
         initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time;
-        rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
     }
 
     if (isLaunchMaxAltitudeReached()) {
@@ -468,7 +461,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
         // Make a smooth transition from the launch state to the current state for pitch angle
         // Do the same for throttle when manual launch throttle isn't used
         if (!navConfig()->fw.launch_manual_throttle) {
-            const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
+            const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
             rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]);
         }
         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);

src/main/navigation/navigation_multicopter.c

@@ -121,7 +121,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
     int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
     rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax);
 
-    posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle);
+    posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, false);
 }
 
 bool adjustMulticopterAltitudeFromRCInput(void)
@@ -176,18 +176,15 @@ bool adjustMulticopterAltitudeFromRCInput(void)
 void setupMulticopterAltitudeController(void)
 {
     const bool throttleIsLow = throttleStickIsLow();
+    const uint8_t throttleType = navConfig()->mc.althold_throttle_type;
 
-    if (navConfig()->general.flags.use_thr_mid_for_althold) {
+    if (throttleType == MC_ALT_HOLD_STICK && !throttleIsLow) {
-        altHoldThrottleRCZero = rcLookupThrottleMid();
+        // Only use current throttle if not LOW - use Thr Mid otherwise
-    }
-    else {
-        // If throttle is LOW - use Thr Mid anyway
-        if (throttleIsLow) {
-            altHoldThrottleRCZero = rcLookupThrottleMid();
-        }
-        else {
         altHoldThrottleRCZero = rcCommand[THROTTLE];
-        }
+    } else if (throttleType == MC_ALT_HOLD_HOVER) {
+        altHoldThrottleRCZero = currentBatteryProfile->nav.mc.hover_throttle;
+    } else {
+        altHoldThrottleRCZero = rcLookupThrottleMid();
     }
 
     // Make sure we are able to satisfy the deadband
@@ -221,7 +218,7 @@ void resetMulticopterAltitudeController(void)
     pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f);
 
     if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-        const float maxSpeed = getActiveWaypointSpeed();
+        const float maxSpeed = getActiveSpeed();
         nav_speed_up = maxSpeed;
         nav_accel_z = maxSpeed;
         nav_speed_down = navConfig()->general.max_auto_climb_rate;
@@ -288,14 +285,15 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
 bool adjustMulticopterHeadingFromRCInput(void)
 {
     if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
-        // Can only allow pilot to set the new heading if doing PH, during RTH copter will target itself to home
+        // Heading during Cruise Hold mode set by Nav function so no adjustment required here
+        if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
             posControl.desiredState.yaw = posControl.actualState.yaw;
+        }
 
         return true;
     }
-    else {
+
     return false;
-    }
 }
 
 /*-----------------------------------------------------------
@@ -405,8 +403,44 @@ void resetMulticopterPositionController(void)
     }
 }
 
+static bool adjustMulticopterCruiseSpeed(int16_t rcPitchAdjustment)
+{
+    static timeMs_t lastUpdateTimeMs;
+    const timeMs_t currentTimeMs = millis();
+    const timeMs_t updateDeltaTimeMs = currentTimeMs - lastUpdateTimeMs;
+    lastUpdateTimeMs = currentTimeMs;
+
+    const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband);
+
+    if (rcVelX > posControl.cruise.multicopterSpeed) {
+        posControl.cruise.multicopterSpeed = rcVelX;
+    } else if (rcVelX < 0 && updateDeltaTimeMs < 100) {
+        posControl.cruise.multicopterSpeed += MS2S(updateDeltaTimeMs) * rcVelX / 2.0f;
+    } else {
+        return false;
+    }
+    posControl.cruise.multicopterSpeed = constrainf(posControl.cruise.multicopterSpeed, 10.0f, navConfig()->general.max_manual_speed);
+
+    return true;
+}
+
+static void setMulticopterStopPosition(void)
+{
+    fpVector3_t stopPosition;
+    calculateMulticopterInitialHoldPosition(&stopPosition);
+    setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
+}
+
 bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment)
 {
+    if (navGetMappedFlightModes(posControl.navState) & NAV_COURSE_HOLD_MODE) {
+        if (rcPitchAdjustment) {
+            return adjustMulticopterCruiseSpeed(rcPitchAdjustment);
+        }
+
+        return false;
+    }
+
     // Process braking mode
     processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment);
 
@@ -428,16 +462,12 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
 
         return true;
     }
-    else {
+    else if (posControl.flags.isAdjustingPosition) {
         // Adjusting finished - reset desired position to stay exactly where pilot released the stick
-        if (posControl.flags.isAdjustingPosition) {
+        setMulticopterStopPosition();
-            fpVector3_t stopPosition;
-            calculateMulticopterInitialHoldPosition(&stopPosition);
-            setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
     }
 
     return false;
-    }
 }
 
 static float getVelocityHeadingAttenuationFactor(void)
@@ -466,15 +496,28 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
 
 static void updatePositionVelocityController_MC(const float maxSpeed)
 {
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
+        // Position held at cruise speeds below 0.5 m/s, otherwise desired neu velocities set directly from cruise speed
+        if (posControl.cruise.multicopterSpeed >= 50) {
+            // Rotate multicopter x velocity from body frame to earth frame
+            posControl.desiredState.vel.x = posControl.cruise.multicopterSpeed * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course));
+            posControl.desiredState.vel.y = posControl.cruise.multicopterSpeed * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course));
+
+            return;
+        } else if (posControl.flags.isAdjustingPosition) {
+            setMulticopterStopPosition();
+        }
+    }
+
     const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
     const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
 
     // Calculate target velocity
-    float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
+    float neuVelX = posErrorX * posControl.pids.pos[X].param.kP;
-    float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
+    float neuVelY = posErrorY * posControl.pids.pos[Y].param.kP;
 
     // Scale velocity to respect max_speed
-    float newVelTotal = calc_length_pythagorean_2D(newVelX, newVelY);
+    float neuVelTotal = calc_length_pythagorean_2D(neuVelX, neuVelY);
 
     /*
      * We override computed speed with max speed in following cases:
@@ -484,26 +527,23 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
     if (
         ((navGetCurrentStateFlags() & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) &&
         !isNavHoldPositionActive() &&
-        newVelTotal < maxSpeed &&
+        neuVelTotal < maxSpeed &&
         !navConfig()->mc.slowDownForTurning
-        ) || newVelTotal > maxSpeed
+        ) || neuVelTotal > maxSpeed
     ) {
-        newVelX = maxSpeed * (newVelX / newVelTotal);
+        neuVelX = maxSpeed * (neuVelX / neuVelTotal);
-        newVelY = maxSpeed * (newVelY / newVelTotal);
+        neuVelY = maxSpeed * (neuVelY / neuVelTotal);
-        newVelTotal = maxSpeed;
+        neuVelTotal = maxSpeed;
     }
 
-    posControl.pids.pos[X].output_constrained = newVelX;
+    posControl.pids.pos[X].output_constrained = neuVelX;
-    posControl.pids.pos[Y].output_constrained = newVelY;
+    posControl.pids.pos[Y].output_constrained = neuVelY;
 
     // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
     const float velHeadFactor = getVelocityHeadingAttenuationFactor();
-    const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
+    const float velExpoFactor = getVelocityExpoAttenuationFactor(neuVelTotal, maxSpeed);
-    posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor;
+    posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor;
-    posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor;
+    posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor;
-
-    navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
-    navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
 }
 
 static float computeNormalizedVelocity(const float value, const float maxValue)
@@ -663,49 +703,53 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
 static void applyMulticopterPositionController(timeUs_t currentTimeUs)
 {
-    static timeUs_t previousTimePositionUpdate = 0;     // Occurs @ GPS update rate
-    bool bypassPositionController;
-
-    // We should passthrough rcCommand is adjusting position in GPS_ATTI mode
-    bypassPositionController = (navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI) && posControl.flags.isAdjustingPosition;
-
     // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
     // and pilots input would be passed thru to PID controller
-    if ((posControl.flags.estPosStatus >= EST_USABLE)) {
+    if (posControl.flags.estPosStatus < EST_USABLE) {
-        // If we have new position - update velocity and acceleration controllers
-        if (posControl.flags.horizontalPositionDataNew) {
-            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
-            previousTimePositionUpdate = currentTimeUs;
-
-            if (!bypassPositionController) {
-                // Update position controller
-                if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
-                    // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
-                    const float maxSpeed = getActiveWaypointSpeed();
-                    updatePositionVelocityController_MC(maxSpeed);
-                    updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
-                }
-                else {
-                    // Position update has not occurred in time (first start or glitch), reset altitude controller
-                    resetMulticopterPositionController();
-                }
-            }
-
-            // Indicate that information is no longer usable
-            posControl.flags.horizontalPositionDataConsumed = true;
-        }
-    }
-    else {
         /* No position data, disable automatic adjustment, rcCommand passthrough */
         posControl.rcAdjustment[PITCH] = 0;
         posControl.rcAdjustment[ROLL] = 0;
-        bypassPositionController = true;
+
+        return;
+    }
+
+    // Passthrough rcCommand if adjusting position in GPS_ATTI mode except when Course Hold active
+    bool bypassPositionController = !FLIGHT_MODE(NAV_COURSE_HOLD_MODE) &&
+                                    navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI &&
+                                    posControl.flags.isAdjustingPosition;
+
+    if (posControl.flags.horizontalPositionDataNew) {
+        // Indicate that information is no longer usable
+        posControl.flags.horizontalPositionDataConsumed = true;
+
+        static timeUs_t previousTimePositionUpdate = 0;     // Occurs @ GPS update rate
+        const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+        previousTimePositionUpdate = currentTimeUs;
+
+        if (bypassPositionController) {
+            return;
+        }
+
+        // If we have new position data - update velocity and acceleration controllers
+        if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
+            // Get max speed for current NAV mode
+            float maxSpeed = getActiveSpeed();
+            updatePositionVelocityController_MC(maxSpeed);
+            updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
+
+            navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
+            navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
+        }
+        else {
+            // Position update has not occurred in time (first start or glitch), reset position controller
+            resetMulticopterPositionController();
+        }
+    } else if (bypassPositionController) {
+        return;
     }
 
-    if (!bypassPositionController) {
     rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
     rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
-    }
 }
 
 bool isMulticopterFlying(void)
@@ -870,13 +914,14 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[YAW] = 0;
     rcCommand[ROLL] = 0;
     rcCommand[PITCH] = 0;
-    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 
     /* Altitude sensors gone haywire, attempt to land regardless */
     if (posControl.flags.estAltStatus < EST_USABLE) {
         if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
             rcCommand[THROTTLE] = getThrottleIdleValue();
+            return;
         }
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
         return;
     }
 
@@ -929,6 +974,10 @@ void resetMulticopterHeadingController(void)
 
 static void applyMulticopterHeadingController(void)
 {
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {    // heading set by Nav during Course Hold so disable yaw stick input
+        rcCommand[YAW] = 0;
+    }
+
     updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
 }
 

src/main/navigation/navigation_pos_estimator.c

@@ -816,11 +816,12 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
 
         /* Publish altitude update and set altitude validity */
         if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+            const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0;
             navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
-            updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus);
+            updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError);
         }
         else {
-            updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
+            updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE, 0);
         }
 
         //Update Blackbox states

src/main/navigation/navigation_private.h

@@ -91,6 +91,7 @@ typedef struct navigationFlags_s {
     navigationEstimateStatus_e estVelStatus;        // Indicates that GPS is working (or not)
     navigationEstimateStatus_e estAglStatus;
     navigationEstimateStatus_e estHeadingStatus;    // Indicate valid heading - wither mag or GPS at certain speed on airplane
+    bool gpsCfEstimatedAltitudeMismatch;            // Indicates a mismatch between GPS altitude and estimated altitude
 
     bool isAdjustingPosition;
     bool isAdjustingAltitude;
@@ -326,6 +327,7 @@ typedef struct {
     int32_t                     course;
     int32_t                     previousCourse;
     timeMs_t                    lastCourseAdjustmentTime;
+    float                       multicopterSpeed;
 } navCruise_t;
 
 typedef struct {
@@ -336,6 +338,7 @@ typedef struct {
     float                   rthFinalAltitude;       // Altitude at end of RTH approach
     float                   rthInitialDistance;     // Distance when starting flight home
     fpVector3_t             homeTmpWaypoint;        // Temporary storage for home target
+    fpVector3_t             originalHomePosition;   // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
 } rthState_t;
 
 typedef enum {
@@ -346,6 +349,13 @@ typedef enum {
     RTH_HOME_FINAL_LAND,            // Home position and altitude
 } rthTargetMode_e;
 
+typedef struct {
+    fpVector3_t nearestSafeHome;    // The nearestSafeHome found during arming
+    uint32_t    distance;           // distance to the nearest safehome
+    int8_t      index;              // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+    bool        isApplied;          // whether the safehome has been applied to home
+} safehomeState_t;
+
 typedef struct {
     /* Flags and navigation system state */
     navigationFSMState_t        navState;
@@ -368,14 +378,15 @@ typedef struct {
     /* INAV GPS origin (position where GPS fix was first acquired) */
     gpsOrigin_t                 gpsOrigin;
 
-    /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */
+    /* Home/RTH parameters - NEU coordinates (geodetic position of home (LLH) is stored in GPS_home variable) */
     rthSanityChecker_t          rthSanityChecker;
     rthState_t                  rthState;
-
-    /* Home parameters */
     uint32_t                    homeDistance;   // cm
     int32_t                     homeDirection;  // deg*100
 
+    /* Safehome parameters */
+    safehomeState_t             safehomeState;
+
     /* Cruise */
     navCruise_t                 cruise;
 
@@ -443,6 +454,7 @@ bool isFixedWingFlying(void);
 bool isMulticopterFlying(void);
 
 navigationFSMStateFlags_t navGetCurrentStateFlags(void);
+flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state);
 
 void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
@@ -452,12 +464,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
 
 bool isNavHoldPositionActive(void);
 bool isLastMissionWaypoint(void);
-float getActiveWaypointSpeed(void);
+float getActiveSpeed(void);
 bool isWaypointNavTrackingActive(void);
 
 void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
-void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);
+void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
 
 bool checkForPositionSensorTimeout(void);
 

src/main/navigation/navigation_rover_boat.c

@@ -138,7 +138,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
         rcCommand[ROLL] = 0;
         rcCommand[PITCH] = 0;
         rcCommand[YAW] = 0;
-        rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
+        rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
     } else if (navStateFlags & NAV_CTL_POS) {
         applyRoverBoatPositionController(currentTimeUs);
         applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);

src/main/sensors/pitotmeter.c

@@ -31,6 +31,7 @@
 
 #include "drivers/pitotmeter/pitotmeter.h"
 #include "drivers/pitotmeter/pitotmeter_ms4525.h"
+#include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h"
 #include "drivers/pitotmeter/pitotmeter_adc.h"
 #include "drivers/pitotmeter/pitotmeter_msp.h"
 #include "drivers/pitotmeter/pitotmeter_virtual.h"
@@ -44,11 +45,18 @@
 #include "scheduler/protothreads.h"
 
 #include "sensors/pitotmeter.h"
+#include "sensors/barometer.h"
 #include "sensors/sensors.h"
 
+
+//#include "build/debug.h"
+
+
 #ifdef USE_PITOT
 
-pitot_t pitot;
+extern baro_t baro;
+
+pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0};
 
 PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2);
 
@@ -86,6 +94,15 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             }
             FALLTHROUGH;
 
+        case PITOT_DLVR:
+
+			// Skip autodetection for DLVR (it is indistinguishable from MS4525) and allow only manual config
+            if (pitotHardwareToUse != PITOT_AUTODETECT && dlvrDetect(dev)) {
+                pitotHardware = PITOT_DLVR;
+                break;
+            }
+            FALLTHROUGH;
+
         case PITOT_ADC:
 #if defined(USE_ADC) && defined(USE_PITOT_ADC)
             if (adcPitotDetect(dev)) {
@@ -169,7 +186,7 @@ bool pitotIsCalibrationComplete(void)
 
 void pitotStartCalibration(void)
 {
-    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00001f, false);
+    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * pitot.dev.calibThreshold, false);
 }
 
 static void performPitotCalibrationCycle(void)
@@ -187,10 +204,12 @@ STATIC_PROTOTHREAD(pitotThread)
     ptBegin(pitotThread);
 
     static float pitotPressureTmp;
+    static float pitotTemperatureTmp;
     timeUs_t currentTimeUs;
 
     // Init filter
     pitot.lastMeasurementUs = micros();
+
     pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
 
     while(1) {
@@ -201,19 +220,23 @@ STATIC_PROTOTHREAD(pitotThread)
     	}
 #endif
 
-        // Start measurement
+        if ( pitot.lastSeenHealthyMs == 0 ) {
             if (pitot.dev.start(&pitot.dev)) {
                 pitot.lastSeenHealthyMs = millis();
             }        
+        }
 
-        ptDelayUs(pitot.dev.delay);
+        if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
+            if (pitot.dev.get(&pitot.dev))          // read current data
+                pitot.lastSeenHealthyMs = millis();
 
-        // Read and calculate data
+            if (pitot.dev.start(&pitot.dev))        // init for next read
-        if (pitot.dev.get(&pitot.dev)) {
                 pitot.lastSeenHealthyMs = millis();        
         }
 
-        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
+
+        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
+
 #ifdef USE_SIMULATOR
     	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
         	pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
@@ -226,14 +249,9 @@ STATIC_PROTOTHREAD(pitotThread)
 #endif
         ptYield();
 
-        // Filter pressure
-        currentTimeUs = micros();
-        pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-        pitot.lastMeasurementUs = currentTimeUs;
-        ptDelayUs(pitot.dev.delay);
-
         // Calculate IAS
         if (pitotIsCalibrationComplete()) {
+            // NOTE ::
             // https://en.wikipedia.org/wiki/Indicated_airspeed
             // Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system.
             // The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for
@@ -242,11 +260,21 @@ STATIC_PROTOTHREAD(pitotThread)
             //
             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
-            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;
+
+            // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!!
+            currentTimeUs = micros();
+            pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+            pitot.lastMeasurementUs = currentTimeUs;
+
+            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+            pitot.temperature = pitotTemperatureTmp;   // Kelvin
+
         } else {
+            pitot.pressure = pitotPressureTmp;
             performPitotCalibrationCycle();
             pitot.airSpeed = 0.0f;
         }
+
 #ifdef USE_SIMULATOR
     	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
             pitot.airSpeed = simulatorData.airSpeed;

src/main/sensors/pitotmeter.h

@@ -31,6 +31,7 @@ typedef enum {
     PITOT_VIRTUAL = 4,
     PITOT_FAKE = 5,
     PITOT_MSP = 6,
+    PITOT_DLVR = 7,
 } pitotSensor_e;
 
 #define PITOT_MAX  PITOT_FAKE
@@ -55,6 +56,7 @@ typedef struct pito_s {
 
     float pressureZero;
     float pressure;
+    float temperature;
 } pitot_t;
 
 #ifdef USE_PITOT

src/main/sensors/sensors.h

@@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u {
 } flightDynamicsTrims_t;
 
 #define CALIBRATING_BARO_TIME_MS            2000
-#define CALIBRATING_PITOT_TIME_MS           2000
+#define CALIBRATING_PITOT_TIME_MS           4000
 #define CALIBRATING_GYRO_TIME_MS            2000
 #define CALIBRATING_ACC_TIME_MS             500
 #define CALIBRATING_GYRO_MORON_THRESHOLD    32

src/main/target/AOCODARCF4V2/target.h

@@ -55,6 +55,7 @@
 
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
+#define USE_BARO_DPS310
 
 //*********** Magnetometer / Compass *************
 #define USE_MAG

src/main/target/BETAFPVF435/CMakeLists.txt

@@ -0,0 +1 @@
+target_at32f43x_xGT7(BETAFPVF435 SKIP_RELEASES)

src/main/target/BETAFPVF435/config.c

@@ -0,0 +1,36 @@
+/*
+ * 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/>.
+ *
+ * This target has been autgenerated by bf2inav.py
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "fc/fc_msp_box.h"
+#include "fc/config.h"
+
+#include "io/ledstrip.h"
+
+void targetConfiguration(void)
+{
+    ledStripConfig_t *config = ledStripConfigMutable();
+    ledConfig_t *lc = config->ledConfigs;
+    DEFINE_LED(lc, 0, 0, COLOR_RED, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0);
+    DEFINE_LED(lc+1, 0, 1, COLOR_GREEN, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0);
+}
+

src/main/target/BETAFPVF435/target.c

@@ -0,0 +1,44 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV 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.
+ *
+ * INAV are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/pinio.h"
+#include "drivers/sensor.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TMR3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),  // TMR3_CH3 motor 1
+    DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2),  // TMR3_CH4 motor 2
+    DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 7),  // TMR2_CH4 motor 3
+    DEF_TIM(TMR2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 6),  // TMR2_CH3 motor 4
+
+    DEF_TIM(TMR8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO |TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 4),  // TMR8_CH3 motor 5
+    DEF_TIM(TMR1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO |TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 5),  // TMR1_CH3 motor 6
+
+    DEF_TIM(TMR4, CH1, PB6, TIM_USE_LED, 0, 0),  // TMR4_CH1 LED_STRIP
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/BETAFPVF435/target.h

@@ -0,0 +1,201 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV 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.
+ *
+ * INAV 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/>.
+ */
+//https://www.arterychip.com/download/DS/DS_AT32F435_437_V2.02-EN.pdf
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "BHER"
+#define USBD_PRODUCT_STRING     "BETAFPVF435"
+
+#define LED0                PB5
+
+#define USE_BEEPER
+#define BEEPER              PB4
+#define BEEPER_INVERTED
+
+//#define ENABLE_DSHOT_DMAR       DSHOT_DMAR_AUTO
+//#define DSHOT_BITBANG_DEFAULT   DSHOT_BITBANG_AUTO
+//#define ENABLE_DSHOT
+
+// *************** Gyro & ACC **********************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define SPI1_NSS_PIN           PA4
+//#define GYRO_1_SPI_INSTANCE     SPI1
+
+//#define USE_EXTI
+//#define USE_GYRO_EXTI
+//#define GYRO_1_EXTI_PIN         PC4
+//#define USE_MPU_DATA_READY_SIGNAL
+//#define ENSURE_MPU_DATA_READY_IS_LOW
+
+// MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG 
+#define MPU6000_SPI_BUS         BUS_SPI1
+#define MPU6000_CS_PIN          SPI1_NSS_PIN
+
+// ICM42605/ICM42688P
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define ICM42605_CS_PIN         SPI1_NSS_PIN
+
+//#define USE_ACC
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN      CW270_DEG
+#define BMI270_SPI_BUS        BUS_SPI1
+#define BMI270_CS_PIN         SPI1_NSS_PIN
+
+// *************** Baro **************************
+#define USE_BARO
+#define USE_BARO_BMP280
+#define SPI3_NSS_PIN    PB3
+#define BMP280_SPI_BUS  BUS_SPI3
+#define BMP280_CS_PIN  SPI3_NSS_PIN
+#define USE_BARO_DPS310
+#define DPS310_SPI_BUS  BUS_SPI3
+#define DPS310_CS_PIN   SPI3_NSS_PIN
+
+// *************** BLACKBOX **************************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define USE_FLASHFS
+#define FLASH_CS_PIN            PB12
+
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS  BUS_SPI2
+#define M25P16_CS_PIN   FLASH_CS_PIN
+#define USE_FLASH_W25N01G          // 1Gb NAND flash support
+#define W25N01G_SPI_BUS BUS_SPI2
+#define W25N01G_CS_PIN   FLASH_CS_PIN
+#define USE_FLASH_W25M             // Stacked die support
+#define W25M_SPI_BUS    BUS_SPI2
+#define W25M_CS_PIN   FLASH_CS_PIN
+#define USE_FLASH_W25M512          // 512Kb (256Kb x 2 stacked) NOR flash support
+#define W25M512_SPI_BUS BUS_SPI2
+#define W25M512_CS_PIN   FLASH_CS_PIN
+#define USE_FLASH_W25M02G          // 2Gb (1Gb x 2 stacked) NAND flash support
+#define W25M02G_SPI_BUS BUS_SPI2
+#define W25M02G_CS_PIN FLASH_CS_PIN
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+
+#if 0
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI3
+#define MAX7456_CS_PIN          PA15
+#endif
+
+#if 0
+// I2C
+#define USE_I2C
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL                PB10        // SCL pad
+#define I2C2_SDA                PB11        // SDA pad
+#define USE_I2C_PULLUP
+#endif
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C2
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+
+// *************** UART *****************************
+#define USE_VCP
+#define USE_USB_DETECT
+//#define USB_DETECT_PIN          PC5
+
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+#define USE_UART_INVERTER
+#define INVERTER_PIN_UART3_RX   PC9
+#define INVERTER_PIN_USART3_RX   PC9
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART5
+#define UART5_RX_PIN            PD2
+#define UART5_TX_PIN            PD2
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_CRSF
+#define SERIALRX_UART           SERIAL_PORT_USART3
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE            ADC1
+#define ADC1_DMA_STREAM             DMA2_CHANNEL5
+#define ADC_CHANNEL1_PIN        PC2
+#define ADC_CHANNEL2_PIN        PC1
+#define ADC_CHANNEL3_PIN        PC0
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL        ADC_CHN_3
+
+#define USE_LED_STRIP
+#define WS2811_PIN              PB6
+
+#define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP )
+#define DEFAULT_VOLTAGE_METER_SOURCE    VOLTAGE_METER_ADC
+#define DEFAULT_CURRENT_METER_SOURCE    CURRENT_METER_ADC
+
+#define VBAT_SCALE_DEFAULT          110
+#define CURRENT_METER_SCALE_DEFAULT 800
+
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         0xffff
+#define TARGET_IO_PORTE         BIT(2)
+
+
+#define MAX_PWM_OUTPUT_PORTS    8
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+//#define USE_DSHOT
+//#define USE_ESC_SENSOR
+#define USE_ESCSERIAL

src/main/target/GEPRCF405/CMakeLists.txt

@@ -0,0 +1 @@
+target_stm32f405xg(GEPRCF405)

src/main/target/GEPRCF405/target.c

@@ -0,0 +1,46 @@
+/*
+* This file is part of INAV Project.
+*
+* Cleanflight is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Cleanflight is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdint.h>
+
+#include "platform.h"
+#include "drivers/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+
+
+
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM8,  CH2,  PC7, TIM_USE_PPM,                        0, 0),
+
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM1, CH2, PA9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED,                         0, 0),
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
+
+

src/main/target/GEPRCF405/target.h

@@ -0,0 +1,177 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "GEPR"
+
+#define USBD_PRODUCT_STRING     "GEPRCF405"
+
+#define LED0                    PC14
+#define LED1                    PC15
+
+
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *************** Gyro & ACC **********************
+
+
+
+#define USE_SPI
+#define USE_SPI_DEVICE_3
+
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN           PB4
+#define SPI3_MOSI_PIN           PB5
+
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
+#define MPU6000_CS_PIN          PA15
+#define MPU6000_SPI_BUS         BUS_SPI3
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_CS_PIN           PA15
+#define BMI270_SPI_BUS          BUS_SPI3
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_CS_PIN         PA15
+#define ICM42605_SPI_BUS        BUS_SPI3
+
+// *************** I2C/Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+// *************** FLASH **************************
+#define M25P16_CS_PIN           PB12
+#define M25P16_SPI_BUS          BUS_SPI2
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PA4
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PB7
+#define UART1_TX_PIN            PB6
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+
+#define USE_UART3
+#define UART3_RX_PIN            PC11
+#define UART3_TX_PIN            PC10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART5
+#define UART5_RX_PIN            PD2
+#define UART5_TX_PIN            PC12
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       7
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC1
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC0
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define RSSI_ADC_CHANNEL            ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
+
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PB1
+
+
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         (BIT(2))
+
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

src/main/target/GEPRCF722/CMakeLists.txt

@@ -0,0 +1 @@
+target_stm32f722xe(GEPRCF722)

src/main/target/GEPRCF722/target.c

@@ -0,0 +1,44 @@
+/*
+* This file is part of INAV Project.
+*
+* Cleanflight is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Cleanflight is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/sensor.h"
+
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM,                         0, 0),
+
+    DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                         0, 0),
+
+    
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/GEPRCF722/target.h

@@ -0,0 +1,173 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "GEPR"
+
+#define USBD_PRODUCT_STRING     "GEPRCF722"
+
+#define LED0                    PA13
+#define LED1                    PA14
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *************** Gyro & ACC **********************
+
+#define USE_SPI
+#define USE_SPI_DEVICE_3
+
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+
+#define MPU6000_CS_PIN          PA15
+#define MPU6000_SPI_BUS         BUS_SPI3
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_CS_PIN           PA15
+#define BMI270_SPI_BUS          BUS_SPI3
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_CS_PIN         PA15
+#define ICM42605_SPI_BUS        BUS_SPI3
+
+// *************** I2C/Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+// *************** FLASH **************************
+#define M25P16_CS_PIN           PB12
+#define M25P16_SPI_BUS          BUS_SPI2
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PB2
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define RSSI_ADC_CHANNEL            ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
+
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+
+// ************PINIO to other
+#define PINIO1_PIN                  PC8                //Enable vsw
+#define PINIO2_PIN                  PC9
+
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA             0xffff
+#define TARGET_IO_PORTB             0xffff
+#define TARGET_IO_PORTC             0xffff
+#define TARGET_IO_PORTD             (BIT(2))
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

src/main/target/MAMBAF405US/target.h

@@ -40,6 +40,10 @@
 #define MPU6000_SPI_BUS                 BUS_SPI1
 #define MPU6000_CS_PIN                  PA4
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN               CW180_DEG
+#define MPU6500_SPI_BUS                 BUS_SPI1
+#define MPU6500_CS_PIN                  PA4
 
 // *************** Baro **************************
 #define USE_I2C

src/main/target/MATEKF405/CMakeLists.txt

@@ -1,3 +1,2 @@
 target_stm32f405xg(MATEKF405)
-target_stm32f405xg(MATEKF405_SERVOS6)
 target_stm32f405xg(MATEKF405OSD)

src/main/target/MATEKF405/target.c

@@ -24,23 +24,15 @@
 timerHardware_t timerHardware[] = {
     DEF_TIM(TIM9, CH2, PA3,  TIM_USE_PPM,   0, 0), // PPM
 
-#ifdef MATEKF405_SERVOS6
+    DEF_TIM(TIM3, CH1, PC6,  TIM_USE_OUTPUT_AUTO, 0, 0), // S1
-    DEF_TIM(TIM3, CH1, PC6,  TIM_USE_MC_MOTOR |                    TIM_USE_FW_MOTOR, 0, 0), // S1
+    DEF_TIM(TIM8, CH2, PC7,  TIM_USE_OUTPUT_AUTO, 0, 1), // S2  UP(2,1)
-#else
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 1), // S3  UP(2,1)
-    DEF_TIM(TIM3, CH1, PC6,  TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0), // S4  UP(2,1)
-#endif
-    DEF_TIM(TIM8, CH2, PC7,  TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO, 0, 1), // S2  UP(2,1)
-    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO, 0, 1), // S3  UP(2,1)
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO, 0, 0), // S4  UP(2,1)
 
     DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED                        , 0, 0), // S5  UP(1,7)
 
-#ifdef MATEKF405_SERVOS6
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_OUTPUT_AUTO, 0, 0), // S6  UP(2,5)
-    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6  UP(2,5)
+    DEF_TIM(TIM4, CH3, PB8,  TIM_USE_OUTPUT_AUTO, 0, 0), // S7  D(1,7)!S5 UP(2,6)
-#else
-    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO, 0, 0), // S6  UP(2,5)
-#endif
-    DEF_TIM(TIM4, CH3, PB8,  TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7  D(1,7)!S5 UP(2,6)
 
     DEF_TIM(TIM5, CH3, PA2,  TIM_USE_PWM,                                            0, 0), // TX2
 

src/main/target/MATEKF405TE/target.h

@@ -178,5 +178,6 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 #define USE_DSHOT
+#define USE_DSHOT_DMAR
 #define USE_ESC_SENSOR
 

src/main/target/MATEKF722/CMakeLists.txt

@@ -1,2 +1 @@
 target_stm32f722xe(MATEKF722)
-target_stm32f722xe(MATEKF722_HEXSERVO)

src/main/target/MATEKF722/target.c

@@ -27,18 +27,14 @@
 timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM,                         0, 0), // PPM
 
-    DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),                     // S1 D(1, 4, 5)
+    DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0),  // S1 D(1, 4, 5)
-    DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),                     // S2 D(2, 3, 7)
+    DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0),  // S2 D(2, 3, 7)
-    DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),                     // S3 D(2, 4, 7)
+    DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0),  // S3 D(2, 4, 7)
-    DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),                     // S4 D(2, 7, 7)
+    DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0),  // S4 D(2, 7, 7)
 
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),                     // S5 DMA1_ST2
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0),  // S5 DMA1_ST2
-#ifdef MATEKF722_HEXSERVO
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0),  // S6 DMA2_ST6
-    DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),                     // S6 DMA2_ST6
+    DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0),  // S7 DMA1_ST7
-#else
-    DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),  // S6 DMA2_ST6
-#endif
-    DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),                     // S7 DMA1_ST7
 
     // DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0),                                           // TX2/S8  DMA1_ST0
 

src/main/target/MATEKH743/target.c

@@ -32,21 +32,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500,  MPU6500_SPI_BUS,   MPU6
 BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS,  ICM42605_CS_PIN,  NONE,  2,  DEVFLAGS_NONE,  IMU_ICM42605_ALIGN);
 
 timerHardware_t timerHardware[] = {
-    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S1
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),   // S1
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),   // S2
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1),   // S2
 
-    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2),   // S3  
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2),   // S3  
-    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3),   // S4
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3),   // S4
-    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4),   // S5
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4),   // S5
-    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5),   // S6
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5),   // S6
 
-    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6),   // S7
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6),   // S7
-    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7),   // S8
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7),   // S8
-    DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S9
+    DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0),   // S9
-    DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
+    DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0),   // S10 DMA_NONE
 
-    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S11
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0),   // S11
-    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S12 DMA_NONE
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0),   // S12 DMA_NONE
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
     DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM

src/main/target/RUSH_BLADE_F7/target.c

@@ -27,8 +27,8 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
     DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
     DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
-    DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S5
+    DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5
-    DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S6
+    DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6
     DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S7
     DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S8
 

src/main/target/SITL/target.c

@@ -51,6 +51,7 @@
 #include "drivers/timer.h"
 #include "drivers/serial.h"
 #include "config/config_streamer.h"
+#include "build/version.h"
 
 #include "target/SITL/sim/realFlight.h"
 #include "target/SITL/sim/xplane.h"
@@ -73,9 +74,12 @@ static int simPort = 0;
 
 static char **c_argv;
 
-void systemInit(void) {
+static void printVersion(void) {
+    fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision);
+}
 
-    fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
+void systemInit(void) {
+    printVersion();
     clock_gettime(CLOCK_MONOTONIC, &start_time);
     fprintf(stderr, "[SYSTEM] Init...\n");
 
@@ -168,6 +172,7 @@ bool parseMapping(char* mapStr)
 
 void printCmdLineOptions(void)
 {
+    printVersion();
     fprintf(stderr, "Avaiable options:\n");
     fprintf(stderr, "--path=[path]                        Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
     fprintf(stderr, "--sim=[rf|xp]                        Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
@@ -197,6 +202,7 @@ void parseArguments(int argc, char *argv[])
             {"simport", required_argument, 0, 'p'},
             {"help", no_argument, 0, 'h'},
             {"path", required_argument, 0, 'e'},
+	    {"version", no_argument, 0, 'v'},
             {NULL, 0, NULL, 0}
         };
 
@@ -236,10 +242,12 @@ void parseArguments(int argc, char *argv[])
                     fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
                 }
                 break;
-            case 'h':
+	    case 'v':
+		printVersion();
+		exit(0);
+	    default:
                 printCmdLineOptions();
                 exit(0);
-                break;
         }
     }
 

src/main/target/SKYSTARSH743HD/target.c

@@ -36,13 +36,10 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2),   // S3
     DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3),   // S4
 
-    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4),   // S5  
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4),   // S5
-    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5),   // S6
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5),   // S6
-    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6),   // S7
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6),   // S7
-    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7),   // S8
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7),   // S8
-
-    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S9
-    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
 };

src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt

@@ -0,0 +1,2 @@
+target_stm32f405xg(SPEEDYBEEF405MINI)
+target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS)

src/main/target/SPEEDYBEEF405MINI/config.c

@@ -0,0 +1,37 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "fc/fc_msp_box.h"
+
+#include "io/piniobox.h"
+#include "io/serial.h"
+
+void targetConfiguration(void)
+{
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_RX_SERIAL;
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL;
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
+
+    pinioBoxConfigMutable()->permanentId[0] = BOXARM;
+}

src/main/target/SPEEDYBEEF405MINI/target.c

@@ -0,0 +1,46 @@
+/*
+ * 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 <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/pinio.h"
+#include "drivers/sensor.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR  | TIM_USE_FW_MOTOR,   1, 0), // S1
+    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR  | TIM_USE_FW_MOTOR,   1, 0), // S2
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S3
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S4
+
+#ifdef SPEEDYBEEF405MINI_6OUTPUTS
+    DEF_TIM(TIM12,  CH1,    PB14,   TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO,     1, 0), // CAM_CTRL
+    DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO,     0, 0), // LED
+#else
+    DEF_TIM(TIM12,  CH1,    PB14,   TIM_USE_ANY,                                                1, 0), // CAM_CTRL
+    DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_LED,                                                0, 0), // LED
+#endif
+
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY,                            0, 0), //TX2  softserial1_Tx
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/SPEEDYBEEF405MINI/target.h

@@ -0,0 +1,186 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "SF4M"
+#define USBD_PRODUCT_STRING     "SPEEDYBEEF405MINI"
+
+#define LED0                    PC13
+
+#define BEEPER                  PC15
+#define BEEPER_INVERTED
+
+/*
+ * SPI Buses
+ */
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PC2
+#define SPI2_MOSI_PIN   	    PC3
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN   	    PB4
+#define SPI3_MOSI_PIN   	    PB5
+
+/*
+ * I2C
+ */
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+/*
+ * Serial
+ */
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+
+#define USE_UART3
+#define UART3_TX_PIN            PC10
+#define UART3_RX_PIN            PC11
+
+#define USE_UART4 //Internally routed to BLE
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1 
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            PD2
+
+#define USE_UART6
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
+
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN      PA2
+#define SOFTSERIAL_1_RX_PIN      PA2
+
+#define SERIAL_PORT_COUNT       8
+
+/*
+ * Gyro
+ */
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG
+#define ICM42605_CS_PIN         PA4
+#define ICM42605_SPI_BUS        BUS_SPI1
+
+/*
+ * Other
+ */
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define PITOT_I2C_BUS           BUS_I2C1
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+/*
+ * OSD
+ */
+#define USE_MAX7456
+#define MAX7456_CS_PIN          PB12
+#define MAX7456_SPI_BUS         BUS_SPI2
+
+/*
+ * Blackbox
+ */
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS          BUS_SPI3
+#define M25P16_CS_PIN           PC14
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PB11 // RF Switch
+#define PINIO1_FLAGS                PINIO_FLAGS_INVERTED
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES        (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
+#define CURRENT_METER_SCALE     250
+#define CURRENT_METER_OFFSET    0
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+
+#ifdef SPEEDYBEEF405MINI_6OUTPUTS
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#else
+
+#define MAX_PWM_OUTPUT_PORTS        4
+
+#endif
+
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR

src/main/target/SPEEDYBEEF405V3/target.h

@@ -152,7 +152,8 @@
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
 
 #define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
-// #define USE_DSHOT
+#define USE_DSHOT
+#define USE_DSHOT_DMAR
 // #define USE_ESC_SENSOR
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 

src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt

@@ -1 +1,2 @@
 target_stm32f722xe(SPEEDYBEEF7MINI)
+target_stm32f722xe(SPEEDYBEEF7MINIV2)

src/main/target/SPEEDYBEEF7MINI/target.c

@@ -26,7 +26,11 @@
 #include "drivers/pinio.h"
 #include "drivers/sensor.h"
 
+#ifdef SPEEDYBEEF7MINIV2
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270,      DEVHW_BMI270,       BMI270_SPI_BUS,     BMI270_CS_PIN,      NONE,       0,  DEVFLAGS_NONE,  IMU_BMI270_ALIGN);
+#else
 BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+#endif
 
 timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH1, PB4,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP1-2   D(1, 4, 5)

src/main/target/SPEEDYBEEF7MINI/target.h

@@ -19,7 +19,12 @@
 #pragma once
 
 #define TARGET_BOARD_IDENTIFIER "SBMN"
+
+#ifdef SPEEDYBEEF7MINIV2
+#define USBD_PRODUCT_STRING  "SPEEDYBEEF7MINIV2"
+#else
 #define USBD_PRODUCT_STRING  "SPEEDYBEEF7MINI"
+#endif
 
 #define LED0                    PA14  //Blue   SWCLK
 
@@ -35,11 +40,22 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#ifdef SPEEDYBEEF7MINIV2
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_CS_PIN           PB2
+#define BMI270_SPI_BUS          BUS_SPI1
+
+#else 
+
 #define USE_IMU_MPU6000
 #define IMU_MPU6000_ALIGN       CW0_DEG
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
 
+#endif
+
 // *************** I2C /Baro/Mag *********************
 #define USE_I2C
 #define USE_I2C_DEVICE_1

src/main/target/common.h

@@ -92,6 +92,7 @@
 #define USE_PITOT
 #define USE_PITOT_MS4525
 #define USE_PITOT_MSP
+#define USE_PITOT_DLVR
 
 #define USE_1WIRE
 #define USE_1WIRE_DS2482
@@ -161,6 +162,7 @@
 #define NAV_MAX_WAYPOINTS       120
 #define USE_RCDEVICE
 #define USE_MULTI_MISSION
+#define USE_MULTI_FUNCTIONS  // defines functions only, warnings always defined
 
 //Enable VTX control
 #define USE_VTX_CONTROL

src/main/target/common_hardware.c

@@ -357,10 +357,18 @@
 #endif
 
 #if defined(USE_PITOT_MS4525) && defined(MS4525_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_ms5425,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
+    BUSDEV_REGISTER_I2C(busdev_ms4525,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
 #endif
 
 
+#if defined(PITOT_I2C_BUS) && !defined(DLVR_I2C_BUS)
+    #define DLVR_I2C_BUS PITOT_I2C_BUS
+#endif
+
+#if defined(USE_PITOT_DLVR) && defined(DLVR_I2C_BUS)
+    BUSDEV_REGISTER_I2C(busdev_dlvr,        DEVHW_DLVR,         DLVR_I2C_BUS,        0x28,              NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
+#endif
+
 /** OTHER HARDWARE **/
 
 #if defined(USE_MAX7456)