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

Dockerfile

@@ -1,4 +1,4 @@
-FROM ubuntu:focal
+FROM ubuntu:jammy
 
 ARG USER_ID
 ARG GROUP_ID
@@ -9,10 +9,10 @@ RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3
 RUN pip install pyyaml
 
 # if either of these are already set the same as the user's machine, leave them be and ignore the error
-RUN addgroup --gid $GROUP_ID users; exit 0;
+RUN addgroup --gid $GROUP_ID inav; exit 0;
-RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID user; exit 0;
+RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0;
 
-USER user
+USER inav
 RUN git config --global --add safe.directory /src
 
 VOLUME /src

cmake/stm32h7.cmake

@@ -185,7 +185,7 @@ function(target_stm32h7xx)
         VCP_SOURCES ${STM32H7_USB_SRC} ${STM32H7_VCP_SRC}
         VCP_INCLUDE_DIRECTORIES ${STM32H7_USB_INCLUDE_DIRS} ${STM32H7_VCP_DIR}
 
-        OPTIMIZATION -O2
+        OPTIMIZATION -Ofast
 
         OPENOCD_TARGET stm32h7x
 

docs/Blackbox.md

@@ -4,8 +4,7 @@
 
 ## Introduction
 
-This feature transmits your flight data information on every control loop iteration over a serial port to an external
+This feature transmits your flight data information on every control loop iteration over a serial port to an external logging device to be recorded, SD card, or to a dataflash chip which is present on some flight controllers.
-logging device to be recorded, SD card, or to a dataflash chip which is present on some flight controllers.
 
 After your flight, you can view the resulting logs using the interactive log viewer:
 
@@ -17,66 +16,43 @@ video using the `blackbox_render` tool. Those tools can be found in this reposit
 https://github.com/iNavFlight/blackbox-tools
 
 ## Logged data
-The blackbox records flight data on every iteration of the flight control loop. It records the current time in
+The blackbox records flight data on every iteration of the flight control loop. It records the current time in microseconds, P, I and D corrections for each axis, your RC command stick positions (after applying expo curves), gyroscope data, accelerometer data (after your configured low-pass filtering), barometer and rangefinder readings, 3-axis magnetometer readings, raw VBAT and current measurements, RSSI, and the command being sent to each motor speed controller. This is all stored without any approximation or loss of precision, so even quite subtle problems should be
-microseconds, P, I and D corrections for each axis, your RC command stick positions (after applying expo curves),
-gyroscope data, accelerometer data (after your configured low-pass filtering), barometer and rangefinder readings, 3-axis
-magnetometer readings, raw VBAT and current measurements, RSSI, and the command being sent to each motor speed
-controller. This is all stored without any approximation or loss of precision, so even quite subtle problems should be
 detectable from the fight data log.
 
-GPS data is logged whenever new GPS data is available. Although the CSV decoder will decode this data, the video
+GPS data is logged whenever new GPS data is available. Although the CSV decoder will decode this data, the video renderer does not yet show any of the GPS information (this will be added later).
-renderer does not yet show any of the GPS information (this will be added later).
 
 ## Supported configurations
 
-The maximum data rate that can be recorded to the flight log is fairly restricted, so anything that increases the load
+The maximum data rate that can be recorded to the flight log is fairly restricted, so anything that increases the load can cause the flight log to drop frames and contain errors.
-can cause the flight log to drop frames and contain errors.
 
-The Blackbox is typically used on tricopters and quadcopters. Although it will work on hexacopters and octocopters,
+The Blackbox is typically used on tricopters and quadcopters. Although it will work on hexacopters and octocopters, because these craft have more motors to record, they must transmit more data to the flight log. This can increase the number of dropped frames. Although the browser-based log viewer supports hexacopters and octocopters, the command-line `blackbox_render` tool currently only supports tri- and quadcopters.
-because these craft have more motors to record, they must transmit more data to the flight log. This can increase the
-number of dropped frames. Although the browser-based log viewer supports hexacopters and octocopters, the command-line
-`blackbox_render` tool currently only supports tri- and quadcopters.
 
-INAV's `looptime` setting decides how frequently an update is saved to the flight log. The default looptime on
+INAV's `looptime` setting decides how frequently an update is saved to the flight log. The default looptime on INAV is 2000us. If you're using a looptime smaller than about 2400, you may experience some dropped frames due to the high required data rate. In that case you will need to reduce the sampling rate in the Blackbox settings, or increase your logger's baudrate to 250000. See the later section on configuring the Blackbox feature for details.
-INAV is 2000us. If you're using a looptime smaller than about 2400, you may experience some dropped frames due to
-the high required data rate. In that case you will need to reduce the sampling rate in the Blackbox settings, or
-increase your logger's baudrate to 250000. See the later section on configuring the Blackbox feature for details.
 
 ## Setting up logging
 
-First, you must enable the Blackbox feature. In the [INAV Configurator][] enter the Configuration tab,
+First, you must enable the Blackbox feature. In the [INAV Configurator][] enter the Configuration tab, tick the "BLACKBOX" feature at the bottom of the page, and click "Save and reboot"
-tick the "BLACKBOX" feature at the bottom of the page, and click "Save and reboot"
 
-Now you must decide which device to store your flight logs on. You can either transmit the log data over a serial port
+Now you must decide which device to store your flight logs on. You can either transmit the log data over a serial port to an external logging device like the [OpenLog serial data logger][] to be recorded to a microSDHC card, or if you have a compatible flight controller you can store the logs on the onboard dataflash storage instead.
-to an external logging device like the [OpenLog serial data logger][] to be recorded to a microSDHC card, or if you have
-a compatible flight controller you can store the logs on the onboard dataflash storage instead.
 
 ### OpenLog serial data logger
 
-The OpenLog is a small logging device which attaches to your flight controller using a serial port and logs your
+The OpenLog is a small logging device which attaches to your flight controller using a serial port and logs your flights to a MicroSD card.
-flights to a MicroSD card.
 
-The OpenLog ships from SparkFun with standard "OpenLog 3" firmware installed. Although this original OpenLog firmware
+The OpenLog ships from SparkFun with standard "OpenLog 3" firmware installed. Although this original OpenLog firmware will work with the Blackbox, in order to reduce the number of dropped frames it should be reflashed with the higher performance [OpenLog Blackbox firmware][]. The special Blackbox variant of the OpenLog firmware also ensures that the OpenLog is using INAV compatible settings, and defaults to 115200 baud.
-will work with the Blackbox, in order to reduce the number of dropped frames it should be reflashed with the
-higher performance [OpenLog Blackbox firmware][]. The special Blackbox variant of the OpenLog firmware also ensures that
-the OpenLog is using INAV compatible settings, and defaults to 115200 baud.
 
-You can find the Blackbox version of the OpenLog firmware [here](https://github.com/iNavFlight/openlog-blackbox-firmware),
+You can find the Blackbox version of the OpenLog firmware [here](https://github.com/iNavFlight/openlog-blackbox-firmware), along with instructions for installing it onto your OpenLog.
-along with instructions for installing it onto your OpenLog.
 
 [OpenLog serial data logger]: https://www.sparkfun.com/products/9530
 [OpenLog Blackbox firmware]: https://github.com/iNavFlight/openlog-blackbox-firmware
 
 #### microSDHC
 
-Your choice of microSDHC card is very important to the performance of the system. The OpenLog relies on being able to
+Your choice of microSDHC card is very important to the performance of the system. The OpenLog relies on being able to make many small writes to the card with minimal delay, which not every card is good at. A faster SD-card speed rating is not a guarantee of better performance.
-make many small writes to the card with minimal delay, which not every card is good at. A faster SD-card speed rating is
-not a guarantee of better performance.
 
 ##### microSDHC cards known to have poor performance
 
- - Generic 4GB Class 4 microSDHC card - the rate of missing frames is about 1%, and is concentrated around the most
+ - Generic 4GB Class 4 microSDHC card - the rate of missing frames is about 1%, and is concentrated around the most interesting parts of the log!
-   interesting parts of the log!
  - Sandisk Ultra 32GB (unlike the smaller 16GB version, this version has poor write latency)
 
 ##### microSDHC cards known to have good performance
@@ -85,33 +61,22 @@ not a guarantee of better performance.
  - Sandisk Extreme 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%)
  - Sandisk Ultra 16GB (it performs only half as well as the Extreme in theory, but still very good)
 
-You should format any card you use with the [SD Association's special formatting tool][] , as it will give the OpenLog
+You should format any card you use with the [SD Association's special formatting tool][] , as it will give the OpenLog the best chance of writing at high speed. You must format it with either FAT, or with FAT32 (recommended).
-the best chance of writing at high speed. You must format it with either FAT, or with FAT32 (recommended).
 
 [SD Association's special formatting tool]: https://www.sdcard.org/downloads/formatter_4/
 
 ### Choosing a serial port for the OpenLog
-First, tell the Blackbox to log using a serial port (rather than to an onboard dataflash chip). Go to the
+First, tell the Blackbox to log using a serial port (rather than to an onboard dataflash chip). Go to the Configurator's CLI tab, enter `set blackbox_device=SERIAL` to switch logging to serial, and save.
-Configurator's CLI tab, enter `set blackbox_device=SERIAL` to switch logging to serial, and
-save.
 
-You need to let INAV know which of [your serial ports][] you connect your OpenLog to (i.e. the Blackbox port),
+You need to let INAV know which of [your serial ports][] you connect your OpenLog to (i.e. the Blackbox port), which you can do on the Configurator's Ports tab.
-which you can do on the Configurator's Ports tab.
 
-You should use a hardware serial port. SoftSerial ports can be used for the Blackbox. However, because they are limited to 19200 baud, your logging
+You should use a hardware serial port. SoftSerial ports can be used for the Blackbox. However, because they are limited to 19200 baud, your logging rate will need to be severely reduced to compensate. Therefore the use of SoftSerial is not recommended.
-rate will need to be severely reduced to compensate. Therefore the use of SoftSerial is not recommended.
 
-When using a hardware serial port, Blackbox should be set to at least 115200 baud on that port. When using fast
+When using a hardware serial port, Blackbox should be set to at least 115200 baud on that port. When using fast looptimes (<2500), a baud rate of 250000 should be used instead in order to reduce dropped frames.
-looptimes (<2500), a baud rate of 250000 should be used instead in order to reduce dropped frames.
 
-The serial port used for Blackbox cannot be shared with any other function (e.g. GPS, telemetry) except the MSP
+The serial port used for Blackbox cannot be shared with any other function (e.g. GPS, telemetry) except the MSP protocol. If MSP is used on the same port as Blackbox, then MSP will be active when the board is disarmed, and Blackbox will be active when the board is armed. This will mean that you can't use the Configurator or any other function that requires MSP, such as an OSD or a Bluetooth wireless configuration app, while the board is armed.
-protocol. If MSP is used on the same port as Blackbox, then MSP will be active when the board is disarmed, and Blackbox
-will be active when the board is armed. This will mean that you can't use the Configurator or any other function that
-requires MSP, such as an OSD or a Bluetooth wireless configuration app, while the board is armed.
 
-Connect the "TX" pin of the serial port you've chosen to the OpenLog's "RXI" pin. Don't connect the serial port's RX
+Connect the "TX" pin of the serial port you've chosen to the OpenLog's "RXI" pin. Don't connect the serial port's RX pin to the OpenLog, as this will cause the OpenLog to interfere with any shared functions on the serial port while disarmed.
-pin to the OpenLog, as this will cause the OpenLog to interfere with any shared functions on the serial port while
-disarmed.
 
 The key criteria to choose a serial port are:
 
@@ -121,15 +86,11 @@ The key criteria to choose a serial port are:
 
 #### OpenLog configuration
 
-Power up the OpenLog with a microSD card inside, wait 10 seconds or so, then power it down and plug the microSD card
+Power up the OpenLog with a microSD card inside, wait 10 seconds or so, then power it down and plug the microSD card into your computer. You should find a "CONFIG.TXT" file on the card, open it up in a text editor. You should see the baud rate that the OpenLog has been configured for (usually 115200 or 9600 from the factory). Set the baud rate to match the rate you entered for the Blackbox in the Configurator's Port tab (typically 115200 or 250000).
-into your computer. You should find a "CONFIG.TXT" file on the card, open it up in a text editor. You should see the
-baud rate that the OpenLog has been configured for (usually 115200 or 9600 from the factory). Set the baud rate to match
-the rate you entered for the Blackbox in the Configurator's Port tab (typically 115200 or 250000).
 
 Save the file and put the card back into your OpenLog, it will use those settings from now on.
 
-If your OpenLog didn't write a CONFIG.TXT file, create a CONFIG.TXT file with these contents and store it in the root
+If your OpenLog didn't write a CONFIG.TXT file, create a CONFIG.TXT file with these contents and store it in the root of the MicroSD card:
-of the MicroSD card:
 
 ```
 115200
@@ -145,15 +106,12 @@ baud,escape,esc#,mode,verb,echo,ignoreRX
 
 #### OpenLog protection
 
-The OpenLog can be wrapped in black electrical tape or heat-shrink in order to insulate it from conductive frames (like
+The OpenLog can be wrapped in black electrical tape or heat-shrink in order to insulate it from conductive frames (like carbon fibre), but this makes its status LEDs impossible to see. I recommend wrapping it with some clear heatshrink tubing instead.
-carbon fiber), but this makes its status LEDs impossible to see. I recommend wrapping it with some clear heatshrink
-tubing instead.
 
 ![OpenLog installed](Wiring/blackbox-installation-1.jpg "OpenLog installed with double-sided tape, SDCard slot pointing outward")
 
 ### Onboard dataflash storage
-Some flight controllers have an onboard SPI NOR dataflash chip which can be used to store flight logs instead of using
+Some flight controllers have an onboard SPI NOR dataflash chip which can be used to store flight logs instead of using an OpenLog.
-an OpenLog.
 
 These chips are also supported:
 
@@ -164,57 +122,47 @@ These chips are also supported:
 * Winbond W25Q128 - 128 Mbit / 16 MByte
 
 #### Enable recording to dataflash
-On the Configurator's CLI tab, you must enter `set blackbox_device=SPIFLASH` to switch to logging to an onboard dataflash chip,
+On the Configurator's CLI tab, you must enter `set blackbox_device=SPIFLASH` to switch to logging to an onboard dataflash chip, then save.
-then save.
 
 [your serial ports]: https://github.com/iNavFlight/inav/blob/master/docs/Serial.md
 [INAV Configurator]: https://chrome.google.com/webstore/detail/inav-configurator/fmaidjmgkdkpafmbnmigkpdnpdhopgel
 
 ## Configuring the Blackbox
 
-The Blackbox currently provides two settings (`blackbox_rate_num` and `blackbox_rate_denom`) that allow you to control
+The Blackbox currently provides two settings (`blackbox_rate_num` and `blackbox_rate_denom`) that allow you to control the rate at which data is logged. These two together form a fraction (`blackbox_rate_num / blackbox_rate_denom`) which decides what portion of the flight controller's control loop iterations should be logged. The default is 1/1 which logs every iteration.
-the rate at which data is logged. These two together form a fraction (`blackbox_rate_num / blackbox_rate_denom`) which
-decides what portion of the flight controller's control loop iterations should be logged. The default is 1/1 which logs
-every iteration.
 
-If you're using a slower MicroSD card, you may need to reduce your logging rate to reduce the number of corrupted
+If you're using a slower MicroSD card, you may need to reduce your logging rate to reduce the number of corrupted logged frames that `blackbox_decode` complains about. A rate of 1/2 is likely to work for most craft.
-logged frames that `blackbox_decode` complains about. A rate of 1/2 is likely to work for most craft.
 
-You can change the logging rate settings by entering the CLI tab in the [INAV Configurator][] and using the `set`
+You can change the logging rate settings by entering the CLI tab in the [INAV Configurator][] and using the `set` command, like so:
-command, like so:
 
 ```
 set blackbox_rate_num = 1
 set blackbox_rate_denom = 2
 ```
 
-The data rate for my quadcopter using a looptime of 2400 and a rate of 1/1 is about 10.25kB/s. This allows about 18
+The data rate for my quadcopter using a looptime of 2400 and a rate of 1/1 is about 10.25kB/s. This allows about 18 days of flight logs to fit on my OpenLog's 16GB MicroSD card, which ought to be enough for anybody :).
-days of flight logs to fit on my OpenLog's 16GB MicroSD card, which ought to be enough for anybody :).
 
-If you are logging using SoftSerial, you will almost certainly need to reduce your logging rate to 1/32. Even at that
+If you are logging using SoftSerial, you will almost certainly need to reduce your logging rate to 1/32. Even at that logging rate, looptimes faster than about 1000 cannot be successfully logged.
-logging rate, looptimes faster than about 1000 cannot be successfully logged.
 
-If you're logging to an onboard dataflash chip instead of an OpenLog, be aware that the 2MB of storage space it offers
+If you're logging to an onboard dataflash chip instead of an OpenLog, be aware that the 2MB of storage space it offers is pretty small. At the default 1/1 logging rate, and a 2400 looptime, this is only enough for about 3 minutes of flight. This could be long enough for you to investigate some flying problem with your craft, but you may want to reduce the logging rate in order to extend your recording time.
-is pretty small. At the default 1/1 logging rate, and a 2400 looptime, this is only enough for about 3 minutes of
-flight. This could be long enough for you to investigate some flying problem with your craft, but you may want to reduce
-the logging rate in order to extend your recording time.
 
-To maximize your recording time, you could drop the rate all the way down to 1/32 (the smallest possible rate) which
+To maximize your recording time, you could drop the rate all the way down to 1/32 which would result in a logging rate of about 10-20Hz and about 650 bytes/second of data. At that logging rate, a 2MB dataflash chip can store around 50 minutes of flight data, though the level of detail is severely reduced and you could not diagnose flight problems like vibration or PID setting issues.
-would result in a logging rate of about 10-20Hz and about 650 bytes/second of data. At that logging rate, a 2MB
-dataflash chip can store around 50 minutes of flight data, though the level of detail is severely reduced and you could
-not diagnose flight problems like vibration or PID setting issues.
 
 The CLI command `blackbox` allows setting which Blackbox fields are recorded to conserve space and bandwidth. Possible fields are:
 
-* NAV_ACC - Navigation accelerometer readouts
+* `NAV_ACC` - Navigation accelerometer readouts
-* NAV_PID - Navigation PID debug
+* `NAV_PID` - Navigation PID debug
-* NAV_POS - Current and target position and altitude
+* `NAV_POS` - Current and target position and altitude
-* MAG - Magnetometer raw values
+* `MAG` - Magnetometer raw values
-* ACC - Accelerometer raw values
+* `ACC` - Accelerometer raw values
-* ATTI - Attitude as computed by INAV position estimator
+* `ATTI` - Attitude as computed by INAV position estimator
-* RC_DATA - RC channels 1-4 as returned by the radio receiver
+* `RC_DATA` - RC channels 1-4 as returned by the radio receiver
-* RC_COMMAND - RC_DATA converted to [-500:500] scale (for A,E,R) with expo and deadband
+* `RC_COMMAND` - RC_DATA converted to [-500:500] scale (for A,E,R) with expo and deadband
-* MOTORS - motor output
+* `MOTORS` - motor output
+* `GYRO_RAW` - Raw Gyro data
+* `PEAKS_R` - Roll axis noise peak
+* `PEAKS_P` - Pitch axis noise peak
+* `PEAKS_Y` - Yaw axis noise peak
 
 Usage:
 
@@ -227,38 +175,28 @@ Usage:
 
 The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm.
 
-If your craft has a buzzer attached, you can use INAV's arming beep to synchronize your Blackbox log with your
+If your craft has a buzzer attached, you can use INAV's arming beep to synchronize your Blackbox log with your flight video. INAV's arming beep is a "long, short" pattern. The beginning of the first long beep will be shown as a blue line in the flight data log, which you can sync against your recorded audio track.
-flight video. INAV's arming beep is a "long, short" pattern. The beginning of the first long beep will be shown
-as a blue line in the flight data log, which you can sync against your recorded audio track.
 
 You should wait a few seconds after disarming your craft to allow the Blackbox to finish saving its data.
 
 ### Usage - OpenLog
-Each time the OpenLog is power-cycled, it begins a fresh new log file. If you arm and disarm several times without
+Each time the OpenLog is power-cycled, it begins a fresh new log file. If you arm and disarm several times without cycling the power (recording several flights), those logs will be combined together into one file. The command line tools will ask you to pick which one of these flights you want to display/decode.
-cycling the power (recording several flights), those logs will be combined together into one file. The command line
-tools will ask you to pick which one of these flights you want to display/decode.
 
 Don't insert or remove the SD card while the OpenLog is powered up.
 
 ### Usage - Dataflash chip
-After your flights, you can use the [INAV Configurator][] to download the contents of the dataflash to your
+After your flights, you can use the [INAV Configurator][] to download the contents of the dataflash to your computer. Go to the "dataflash" tab and click the "save flash to file..." button. Saving the log can take 2 or 3 minutes.
-computer. Go to the "dataflash" tab and click the "save flash to file..." button. Saving the log can take 2 or 3
-minutes.
 
 ![Dataflash tab in Configurator](Screenshots/blackbox-dataflash.png)
 
 After downloading the log, be sure to erase the chip to make it ready for reuse by clicking the "erase flash" button.
 
-If you try to start recording a new flight when the dataflash is already full, Blackbox logging will be disabled and
+If you try to start recording a new flight when the dataflash is already full, Blackbox logging will be disabled and nothing will be recorded.
-nothing will be recorded.
 
 ### Usage - Logging switch
-If you're recording to an onboard flash chip, you probably want to disable Blackbox recording when not required in order
+If you're recording to an onboard flash chip, you probably want to disable Blackbox recording when not required in order to save storage space. To do this, you can add a Blackbox flight mode to one of your AUX channels on the Configurator's modes tab. Once you've added a mode, Blackbox will only log flight data when the mode is active.
-to save storage space. To do this, you can add a Blackbox flight mode to one of your AUX channels on the Configurator's
-modes tab. Once you've added a mode, Blackbox will only log flight data when the mode is active.
 
-A log header will always be recorded at arming time, even if logging is paused. You can freely pause and resume logging
+A log header will always be recorded at arming time, even if logging is paused. You can freely pause and resume logging while in flight.
-while in flight.
 
 ## Viewing recorded logs
 After your flights, you'll have a series of flight log files with a .TXT extension.
@@ -267,12 +205,9 @@ You can view these .TXT flight log files interactively using your web browser wi
 
 https://github.com/iNavFlight/blackbox-log-viewer
 
-This allows you to scroll around a graphed version of your log and examine your log in detail. You can also export a
+This allows you to scroll around a graphed version of your log and examine your log in detail. You can also export a video of your log to share it with others!
-video of your log to share it with others!
 
-You can decode your logs with the `blackbox_decode` tool to create CSV (comma-separated values) files for analysis,
+You can decode your logs with the `blackbox_decode` tool to create CSV (comma-separated values) files for analysis, or render them into a series of PNG frames with `blackbox_render` tool, which you could then convert into a video using another software package.
-or render them into a series of PNG frames with `blackbox_render` tool, which you could then convert into a video using
-another software package.
 
 You'll find those tools along with instructions for using them in this repository:
 

docs/LedStrip.md

@@ -125,6 +125,7 @@ And each LED has overlays:
 * `B` - `B`link (flash twice) mode.
 * `O` - Lars`O`n Scanner (Cylon Effect).
 * `N` - Blink on la`N`ding (throttle < 50%).
+* `E` - Strob`E` Blink white on top of selected color.
 
 `cc` specifies the color number (0 based index), or Channel number to adjust Hue
 

docs/Programming Framework.md

@@ -125,24 +125,62 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 20            | IS_POSITION_CONTROL           | boolean `0`/`1` |
 | 21            | IS_EMERGENCY_LANDING          | boolean `0`/`1` |
 | 22            | IS_RTH                        | boolean `0`/`1` |
-| 23            | IS_WP                         | boolean `0`/`1` |
+| 23            | IS_LANDING                    | boolean `0`/`1` |
-| 24            | IS_LANDING                    | boolean `0`/`1` |
+| 24            | IS_FAILSAFE                   | boolean `0`/`1` |
-| 25            | IS_FAILSAFE                   | boolean `0`/`1` |
+| 25            | STABILIZED_ROLL               | Roll PID controller output `[-500:500]` |
-| 26            | STABILIZED_ROLL               | Roll PID controller output `[-500:500]` |
+| 26            | STABILIZED_PITCH              | Pitch PID controller output `[-500:500]` |
-| 27            | STABILIZED_PITCH              | Pitch PID controller output `[-500:500]` |
+| 27            | STABILIZED_YAW                | Yaw PID controller output `[-500:500]` |
-| 28            | STABILIZED_YAW                | Yaw PID controller output `[-500:500]` |
+| 28            | 3D HOME_DISTANCE              | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
-| 29            | ACTIVE_WAYPOINT_INDEX         | Indexed from `1`. To verify WP is in progress, use `IS_WP` |
+| 29            | CROSSFIRE LQ                  | Crossfire Link quality as returned by the CRSF protocol | 
-| 30            | ACTIVE_WAYPOINT_ACTION        | See ACTIVE_WAYPOINT_ACTION paragraph |
+| 30            | CROSSFIRE SNR                 | Crossfire SNR as returned by the CRSF protocol |
-| 31            | 3D HOME_DISTANCE              | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
+| 31            | GPS_VALID                     | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
-| 32            | CROSSFIRE LQ                  | Crossfire Link quality as returned by the CRSF protocol | 
+| 32            | LOITER_RADIUS                 | The current loiter radius in cm. |
-| 33            | CROSSFIRE SNR                 | Crossfire SNR as returned by the CRSF protocol |
+| 33            | ACTIVE_PROFILE                | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
-| 34            | GPS_VALID                     | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
+| 34            | BATT_CELLS                    | Number of battery cells detected |
-| 35            | LOITER_RADIUS                 | The current loiter radius in cm. |
+| 35            | AGL_STATUS                    | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
-| 36            | ACTIVE_PROFILE                | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
+| 36            | AGL                           | integer Above The Groud Altitude in `cm` |
-| 37            | BATT_CELLS                    | Number of battery cells detected |
+| 37            | RANGEFINDER_RAW               | integer raw distance provided by the rangefinder in `cm` |
-| 38            | AGL_STATUS                    | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
+
-| 39            | AGL                           | integer Above The Groud Altitude in `cm` |
+#### FLIGHT_MODE
-| 40            | RANGEFINDER_RAW               | integer raw distance provided by the rangefinder in `cm` |
+
+| Operand Value | Name              | Notes |
+|---------------|-------------------|-------|
+| 0             | FAILSAFE          |  |
+| 1             | MANUAL            |  |
+| 2             | RTH               |  |
+| 3             | POSHOLD           |  |
+| 4             | CRUISE            |  |
+| 5             | ALTHOLD           |  |
+| 6             | ANGLE             |  |
+| 7             | HORIZON           |  |
+| 8             | AIR               |  |
+| 9             | USER1             |  |
+| 10            | USER2             |  |
+| 11            | COURSE_HOLD       |  |
+| 12            | USER3             |  |
+| 13            | USER4             |  |
+| 14            | ACRO              |  |
+| 15            | WAYPOINT_MISSION  |  | 
+
+#### WAYPOINTS
+
+| Operand Value | Name                          | Notes |
+|---------------|-------------------------------|-------|
+| 0             | Is WP                         | boolean `0`/`1` |
+| 1             | Current Waypoint Index        | Current waypoint leg. Indexed from `1`. To verify WP is in progress, use `Is WP` |
+| 2             | Current Waypoint Action       | Action active in current leg. See ACTIVE_WAYPOINT_ACTION table |
+| 3             | Next Waypoint Action          | Action active in next leg. See ACTIVE_WAYPOINT_ACTION table |
+| 4             | Distance to next Waypoint     | Distance to next WP in metres |
+| 5             | Distance from Waypoint        | Distance from the last WP in metres |
+| 6             | User Action 1                 | User Action 1 is active on this waypoint leg [boolean `0`/`1`] |
+| 7             | User Action 2                 | User Action 2 is active on this waypoint leg [boolean `0`/`1`] |
+| 8             | User Action 3                 | User Action 3 is active on this waypoint leg [boolean `0`/`1`] |
+| 9             | User Action 4                 | User Action 4 is active on this waypoint leg [boolean `0`/`1`] |
+| 10            | Next Waypoint User Action 1   | User Action 1 is active on the next waypoint leg [boolean `0`/`1`] |
+| 11            | Next Waypoint User Action 2   | User Action 2 is active on the next waypoint leg [boolean `0`/`1`] |
+| 12            | Next Waypoint User Action 3   | User Action 3 is active on the next waypoint leg [boolean `0`/`1`] |
+| 13            | Next Waypoint User Action 4   | User Action 4 is active on the next waypoint leg [boolean `0`/`1`] |
+
 
 #### ACTIVE_WAYPOINT_ACTION
 
@@ -155,24 +193,6 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | JUMP          | 6     |
 | SET_HEAD      | 7     |
 | LAND          | 8     |
-
-
-#### FLIGHT_MODE
-
-| Operand Value | Name      | Notes |
-|---------------|-----------|-------|
-| 0             | FAILSAFE  |  |
-| 1             | MANUAL    |  |
-| 2             | RTH       |  |
-| 3             | POSHOLD   |  |
-| 4             | CRUISE    |  |
-| 5             | ALTHOLD   |  |
-| 6             | ANGLE     |  |
-| 7             | HORIZON   |  |
-| 8             | AIR       |  |
-| 9             | USER1     |  |
-| 10            | USER2     |  |
-
     
 ### Flags
 
@@ -180,7 +200,8 @@ All flags are reseted on ARM and DISARM event.
 
 | bit   | Decimal   | Function  |
 |-------|-----------|-----------|
-| 0     | 1         | Latch - after activation LC will stay active until LATCH flag is reseted |
+| 0     | 1         | Latch - after activation LC will stay active until LATCH flag is reset |
+| 1     | 2         | Timeout satisfied - Used in timed operands to determine if the timeout has been met |
 
 ## Global variables
 

docs/Settings.md

@@ -182,6 +182,86 @@ Calculated value after '6 position avanced calibration'. See Wiki page.
 
 ---
 
+### ahrs_acc_ignore_rate
+
+Total gyro rotation rate threshold [deg/s] before scaling to consider accelerometer trustworthy
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 15 | 0 | 30 |
+
+---
+
+### ahrs_acc_ignore_slope
+
+Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5 | 0 | 10 |
+
+---
+
+### ahrs_dcm_ki
+
+Inertial Measurement Unit KI Gain for accelerometer measurements
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50 |  | 65535 |
+
+---
+
+### ahrs_dcm_ki_mag
+
+Inertial Measurement Unit KI Gain for compass measurements
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50 |  | 65535 |
+
+---
+
+### ahrs_dcm_kp
+
+Inertial Measurement Unit KP Gain for accelerometer measurements
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2000 |  | 65535 |
+
+---
+
+### ahrs_dcm_kp_mag
+
+Inertial Measurement Unit KP Gain for compass measurements
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2000 |  | 65535 |
+
+---
+
+### ahrs_gps_yaw_windcomp
+
+Wind compensation in heading estimation from gps groundcourse(fixed wing only)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON | OFF | ON |
+
+---
+
+### ahrs_inertia_comp_method
+
+Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop
+
+| Default | Min | Max |
+| --- | --- | --- |
+| VELNED |  |  |
+
+---
+
 ### airmode_throttle_threshold
 
 Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used
@@ -1632,86 +1712,6 @@ Power draw at zero throttle used for remaining flight time/distance estimation i
 
 ---
 
-### imu_acc_ignore_rate
-
-Total gyro rotation rate threshold [deg/s] before scaling to consider accelerometer trustworthy
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 15 | 0 | 30 |
-
----
-
-### imu_acc_ignore_slope
-
-Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight)
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 5 | 0 | 10 |
-
----
-
-### imu_dcm_ki
-
-Inertial Measurement Unit KI Gain for accelerometer measurements
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 50 |  | 65535 |
-
----
-
-### imu_dcm_ki_mag
-
-Inertial Measurement Unit KI Gain for compass measurements
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 50 |  | 65535 |
-
----
-
-### imu_dcm_kp
-
-Inertial Measurement Unit KP Gain for accelerometer measurements
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 2000 |  | 65535 |
-
----
-
-### imu_dcm_kp_mag
-
-Inertial Measurement Unit KP Gain for compass measurements
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 2000 |  | 65535 |
-
----
-
-### imu_gps_yaw_windcomp
-
-Wind compensation in heading estimation from gps groundcourse(fixed wing only)
-
-| Default | Min | Max |
-| --- | --- | --- |
-| ON | OFF | ON |
-
----
-
-### imu_inertia_comp_method
-
-Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop
-
-| Default | Min | Max |
-| --- | --- | --- |
-| VELNED |  |  |
-
----
-
 ### inav_allow_dead_reckoning
 
 Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation
@@ -2708,7 +2708,7 @@ Delay before craft disarms when `nav_disarm_on_landing` is set (ms)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 2000 | 100 | 10000 |
+| 1000 | 100 | 10000 |
 
 ---
 
@@ -2728,7 +2728,7 @@ If set to ON, INAV disarms the FC after landing
 
 | Default | Min | Max |
 | --- | --- | --- |
-| OFF | OFF | ON |
+| ON | OFF | ON |
 
 ---
 
@@ -4382,6 +4382,16 @@ Number of decimals for the battery voltages displayed in the OSD [1-2].
 
 ---
 
+### osd_msp_displayport_fullframe_interval
+
+Full Frame redraw interval for MSP DisplayPort [deciseconds]. This is how often a full frame update is sent to the DisplayPort, to cut down on OSD artifacting. The default value should be fine for most pilots. Though long range pilots may benefit from increasing the refresh time, especially near the edge of range. -1 = disabled (legacy mode) | 0 = every frame (not recommended) | default = 10 (1 second)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | -1 | 600 |
+
+---
+
 ### osd_neg_alt_alarm
 
 Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters)
@@ -4724,7 +4734,7 @@ IMPERIAL, METRIC, UK
 
 ### osd_video_system
 
-Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, `DJIWTF` and `BF43COMPAT`
+Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR' and `BF43COMPAT`
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -4772,6 +4782,16 @@ A limitation to overall amount of correction Flight PID can request on each axis
 
 ---
 
+### pilot_name
+
+Pilot name
+
+| Default | Min | Max |
+| --- | --- | --- |
+| _empty_ |  | MAX_NAME_LENGTH |
+
+---
+
 ### pinio_box1
 
 Mode assignment for PINIO#1
@@ -5678,7 +5698,7 @@ Time zone offset from UTC, in minutes. This is applied to the GPS time for loggi
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0 | -1440 | 1440 |
+| 0 | -720 | 840 |
 
 ---
 

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

@@ -1,87 +1,109 @@
-# General Info
+# Building in Windows with MSYS2
+- This environment does not require installing WSL which may not be available or would get in the way of other virtualization and/or anti-cheat softwares. 
+- It is also much faster to install and get set up because of its small size(~2 GB download and ~3.12 GB total after building hex file).
 
-This is a guide on how to use Windows MSYS2 distribution and building platform to build INAV firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com
+## Setting up the environment
 
-Some of those benefits are described here:
+1. Download and install the latest MSYS2 release from https://repo.msys2.org/distrib/x86_64/
+    - Scroll all the way down for an executable
+    - Scroll halfway down for a self-extracting archive
 
-https://xpack.github.io/arm-none-eabi-gcc/
+1. Open an MSYS2 terminal by running C:\msys64\msys2_shell.cmd
 
-## Setting up build environment
+1. In the newly opened shell, set up your work path
+```
+mkdir /c/Workspace
+cd /c/Workspace
+```
 
-Download MSYS2 for your architecture (most likely 64-bit)
+## Downloading the INAV repository
 
-https://www.msys2.org/wiki/MSYS2-installation/
+- For master:
-
+```
-Click on 64-bit, scroll all the way down for the latest release
+git clone https://github.com/iNavFlight/inav
-
+```
-pacman is the package manager which makes it a lot easier to install and maintain all the dependencies
+- For [a branch](https://github.com/iNavFlight/inav/branches) or [a tag](https://github.com/iNavFlight/inav/tags): 
+```
+# "5.1.0" here can be the name of a branch or a tag 
+git clone --branch 5.1.0 https://github.com/iNavFlight/inav
+```
+- If you are internet speed or space restrained, you can also use `--depth 1` which won't download the whole history and `--single-branch` which won't download other branches:
+```
+git clone --depth 1 --single-branch --branch 5.1.0 https://github.com/iNavFlight/inav
+```
+This results in ~315 MB instead of ~476 MB download/install size
 
 ## Installing dependencies
 
-Once MSYS2 is installed, you can open up a new terminal window by running:
+### Installing other dependencies:
-
-"C:\msys64\mingw64.exe"
-
-You can also make a shortcut of this somewhere on your taskbar, desktop, etc, or even setup a shortcut key to open it up every time you need to get a terminal window. If you right click on the window you can customize the font and layout to make it more comfortable to work with. This is very similar to cygwin or any other terminal program you might have used before
-
-This is the best part:
 ```
 pacman -S git ruby make cmake gcc mingw-w64-x86_64-libwinpthread-git unzip wget
 ```
 
-Now, each release needs a different version of arm toolchain. To setup the xPack ARM toolchain, use the following process:
+### Installing xPack 
-
+1. Create xPack directory:
-First, setup your work path, get the release you want to build or master if you want the latest/greatest
 ```
-mkdir /c/Workspace
-cd /c/Workspace
-# you can also check out your own fork here which makes contributing easier
-git clone https://github.com/iNavFlight/inav
-cd inav
-```
-
-(Optional) Switch to a release instead of master
-```
-git fetch origin
-# on the next line, tags/5.0.0 is the release's tag, and local_5.0.0 is the name of a local branch you will create.
-# tags can be found on https://github.com/iNavFlight/inav/tags as well as the releases page
-git checkout tags/5.0.0 -b local_5.0.0
-# you can also checkout with a branch if applicable:
-# git checkout -b release_5.1.0 origin/release_5.1.0
-```
-Now create the build and xpack directories and get the toolkit version you need for your INAV version
-```
-mkdir build
-cd build
 mkdir /c/Workspace/xpack
 cd /c/Workspace/xpack
+```
+2. Get the xPack version you need for your INAV version:
+```
 cat /c/Workspace/inav/cmake/arm-none-eabi-checks.cmake | grep "set(arm_none_eabi_gcc_version" | cut -d\" -f2
 ```
-This will give you the version you need for any given release or master branch. You can get to all the releases here and find the version you need
+3. Find the version you need from the [releases page](https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/), then either:
-
+- Download the "...-win32-x64.zip" and copy the folder inside, or
-https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/
+- Right click, choose "Copy link address" and paste it into the following commands:
 ```
-# for INAV version 5.0.0, toolchain version needed is 10.2.1
+cd /c/Workspace/xpack
+# paste the link after "wget"
 wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v10.2.1-1.1/xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip
+# paste the file name after "unzip"
 unzip xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip
+# you can delete the zip file after as it is no longer needed
+rm xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip
 ```
-This is important, put the toolkit first before your path so that it is  picked up ahead of any other versions that may be present on your system
+3. This is important, put the toolkit first before your path so that it is picked up ahead of any other versions that may be present on your system:
 ```
 export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-10.2.1-1.1/bin:$PATH
 cd /c/Workspace/inav/build
 ```
-You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake
+
+## Building the INAV firmware
+1. Create the build directory:
+```
+cd /c/Workspace/inav
+mkdir build
+```
+2. Go into the build directory:
+```
+cd build
+```
+3. Run cmake
 ```
-# while inside the build directory
 cmake ..
 ```
-Once that's done you can compile the firmware for your flight controller
+4. Compile the firmware for your flight controller.
 ```
-make DALRCF405
+make MATEKF405
 ```
-To get a list of available targets in INAV, see the target src folder
+- The list of available targets in INAV can be found here: https://github.com/inavflight/inav/tree/master/src/main/target
-[https://github.com/tednv/inav/tree/master/src/main/target](https://github.com/inavflight/inav/tree/master/src/main/target)
 
-The generated hex file will be in /c/Workspace/inav/build folder
+- The generated hex file will be in the /c/Workspace/inav/build folder
 
-At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building INAV 2.1 and 2.2, and now I'm getting back into it so figured I would share my method
+- For the following runs, repeat steps 2 to 4
+
+## Troubleshooting
+
+### *** multiple target patterns.  Stop. | Error 2
+#### Delete everything in the build directory that contains previous runs
+You can either use file explorer and delete everything inside C:\Workspace\inav\build
+or run:
+```
+cd /c/Workspace/inav/build
+rm -rf *
+```
+### -- could not find arm-none-eabi-gcc
+#### Redo export PATH:
+```
+export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-10.2.1-1.1/bin:$PATH
+```

docs/development/serial_printf_debugging.md

@@ -17,7 +17,7 @@ For example, on a VCP port.
 serial 20 32769 115200 115200 0 115200
 ```
 
-If the port is shared, it will be resused with extant settings; if the port is not shared it is opened at 921600 baud.
+If the port is shared, it will be reused with extant baud rate settings; if the port is not shared it is opened at 921600 baud.
 
 There are two run time settings that control the verbosity, the most verbose settings being:
 
@@ -36,11 +36,11 @@ The use of level and topics is described in the following sections.
 
 Log levels are defined in `src/main/common/log.h`, at the time of writing these include (in ascending order):
 
-* ERROR
+* LOG_LEVEL_ERROR
-* WARNING
+* LOG_LEVEL_WARNING
-* INFO
+* LOG_LEVEL_INFO
-* VERBOSE
+* LOG_LEVEL_VERBOSE
-* DEBUG
+* LOG_LEVEL_DEBUG
 
 These are used at both compile time and run time.
 
@@ -56,17 +56,17 @@ then only `ERROR`, `WARNING` and `INFO` levels will be output.
 
 Log topics are defined in `src/main/common/log.h`, at the time of writing:
 
-* SYSTEM
+* LOG_TOPIC_SYSTEM
-* GYRO
+* LOG_TOPIC_GYRO
-* BARO
+* LOG_TOPIC_BARO
-* PITOT
+* LOG_TOPIC_PITOT
-* PWM
+* LOG_TOPIC_PWM
-* TIMER
+* LOG_TOPIC_TIMER
-* IMU
+* LOG_TOPIC_IMU
-* TEMPERATURE
+* LOG_TOPIC_TEMPERATURE
-* POS_ESTIMATOR
+* LOG_TOPIC_POS_ESTIMATOR
-* VTX
+* LOG_TOPIC_VTX
-* OSD
+* LOG_TOPIC_OSD
 
 Topics are stored as masks (SYSTEM=1 ... OSD=1024) and may be used to unconditionally display log messages.
 
@@ -74,36 +74,37 @@ If the CLI `log_topics` is non-zero, then all topics matching the mask will be d
 
 ## Code usage
 
-A set of macros `LOG_E()` (log error) through `LOG_D()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
+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.
 
 ```
-//  LOG_D(topic, fmt, ...)
+//  LOG_DEBUG(topic, fmt, ...)
-LOG_D(SYSTEM, "This is %s topic debug message, value %d", "system", 42);
+LOG_DEBUG(LOG_TOPIC_SYSTEM, "This is %s topic debug message, value %d", "system", 42);
 ```
 
-It is also possible to dump a hex representation of arbitrary  data:
+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.:
 
 ```
-// LOG_BUF_D(topic, buf, size)
+// LOG_BUFFER_ERROR(topic, buf, size)
+// LOG_BUF_DEBUG(topic, buf, size)
 
 struct {...} tstruct;
 ...
-LOG_BUF_D(TEMPERATURE, &tstruct, sizeof(tstruct));
+LOG_BUF_DEBUG(LOG_TOPIC_TEMPERATURE, &tstruct, sizeof(tstruct));
-
 ```
 
 ## Output Support
 
 Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messages (`MSP_DEBUGMSG`). It is possible to use any serial terminal to display these messages, however it is advisable to use an application that understands `MSP_DEBUGMSG` in order to maintain readability (in a raw serial terminal the MSP message envelope may result in the display of strange characters). `MSP_DEBUGMSG` aware applications include:
 
-* msp-tool https://github.com/fiam/msp-tool
+* [msp-tool](https://github.com/fiam/msp-tool)
-* mwp https://github.com/stronnag/mwptools
+* [mwp](https://github.com/stronnag/mwptools)
-* INAV Configurator
+* [dbg-tool](https://github.com/stronnag/mwptools)
+* [INAV Configurator](https://github.com/iNavFlight/inav-configurator)
 
 For example, with the final lines of `src/main/fc/fc_init.c` set to:
 
 ```
- LOG_E(SYSTEM, "Init is complete");
+ LOG_ERROR(LOG_TOPIC_SYSTEM, "Init is complete");
  systemState |= SYSTEM_STATE_READY;
 ```
 

readme.md

@@ -4,7 +4,7 @@
 
 # INAV 6 Horizon Hawk feature freeze
 
-> INAV 6 feature freeze will happen on 29th of January 2023. No new features for INAv 6 will be accepted after that date. 
+> INAV 6 feature freeze will happen on 29th of January 2023. No new features for INAV 6 will be accepted after that date. 
 
 # INAV Community
 

src/main/blackbox/blackbox.c

@@ -1809,7 +1809,7 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "INAV %s (%s) %s",  FC_VERSION_STRING, shortGitRevision, targetName);
         BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s",                buildDate, buildTime);
         BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s",              blackboxGetStartDateTime(buf));
-        BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s",                      systemConfig()->name);
+        BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s",                      systemConfig()->craftName);
         BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u",                   blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
         BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d",                     getThrottleIdleValue());
         BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d",                     motorConfig()->maxthrottle);

src/main/cms/cms_menu_navigation.c

@@ -50,6 +50,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
     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 HOVER THR", SETTING_NAV_MC_HOVER_THR),
+    OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING),
 
     OSD_BACK_AND_END_ENTRY,
  };

src/main/common/time.h

@@ -51,10 +51,13 @@ typedef uint32_t timeUs_t;
 #define USECS_PER_SEC (1000 * 1000)
 
 #define HZ2US(hz)   (1000000 / (hz))
+#define HZ2MS(hz)   (1000 / (hz))
 #define US2S(us)    ((us) * 1e-6f)
 #define US2MS(us)   ((us) * 1e-3f)
 #define MS2US(ms)   ((ms) * 1000)
 #define MS2S(ms)    ((ms) * 1e-3f)
+#define S2MS(s)     ((s) * MILLISECS_PER_SEC)
+#define DS2MS(ds)   ((ds) * 100)
 #define HZ2S(hz)    US2S(HZ2US(hz))
 
 // Use this function only to get small deltas (difference overflows at ~35 minutes)

src/main/drivers/display.h

@@ -69,6 +69,7 @@ typedef struct displayPortVTable_s displayPortVTable_t;
 typedef struct displayPort_s {
     const displayPortVTable_t *vTable;
     void *device;
+    const char* displayPortType;
     uint8_t rows;
     uint8_t cols;
     uint8_t posX;

src/main/drivers/osd.h

@@ -48,6 +48,7 @@ typedef enum {
     VIDEO_SYSTEM_NTSC,
     VIDEO_SYSTEM_HDZERO,
     VIDEO_SYSTEM_DJIWTF,
+    VIDEO_SYSTEM_AVATAR,
     VIDEO_SYSTEM_BFCOMPAT
 } videoSystem_e;
 

src/main/fc/cli.c

@@ -1557,7 +1557,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC
         bool equalsDefault = false;
         if (defaultLedConfigs) {
             ledConfig_t ledConfigDefault = defaultLedConfigs[i];
-            equalsDefault = ledConfig == ledConfigDefault;
+            equalsDefault = !memcmp(&ledConfig, &ledConfigDefault, sizeof(ledConfig_t));
             generateLedConfig(&ledConfigDefault, ledConfigDefaultBuffer, sizeof(ledConfigDefaultBuffer));
             cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, ledConfigDefaultBuffer);
         }
@@ -3274,6 +3274,17 @@ static void cliStatus(char *cmdline)
     char buf[MAX(FORMATTED_DATE_TIME_BUFSIZE, SETTING_MAX_NAME_LENGTH)];
     dateTime_t dt;
 
+    cliPrintLinef("%s/%s %s %s / %s (%s)",
+        FC_FIRMWARE_NAME,
+        targetName,
+        FC_VERSION_STRING,
+        buildDate,
+        buildTime,
+        shortGitRevision
+    );
+    cliPrintLinef("GCC-%s",
+        compilerVersion
+    );
     cliPrintLinef("System Uptime: %d seconds", millis() / 1000);
     rtcGetDateTime(&dt);
     dateTimeFormatLocal(buf, &dt);
@@ -3399,9 +3410,18 @@ static void cliStatus(char *cmdline)
     cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS);
 #endif
 
-#if defined(USE_VTX_CONTROL) && !defined(CLI_MINIMAL_VERBOSITY)
+#if !defined(CLI_MINIMAL_VERBOSITY)
-    cliPrint("VTX: ");
+    cliPrint("OSD: ");
+#if defined(USE_OSD)
+    displayPort_t *osdDisplayPort = osdGetDisplayPort();
+    cliPrintf("%s [%u x %u]", osdDisplayPort->displayPortType, osdDisplayPort->cols, osdDisplayPort->rows);
+#else 
+    cliPrint("not used");
+#endif
+    cliPrintLinefeed();
 
+    cliPrint("VTX: ");
+#if defined(USE_VTX_CONTROL)
     if (vtxCommonDeviceIsReady(vtxCommonDevice())) {
         vtxDeviceOsdInfo_t osdInfo;
         vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
@@ -3418,6 +3438,9 @@ static void cliStatus(char *cmdline)
     else {
         cliPrint("not detected");
     }
+#else
+    cliPrint("no VTX control");
+#endif
 
     cliPrintLinefeed();
 #endif
@@ -3588,18 +3611,18 @@ static void printConfig(const char *cmdline, bool doDiff)
         cliPrintHashLine("resources");
         //printResource(dumpMask, &defaultConfig);
 
-        cliPrintHashLine("mixer");
+        cliPrintHashLine("Mixer: motor mixer");
         cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n");
 
         printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0));
 
         // print custom servo mixer if exists
-        cliPrintHashLine("servo mixer");
+        cliPrintHashLine("Mixer: servo mixer");
         cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n");
         printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0));
 
         // print servo parameters
-        cliPrintHashLine("servo");
+        cliPrintHashLine("Outputs [servo]");
         printServo(dumpMask, servoParams_CopyArray, servoParams(0));
 
 #if defined(USE_SAFE_HOME)
@@ -3607,7 +3630,7 @@ static void printConfig(const char *cmdline, bool doDiff)
         printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0));
 #endif
 
-        cliPrintHashLine("feature");
+        cliPrintHashLine("features");
         printFeature(dumpMask, &featureConfig_Copy, featureConfig());
 
 #if defined(BEEPER) || defined(USE_DSHOT)
@@ -3620,30 +3643,30 @@ static void printConfig(const char *cmdline, bool doDiff)
         printBlackbox(dumpMask, &blackboxConfig_Copy, blackboxConfig());
 #endif
 
-        cliPrintHashLine("map");
+        cliPrintHashLine("Receiver: Channel map");
         printMap(dumpMask, &rxConfig_Copy, rxConfig());
 
-        cliPrintHashLine("serial");
+        cliPrintHashLine("Ports");
         printSerial(dumpMask, &serialConfig_Copy, serialConfig());
 
 #ifdef USE_LED_STRIP
-        cliPrintHashLine("led");
+        cliPrintHashLine("LEDs");
         printLed(dumpMask, ledStripConfig_Copy.ledConfigs, ledStripConfig()->ledConfigs);
 
-        cliPrintHashLine("color");
+        cliPrintHashLine("LED color");
         printColor(dumpMask, ledStripConfig_Copy.colors, ledStripConfig()->colors);
 
-        cliPrintHashLine("mode_color");
+        cliPrintHashLine("LED mode_color");
         printModeColor(dumpMask, &ledStripConfig_Copy, ledStripConfig());
 #endif
 
-        cliPrintHashLine("aux");
+        cliPrintHashLine("Modes [aux]");
         printAux(dumpMask, modeActivationConditions_CopyArray, modeActivationConditions(0));
 
-        cliPrintHashLine("adjrange");
+        cliPrintHashLine("Adjustments [adjrange]");
         printAdjustmentRange(dumpMask, adjustmentRanges_CopyArray, adjustmentRanges(0));
 
-        cliPrintHashLine("rxrange");
+        cliPrintHashLine("Receiver rxrange");
         printRxRange(dumpMask, rxChannelRangeConfigs_CopyArray, rxChannelRangeConfigs(0));
 
 #ifdef USE_TEMPERATURE_SENSOR
@@ -3652,23 +3675,23 @@ static void printConfig(const char *cmdline, bool doDiff)
 #endif
 
 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
-        cliPrintHashLine("wp");
+        cliPrintHashLine("Mission Control Waypoints [wp]");
         printWaypoints(dumpMask, posControl.waypointList, nonVolatileWaypointList(0));
 #endif
 
 #ifdef USE_OSD
-        cliPrintHashLine("osd_layout");
+        cliPrintHashLine("OSD [osd_layout]");
         printOsdLayout(dumpMask, &osdLayoutsConfig_Copy, osdLayoutsConfig(), -1, -1);
 #endif
 
 #ifdef USE_PROGRAMMING_FRAMEWORK
-        cliPrintHashLine("logic");
+        cliPrintHashLine("Programming: logic");
         printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0), -1);
 
-        cliPrintHashLine("global vars");
+        cliPrintHashLine("Programming: global variables");
         printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
 
-        cliPrintHashLine("programmable pid controllers");
+        cliPrintHashLine("Programming: PID controllers");
         printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif
 

src/main/fc/config.c

@@ -118,7 +118,8 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
     .cpuUnderclock = SETTING_CPU_UNDERCLOCK_DEFAULT,
 #endif
     .throttle_tilt_compensation_strength = SETTING_THROTTLE_TILT_COMP_STR_DEFAULT,      // 0-100, 0 - disabled
-    .name = SETTING_NAME_DEFAULT
+    .craftName = SETTING_NAME_DEFAULT,
+    .pilotName = SETTING_NAME_DEFAULT
 );
 
 PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 2);
@@ -142,6 +143,12 @@ PG_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig,
     }
 );
 
+#define SAVESTATE_NONE 0
+#define SAVESTATE_SAVEONLY 1
+#define SAVESTATE_SAVEANDNOTIFY 2
+
+static uint8_t saveState = SAVESTATE_NONE;
+
 void validateNavConfig(void)
 {
     // Make sure minAlt is not more than maxAlt, maxAlt cannot be set lower than 500.
@@ -160,7 +167,6 @@ __attribute__((weak)) void targetConfiguration(void)
     __NOP();
 }
 
-
 #ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
 #define FIRST_PORT_INDEX 1
 #define SECOND_PORT_INDEX 0
@@ -169,11 +175,13 @@ __attribute__((weak)) void targetConfiguration(void)
 #define SECOND_PORT_INDEX 1
 #endif
 
-uint32_t getLooptime(void) {
+uint32_t getLooptime(void)
+{
     return gyroConfig()->looptime;
 }
 
-uint32_t getGyroLooptime(void) {
+uint32_t getGyroLooptime(void)
+{
     return gyro.targetLooptime;
 }
 
@@ -327,12 +335,18 @@ void readEEPROM(void)
     resumeRxSignal();
 }
 
+void processSaveConfigAndNotify(void)
+{
+    writeEEPROM();
+    readEEPROM();
+    beeperConfirmationBeeps(1);
+    osdShowEEPROMSavedNotification();
+}
+
 void writeEEPROM(void)
 {
     suspendRxSignal();
-
     writeConfigToEEPROM();
-
     resumeRxSignal();
 }
 
@@ -350,11 +364,37 @@ void ensureEEPROMContainsValidData(void)
     resetEEPROM();
 }
 
+/*
+ * Used to save the EEPROM and notify the user with beeps and OSD notifications.
+ * This consolidates all save calls in the loop in to a single save operation. This save is actioned in the next loop, if the model is disarmed.
+ */
 void saveConfigAndNotify(void)
 {
-    writeEEPROM();
+    osdStartedSaveProcess();
-    readEEPROM();
+    saveState = SAVESTATE_SAVEANDNOTIFY;
-    beeperConfirmationBeeps(1);
+}
+
+/*
+ * Used to save the EEPROM without notifications. Can be used instead of writeEEPROM() if no reboot is called after the write.
+ * This consolidates all save calls in the loop in to a single save operation. This save is actioned in the next loop, if the model is disarmed.
+ * If any save with notifications are requested, notifications are shown.
+ */
+void saveConfig(void)
+{
+    if (saveState != SAVESTATE_SAVEANDNOTIFY) {
+        saveState = SAVESTATE_SAVEONLY;
+    }
+}
+
+void processDelayedSave(void)
+{
+    if (saveState == SAVESTATE_SAVEANDNOTIFY) {
+        processSaveConfigAndNotify();
+        saveState = SAVESTATE_NONE;
+    } else if (saveState == SAVESTATE_SAVEONLY) {
+        writeEEPROM();
+        saveState = SAVESTATE_NONE;
+    }
 }
 
 uint8_t getConfigProfile(void)
@@ -417,13 +457,15 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex)
     beeperConfirmationBeeps(profileIndex + 1);
 }
 
-void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) {
+void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT])
+{
     gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X];
     gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y];
     gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z];
 }
 
-void setGravityCalibrationAndWriteEEPROM(float getGravity) {
+void setGravityCalibration(float getGravity)
+{
     gyroConfigMutable()->gravity_cmss_cal = getGravity;
 }
 

src/main/fc/config.h

@@ -80,7 +80,8 @@ typedef struct systemConfig_s {
     uint8_t cpuUnderclock;
 #endif
     uint8_t throttle_tilt_compensation_strength;    // the correction that will be applied at throttle_correction_angle.
-    char name[MAX_NAME_LENGTH + 1];
+    char craftName[MAX_NAME_LENGTH + 1];
+    char pilotName[MAX_NAME_LENGTH + 1];
 } systemConfig_t;
 
 PG_DECLARE(systemConfig_t, systemConfig);
@@ -122,7 +123,9 @@ void resetEEPROM(void);
 void readEEPROM(void);
 void writeEEPROM(void);
 void ensureEEPROMContainsValidData(void);
+void processDelayedSave(void);
 
+void saveConfig(void);
 void saveConfigAndNotify(void);
 void validateAndFixConfig(void);
 void validateAndFixTargetConfig(void);
@@ -135,8 +138,8 @@ uint8_t getConfigBatteryProfile(void);
 bool setConfigBatteryProfile(uint8_t profileIndex);
 void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex);
 
-void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]);
+void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT]);
-void setGravityCalibrationAndWriteEEPROM(float getGravity);
+void setGravityCalibration(float getGravity);
 
 bool canSoftwareSerialBeUsed(void);
 void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch);

src/main/fc/fc_core.c

@@ -104,19 +104,8 @@ enum {
 };
 
 #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000
-#define EMERGENCY_ARMING_COUNTER_STEP_MS 100
+#define EMERGENCY_ARMING_COUNTER_STEP_MS 1000
-
+#define EMERGENCY_ARMING_MIN_ARM_COUNT 10
-typedef struct emergencyArmingState_s {
-    bool armingSwitchWasOn;
-    // Each entry in the queue is an offset from start,
-    // in 0.1s increments. This lets us represent up to 25.5s
-    // so it will work fine as long as the window for the triggers
-    // is smaller (see EMERGENCY_ARMING_TIME_WINDOW_MS). First
-    // entry of the queue is implicit.
-    timeMs_t start;
-    uint8_t queue[9];
-    uint8_t queueCount;
-} emergencyArmingState_t;
 
 timeDelta_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 static timeUs_t flightTime = 0;
@@ -131,7 +120,6 @@ uint8_t motorControlEnable = false;
 static bool isRXDataNew;
 static disarmReason_t lastDisarmReason = DISARM_NONE;
 timeUs_t lastDisarmTimeUs = 0;
-static emergencyArmingState_t emergencyArming;
 
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;
@@ -368,14 +356,6 @@ static void updateArmingStatus(void)
     }
 }
 
-static bool emergencyArmingIsTriggered(void)
-{
-    int threshold = (EMERGENCY_ARMING_TIME_WINDOW_MS / EMERGENCY_ARMING_COUNTER_STEP_MS);
-    return emergencyArming.queueCount == ARRAYLEN(emergencyArming.queue) + 1 &&
-        emergencyArming.queue[ARRAYLEN(emergencyArming.queue) - 1] < threshold &&
-        emergencyArming.start >= millis() - EMERGENCY_ARMING_TIME_WINDOW_MS;
-}
-
 static bool emergencyArmingCanOverrideArmingDisabled(void)
 {
     uint32_t armingPrevention = armingFlags & ARMING_DISABLED_ALL_FLAGS;
@@ -385,7 +365,7 @@ static bool emergencyArmingCanOverrideArmingDisabled(void)
 
 static bool emergencyArmingIsEnabled(void)
 {
-    return emergencyArmingIsTriggered() && emergencyArmingCanOverrideArmingDisabled();
+    return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM)) && emergencyArmingCanOverrideArmingDisabled();
 }
 
 static void processPilotAndFailSafeActions(float dT)
@@ -477,37 +457,36 @@ disarmReason_t getDisarmReason(void)
     return lastDisarmReason;
 }
 
-void emergencyArmingUpdate(bool armingSwitchIsOn)
+bool emergencyArmingUpdate(bool armingSwitchIsOn)
 {
-    if (armingSwitchIsOn == emergencyArming.armingSwitchWasOn) {
+    if (ARMING_FLAG(ARMED)) {
-        return;
+        return false;
     }
-    if (armingSwitchIsOn) {
+
-        timeMs_t now = millis();
+    static timeMs_t timeout = 0;
-        if (emergencyArming.queueCount == 0) {
+    static int8_t counter = 0;
-            emergencyArming.queueCount = 1;
+    static bool toggle;
-            emergencyArming.start = now;
+    timeMs_t currentTimeMs = millis();
-        } else {
+
-            while (emergencyArming.start < now - EMERGENCY_ARMING_TIME_WINDOW_MS || emergencyArmingIsTriggered()) {
+    if (timeout && currentTimeMs > timeout) {
-                if (emergencyArming.queueCount > 1) {
+        timeout += EMERGENCY_ARMING_COUNTER_STEP_MS;
-                    uint8_t delta = emergencyArming.queue[0];
+        counter -= counter ? 1 : 0;
-                    emergencyArming.start += delta * EMERGENCY_ARMING_COUNTER_STEP_MS;
+        if (!counter) {
-                    for (int ii = 0; ii < emergencyArming.queueCount - 2; ii++) {
+            timeout = 0;
-                        emergencyArming.queue[ii] = emergencyArming.queue[ii + 1] - delta;
-                    }
-                    emergencyArming.queueCount--;
-                } else {
-                    emergencyArming.start = now;
-                }
-            }
-            uint8_t delta = (now - emergencyArming.start) / EMERGENCY_ARMING_COUNTER_STEP_MS;
-            if (delta > 0) {
-                emergencyArming.queue[emergencyArming.queueCount - 1] = delta;
-                emergencyArming.queueCount++;
-            }
         }
     }
-    emergencyArming.armingSwitchWasOn = !emergencyArming.armingSwitchWasOn;
+
+    if (armingSwitchIsOn) {
+        if (!timeout && toggle) {
+            timeout = currentTimeMs + EMERGENCY_ARMING_TIME_WINDOW_MS;
+        }
+        counter += toggle;
+        toggle = false;
+    } else {
+        toggle = true;
+    }
+
+    return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
 }
 
 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
@@ -524,15 +503,14 @@ void tryArm(void)
 {
     updateArmingStatus();
 
+    if (ARMING_FLAG(ARMED)) {
+        return;
+    }
+
 #ifdef USE_DSHOT
-    if (
+    if (STATE(MULTIROTOR) && IS_RC_MODE_ACTIVE(BOXTURTLE) && !FLIGHT_MODE(TURTLE_MODE) &&
-            STATE(MULTIROTOR) &&
+        emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()
-            IS_RC_MODE_ACTIVE(BOXTURTLE) &&
+        ) {
-            emergencyArmingCanOverrideArmingDisabled() &&
-            isMotorProtocolDshot() &&
-            !ARMING_FLAG(ARMED) &&
-            !FLIGHT_MODE(TURTLE_MODE)
-            ) {
         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
         ENABLE_ARMING_FLAG(ARMED);
         enableFlightMode(TURTLE_MODE);
@@ -541,21 +519,10 @@ void tryArm(void)
 #endif
 
 #ifdef USE_PROGRAMMING_FRAMEWORK
-    if (
+    if (!isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
-        !isArmingDisabled() ||
-        emergencyArmingIsEnabled() ||
-        LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)
-    ) {
 #else
-    if (
+    if (!isArmingDisabled() || emergencyArmingIsEnabled()) {
-        !isArmingDisabled() ||
-        emergencyArmingIsEnabled()
-    ) {
 #endif
-        if (ARMING_FLAG(ARMED)) {
-            return;
-        }
-
         // If nav_extra_arming_safety was bypassed we always
         // allow bypassing it even without the sticks set
         // in the correct position to allow re-arming quickly
@@ -868,8 +835,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         armTime += cycleTime;
         updateAccExtremes();
     }
+
     if (!ARMING_FLAG(ARMED)) {
         armTime = 0;
+
+        processDelayedSave();
     }
 
     gyroFilter();
@@ -888,7 +858,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     if (isRXDataNew) {
         updateWaypointsAndNavigationMode();
     }
-
     isRXDataNew = false;
 
     updatePositionEstimator();
@@ -951,10 +920,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         writeMotors();
     }
 #endif
-
     // Check if landed, FW and MR
     if (STATE(ALTITUDE_CONTROL)) {
-        updateLandingStatus();
+        updateLandingStatus(US2MS(currentTimeUs));
     }
 
 #ifdef USE_BLACKBOX

src/main/fc/fc_core.h

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

src/main/fc/fc_msp.c

@@ -1051,10 +1051,31 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP_LED_STRIP_CONFIG:
         for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
             const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
-            sbufWriteU32(dst, *ledConfig);
+
+            uint32_t legacyLedConfig = ledConfig->led_position;
+            int shiftCount = 8;
+            legacyLedConfig |= ledConfig->led_function << shiftCount;
+            shiftCount += 4;
+            legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
+            shiftCount += 6; 
+            legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
+            shiftCount += 4;
+            legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
+            shiftCount += 6;
+            legacyLedConfig |= (ledConfig->led_params) << (shiftCount);
+
+            sbufWriteU32(dst, legacyLedConfig);
         }
         break;
 
+    case MSP2_INAV_LED_STRIP_CONFIG_EX:
+        for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
+            const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
+            sbufWriteDataSafe(dst, ledConfig, sizeof(ledConfig_t));
+        }
+        break;
+
+
     case MSP_LED_STRIP_MODECOLOR:
         for (int i = 0; i < LED_MODE_COUNT; i++) {
             for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
@@ -1398,7 +1419,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 
     case MSP_NAME:
         {
-            const char *name = systemConfig()->name;
+            const char *name = systemConfig()->craftName;
             while (*name) {
                 sbufWriteU8(dst, *name++);
             }
@@ -2680,13 +2701,35 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_LED_STRIP_CONFIG:
-        if (dataSize == 5) {
+        if (dataSize == (1 + sizeof(uint32_t))) {
             tmp_u8 = sbufReadU8(src);
-            if (tmp_u8 >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
+            if (tmp_u8 >= LED_MAX_STRIP_LENGTH) {
                 return MSP_RESULT_ERROR;
             }
             ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[tmp_u8];
-            *ledConfig = sbufReadU32(src);
+
+            uint32_t legacyConfig = sbufReadU32(src);
+
+            ledConfig->led_position = legacyConfig & 0xFF;
+            ledConfig->led_function = (legacyConfig >> 8) & 0xF;
+            ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
+            ledConfig->led_color = (legacyConfig >> 18) & 0xF; 
+            ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
+            ledConfig->led_params = (legacyConfig >> 28) & 0xF;
+
+            reevaluateLedConfig();
+        } else
+            return MSP_RESULT_ERROR;
+        break;
+
+    case MSP2_INAV_SET_LED_STRIP_CONFIG_EX:
+        if (dataSize == (1 + sizeof(ledConfig_t))) {
+            tmp_u8 = sbufReadU8(src);
+            if (tmp_u8 >= LED_MAX_STRIP_LENGTH) {
+                return MSP_RESULT_ERROR;
+            }
+            ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[tmp_u8];
+            sbufReadDataSafe(src, ledConfig, sizeof(ledConfig_t));
             reevaluateLedConfig();
         } else
             return MSP_RESULT_ERROR;
@@ -2746,7 +2789,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_NAME:
         if (dataSize <= MAX_NAME_LENGTH) {
-            char *name = systemConfigMutable()->name;
+            char *name = systemConfigMutable()->craftName;
             int len = MIN(MAX_NAME_LENGTH, (int)dataSize);
             sbufReadData(src, name, len);
             memset(&name[len], '\0', (MAX_NAME_LENGTH + 1) - len);

src/main/fc/rc_controls.c

@@ -210,7 +210,6 @@ void processRcStickPositions(bool isThrottleLow)
 
     // perform actions
     bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
-    emergencyArmingUpdate(armingSwitchIsActive);
 
     if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
         // Auto arm on throttle when using fixedwing and motorstop
@@ -224,6 +223,7 @@ void processRcStickPositions(bool isThrottleLow)
             rcDisarmTimeMs = currentTimeMs;
             tryArm();
         } else {
+            emergencyArmingUpdate(armingSwitchIsActive);
             // 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/settings.yaml

@@ -72,7 +72,7 @@ tables:
     values: ["BATTERY", "CELL"]
     enum: osd_stats_min_voltage_unit_e
   - name: osd_video_system
-    values: ["AUTO", "PAL", "NTSC", "HDZERO", "DJIWTF", "BF43COMPAT"]
+    values: ["AUTO", "PAL", "NTSC", "HDZERO", "DJIWTF", "AVATAR", "BF43COMPAT"]
     enum: videoSystem_e
   - name: osd_telemetry
     values: ["OFF", "ON","TEST"]
@@ -1398,22 +1398,22 @@ groups:
     type: imuConfig_t
     headers: ["flight/imu.h"]
     members:
-      - name: imu_dcm_kp
+      - name: ahrs_dcm_kp
         description: "Inertial Measurement Unit KP Gain for accelerometer measurements"
         default_value: 2000
         field: dcm_kp_acc
         max: UINT16_MAX
-      - name: imu_dcm_ki
+      - name: ahrs_dcm_ki
         description: "Inertial Measurement Unit KI Gain for accelerometer measurements"
         default_value: 50
         field: dcm_ki_acc
         max: UINT16_MAX
-      - name: imu_dcm_kp_mag
+      - name: ahrs_dcm_kp_mag
         description: "Inertial Measurement Unit KP Gain for compass measurements"
         default_value: 2000
         field: dcm_kp_mag
         max: UINT16_MAX
-      - name: imu_dcm_ki_mag
+      - name: ahrs_dcm_ki_mag
         description: "Inertial Measurement Unit KI Gain for compass measurements"
         default_value: 50
         field: dcm_ki_mag
@@ -1423,24 +1423,24 @@ groups:
         default_value: 25
         min: 0
         max: 180
-      - name: imu_acc_ignore_rate
+      - name: ahrs_acc_ignore_rate
         description: "Total gyro rotation rate threshold [deg/s] before scaling to consider accelerometer trustworthy"
         default_value: 15
         field: acc_ignore_rate
         min: 0
         max: 30
-      - name: imu_acc_ignore_slope
+      - name: ahrs_acc_ignore_slope
         description: "Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight)"
         default_value: 5
         field: acc_ignore_slope
         min: 0
         max: 10
-      - name: imu_gps_yaw_windcomp
+      - name: ahrs_gps_yaw_windcomp
         description: "Wind compensation in heading estimation from gps groundcourse(fixed wing only)"
         default_value: ON
         field: gps_yaw_windcomp
         type: bool
-      - name: imu_inertia_comp_method
+      - name: ahrs_inertia_comp_method
         description: "Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop"
         default_value: VELNED
         field: inertia_comp_method
@@ -2306,7 +2306,7 @@ groups:
     members:
       - name: nav_disarm_on_landing
         description: "If set to ON, INAV disarms the FC after landing"
-        default_value: OFF
+        default_value: ON
         field: general.flags.disarm_on_landing
         type: bool
       - name: nav_land_detect_sensitivity
@@ -2573,7 +2573,7 @@ groups:
         max: 45
       - name: nav_auto_disarm_delay
         description: "Delay before craft disarms when `nav_disarm_on_landing` is set (ms)"
-        default_value: 2000
+        default_value: 1000
         field: general.auto_disarm_delay
         min: 100
         max: 10000
@@ -3025,7 +3025,7 @@ groups:
         type: uint8_t
         default_value: "OFF"
       - name: osd_video_system
-        description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, `DJIWTF` and `BF43COMPAT`"
+        description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR' and `BF43COMPAT`"
         default_value: "AUTO"
         table: osd_video_system
         field: video_system
@@ -3036,6 +3036,13 @@ groups:
         field: row_shiftdown
         min: 0
         max: 1
+      - name: osd_msp_displayport_fullframe_interval
+        description: "Full Frame redraw interval for MSP DisplayPort [deciseconds]. This is how often a full frame update is sent to the DisplayPort, to cut down on OSD artifacting. The default value should be fine for most pilots. Though long range pilots may benefit from increasing the refresh time, especially near the edge of range. -1 = disabled (legacy mode) | 0 = every frame (not recommended) | default = 10 (1 second)"
+        default_value: 10
+        min: -1
+        max: 600
+        type: int16_t
+        field: msp_displayport_fullframe_interval
       - name: osd_units
         description: "IMPERIAL, METRIC, UK"
         default_value: "METRIC"
@@ -3579,6 +3586,12 @@ groups:
       - name: name
         description: "Craft name"
         default_value: ""
+        field: craftName
+        max: MAX_NAME_LENGTH
+      - name: pilot_name
+        description: "Pilot name"
+        default_value: ""
+        field: pilotName
         max: MAX_NAME_LENGTH
 
   - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG
@@ -3623,8 +3636,8 @@ groups:
       - name: tz_offset
         description: "Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs"
         default_value: 0
-        min: -1440
+        min: -720
-        max: 1440
+        max: 840
       - name: tz_automatic_dst
         description: "Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`."
         default_value: "OFF"

src/main/fc/stats.c

@@ -64,7 +64,7 @@ void statsOnDisarm(void)
                 flyingEnergy += energy;
             }
 #endif
-            writeEEPROM();
+            saveConfigAndNotify();
         }
     }
 }

src/main/flight/failsafe.c

@@ -347,6 +347,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
         }
     }
 
+    // Inhibit Failsafe if emergency landing triggered manually
+    if (posControl.flags.manualEmergLandActive) {
+        return FAILSAFE_PROCEDURE_NONE;
+    }
+
     // 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 &&

src/main/flight/imu.c

@@ -114,15 +114,15 @@ FASTRAM bool gpsHeadingInitialized;
 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
 
 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
-    .dcm_kp_acc = SETTING_IMU_DCM_KP_DEFAULT,                   // 0.20 * 10000
+    .dcm_kp_acc = SETTING_AHRS_DCM_KP_DEFAULT,                   // 0.20 * 10000
-    .dcm_ki_acc = SETTING_IMU_DCM_KI_DEFAULT,                   // 0.005 * 10000
+    .dcm_ki_acc = SETTING_AHRS_DCM_KI_DEFAULT,                   // 0.005 * 10000
-    .dcm_kp_mag = SETTING_IMU_DCM_KP_MAG_DEFAULT,               // 0.20 * 10000
+    .dcm_kp_mag = SETTING_AHRS_DCM_KP_MAG_DEFAULT,               // 0.20 * 10000
-    .dcm_ki_mag = SETTING_IMU_DCM_KI_MAG_DEFAULT,               // 0.005 * 10000
+    .dcm_ki_mag = SETTING_AHRS_DCM_KI_MAG_DEFAULT,               // 0.005 * 10000
     .small_angle = SETTING_SMALL_ANGLE_DEFAULT,
-    .acc_ignore_rate = SETTING_IMU_ACC_IGNORE_RATE_DEFAULT,
+    .acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT,
-    .acc_ignore_slope = SETTING_IMU_ACC_IGNORE_SLOPE_DEFAULT,
+    .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
-    .gps_yaw_windcomp = 1,
+    .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
-    .inertia_comp_method = COMPMETHOD_VELNED
+    .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT
 );
 
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)

src/main/flight/servos.c

@@ -556,7 +556,7 @@ void processContinuousServoAutotrim(const float dT)
         }
     } else if (trimState == AUTOTRIM_COLLECTING) {
         // We have disarmed, save midpoints to EEPROM
-        writeEEPROM();
+        saveConfigAndNotify();
         trimState = AUTOTRIM_IDLE;
     }
 

src/main/flight/wind_estimator.c

@@ -41,7 +41,8 @@
 #include "io/gps.h"
 
 
-#define WINDESTIMATOR_TIMEOUT       60 //60s
+#define WINDESTIMATOR_TIMEOUT       60*15 // 15min with out altitude change
+#define WINDESTIMATOR_ALTITUDE_SCALE WINDESTIMATOR_TIMEOUT/500.0f //or 500m altitude change
 // Based on WindEstimation.pdf paper
 
 static bool hasValidWindEstimate = false;
@@ -79,8 +80,10 @@ void updateWindEstimator(timeUs_t currentTimeUs)
 {
     static timeUs_t lastUpdateUs = 0;
     static timeUs_t lastValidWindEstimate = 0;
+    static float lastValidEstimateAltitude = 0.0f;
+    float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m
 
-    if (US2S(currentTimeUs - lastValidWindEstimate) > WINDESTIMATOR_TIMEOUT)
+    if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT)
     {
         hasValidWindEstimate = false;
     }
@@ -166,6 +169,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         float prevWindLength = calc_length_pythagorean_3D(estimatedWind[X], estimatedWind[Y], estimatedWind[Z]);
         float windLength = calc_length_pythagorean_3D(wind[X], wind[Y], wind[Z]);
 
+        //is this really needed? The reason it is here might be above equation was wrong in early implementations
         if (windLength < prevWindLength + 4000) {
             // TODO: Better filtering
             estimatedWind[X] = estimatedWind[X] * 0.98f + wind[X] * 0.02f;
@@ -176,6 +180,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         lastUpdateUs = currentTimeUs;
         lastValidWindEstimate = currentTimeUs;
         hasValidWindEstimate = true;
+        lastValidEstimateAltitude = currentAltitude;
     }
 }
 

src/main/io/bf_osd_symbols.h

@@ -28,7 +28,7 @@
 #define BF_SYM_HYPHEN                  0x2D
 #define BF_SYM_BBLOG                   0x10
 #define BF_SYM_HOMEFLAG                0x11
-//#define SYM_RPM                     0x12
+#define BF_SYM_RPM                     0x12
 #define BF_SYM_ROLL                    0x14
 #define BF_SYM_PITCH                   0x15
 #define BF_SYM_TEMPERATURE             0x7A

src/main/io/displayport_frsky_osd.c

@@ -631,6 +631,7 @@ displayPort_t *frskyOSDDisplayPortInit(const videoSystem_e videoSystem)
     if (frskyOSDInit(videoSystem)) {
         displayInit(&frskyOSDDisplayPort, &frskyOSDVTable);
         resync(&frskyOSDDisplayPort);
+        frskyOSDDisplayPort.displayPortType = "FrSky PixelOSD";
         return &frskyOSDDisplayPort;
     }
     return NULL;

src/main/io/displayport_max7456.c

@@ -193,6 +193,7 @@ displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem)
     max7456Init(videoSystem);
     displayInit(&max7456DisplayPort, &max7456VTable);
     resync(&max7456DisplayPort);
+    max7456DisplayPort.displayPortType = "MAX7456";
     return &max7456DisplayPort;
 }
 #endif // USE_MAX7456

src/main/io/displayport_msp.c

@@ -162,6 +162,8 @@ displayPort_t *displayPortMspInit(void)
 {
     displayInit(&mspDisplayPort, &mspDisplayPortVTable);
     resync(&mspDisplayPort);
+    mspDisplayPort.displayPortType = "MSP";
     return &mspDisplayPort;
 }
+
 #endif // USE_MSP_DISPLAYPORT

src/main/io/displayport_msp.h

@@ -21,10 +21,16 @@
 #include "drivers/display.h"
 
 // MSP Display Port commands
-#define MSP_DP_RELEASE      1
+typedef enum {
-#define MSP_DP_CLEAR_SCREEN 2
+    MSP_DP_HEARTBEAT = 0,       // Ensure display is not released, and prevent 'disconnected' status
-#define MSP_DP_WRITE_STRING 3
+    MSP_DP_RELEASE = 1,         // Release the display after clearing and updating
-#define MSP_DP_DRAW_SCREEN  4
+    MSP_DP_CLEAR_SCREEN = 2,    // Clear the display
+    MSP_DP_WRITE_STRING = 3,    // Write a string at given coordinates
+    MSP_DP_DRAW_SCREEN = 4,     // Trigger a screen draw
+    MSP_DP_OPTIONS = 5,         // Not used by Betaflight. Reserved by Ardupilot and INAV
+    MSP_DP_SYS = 6,             // Display system element displayportSystemElement_e at given coordinates
+    MSP_DP_COUNT,
+} displayportMspCommand_e;
 
 struct displayPort_s;
-struct displayPort_s *displayPortMspInit(void);
+struct displayPort_s *displayPortMspInit(void);

src/main/io/displayport_msp_bf_compat.c

@@ -228,22 +228,23 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
     case SYM_WIND_VERTICAL:
         return BF_SYM_WIND_VERTICAL;
 
-    case SYM_3D_KMH:
-        return BF_SYM_3D_KMH;
-
-    case SYM_3D_MPH:
-        return BF_SYM_3D_MPH;
-
     case SYM_3D_KT:
         return BF_SYM_3D_KT;
 
-    case SYM_RPM:
-        return BF_SYM_RPM;
 
     case SYM_AIR:
         return BF_SYM_AIR;
 */
 
+    case SYM_3D_KMH:
+        return BF_SYM_KPH;
+
+    case SYM_3D_MPH:
+        return BF_SYM_MPH;
+
+    case SYM_RPM:
+        return BF_SYM_RPM;
+
     case SYM_FTS:
         return BF_SYM_FTPS;
 /*
@@ -554,10 +555,15 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
     case (SYM_AH_H_START+8):
         return BF_SYM_AH_BAR9_8;
 
-/*
+    // BF does not have vertical artificial horizon. replace with middle horizontal one
     case SYM_AH_V_START:
-        return BF_SYM_AH_V_START;
+    case (SYM_AH_V_START+1):
-
+    case (SYM_AH_V_START+2):
+    case (SYM_AH_V_START+3):
+    case (SYM_AH_V_START+4):
+    case (SYM_AH_V_START+5):
+        return BF_SYM_AH_BAR9_4;
+/*
     case SYM_VARIO_UP_2A:
         return BF_SYM_VARIO_UP_2A;
 
@@ -681,7 +687,6 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
     return '?'; // Missing/not mapped character
 }
 
-
 #endif
 
-#endif
+#endif

src/main/io/displayport_msp_bf_compat.h

@@ -22,15 +22,11 @@
 
 #include "platform.h"
 
-#ifdef USE_MSP_DISPLAYPORT
+#if defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_BF_COMPAT)
-
-#ifndef DISABLE_MSP_BF_COMPAT
 #include "osd.h"
 uint8_t getBfCharacter(uint8_t ch, uint8_t page);
 #define isBfCompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT)
 #else
 #define getBfCharacter(x, page) (x)
 #define isBfCompatibleVideoSystem(osdConfigPtr) (false)
-#endif
-
 #endif

src/main/io/displayport_msp_osd.c

@@ -46,23 +46,16 @@ FILE_COMPILE_FOR_SPEED
 #include "fc/rc_modes.h"
 
 #include "io/osd.h"
+#include "io/displayport_msp.h"
 
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
 
 #include "displayport_msp_osd.h"
-
 #include "displayport_msp_bf_compat.h"
 
 #define FONT_VERSION 3
 
-#define MSP_HEARTBEAT    0
-#define MSP_RELEASE      1
-#define MSP_CLEAR_SCREEN 2
-#define MSP_WRITE_STRING 3
-#define MSP_DRAW_SCREEN  4
-#define MSP_SET_OPTIONS  5
-
 typedef enum {          // defines are from hdzero code
     SD_3016,
     HD_5018,
@@ -79,6 +72,7 @@ static mspPort_t mspPort;
 static displayPort_t mspOsdDisplayPort;
 static bool vtxSeen, vtxActive, vtxReset;
 static timeMs_t vtxHeartbeat;
+static timeMs_t sendSubFrameMs = 0;
 
 // PAL screen size
 #define PAL_COLS 30
@@ -90,7 +84,7 @@ static timeMs_t vtxHeartbeat;
 #define HDZERO_COLS 50
 #define HDZERO_ROWS 18
 // Avatar screen size
-#define AVATAR_COLS 54
+#define AVATAR_COLS 53
 #define AVATAR_ROWS 20
 // DJIWTF screen size
 #define DJI_COLS 60
@@ -106,8 +100,9 @@ static timeMs_t vtxHeartbeat;
 static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
 
 static uint8_t screen[SCREENSIZE];
-static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen
+static BITARRAY_DECLARE(fontPage, SCREENSIZE);  // font page for each character on the screen
-static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen
+static BITARRAY_DECLARE(dirty, SCREENSIZE);     // change status for each character on the screen
+static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink?
 static bool screenCleared;
 static uint8_t screenRows, screenCols;
 static videoSystem_e osdVideoSystem;
@@ -165,7 +160,7 @@ static int setDisplayMode(displayPort_t *displayPort)
         currentOsdMode = determineHDZeroOsdMode(); // Can change between layouts
     }
 
-    uint8_t subcmd[] = { MSP_SET_OPTIONS, 0, currentOsdMode }; // Font selection, mode (SD/HD)
+    uint8_t subcmd[] = { MSP_DP_OPTIONS, 0, currentOsdMode }; // Font selection, mode (SD/HD)
     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 }
 
@@ -174,15 +169,16 @@ static void init(void)
     memset(screen, SYM_BLANK, sizeof(screen));
     BITARRAY_CLR_ALL(fontPage);
     BITARRAY_CLR_ALL(dirty);
+    BITARRAY_CLR_ALL(blinkChar);
 }
 
 static int clearScreen(displayPort_t *displayPort)
 {
-    uint8_t subcmd[] = { MSP_CLEAR_SCREEN };
+    uint8_t subcmd[] = { MSP_DP_CLEAR_SCREEN };
 
     if (!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) { // OSD is off
         output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
-        subcmd[0] = MSP_DRAW_SCREEN;
+        subcmd[0] = MSP_DP_DRAW_SCREEN;
         vtxReset = true;
     }
     else {
@@ -215,14 +211,15 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1
     return true;
 }
 
-static int setChar(const uint16_t pos, const uint16_t c)
+static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr)
 {
     if (pos < SCREENSIZE) {
         uint8_t ch = c & 0xFF;
         bool page = (c >> 8);
-        if (screen[pos] != ch || bitArrayGet(fontPage, pos) !=  page) {
+        if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) {
             screen[pos] = ch;
             (page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos);
+            (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos);
             bitArraySet(dirty, pos);
         }
     }
@@ -232,19 +229,17 @@ static int setChar(const uint16_t pos, const uint16_t c)
 static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
 {
     UNUSED(displayPort);
-    UNUSED(attr);
 
-    return setChar((row * COLS) + col, c);
+    return setChar((row * COLS) + col, c, attr);
 }
 
 static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr)
 {
     UNUSED(displayPort);
-    UNUSED(attr);
 
     uint16_t pos = (row * COLS) + col;
     while (*string) {
-        setChar(pos++, *string++);
+        setChar(pos++, *string++, attr);
     }
     return 0;
 }
@@ -256,14 +251,29 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
 {
     static uint8_t counter = 0;
 
-    if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) {
+    if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) { // 62.5Hz
         return 0;
     }
 
+    if (osdConfig()->msp_displayport_fullframe_interval >= 0 && (millis() > sendSubFrameMs)) {
+        // For full frame update, first clear the OSD completely
+        uint8_t refreshSubcmd[1];
+        refreshSubcmd[0] = MSP_DP_CLEAR_SCREEN;
+        output(displayPort, MSP_DISPLAYPORT, refreshSubcmd, sizeof(refreshSubcmd));
+        
+        // Then dirty the characters that are not blank, to send all data on this draw.
+        for (unsigned int pos = 0; pos < sizeof(screen); pos++) {
+            if (screen[pos] != SYM_BLANK) {
+                bitArraySet(dirty, pos);
+            }
+        }
+            
+        sendSubFrameMs = (osdConfig()->msp_displayport_fullframe_interval > 0) ? (millis() + DS2MS(osdConfig()->msp_displayport_fullframe_interval)) : 0;
+    }
 
     uint8_t subcmd[COLS + 4];
     uint8_t updateCount = 0;
-    subcmd[0] = MSP_WRITE_STRING;
+    subcmd[0] = MSP_DP_WRITE_STRING;
 
     int next = BITARRAY_FIND_FIRST_SET(dirty, 0);
     while (next >= 0) {
@@ -271,8 +281,10 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
         int pos = next;
         uint8_t row = pos / COLS;
         uint8_t col = pos % COLS;
+        uint8_t attributes = 0;
         int endOfLine = row * COLS + screenCols;
         bool page = bitArrayGet(fontPage, pos);
+        bool blink = bitArrayGet(blinkChar, pos);
 
         uint8_t len = 4;
         do {
@@ -282,11 +294,20 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
             if (bitArrayGet(dirty, pos)) {
                 next = pos;
             }
-        } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page);
+        } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page && bitArrayGet(blinkChar, next) == blink);
+
+        if (osdVideoSystem != VIDEO_SYSTEM_BFCOMPAT) {
+            attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
+            //attributes = page;
+        }
+
+        if (blink) {
+            attributes |= (1 << DISPLAYPORT_MSP_ATTR_BLINK);
+        }
 
         subcmd[1] = row;
         subcmd[2] = col;
-        subcmd[3] = (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) ? 0 : page;
+        subcmd[3] = attributes;
         output(displayPort, MSP_DISPLAYPORT, subcmd, len);
         updateCount++;
         next = BITARRAY_FIND_FIRST_SET(dirty, pos);
@@ -297,7 +318,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
             screenCleared = false;
         }
 
-        subcmd[0] = MSP_DRAW_SCREEN;
+        subcmd[0] = MSP_DP_DRAW_SCREEN;
         output(displayPort, MSP_DISPLAYPORT, subcmd, 1);
     }
 
@@ -354,7 +375,11 @@ static bool isReady(displayPort_t *displayPort)
 
 static int heartbeat(displayPort_t *displayPort)
 {
-    uint8_t subcmd[] = { MSP_HEARTBEAT };
+    uint8_t subcmd[] = { MSP_DP_HEARTBEAT };
+
+    // heartbeat is used to:
+    // a) ensure display is not released by MW OSD software
+    // b) prevent OSD Slave boards from displaying a 'disconnected' status.
     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 }
 
@@ -365,7 +390,7 @@ static int grab(displayPort_t *displayPort)
 
 static int release(displayPort_t *displayPort)
 {
-    uint8_t subcmd[] = { MSP_RELEASE };
+    uint8_t subcmd[] = { MSP_DP_RELEASE };
     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 }
 
@@ -439,6 +464,11 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
             screenRows = DJI_ROWS;
             screenCols = DJI_COLS;
             break;
+        case VIDEO_SYSTEM_AVATAR:
+            currentOsdMode = HD_5018;
+            screenRows = AVATAR_ROWS;
+            screenCols = AVATAR_COLS;
+            break;
         default:
             break;
         }
@@ -447,6 +477,12 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
         init();
         displayInit(&mspOsdDisplayPort, &mspOsdVTable);
 
+        if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) {
+            mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode";
+        } else {
+            mspOsdDisplayPort.displayPortType = "MSP DisplayPort";
+        }
+
         return &mspOsdDisplayPort;
     }
     return NULL;

src/main/io/displayport_msp_osd.h

@@ -31,3 +31,8 @@ typedef struct displayPort_s displayPort_t;
 
 displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem);
 void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
+
+// MSP displayport V2 attribute byte bit functions
+#define DISPLAYPORT_MSP_ATTR_FONTPAGE   0 // Select bank of 256 characters as per displayPortSeverity_e
+#define DISPLAYPORT_MSP_ATTR_BLINK      6 // Device local blink
+#define DISPLAYPORT_MSP_ATTR_VERSION    7 // Format indicator; must be zero for V2 (and V1)

src/main/io/ledstrip.c

@@ -138,6 +138,7 @@ static const specialColorIndexes_t defaultSpecialColors[] = {
        [LED_SCOLOR_GPSNOSATS]       = COLOR_RED,
        [LED_SCOLOR_GPSNOLOCK]       = COLOR_ORANGE,
        [LED_SCOLOR_GPSLOCKED]       = COLOR_GREEN,
+       [LED_SCOLOR_STROBE]          = COLOR_WHITE,
     }}
 };
 
@@ -185,10 +186,11 @@ STATIC_UNIT_TESTED void updateLedCount(void)
 {
     int count = 0, countRing = 0, countScanner= 0;
 
+    const ledConfig_t configNotSet = {};
     for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) {
         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 
-        if (!(*ledConfig))
+        if (!memcmp(ledConfig, &configNotSet, sizeof(ledConfig_t)))
             break;
 
         count++;
@@ -221,7 +223,7 @@ static const hsvColor_t* getSC(ledSpecialColorIds_e index)
 
 static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' };
 static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT]   = { 'C', 'F', 'A', 'L', 'S', 'G', 'R', 'H' };
-static const char overlayCodes[LED_OVERLAY_COUNT]   = { 'T', 'O', 'B', 'N', 'I', 'W' };
+static const char overlayCodes[LED_OVERLAY_COUNT]   = { 'T', 'O', 'B', 'N', 'I', 'W', 'E' };
 
 #define CHUNK_BUFFER_SIZE 11
 
@@ -305,7 +307,7 @@ bool parseLedStripConfig(int ledIndex, const char *config)
         }
     }
 
-    *ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags, 0);
+    DEFINE_LED(ledConfig, x, y, color, direction_flags, baseFunction, overlay_flags, 0);
 
     reevaluateLedConfig();
 
@@ -502,7 +504,7 @@ static void applyLedHsv(uint32_t mask, uint32_t ledOperation, const hsvColor_t *
 {
     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
-        if ((*ledConfig & mask) == ledOperation)
+        if ((*((uint32_t *)ledConfig) & mask) == ledOperation)
             setLedHsv(ledIndex, color);
     }
 }
@@ -842,10 +844,14 @@ static void applyLedBlinkLayer(bool updateNow, timeUs_t *timer)
     }
 
     bool ledOn = (blinkMask & 1);  // b_b_____...
-    if (!ledOn) {
+    for (int i = 0; i < ledCounts.count; ++i) {
-        for (int i = 0; i < ledCounts.count; ++i) {
+        const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
-            const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
 
+        if (ledOn) {
+            if (ledGetOverlayBit(ledConfig, LED_OVERLAY_STROBE)) {
+                setLedHsv(i, getSC(LED_SCOLOR_STROBE));
+            }
+        } else {
             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) ||
                     (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 55 && scaledThrottle > 10)) {
                 setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND));

src/main/io/ledstrip.h

@@ -25,41 +25,26 @@
 #define LED_MODE_COUNT                  6
 #define LED_DIRECTION_COUNT             6
 #define LED_BASEFUNCTION_COUNT          8
-#define LED_OVERLAY_COUNT               6
+#define LED_OVERLAY_COUNT               7
-#define LED_SPECIAL_COLOR_COUNT        11
+#define LED_SPECIAL_COLOR_COUNT         9
 
-#define LED_POS_OFFSET                  0
 #define LED_FUNCTION_OFFSET             8
-#define LED_OVERLAY_OFFSET             12
+#define LED_OVERLAY_OFFSET             16
-#define LED_COLOR_OFFSET               18
-#define LED_DIRECTION_OFFSET           22
-#define LED_PARAMS_OFFSET              28
 
 #define LED_POS_BITCNT                  8
-#define LED_FUNCTION_BITCNT             4
+#define LED_FUNCTION_BITCNT             8
-#define LED_OVERLAY_BITCNT              6
+#define LED_OVERLAY_BITCNT              8
 #define LED_COLOR_BITCNT                4
 #define LED_DIRECTION_BITCNT            6
-#define LED_PARAMS_BITCNT               4
+#define LED_PARAMS_BITCNT               6
 
 #define LED_FLAG_OVERLAY_MASK ((1 << LED_OVERLAY_BITCNT) - 1)
-#define LED_FLAG_DIRECTION_MASK ((1 << LED_DIRECTION_BITCNT) - 1)
 
-#define LED_MOV_POS(pos) ((pos) << LED_POS_OFFSET)
 #define LED_MOV_FUNCTION(func) ((func) << LED_FUNCTION_OFFSET)
 #define LED_MOV_OVERLAY(overlay) ((overlay) << LED_OVERLAY_OFFSET)
-#define LED_MOV_COLOR(colorId) ((colorId) << LED_COLOR_OFFSET)
-#define LED_MOV_DIRECTION(direction) ((direction) << LED_DIRECTION_OFFSET)
-#define LED_MOV_PARAMS(param) ((param) << LED_PARAMS_OFFSET)
 
-#define LED_BIT_MASK(len) ((1 << (len)) - 1)
-
-#define LED_POS_MASK LED_MOV_POS(((1 << LED_POS_BITCNT) - 1))
 #define LED_FUNCTION_MASK LED_MOV_FUNCTION(((1 << LED_FUNCTION_BITCNT) - 1))
 #define LED_OVERLAY_MASK LED_MOV_OVERLAY(LED_FLAG_OVERLAY_MASK)
-#define LED_COLOR_MASK LED_MOV_COLOR(((1 << LED_COLOR_BITCNT) - 1))
-#define LED_DIRECTION_MASK LED_MOV_DIRECTION(LED_FLAG_DIRECTION_MASK)
-#define LED_PARAMS_MASK LED_MOV_PARAMS(((1 << LED_PARAMS_BITCNT) - 1))
 
 #define LED_FLAG_OVERLAY(id) (1 << (id))
 #define LED_FLAG_DIRECTION(id) (1 << (id))
@@ -104,7 +89,8 @@ typedef enum {
     LED_SCOLOR_BLINKBACKGROUND,
     LED_SCOLOR_GPSNOSATS,
     LED_SCOLOR_GPSNOLOCK,
-    LED_SCOLOR_GPSLOCKED
+    LED_SCOLOR_GPSLOCKED,
+    LED_SCOLOR_STROBE
 } ledSpecialColorIds_e;
 
 typedef enum {
@@ -134,6 +120,7 @@ typedef enum {
     LED_OVERLAY_LANDING_FLASH,
     LED_OVERLAY_INDICATOR,
     LED_OVERLAY_WARNING,
+    LED_OVERLAY_STROBE
 } ledOverlayId_e;
 
 typedef struct modeColorIndexes_s {
@@ -144,7 +131,14 @@ typedef struct specialColorIndexes_s {
     uint8_t color[LED_SPECIAL_COLOR_COUNT];
 } specialColorIndexes_t;
 
-typedef uint32_t ledConfig_t;
+typedef struct ledConfig_s {
+    uint16_t led_position  : LED_POS_BITCNT;
+    uint16_t led_function  : LED_FUNCTION_BITCNT;
+    uint16_t led_overlay   : LED_OVERLAY_BITCNT;
+    uint16_t led_color     : LED_COLOR_BITCNT;
+    uint16_t led_direction : LED_DIRECTION_BITCNT;
+    uint16_t led_params    : LED_PARAMS_BITCNT;
+} __attribute__((packed)) ledConfig_t;
 
 typedef struct ledCounts_s {
     uint8_t count;
@@ -163,16 +157,22 @@ typedef struct ledStripConfig_s {
 
 PG_DECLARE(ledStripConfig_t, ledStripConfig);
 
-#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params))
+#define DEFINE_LED(ledConfigPtr, x, y, col, dir, func, ol, params) { \
+  ledConfigPtr->led_position = CALCULATE_LED_XY(x, y); \
+  ledConfigPtr->led_color = (col); \
+  ledConfigPtr->led_direction = (dir); \
+  ledConfigPtr->led_function = (func); \
+  ledConfigPtr->led_overlay = (ol); \
+  ledConfigPtr->led_params = (params); }
 
-static inline uint8_t ledGetXY(const ledConfig_t *lcfg)         { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); }
+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_POS_OFFSET + LED_X_BIT_OFFSET)) & LED_XY_MASK); }
+static inline uint8_t ledGetX(const ledConfig_t *lcfg)          { return ((lcfg->led_position >> (LED_X_BIT_OFFSET)) & LED_XY_MASK); }
-static inline uint8_t ledGetY(const ledConfig_t *lcfg)          { return ((*lcfg >> (LED_POS_OFFSET + LED_Y_BIT_OFFSET)) & LED_XY_MASK); }
+static inline uint8_t ledGetY(const ledConfig_t *lcfg)          { return ((lcfg->led_position >> (LED_Y_BIT_OFFSET)) & LED_XY_MASK); }
-static inline uint8_t ledGetFunction(const ledConfig_t *lcfg)   { return ((*lcfg >> LED_FUNCTION_OFFSET) & LED_BIT_MASK(LED_FUNCTION_BITCNT)); }
+static inline uint8_t ledGetFunction(const ledConfig_t *lcfg)   { return (lcfg->led_function); }
-static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg)    { return ((*lcfg >> LED_OVERLAY_OFFSET) & LED_BIT_MASK(LED_OVERLAY_BITCNT)); }
+static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg)    { return (lcfg->led_overlay); }
-static inline uint8_t ledGetColor(const ledConfig_t *lcfg)      { return ((*lcfg >> LED_COLOR_OFFSET) & LED_BIT_MASK(LED_COLOR_BITCNT)); }
+static inline uint8_t ledGetColor(const ledConfig_t *lcfg)      { return (lcfg->led_color); }
-static inline uint8_t ledGetDirection(const ledConfig_t *lcfg)  { return ((*lcfg >> LED_DIRECTION_OFFSET) & LED_BIT_MASK(LED_DIRECTION_BITCNT)); }
+static inline uint8_t ledGetDirection(const ledConfig_t *lcfg)  { return (lcfg->led_direction); }
-static inline uint8_t ledGetParams(const ledConfig_t *lcfg)     { return ((*lcfg >> LED_PARAMS_OFFSET) & LED_BIT_MASK(LED_PARAMS_BITCNT)); }
+static inline uint8_t ledGetParams(const ledConfig_t *lcfg)     { return (lcfg->led_params); }
 
 static inline bool ledGetOverlayBit(const ledConfig_t *lcfg, int id) { return ((ledGetOverlay(lcfg) >> id) & 1); }
 static inline bool ledGetDirectionBit(const ledConfig_t *lcfg, int id) { return ((ledGetDirection(lcfg) >> id) & 1); }

src/main/io/osd.c

@@ -149,6 +149,9 @@ FILE_COMPILE_FOR_SPEED
 
 #define OSD_MIN_FONT_VERSION 3
 
+static timeMs_t notify_settings_saved = 0;
+static bool     savingSettings = false;
+
 static unsigned currentLayout = 0;
 static int layoutOverride = -1;
 static bool hasExtendedFont = false; // Wether the font supports characters > 256
@@ -201,6 +204,15 @@ static bool osdDisplayHasCanvas;
 PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 7);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
 
+void osdStartedSaveProcess() {
+    savingSettings = true;
+}
+
+void osdShowEEPROMSavedNotification() {
+    savingSettings = false;
+    notify_settings_saved = millis() + 5000;
+}
+
 static int digitCount(int32_t value)
 {
     int digits = 1;
@@ -520,10 +532,10 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
             suffix = SYM_KMH;
             break;
     }
-    if (isValid) {
+    osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3);
-        osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3);
+    if (!isValid)
-    } else {
+    {
-        buff[0] = buff[1] = buff[2] = '-';
+        suffix = '*';
     }
     buff[3] = suffix;
     buff[4] = '\0';
@@ -751,12 +763,25 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val)
 
 static void osdFormatCraftName(char *buff)
 {
-    if (strlen(systemConfig()->name) == 0)
+    if (strlen(systemConfig()->craftName) == 0)
             strcpy(buff, "CRAFT_NAME");
     else {
         for (int i = 0; i < MAX_NAME_LENGTH; i++) {
-            buff[i] = sl_toupper((unsigned char)systemConfig()->name[i]);
+            buff[i] = sl_toupper((unsigned char)systemConfig()->craftName[i]);
-            if (systemConfig()->name[i] == 0)
+            if (systemConfig()->craftName[i] == 0)
+                break;
+        }
+    }
+}
+
+void osdFormatPilotName(char *buff)
+{
+    if (strlen(systemConfig()->pilotName) == 0)
+            strcpy(buff, "PILOT_NAME");
+    else {
+        for (int i = 0; i < MAX_NAME_LENGTH; i++) {
+            buff[i] = sl_toupper((unsigned char)systemConfig()->pilotName[i]);
+            if (systemConfig()->pilotName[i] == 0)
                 break;
         }
     }
@@ -1614,14 +1639,22 @@ static bool osdDrawSingleElement(uint8_t item)
         break;
 
     case OSD_MAH_DRAWN: {
-        if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (osdConfig()->mAh_used_precision - 2), osdConfig()->mAh_used_precision)) {
+
-           // Shown in mAh
+        if (isBfCompatibleVideoSystem(osdConfig())) {
-           buff[osdConfig()->mAh_used_precision] = SYM_AH;
+            //BFcompat is unable to work with scaled values and it only has mAh symbol to work with
+            tfp_sprintf(buff, "%4d", (int)getMAhDrawn());
+            buff[4] = SYM_MAH;
+            buff[5] = '\0';
         } else {
-          // Shown in Ah
+            if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (osdConfig()->mAh_used_precision - 2), osdConfig()->mAh_used_precision)) {
-            buff[osdConfig()->mAh_used_precision] = SYM_MAH;
+            // Shown in mAh
+            buff[osdConfig()->mAh_used_precision] = SYM_AH;
+            } else {
+            // Shown in Ah
+                buff[osdConfig()->mAh_used_precision] = SYM_MAH;
+            }
+            buff[(osdConfig()->mAh_used_precision + 1)] = '\0';
         }
-        buff[(osdConfig()->mAh_used_precision + 1)] = '\0';
         osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
         break;
     }
@@ -1738,7 +1771,8 @@ static bool osdDrawSingleElement(uint8_t item)
                     if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
                         panHomeDirOffset = osdPanServoHomeDirectionOffset();
                     }
-                    int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset;
+                    int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
+                    int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset;
                     osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
                 }
             } else {
@@ -1758,7 +1792,7 @@ static bool osdDrawSingleElement(uint8_t item)
             buff[1] = SYM_HEADING;
 
             if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) {
-                int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading()))));
+                int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - (STATE(AIRPLANE) ? posControl.actualState.cog : DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading())))));
                 tfp_sprintf(buff + 2, "%4d", h);
             } else {
                 strcpy(buff + 2, "----");
@@ -1785,23 +1819,6 @@ static bool osdDrawSingleElement(uint8_t item)
         osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0);
         break;
 
-    case OSD_HEADING:
-        {
-            buff[0] = SYM_HEADING;
-            if (osdIsHeadingValid()) {
-                int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading());
-                if (h < 0) {
-                    h += 360;
-                }
-                tfp_sprintf(&buff[1], "%3d", h);
-            } else {
-                buff[1] = buff[2] = buff[3] = '-';
-            }
-            buff[4] = SYM_DEGREES;
-            buff[5] = '\0';
-            break;
-        }
-
     case OSD_GROUND_COURSE:
         {
             buff[0] = SYM_GROUND_COURSE;
@@ -2084,6 +2101,10 @@ static bool osdDrawSingleElement(uint8_t item)
         osdFormatCraftName(buff);
         break;
 
+    case OSD_PILOT_NAME:
+        osdFormatPilotName(buff);
+        break;
+
     case OSD_THROTTLE_POS:
     {
         osdFormatThrottlePosition(buff, false, &elemAttr);
@@ -2724,6 +2745,23 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
+    case OSD_HEADING:
+        {
+            buff[0] = SYM_HEADING;
+            if (osdIsHeadingValid()) {
+                int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading());
+                if (h < 0) {
+                    h += 360;
+                }
+                tfp_sprintf(&buff[1], "%3d", h);
+            } else {
+                buff[1] = buff[2] = buff[3] = '-';
+            }
+            buff[4] = SYM_DEGREES;
+            buff[5] = '\0';
+            break;
+        }
+
     case OSD_HEADING_GRAPH:
         {
             if (osdIsHeadingValid()) {
@@ -2932,16 +2970,11 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             bool valid = isEstimatedWindSpeedValid();
             float horizontalWindSpeed;
-            if (valid) {
+            uint16_t angle;
-                uint16_t angle;
+            horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
-                horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
+            int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
-                int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
-                buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
-            } else {
-                horizontalWindSpeed = 0;
-                buff[1] = SYM_BLANK;
-            }
             buff[0] = SYM_WIND_HORIZONTAL;
+            buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
             osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid);
             break;
         }
@@ -2956,16 +2989,12 @@ static bool osdDrawSingleElement(uint8_t item)
             buff[1] = SYM_BLANK;
             bool valid = isEstimatedWindSpeedValid();
             float verticalWindSpeed;
-            if (valid) {
+            verticalWindSpeed = -getEstimatedWindSpeed(Z);  //from NED to NEU
-                verticalWindSpeed = -getEstimatedWindSpeed(Z);  //from NED to NEU
+            if (verticalWindSpeed < 0) {
-                if (verticalWindSpeed < 0) {
+                buff[1] = SYM_AH_DECORATION_DOWN;
-                    buff[1] = SYM_AH_DECORATION_DOWN;
+                verticalWindSpeed = -verticalWindSpeed;
-                    verticalWindSpeed = -verticalWindSpeed;
-                } else if (verticalWindSpeed > 0) {
-                    buff[1] = SYM_AH_DECORATION_UP;
-                }
             } else {
-                verticalWindSpeed = 0;
+                buff[1] = SYM_AH_DECORATION_UP;
             }
             osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid);
             break;
@@ -3270,73 +3299,102 @@ uint8_t osdIncElementIndex(uint8_t elementIndex)
 {
     ++elementIndex;
 
-    if (elementIndex == OSD_ARTIFICIAL_HORIZON)
+    if (elementIndex == OSD_ARTIFICIAL_HORIZON) {   // always drawn last so skip
-        ++elementIndex;
+        elementIndex++;
-
-#ifndef USE_TEMPERATURE_SENSOR
-    if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE)
-        elementIndex = OSD_ALTITUDE_MSL;
-#endif
-
-    if (!sensors(SENSOR_ACC)) {
-        if (elementIndex == OSD_CROSSHAIRS) {
-            elementIndex = OSD_ONTIME;
-        }
     }
 
-    if (!feature(FEATURE_VBAT)) {
+#ifndef USE_TEMPERATURE_SENSOR
+    if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE) {
+        elementIndex = OSD_ALTITUDE_MSL;
+    }
+#endif
+
+    if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))) {
+        if (elementIndex == OSD_POWER) {
+            elementIndex = OSD_GPS_LON;
+        }
         if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
             elementIndex = OSD_LEVEL_PIDS;
         }
+#ifdef USE_POWER_LIMITS
+        if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) {
+            elementIndex = OSD_GLIDESLOPE;
+        }
+#endif
     }
 
+#ifndef USE_POWER_LIMITS
+    if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) {
+        elementIndex = OSD_GLIDESLOPE;
+    }
+#endif
+
     if (!feature(FEATURE_CURRENT_METER)) {
         if (elementIndex == OSD_CURRENT_DRAW) {
             elementIndex = OSD_GPS_SPEED;
         }
         if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) {
+            elementIndex = OSD_BATTERY_REMAINING_PERCENT;
+        }
+        if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) {
             elementIndex = OSD_TRIP_DIST;
         }
         if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) {
             elementIndex = OSD_HOME_HEADING_ERROR;
         }
-        if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
+        if (elementIndex == OSD_CLIMB_EFFICIENCY) {
-            elementIndex = OSD_LEVEL_PIDS;
+            elementIndex = OSD_NAV_WP_MULTI_MISSION_INDEX;
-        }
-    }
-
-    if (!feature(FEATURE_GPS)) {
-        if (elementIndex == OSD_GPS_SPEED) {
-            elementIndex = OSD_ALTITUDE;
-        }
-        if (elementIndex == OSD_GPS_LON) {
-            elementIndex = OSD_VARIO;
-        }
-        if (elementIndex == OSD_GPS_HDOP) {
-            elementIndex = OSD_MAIN_BATT_CELL_VOLTAGE;
-        }
-        if (elementIndex == OSD_TRIP_DIST) {
-            elementIndex = OSD_ATTITUDE_PITCH;
-        }
-        if (elementIndex == OSD_WIND_SPEED_HORIZONTAL) {
-            elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE;
-        }
-        if (elementIndex == OSD_3D_SPEED) {
-            elementIndex++;
         }
     }
 
     if (!STATE(ESC_SENSOR_ENABLED)) {
         if (elementIndex == OSD_ESC_RPM) {
-            elementIndex++;
+            elementIndex = OSD_AZIMUTH;
         }
     }
 
-#ifndef USE_POWER_LIMITS
+    if (!feature(FEATURE_GPS)) {
-    if (elementIndex == OSD_NAV_FW_CONTROL_SMOOTHNESS) {
+        if (elementIndex == OSD_GPS_HDOP || elementIndex == OSD_TRIP_DIST || elementIndex == OSD_3D_SPEED || elementIndex == OSD_MISSION ||
-        elementIndex = OSD_ITEM_COUNT;
+            elementIndex == OSD_AZIMUTH || elementIndex == OSD_BATTERY_REMAINING_CAPACITY || elementIndex == OSD_EFFICIENCY_MAH_PER_KM) {
+            elementIndex++;
+        }
+        if (elementIndex == OSD_HEADING_GRAPH && !sensors(SENSOR_MAG)) {
+            elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_WH_DRAWN : OSD_BATTERY_REMAINING_PERCENT;
+        }
+        if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) {
+            elementIndex = OSD_ATTITUDE_PITCH;
+        }
+        if (elementIndex == OSD_GPS_SPEED) {
+            elementIndex = OSD_ALTITUDE;
+        }
+        if (elementIndex == OSD_GPS_LON) {
+            elementIndex = sensors(SENSOR_MAG) ? OSD_HEADING : OSD_VARIO;
+        }
+        if (elementIndex == OSD_MAP_NORTH) {
+            elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE : OSD_LEVEL_PIDS;
+        }
+        if (elementIndex == OSD_PLUS_CODE) {
+            elementIndex = OSD_GFORCE;
+        }
+        if (elementIndex == OSD_GLIDESLOPE) {
+            elementIndex = OSD_AIR_MAX_SPEED;
+        }
+        if (elementIndex == OSD_GLIDE_RANGE) {
+            elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_ITEM_COUNT;
+        }
+        if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) {
+            elementIndex = OSD_ITEM_COUNT;
+        }
+    }
+
+    if (!sensors(SENSOR_ACC)) {
+        if (elementIndex == OSD_CROSSHAIRS) {
+            elementIndex = OSD_ONTIME;
+        }
+        if (elementIndex == OSD_GFORCE) {
+            elementIndex = OSD_RC_SOURCE;
+        }
     }
-#endif
 
     if (elementIndex == OSD_ITEM_COUNT) {
         elementIndex = 0;
@@ -3347,18 +3405,18 @@ uint8_t osdIncElementIndex(uint8_t elementIndex)
 void osdDrawNextElement(void)
 {
     static uint8_t elementIndex = 0;
-    // Prevent infinite loop when no elements are enabled
+    // Flag for end of loop, also prevents infinite loop when no elements are enabled
     uint8_t index = elementIndex;
     do {
         elementIndex = osdIncElementIndex(elementIndex);
-    } while(!osdDrawSingleElement(elementIndex) && index != elementIndex);
+    } while (!osdDrawSingleElement(elementIndex) && index != elementIndex);
 
     // Draw artificial horizon + tracking telemtry last
-		osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
+    osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
-		if (osdConfig()->telemetry>0){
+    if (osdConfig()->telemetry>0){
-		  osdDisplayTelemetry();
+        osdDisplayTelemetry();
-		}
     }
+}
 
 PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT,
@@ -3396,6 +3454,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
 
     .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT,
     .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT,
+    .msp_displayport_fullframe_interval = SETTING_OSD_MSP_DISPLAYPORT_FULLFRAME_INTERVAL_DEFAULT,
 
     .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT,
     .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT,
@@ -3511,6 +3570,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6);
 
     osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
+    osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3);
     osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
 
 #ifdef USE_SERIALRX_CRSF
@@ -3930,6 +3990,17 @@ static void osdShowStatsPage1(void)
 
     displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
     displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
+
+    if (savingSettings == true) {
+        displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS));
+    } else if (notify_settings_saved > 0) {
+        if (millis() > notify_settings_saved) {
+            notify_settings_saved = 0;
+        } else {
+            displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED));
+        }
+    }
+
     displayCommitTransaction(osdDisplayPort);
 }
 
@@ -4071,6 +4142,17 @@ static void osdShowStatsPage2(void)
     displayWrite(osdDisplayPort, statValuesX - 1, top, buff);
     osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3);
     displayWrite(osdDisplayPort, statValuesX + 4, top++, buff);
+
+    if (savingSettings == true) {
+        displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS));
+    } else if (notify_settings_saved > 0) {
+        if (millis() > notify_settings_saved) {
+            notify_settings_saved = 0;
+        } else {
+            displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED));
+        }
+    }
+
     displayCommitTransaction(osdDisplayPort);
 }
 
@@ -4091,9 +4173,9 @@ static void osdShowArmed(void)
     displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
     y += 2;
 
-    if (strlen(systemConfig()->name) > 0) {
+    if (strlen(systemConfig()->craftName) > 0) {
         osdFormatCraftName(craftNameBuf);
-        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig() -> name)) / 2, y, craftNameBuf );
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, y, craftNameBuf );
         y += 1;
     }
     if (posControl.waypointListValid && posControl.waypointCount > 0) {
@@ -4557,6 +4639,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
         /* Messages that are shown regardless of Arming state */
 
+        if (savingSettings == true) {
+           messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS);
+        } else if (notify_settings_saved > 0) {
+            if (millis() > notify_settings_saved) {
+                notify_settings_saved = 0;
+            } else {
+                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED);
+            }
+        }
+
 #ifdef USE_DEV_TOOLS
         if (systemConfig()->groundTestMode) {
             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE);

src/main/io/osd.h

@@ -115,6 +115,8 @@
 #define OSD_MSG_HEADFREE            "(HEADFREE)"
 #define OSD_MSG_NAV_SOARING         "(SOARING)"
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"
+#define OSD_MSG_SAVING_SETTNGS      "** SAVING SETTINGS **"
+#define OSD_MSG_SETTINGS_SAVED      "** SETTINGS SAVED **"
 
 #ifdef USE_DEV_TOOLS
 #define OSD_MSG_GRD_TEST_MODE       "GRD TEST > MOTORS DISABLED"
@@ -268,6 +270,7 @@ typedef enum {
     OSD_NAV_WP_MULTI_MISSION_INDEX,
     OSD_GROUND_COURSE,      // 140
     OSD_CROSS_TRACK_ERROR,
+    OSD_PILOT_NAME,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -371,6 +374,7 @@ typedef struct osdConfig_s {
 
     videoSystem_e video_system;
     uint8_t row_shiftdown;
+    int16_t msp_displayport_fullframe_interval;
 
     // Preferences
     uint8_t main_voltage_decimals;
@@ -468,6 +472,9 @@ displayCanvas_t *osdGetDisplayPortCanvas(void);
 int16_t osdGetHeading(void);
 int32_t osdGetAltitude(void);
 
+void osdStartedSaveProcess(void);
+void osdShowEEPROMSavedNotification(void);
+
 void osdCrosshairPosition(uint8_t *x, uint8_t *y);
 bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
 void osdFormatAltitudeSymbol(char *buff, int32_t alt);

src/main/io/osd_dji_hd.c

@@ -1192,7 +1192,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
                     djiSerializeCraftNameOverride(dst);
                 } else {
 #endif
-                    sbufWriteData(dst, systemConfig()->name, (int)strlen(systemConfig()->name));
+                    sbufWriteData(dst, systemConfig()->craftName, (int)strlen(systemConfig()->craftName));
 #if defined(USE_OSD)
                 }
 #endif

src/main/msp/msp_protocol.h

@@ -59,7 +59,7 @@
 #define MSP_PROTOCOL_VERSION                0   // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible
 
 #define API_VERSION_MAJOR                   2   // increment when major changes are made
-#define API_VERSION_MINOR                   4   // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
+#define API_VERSION_MINOR                   5   // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
 
 #define API_VERSION_LENGTH                  2
 

src/main/msp/msp_protocol_v2_inav.h

@@ -84,4 +84,8 @@
 #define MSP2_INAV_MISC2                         0x203A
 #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE       0x203B
 
-#define MSP2_INAV_ESC_RPM                       0x2040
+#define MSP2_INAV_ESC_RPM                       0x2040
+
+#define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
+#define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049
+

src/main/navigation/navigation.c

@@ -257,7 +257,6 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
-void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance);
 static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
 bool isWaypointAltitudeReached(void);
 static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
@@ -1059,7 +1058,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
 {
-    const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
+    UNUSED(previousState);
 
     if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
 
@@ -1068,19 +1067,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
     }  // Switch to IDLE if we do not have an healty position. Try the next iteration.
 
-    if (!(prevFlags & NAV_CTL_POS)) {
+    resetPositionController();
-        resetPositionController();
-    }
 
     posControl.cruise.course = posControl.actualState.cog;  // Store the course to follow
     posControl.cruise.previousCourse = posControl.cruise.course;
     posControl.cruise.lastCourseAdjustmentTime = 0;
 
-    return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
+    return NAV_FSM_EVENT_SUCCESS; // Go to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
 }
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState)
 {
+    UNUSED(previousState);
+
     const timeMs_t currentTimeMs = millis();
 
     if (checkForPositionSensorTimeout()) {
@@ -1097,30 +1096,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
     // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
     if (posControl.flags.isAdjustingHeading) {
         timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
-        if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
+        if (timeDifference > 100) timeDifference = 0;   // if adjustment was called long time ago, reset the time difference.
         float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
-        float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
+        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));
         posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
-    }
+    } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) {
-
-    if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000)
         posControl.cruise.previousCourse = posControl.cruise.course;
-
-    uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
-
-    if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING)
-            || (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING)
-            || posControl.flags.isAdjustingHeading) {
-        calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.course, distance);
-        DEBUG_SET(DEBUG_CRUISE, 2, 1);
-    } else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
-        calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.course, distance);
-        DEBUG_SET(DEBUG_CRUISE, 2, 2);
     }
 
-    setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.course, NAV_POS_UPDATE_XY);
+    setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
 
     return NAV_FSM_EVENT_NONE;
 }
@@ -1139,7 +1125,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
 
     resetPositionController();
 
-    return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
+    return NAV_FSM_EVENT_SUCCESS; // go back to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
 }
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
@@ -1788,9 +1774,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
 {
-    // TODO:
     UNUSED(previousState);
 
+    if ((posControl.flags.estPosStatus >= EST_USABLE)) {
+        resetPositionController();
+        setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
+    }
+
     // Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
     // Make sure terrain following is not enabled
     resetAltitudeController(false);
@@ -1802,6 +1792,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PR
 {
     UNUSED(previousState);
 
+    // Reset target position if too far away for some reason, e.g. GPS recovered since start landing.
+    if (posControl.flags.estPosStatus >= EST_USABLE) {
+        float targetPosLimit = STATE(MULTIROTOR) ? 2000.0f : navConfig()->fw.loiter_radius * 2.0f;
+        if (calculateDistanceToDestination(&posControl.desiredState.pos) > targetPosLimit) {
+            setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
+        }
+    }
+
     if (STATE(LANDING_DETECTED)) {
         return NAV_FSM_EVENT_SUCCESS;
     }
@@ -1934,7 +1932,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
     if (posControl.flags.isAdjustingPosition)   NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
     if (posControl.flags.isAdjustingAltitude)   NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
 
-    NAV_Status.activeWpNumber = posControl.activeWaypointIndex - posControl.startWpIndex + 1;
+    NAV_Status.activeWpIndex = posControl.activeWaypointIndex - posControl.startWpIndex;
+    NAV_Status.activeWpNumber = NAV_Status.activeWpIndex + 1;
+
     NAV_Status.activeWpAction = 0;
     if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
         NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
@@ -2663,7 +2663,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
         suspendTracking = false;
     }
 
-    if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED || suspendTracking)) {
+    if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) {
         return;
     }
 
@@ -2819,22 +2819,21 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
     farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
 }
 
-void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance)
-{
-    origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(course));
-    origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(course));
-    origin->z = origin->z;
-}
-
 /*-----------------------------------------------------------
  * NAV land detector
  *-----------------------------------------------------------*/
-void updateLandingStatus(void)
+void updateLandingStatus(timeMs_t currentTimeMs)
 {
     if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
         return;     // no point using this with a fixed wing if not set to disarm
     }
 
+    static timeMs_t lastUpdateTimeMs = 0;
+    if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) {  // limit update to 100Hz
+        return;
+    }
+    lastUpdateTimeMs = currentTimeMs;
+
     static bool landingDetectorIsActive;
 
     DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
@@ -2859,7 +2858,7 @@ void updateLandingStatus(void)
         if (navConfig()->general.flags.disarm_on_landing) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
             disarm(DISARM_LANDING);
-        } else if (!navigationIsFlyingAutonomousMode()) {
+        } else if (!navigationInAutomaticThrottleMode()) {
             // for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
             landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
         }
@@ -2915,9 +2914,9 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
         if (STATE(FIXED_WING_LEGACY)) {
             // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
             float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
+            static bool targetHoldActive = false;
 
             if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
-                static bool targetHoldActive = false;
                 // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
                 if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
                     posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
@@ -2926,6 +2925,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
                     posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
                     targetHoldActive = true;
                 }
+            } else {
+                targetHoldActive = false;
             }
         }
         else {
@@ -3441,7 +3442,10 @@ bool isNavHoldPositionActive(void)
         return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
     }
     // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
-    return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || FLIGHT_MODE(NAV_POSHOLD_MODE);
+    // Also hold position during emergency landing if position valid
+    return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
+            FLIGHT_MODE(NAV_POSHOLD_MODE) ||
+            navigationIsExecutingAnEmergencyLanding();
 }
 
 float getActiveWaypointSpeed(void)
@@ -3540,6 +3544,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
         // If we are disarmed, abort forced RTH or Emergency Landing
         posControl.flags.forcedRTHActivated = false;
         posControl.flags.forcedEmergLandingActivated = false;
+        posControl.flags.manualEmergLandActive = false;
         //  ensure WP missions always restart from first waypoint after disarm
         posControl.activeWaypointIndex = posControl.startWpIndex;
         // Reset RTH trackback
@@ -3616,6 +3621,41 @@ static bool isWaypointMissionValid(void)
     return posControl.waypointListValid && (posControl.waypointCount > 0);
 }
 
+static void checkManualEmergencyLandingControl(void)
+{
+    static timeMs_t timeout = 0;
+    static int8_t counter = 0;
+    static bool toggle;
+    timeMs_t currentTimeMs = millis();
+
+    if (timeout && currentTimeMs > timeout) {
+        timeout += 1000;
+        counter -= counter ? 1 : 0;
+        if (!counter) {
+            timeout = 0;
+        }
+    }
+    if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
+        if (!timeout && toggle) {
+            timeout = currentTimeMs + 4000;
+        }
+        counter += toggle;
+        toggle = false;
+    } else {
+        toggle = true;
+    }
+
+    // Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
+    if (counter >= 5) {
+        counter = 0;
+        posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
+
+        if (!posControl.flags.manualEmergLandActive) {
+            navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
+        }
+    }
+}
+
 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
 {
     static bool canActivateWaypoint = false;
@@ -3641,8 +3681,12 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             posControl.flags.rthTrackbackActive = isExecutingRTH;
         }
 
-        /* Emergency landing triggered by failsafe when Failsafe procedure set to Landing */
+        /* Emergency landing controlled manually by rapid switching of Poshold mode.
-        if (posControl.flags.forcedEmergLandingActivated) {
+         * Landing toggled ON or OFF for each Poshold activation sequence */
+        checkManualEmergencyLandingControl();
+
+        /* Emergency landing triggered by failsafe Landing or manually initiated */
+        if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
             return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
         }
 
@@ -3760,13 +3804,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         // PH has priority over COURSE_HOLD
         // CRUISE has priority on AH
         if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
-
+            if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
-            if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
                 return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
+            }
 
-            if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold))
+            if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) {
                 return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
-
+            }
         }
 
         if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {

src/main/navigation/navigation.h

@@ -473,6 +473,7 @@ typedef struct {
     navSystemStatus_Error_e error;
     navSystemStatus_Flags_e flags;
     uint8_t                 activeWpNumber;
+    uint8_t                 activeWpIndex;
     navWaypointActions_e    activeWpAction;
 } navSystemStatus_t;
 
@@ -607,7 +608,7 @@ const char * fixedWingLaunchStateMessage(void);
 
 float calculateAverageSpeed(void);
 
-void updateLandingStatus(void);
+void updateLandingStatus(timeMs_t currentTimeMs);
 
 const navigationPIDControllers_t* getNavigationPIDControllers(void);
 

src/main/navigation/navigation_fixedwing.c

@@ -267,6 +267,10 @@ static int8_t loiterDirection(void) {
 
 static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 {
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
+        return;
+    }
+
     float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
     float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
 
@@ -391,9 +395,14 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     static int32_t previousHeadingError;
     static bool errorIsDecreasing;
     static bool forceTurnDirection = false;
+    int32_t virtualTargetBearing;
 
-    // We have virtual position target, calculate heading error
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
-    int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
+        virtualTargetBearing = posControl.desiredState.yaw;
+    } else {
+        // We have virtual position target, calculate heading error
+        virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
+    }
 
     /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
@@ -702,7 +711,7 @@ bool isFixedWingLandingDetected(void)
     static int16_t fwLandSetPitchDatum;
     const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f;
 
-    timeMs_t currentTimeMs = millis();
+    const timeMs_t currentTimeMs = millis();
 
     // Check horizontal and vertical velocities are low (cm/s)
     bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
@@ -727,9 +736,12 @@ bool isFixedWingLandingDetected(void)
             DEBUG_SET(DEBUG_LANDING, 6, isRollAxisStatic);
             DEBUG_SET(DEBUG_LANDING, 7, isPitchAxisStatic);
             if (isRollAxisStatic && isPitchAxisStatic) {
-                // Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
+                /* Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
-                timeMs_t safetyTimeDelay = 2000 + navConfig()->general.auto_disarm_delay;
+                 * Conditions need to be held for fixed safety time + optional extra delay.
-                return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay; // check conditions stable for 2s + optional extra delay
+                 * Fixed time increased if velocities invalid to provide extra safety margin against false triggers */
+                const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE || posControl.flags.estVelStatus == EST_NONE ? 5000 : 1000;
+                timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay;
+                return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay;
             } else {
                 fixAxisCheck = false;
             }
@@ -743,8 +755,6 @@ bool isFixedWingLandingDetected(void)
  *-----------------------------------------------------------*/
 void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
 {
-    rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
-    rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
     rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
@@ -756,6 +766,18 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
     } else {
         rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
     }
+
+    if (posControl.flags.estPosStatus >= EST_USABLE) {  // Hold position if possible
+        applyFixedWingPositionController(currentTimeUs);
+        int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL],
+                                            -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle),
+                                            DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
+        rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
+        rcCommand[YAW] = 0;
+    } else {
+        rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
+        rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
+    }
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation_multicopter.c

@@ -723,7 +723,7 @@ bool isMulticopterFlying(void)
 bool isMulticopterLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
-    static timeUs_t landingDetectorStartedAt;
+    static timeMs_t landingDetectorStartedAt;
 
     /* Basic condition to start looking for landing
     *  Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
@@ -747,7 +747,7 @@ bool isMulticopterLandingDetected(void)
     DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
 
     bool possibleLandingDetected = false;
-    const timeUs_t currentTimeUs = micros();
+    const timeMs_t currentTimeMs = millis();
 
     if (navGetCurrentStateFlags() & NAV_CTL_LAND) {
         // We have likely landed if throttle is 40 units below average descend throttle
@@ -761,13 +761,13 @@ bool isMulticopterLandingDetected(void)
 
         if (!landingDetectorStartedAt) {
             landingThrSum = landingThrSamples = 0;
-            landingDetectorStartedAt = currentTimeUs;
+            landingDetectorStartedAt = currentTimeMs;
         }
         if (!landingThrSamples) {
-            if (currentTimeUs - landingDetectorStartedAt < (USECS_PER_SEC * MC_LAND_THR_STABILISE_DELAY)) {   // Wait for 1 second so throttle has stabilized.
+            if (currentTimeMs - landingDetectorStartedAt < S2MS(MC_LAND_THR_STABILISE_DELAY)) {   // Wait for 1 second so throttle has stabilized.
                 return false;
             } else {
-                landingDetectorStartedAt = currentTimeUs;
+                landingDetectorStartedAt = currentTimeMs;
             }
         }
         landingThrSamples += 1;
@@ -783,7 +783,7 @@ bool isMulticopterLandingDetected(void)
         if (landingDetectorStartedAt) {
             possibleLandingDetected = velCondition && gyroCondition;
         } else {
-            landingDetectorStartedAt = currentTimeUs;
+            landingDetectorStartedAt = currentTimeMs;
             return false;
         }
     }
@@ -798,10 +798,13 @@ bool isMulticopterLandingDetected(void)
     DEBUG_SET(DEBUG_LANDING, 5, possibleLandingDetected);
 
     if (possibleLandingDetected) {
-        timeUs_t safetyTimeDelay = MS2US(2000 + navConfig()->general.auto_disarm_delay);  // check conditions stable for 2s + optional extra delay
+        /* Conditions need to be held for fixed safety time + optional extra delay.
-        return (currentTimeUs - landingDetectorStartedAt > safetyTimeDelay);
+         * Fixed time increased if Z velocity invalid to provide extra safety margin against false triggers */
+        const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE ? 5000 : 1000;
+        timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay;
+        return currentTimeMs - landingDetectorStartedAt > safetyTimeDelay;
     } else {
-        landingDetectorStartedAt = currentTimeUs;
+        landingDetectorStartedAt = currentTimeMs;
         return false;
     }
 }
@@ -814,8 +817,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     static timeUs_t previousTimePositionUpdate = 0;
 
     /* Attempt to stabilise */
-    rcCommand[ROLL] = 0;
-    rcCommand[PITCH] = 0;
     rcCommand[YAW] = 0;
 
     if ((posControl.flags.estAltStatus < EST_USABLE)) {
@@ -852,6 +853,14 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
 
     // Update throttle controller
     rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
+
+    // Hold position if possible
+    if ((posControl.flags.estPosStatus >= EST_USABLE)) {
+        applyMulticopterPositionController(currentTimeUs);
+    } else {
+        rcCommand[ROLL] = 0;
+        rcCommand[PITCH] = 0;
+    }
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation_pos_estimator.c

@@ -330,7 +330,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
         posEstimator.baro.lastUpdateTime = currentTimeUs;
 
         if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) {
-            pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs));
+            posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs));
         }
     }
     else {
@@ -443,7 +443,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
 
                 if (gravityCalibrationComplete()) {
                     zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
-                    setGravityCalibrationAndWriteEEPROM(posEstimator.imu.calibratedGravityCMSS);
+                    setGravityCalibration(posEstimator.imu.calibratedGravityCMSS);
                     LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
                 }
             }
@@ -564,6 +564,15 @@ static void estimationPredict(estimationContext_t * ctx)
 
 static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 {
+    DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
+    DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt);        // Baro altitude
+    DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
+    DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
+    DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
+    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
+    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
+    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
+
     if (ctx->newFlags & EST_BARO_VALID) {
         timeUs_t currentTimeUs = micros();
 
@@ -634,15 +643,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         return true;
     }
 
-    DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
-    DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt);        // Baro altitude
-    DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
-    DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
-    DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
-    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
-    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
-    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
-
     return false;
 }
 

src/main/navigation/navigation_private.h

@@ -42,7 +42,7 @@
 #define MC_LAND_CHECK_VEL_XY_MOVING         100.0f  // cm/s
 #define MC_LAND_CHECK_VEL_Z_MOVING          100.0f  // cm/s
 #define MC_LAND_THR_STABILISE_DELAY         1       // seconds
-#define MC_LAND_DESCEND_THROTTLE            40      // uS
+#define MC_LAND_DESCEND_THROTTLE            40      // RC pwm units (us)
 #define MC_LAND_SAFE_SURFACE                5.0f    // cm
 
 #define NAV_RTH_TRACKBACK_POINTS            50      // max number RTH trackback points
@@ -104,14 +104,13 @@ typedef struct navigationFlags_s {
     bool forcedRTHActivated;
     bool forcedEmergLandingActivated;
 
-    bool wpMissionPlannerActive;            // Activation status of WP mission planner
-
     /* Landing detector */
     bool resetLandingDetector;
 
+    bool wpMissionPlannerActive;            // Activation status of WP mission planner
     bool rthTrackbackActive;                // Activation status of RTH trackback
-
     bool wpTurnSmoothingActive;             // Activation status WP turn smoothing
+    bool manualEmergLandActive;             // Activation status of manual emergency landing
 } navigationFlags_t;
 
 typedef struct {
@@ -323,7 +322,6 @@ typedef struct {
 } rthSanityChecker_t;
 
 typedef struct {
-    fpVector3_t                 targetPos;
     int32_t                     course;
     int32_t                     previousCourse;
     timeMs_t                    lastCourseAdjustmentTime;

src/main/programming/logic_condition.c

@@ -520,6 +520,105 @@ void logicConditionProcess(uint8_t i) {
     }
 }
 
+static int logicConditionGetWaypointOperandValue(int operand) {
+
+    switch (operand) {
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP: // 0/1
+            return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX:
+            return NAV_Status.activeWpNumber;
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION:
+            return NAV_Status.activeWpAction;
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION:
+            {
+                uint8_t wpIndex = posControl.activeWaypointIndex + 1;
+                if ((wpIndex > 0) && (wpIndex < NAV_MAX_WAYPOINTS)) {
+                    return posControl.waypointList[wpIndex].action;
+                }
+                return false;
+            }
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE:
+            {
+                uint32_t distance = 0;
+                if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
+                    fpVector3_t poi;
+                    gpsLocation_t wp;
+                    wp.lat = posControl.waypointList[NAV_Status.activeWpIndex].lat;
+                    wp.lon = posControl.waypointList[NAV_Status.activeWpIndex].lon;
+                    wp.alt = posControl.waypointList[NAV_Status.activeWpIndex].alt;
+                    geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
+
+                    distance = calculateDistanceToDestination(&poi) / 100;
+                }
+
+                return distance;
+            }
+            break;
+
+        case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT:
+            {
+                uint32_t distance = 0;
+                if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpIndex > 0) {
+                    fpVector3_t poi;
+                    gpsLocation_t wp;
+                    wp.lat = posControl.waypointList[NAV_Status.activeWpIndex-1].lat;
+                    wp.lon = posControl.waypointList[NAV_Status.activeWpIndex-1].lon;
+                    wp.alt = posControl.waypointList[NAV_Status.activeWpIndex-1].alt;
+                    geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
+
+                    distance = calculateDistanceToDestination(&poi) / 100;
+                }
+
+                return distance;
+            }
+            break;
+        
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION:
+            return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION:
+            return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0;
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION:
+            return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0;
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION:
+            return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0;
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP:
+            return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1);
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP:
+            return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2);
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP:
+            return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3);
+            break;
+
+        case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP:
+            return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4);
+            break;
+
+        default:
+            return 0;
+            break;
+    }
+}
+
 static int logicConditionGetFlightOperandValue(int operand) {
 
     switch (operand) {
@@ -633,10 +732,6 @@ static int logicConditionGetFlightOperandValue(int operand) {
             return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
             break; 
 
-        case LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP: // 0/1
-            return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
-            break; 
-
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
             return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
             break;
@@ -657,14 +752,6 @@ static int logicConditionGetFlightOperandValue(int operand) {
             return axisPID[PITCH];
             break;
 
-        case LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX:
-            return NAV_Status.activeWpNumber;
-            break;
-
-        case LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION:
-            return NAV_Status.activeWpAction;
-            break;
-
         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
             return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT16_MAX);
             break;
@@ -730,12 +817,16 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
             return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE);
             break;
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION:
+            return (bool) FLIGHT_MODE(NAV_WP_MODE);
+            break;
+
         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD:
-            return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
+            return ((bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !(bool)FLIGHT_MODE(NAV_ALTHOLD_MODE));
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
-            return (bool)(FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
+            return (bool) (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
@@ -754,6 +845,11 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
             return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
             break;
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO:
+            return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) || 
+                    (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) || (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
+            break;
+
         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
             return IS_RC_MODE_ACTIVE(BOXUSER1);
             break;
@@ -787,7 +883,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
 
         case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
             //Extract RC channel raw value
-            if (operand >= 1 && operand <= 16) {
+            if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
                 retVal = rxGetChannelValue(operand - 1);
             } 
             break;
@@ -817,6 +913,10 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
                 retVal = programmingPidGetOutput(operand);
             }
             break;
+        
+        case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
+            retVal = logicConditionGetWaypointOperandValue(operand);
+            break;
 
         default:
             break;

src/main/programming/logic_condition.h

@@ -93,6 +93,7 @@ typedef enum logicOperandType_s {
     LOGIC_CONDITION_OPERAND_TYPE_LC,    // Result of different LC and LC operand
     LOGIC_CONDITION_OPERAND_TYPE_GVAR,  // Value from a global variable
     LOGIC_CONDITION_OPERAND_TYPE_PID,  // Value from a Programming PID
+    LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS,
     LOGIC_CONDITION_OPERAND_TYPE_LAST
 } logicOperandType_e;
 
@@ -120,24 +121,21 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL, // 0/1              // 20
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING, // 0/1             // 21
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH, // 0/1                           // 22
-    LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP, // 0/1                            // 23
+    LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1                       // 23
-    LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1                       // 24
+    LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1                      // 24
-    LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1                      // 25
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL,                         // 25
-    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL,                         // 26
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 26
-    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 27
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 27
-    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 28
+    LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE,                        // 28
-    LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX,                          // 29
+    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ,                                 // 29
-    LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION,                         // 30
+    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR,                                // 39
-    LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE,                        // 31
+    LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1                        // 31
-    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ,                                 // 32
+    LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS,                           // 32
-    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR,                                // 33
+    LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int                    // 33
-    LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1                        // 34
+    LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS,                              // 34
-    LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS,                           // 35
+    LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2                      // 35
-    LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int                    // 36
+    LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2                             // 36
-    LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS,                              // 37
+    LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int                   // 37
-    LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2                      // 38
-    LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2                             // 39
-    LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int                   // 40
 } logicFlightOperands_e;
 
 typedef enum {
@@ -155,8 +153,27 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD,                        // 11
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3,                              // 12
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4,                              // 13
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO,                               // 14
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION,                   // 15
 } logicFlightModeOperands_e;
 
+typedef enum {
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1                         // 0
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX,                       // 1
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION,                      // 2
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION,                 // 3
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE,                    // 4
+    LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT,               // 5
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION,                         // 6
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION,                         // 7
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION,                         // 8
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION,                         // 9
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP,                 // 10
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP,                 // 11
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP,                 // 12
+    LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP,                 // 13
+} logicWaypointOperands_e;
+
 typedef enum {
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0),
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1),

src/main/sensors/gyro.c

@@ -362,7 +362,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe
         dev->gyroZero[Z] = v.v[Z];
 
 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
-        setGyroCalibrationAndWriteEEPROM(dev->gyroZero);
+        setGyroCalibration(dev->gyroZero);
 #endif
 
         LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]);

src/main/target/AOCODARCF7MINI/CMakeLists.txt

@@ -0,0 +1,2 @@
+target_stm32f722xe(AOCODARCF7MINI_V1)
+target_stm32f722xe(AOCODARCF7MINI_V2)

src/main/target/AOCODARCF7MINI/target.c

@@ -0,0 +1,54 @@
+/*
+ * 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"
+
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM1, CH3, PA10,  TIM_USE_PPM, 0, 0),                            // PPM, RX1  
+    
+    DEF_TIM(TIM3, CH1, PB4,   TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),     // S1   D(1, 4, 5)
+    DEF_TIM(TIM3, CH2, PB5,   TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),     // S2   D(1, 5, 4)   
+    DEF_TIM(TIM3, CH3, PB0,   TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),     // S3   D(1, 7, 5)
+    DEF_TIM(TIM3, CH4, PB1,   TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),     // S4   D(1, 2, 5)
+
+#if defined(AOCODARCF7MINI_V2)
+    DEF_TIM(TIM8, CH3, PC8,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),    // S5   D(2, 4, 7)
+    DEF_TIM(TIM8, CH4, PC9,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),    // S6   D(2, 7, 7)
+#else
+    DEF_TIM(TIM2, CH2, PB3,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),    // S6   D(1, 6, 3)
+#endif
+
+    DEF_TIM(TIM4, CH1, PB6,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),    // S7   D(1, 0, 2)    
+    DEF_TIM(TIM4, CH2, PB7,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),    // S8   D(1, 3, 2)
+
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_LED, 0, 0),                             // LED     
+    
+};
+
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/AOCODARCF7MINI/target.h

@@ -0,0 +1,171 @@
+/*
+ * 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 "AO7M"
+
+#if defined(AOCODARCF7MINI_V2)
+#define USBD_PRODUCT_STRING  "AocodaRCF7MiniV2"
+#else
+#define USBD_PRODUCT_STRING  "AocodaRCF7MiniV1"
+#endif
+
+#define LED0                    PA13
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *************** SPI1 Gyro & ACC *******************
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+
+#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_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
+#define MPU6500_CS_PIN          PB2
+#define MPU6500_SPI_BUS         BUS_SPI1
+
+#define USE_EXTI
+#define MPU6500_EXTI_PIN        PC4
+
+#define USE_MPU_DATA_READY_SIGNAL
+
+// *************** 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_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 USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+// *************** SPI2 OSD ***********************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PB12
+
+// *************** SPI3 FLASH BLACKBOX*******************
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS          BUS_SPI3
+#define M25P16_CS_PIN           PD2
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+
+// *************** UART *****************************
+#define USE_VCP
+#define USB_DETECT_PIN          PC14
+#define USE_USB_DETECT
+
+#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            PB10
+#define UART3_RX_PIN            PB11
+
+#define USE_UART4
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1
+
+#define USE_UART6
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART1
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+
+#define ADC_CHANNEL_1_PIN           PC2
+#define ADC_CHANNEL_2_PIN           PC1
+#define ADC_CHANNEL_3_PIN           PC0
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+
+// *************** PINIO ***************************
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES                (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_TX_PROF_SEL)
+#define CURRENT_METER_SCALE     400
+
+#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
+
+#define MAX_PWM_OUTPUT_PORTS        10
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR

src/main/target/IFLIGHT_BLITZ_F722/target.c

@@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM2, CH4, PB11,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),  // S8 DMA1_S6_CH3
     
 
-    DEF_TIM(TIM1, CH2N, PB0,  TIM_USE_LED,                         0, 0),  // WS2812B DMA2_S6_CH0
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_LED,                         0, 0),  // WS2812B
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/MATEKF405SE/target.h

@@ -135,7 +135,11 @@
 #define SOFTSERIAL_1_TX_PIN      PA2
 #define SOFTSERIAL_1_RX_PIN      PA2
 
+#ifdef MATEKF405SE_PINIO
+#define SERIAL_PORT_COUNT       7
+#else
 #define SERIAL_PORT_COUNT       8
+#endif
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS

src/main/target/MATEKF411TE/target.h

@@ -38,6 +38,10 @@
 #define BMI270_CS_PIN           PC13
 #define IMU_BMI270_ALIGN        CW270_DEG_FLIP
 
+#define USE_IMU_ICM42605
+#define ICM42605_SPI_BUS        BUS_SPI2
+#define ICM42605_CS_PIN         PC13
+#define IMU_ICM42605_ALIGN      CW180_DEG_FLIP
 
 #define USE_MAX7456
 #define MAX7456_SPI_BUS         BUS_SPI2

src/main/target/MATEKF722PX/target.h

@@ -125,7 +125,11 @@
 #define SOFTSERIAL_1_TX_PIN      PA2 //TX2 pad
 #define SOFTSERIAL_1_RX_PIN      NONE
 
+#ifdef MATEKF722PX_PINIO
+#define SERIAL_PORT_COUNT       7
+#else
 #define SERIAL_PORT_COUNT       8
+#endif
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS

src/main/target/OMNIBUSF7NXT/config.c

@@ -32,6 +32,6 @@
 
 void targetConfiguration(void)
 {
-    ledStripConfigMutable()->ledConfigs[0] = DEFINE_LED(0, 0, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0);
+    DEFINE_LED(ledStripConfigMutable()->ledConfigs, 0, 0, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0);
     // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
 }

src/main/target/RUSH_BLADE_F7/CMakeLists.txt

@@ -1 +1,2 @@
-target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES)
+target_stm32f722xe(RUSH_BLADE_F7)
+target_stm32f722xe(RUSH_BLADE_F7_HD)

src/main/target/RUSH_BLADE_F7/target.c

@@ -23,15 +23,19 @@
 #include "drivers/timer.h"
 
 timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
+    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, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 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
 
-    DEF_TIM(TIM8, CH3, PC8,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP2-1   D(2, 4, 7)
+    DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP
-    DEF_TIM(TIM8, CH4, PC9,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S2   UP2-1   D(2, 7, 7)
+    #ifdef RUSH_BLADE_F7
-    DEF_TIM(TIM3, CH4, PB1,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S6   UP1-2   D(1, 2, 5)
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // CAM CONTROL
-    DEF_TIM(TIM3, CH3, PB0,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S5   UP1-2   D(1, 7, 5)
+    #endif
-    DEF_TIM(TIM8, CH2, PC7,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5 D(2, 3, 7)
-    DEF_TIM(TIM8, CH1, PC6,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6 D(1, 4, 5)
-
-    DEF_TIM(TIM2, CH1, PA15,    TIM_USE_LED, 0, 0),                            // LED 
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/RUSH_BLADE_F7/target.h

@@ -15,32 +15,55 @@
  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-
 #pragma once
 
+#ifdef RUSH_BLADE_F7
 #define TARGET_BOARD_IDENTIFIER "RBF7"
 #define USBD_PRODUCT_STRING  "RUSH_BLADE_F7"
+#endif
 
-#define LED0                    PB10  //Blue   SWCLK
+#ifdef RUSH_BLADE_F7_HD
-// #define LED1                    PA13  //Green  SWDIO
+#define TARGET_BOARD_IDENTIFIER "RBF7HD"
+#define USBD_PRODUCT_STRING     "RUSH_BLADE_F7_HD"
+#endif
 
+#define LED0                    PB10
 #define BEEPER                  PB2
 #define BEEPER_INVERTED
 
-// *************** SPI1 Gyro & ACC *******************
+// *************** SPI **********************
 #define USE_SPI
+
 #define USE_SPI_DEVICE_1
+#define SPI1_NSS_PIN            PC4
 #define SPI1_SCK_PIN            PA5
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#define USE_SPI_DEVICE_2
+#define SPI2_NSS_FLASH_PIN      PB12
+#define SPI2_NSS_OSD_PIN        PB11
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+
+// *************** Gyro & ACC **********************
 #define USE_IMU_MPU6000
 #define IMU_MPU6000_ALIGN       CW270_DEG
-#define MPU6000_CS_PIN          PC4
 #define MPU6000_SPI_BUS         BUS_SPI1
+#define MPU6000_CS_PIN          SPI1_NSS_PIN
+#define MPU6000_EXTI_PIN        GYRO_INT_EXTI
 
-// *************** I2C /Baro/Mag *********************
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define ICM42605_CS_PIN         SPI1_NSS_PIN
+#define ICM42605_EXTI_PIN       GYRO_INT_EXTI
+
+// *************** I2C/Baro/Mag *********************
 #define USE_I2C
+
 #define USE_I2C_DEVICE_1
 #define I2C1_SCL                PB8
 #define I2C1_SDA                PB9
@@ -49,6 +72,7 @@
 #define BARO_I2C_BUS            BUS_I2C1
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
+#define USE_BARO_SPL06
 #define USE_BARO_DPS310
 
 #define USE_MAG
@@ -61,28 +85,24 @@
 #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
-
-// *************** SPI2 Flash ***********************
-#define USE_SPI_DEVICE_2
-#define SPI2_SCK_PIN            PB13
-#define SPI2_MISO_PIN           PB14
-#define SPI2_MOSI_PIN           PB15
 
+// *************** Flash ****************************
 #define USE_FLASHFS
 #define USE_FLASH_M25P16
 #define M25P16_SPI_BUS          BUS_SPI2
-#define M25P16_CS_PIN           PB12
+#define M25P16_CS_PIN           SPI2_NSS_FLASH_PIN
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
 
+
+// *************** Analog OSD ************************
+#ifdef RUSH_BLADE_F7
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          SPI2_NSS_OSD_PIN
+#endif
+
 // *************** UART *****************************
 #define USE_VCP
-#define USB_DETECT_PIN          PC14
-#define USE_USB_DETECT
 
 #define USE_UART1
 #define UART1_TX_PIN            PA9
@@ -108,12 +128,11 @@
 
 #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 ADC1_DMA_STREAM             DMA2_Stream4
 
 #define ADC_CHANNEL_1_PIN           PC0
 #define ADC_CHANNEL_2_PIN           PC1
@@ -121,20 +140,28 @@
 #define VBAT_ADC_CHANNEL            ADC_CHN_1
 #define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
 
-// *************** LEDSTRIP ************************
+#define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_LED_STRIP)
-#define USE_LED_STRIP
-#define WS2811_PIN                  PA8
 
-#define DEFAULT_FEATURES   (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL)
 #define CURRENT_METER_SCALE     179
 
+
+// ********** Optiical Flow and Lidar **************
+#define USE_RANGEFINDER
+#define USE_RANGEFINDER_MSP
+#define USE_OPFLOW
+#define USE_OPFLOW_MSP
+
+// *************** LED *****************************
+#define USE_LED_STRIP
+#define WS2811_PIN              PA15
+
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTA         0xffff
-#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTB         0xffff
-#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTC         0xffff
-#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTD         (BIT(2))
 
-#define MAX_PWM_OUTPUT_PORTS        10
+#define MAX_PWM_OUTPUT_PORTS    8
 #define USE_DSHOT
 #define USE_ESC_SENSOR

src/main/target/common_post.h

@@ -65,6 +65,17 @@ extern uint8_t __config_end;
 #endif
 
 //Defines for compiler optimizations
+#ifdef STM32H7
+
+#define FUNCTION_COMPILE_FOR_SIZE
+#define FUNCTION_COMPILE_NORMAL
+#define FUNCTION_COMPILE_FOR_SPEED
+#define FILE_COMPILE_FOR_SIZE
+#define FILE_COMPILE_NORMAL
+#define FILE_COMPILE_FOR_SPEED
+
+#else
+
 #define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize("-Os")))
 #define FUNCTION_COMPILE_NORMAL __attribute__((optimize("-O2")))
 #define FUNCTION_COMPILE_FOR_SPEED __attribute__((optimize("-Ofast")))
@@ -72,6 +83,8 @@ extern uint8_t __config_end;
 #define FILE_COMPILE_NORMAL _Pragma("GCC optimize(\"O2\")")
 #define FILE_COMPILE_FOR_SPEED _Pragma("GCC optimize(\"Ofast\")")
 
+#endif
+
 #if defined(CONFIG_IN_RAM) || defined(CONFIG_IN_EXTERNAL_FLASH)
 #ifndef EEPROM_SIZE
 #define EEPROM_SIZE     8192

src/main/telemetry/frsky_d.c

@@ -195,10 +195,7 @@ static void sendThrottleOrBatterySizeAsRpm(void)
 {
     sendDataHead(ID_RPM);
     if (ARMING_FLAG(ARMED)) {
-        uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
+        uint16_t throttleForRPM = getThrottlePercent() / BLADE_NUMBER_DIVIDER;
-        if (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) {
-            throttleForRPM = 0;
-        }
         serialize16(throttleForRPM);
     } else {
         serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));

src/main/telemetry/ibus.c

@@ -51,6 +51,7 @@
 
 #include "flight/imu.h"
 #include "flight/failsafe.h"
+#include "flight/mixer.h"
 
 #include "navigation/navigation.h"
 

src/main/telemetry/ibus_shared.c

@@ -44,6 +44,7 @@
 
 #include "flight/imu.h"
 #include "flight/failsafe.h"
+#include "flight/mixer.h"
 
 #include "navigation/navigation.h"
 
@@ -146,7 +147,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
         if (!temp_valid || (temperature < -400)) temperature = -400; // Minimum reported temperature is -40°C
         return sendIbusMeasurement2(address, (uint16_t)(temperature  + IBUS_TEMPERATURE_OFFSET));
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_RPM) {
-        return sendIbusMeasurement2(address, (uint16_t) (rcCommand[THROTTLE]));
+        return sendIbusMeasurement2(address, (uint16_t)getThrottlePercent() );
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE) { //VBAT
         if (telemetryConfig()->report_cell_voltage) {
             return sendIbusMeasurement2(address, getBatteryAverageCellVoltage());

src/main/telemetry/mavlink.c

@@ -653,10 +653,7 @@ void mavlinkSendHUDAndHeartbeat(void)
     mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
     mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
 
-    int16_t thr = rxGetChannelValue(THROTTLE);
+    int16_t thr = getThrottlePercent();
-    if (navigationIsControllingThrottle()) {
-        thr = rcCommand[THROTTLE];
-    }
     mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
         // airspeed Current airspeed in m/s
         mavAirSpeed,
@@ -665,7 +662,7 @@ void mavlinkSendHUDAndHeartbeat(void)
         // heading Current heading in degrees, in compass units (0..360, 0=north)
         DECIDEGREES_TO_DEGREES(attitude.values.yaw),
         // throttle Current throttle setting in integer percent, 0 to 100
-        scaleRange(constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
+        thr, 
         // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
         mavAltitude,
         // climb Current climb rate in meters/second

src/utils/draft_rn_settings.rb

@@ -0,0 +1,312 @@
+#!/usr/bin/env ruby
+# coding: utf-8
+
+##
+## This file is part of INAV.
+##
+## INAV is 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 is 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/>.
+##
+
+#########################################################################
+#
+# Generates a formatted (Markdown) report of CLI settings differences
+# between two INAV branches or tags. Primarily intended for inclusion
+# in release notes.
+#
+# Note that the report will require review / editing before inclusing
+# in the RN, but it will jabe done the vast majority of the work for
+# you.
+#
+# Example:
+#
+# draft_rn_settings.rb -f 5.1.0 -t release_6.0.0 > /tmp/draft-set6.0RN.md
+# # now review  /tmp/draft-set6.0RN.md for inclusion in the offical RN
+#
+# Requirments:
+# * ruby (v3 recommended)
+# * git
+#
+#########################################################################
+
+require 'yaml'
+require 'optparse'
+require 'tmpdir'
+
+base = File.join(ENV["HOME"], "Projects/fc/inav")
+ntag = nil
+otag = nil
+
+ARGV.options do |opt|
+  opt.banner = "#{File.basename($0)} [options]"
+  opt.on('-t','--tag=TAG',  'New tag or branch') {|o|ntag=o}
+  opt.on('-f','--from-tag=TAG', 'previous release tag or branch'){|o|otag=o}
+  opt.on('-b','--base=DIR',  "Base of the inav repo [#{base}]" ) {|o|base=o}
+  opt.on('-?', "--help", "Show this message") {puts opt.to_s; exit}
+  begin
+    opt.parse!
+  rescue
+    puts opt
+    exit
+  end
+end
+
+abort 'New tag / branch is mandatory' unless ntag
+abort 'Old tag / branch is mandatory' unless otag
+
+begin
+  Dir.chdir(base)
+rescue
+  abort "Cannot chdir to #{base}"
+end
+
+oldyaml = File.join(Dir.tmpdir, ".setting_old.yaml")
+newyaml = File.join(Dir.tmpdir, ".setting_new.yaml")
+
+system "> #{oldyaml} git show #{otag}:src/main/fc/settings.yaml"
+system "> #{newyaml} git show #{ntag}:src/main/fc/settings.yaml"
+
+h_old = {}
+begin
+  h_old = YAML.load_file(oldyaml)
+rescue
+  abort "Error reading old settings yaml"
+end
+
+h_new = {}
+begin
+  h_new = YAML.load_file(newyaml)
+rescue
+  abort "Error reading new settings yaml"
+end
+
+ot_names = []
+h_old["tables"].each do |t|
+  ot_names << t["name"]
+end
+
+nt_names = []
+h_new["tables"].each do |t|
+  nt_names << t["name"]
+end
+
+puts "# Draft Rel Notes -- settings helper"
+puts
+puts "The following should make a reasonable first pass at new / changed / removed settings"
+puts "Will require manual checking / refining / formatting."
+
+d = nt_names.difference(ot_names).sort
+unless d.empty?
+  puts
+  puts "# New (Tables) -- to update group values"
+  puts
+  puts "| Name | Values |"
+  puts "| ---- | ------ |"
+  d.each do |dn|
+    t = h_new["tables"].select{|k| k["name"] == dn}.first
+    unless t.nil?
+      vals = t["values"]
+      if vals[0].class == String
+        vals.map!{|a| "`#{a}`"}
+      end
+      vs = vals.join(", ")
+      puts "| #{dn} | #{vs} |"
+    end
+  end
+end
+puts
+
+d= ot_names.difference(nt_names).sort
+unless d.empty?
+  puts
+  puts "## Removed (tables -- check for group value later)"
+  puts
+  puts "| Name | Comment |"
+  puts "| ---- | ------ |"
+  d.each do |dn|
+    t = h_old["tables"].select{|k| k["name"] == dn}.first
+    unless t.nil?
+      puts "| #{dn} |  |"
+    end
+  end
+end
+
+d=ot_names.intersection(nt_names).sort
+unless d.empty?
+  puts
+  puts "## Changed (table values -- check for group value later)"
+  puts
+  puts "| Name | Values |"
+  puts "| ---- | ------ |"
+  d.each do |dn|
+    tnew = h_new["tables"].select{|k| k["name"] == dn}.first
+    told = h_old["tables"].select{|k| k["name"] == dn}.first
+    unless told.nil? && tnew.nil?
+      if told["values"] != tnew["values"]
+        vals = tnew["values"].difference(told["values"])
+        if !vals.empty?
+          if vals[0].class == String
+            vals.map!{|a| "`#{a}`"}
+          end
+          newvals = vals.join(", ")
+          puts "| #{dn} | New: #{newvals} |"
+        end
+
+        vals = told["values"].difference(tnew["values"])
+        if !vals.empty?
+          if vals[0].class == String
+            vals.map!{|a| "`#{a}`"}
+          end
+          remvals = vals.join(", ")
+          puts "| #{dn} | Removed: #{remvals} |"
+        end
+      end
+    end
+  end
+end
+
+og_names = []
+h_old["groups"].each do |h|
+  h["members"].each do |m|
+    og_names << m["name"]
+  end
+end
+
+ng_names = []
+h_new["groups"].each do |h|
+  h["members"].each do |m|
+    ng_names << m["name"]
+  end
+end
+
+d = ng_names.difference(og_names).sort
+unless d.empty?
+  puts
+  puts "## New Items"
+  puts
+  puts "| Name | Description |"
+  puts "| ---- | ------ |"
+  d.each do |dn|
+    t=nil
+    h_new["groups"].each do |hh|
+      t = hh["members"].select{|k| k["name"] == dn}.first
+      unless t.nil?
+        desc = t["description"]||""
+        minmax = []
+        if t.has_key?("min")
+          case t["min"]
+          when Integer
+            minmax[0] = t["min"]
+          when "INT16_MIN"
+            minmax[0] = -32768
+          when "INT8_MIN"
+            minmax[0] = -128
+          end
+        end
+
+        if t.has_key?("max")
+          case t["max"]
+          when Integer
+            minmax[1] = t["max"]
+          when "UINT16_MAX"
+            minmax[1] = 65535
+          when "INT16_MAX"
+            minmax[1] = 32767
+          when "INT8_MAX"
+            minmax[1] = 127
+          when "UINT8_MAX"
+            minmax[1] = 255
+          end
+          minmax[0] = 0 if !minmax[1].nil? && minmax[0].nil?
+        end
+        if minmax.size == 2
+          desc = [desc," Values: #{minmax[0]} - #{minmax[1]}"].join
+        end
+
+        default = nil
+        if t.has_key?("default_value")
+          default = t["default_value"].to_s.upcase
+          unless default.nil?
+            desc = "#{desc} Default: #{default}"
+          end
+        end
+        desc.gsub!('|',',')
+        # default_value max min
+        puts "| #{dn} | #{desc} |"
+        break
+      end
+    end
+  end
+end
+
+
+d = og_names.difference(ng_names).sort
+unless d.empty?
+  puts
+  puts "## Removed Items"
+  puts
+  puts "| Name | Description |"
+  puts "| ---- | ------ |"
+  d.each do |dn|
+    puts "| #{dn} |  |"
+  end
+end
+
+
+d=ot_names.intersection(nt_names).sort
+unless d.empty?
+  puts
+  puts "## Changed Items (desc, range, default -- requires checking)"
+  puts
+  puts "Most of the content here will be indicated by a change at the table level"
+  puts
+  puts "| Name | Values |"
+  puts "| ---- | ------ |"
+  d.each do |dn|
+    tnew = told = nil
+    h_new["groups"].each do |hh|
+      tnew = hh["members"].select{|k| k["name"] == dn}.first
+      break unless tnew.nil?
+    end
+    h_old["groups"].each do |hh|
+      told = hh["members"].select{|k| k["name"] == dn}.first
+      break unless told.nil?
+    end
+    # in the following tests we discard just the addtion of an attribute
+    unless (told.nil? && tnew.nil?)
+      diffs = []
+      if told["default_value"] && told["default_value"] != tnew["default_value"]
+        diffs << "Default:  #{tnew["default_value"]}"
+      end
+
+      if told["description"] && told["description"] != tnew["description"]
+        diffs << tnew["description"]
+      end
+
+      unless diffs.empty?
+        diffstr = diffs.join(" ")
+        puts "| #{dn} | #{diffstr} |"
+      end
+
+    end
+  end
+end
+
+puts "Last updated: #{Time.now.utc.strftime("%FT%T%Z")}"
+begin
+  File.unlink(oldyaml, newyaml)
+rescue
+end