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

Dockerfile

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

build.sh

@@ -21,19 +21,29 @@ fi
 
 if [ -z "$(docker images -q inav-build)" ]; then
   echo -e "*** Building image\n"
-  docker build -t inav-build .
+  docker build -t inav-build --build-arg USER_ID="$(id -u)" --build-arg GROUP_ID="$(id -g)" .
   echo -ne "\n"
 fi
 
 if [ ! -d ./build ]; then
   echo -e "*** Creating build directory\n"
-  mkdir ./build
+  mkdir ./build && chmod 777 ./build
+fi
+
+if [ ! -d ./downloads ]; then
+  echo -e "*** Creating downloads directory\n"
+  mkdir ./downloads && chmod 777 ./downloads
+fi
+
+if [ ! -d ./tools ]; then
+  echo -e "*** Creating tools directory\n"
+  mkdir ./tools && chmod 777 ./tools
 fi
 
 echo -e "*** Building targets [$@]\n"
 docker run --rm -it -v "$(pwd)":/src inav-build $@
 
-if ls ./build/*.hex &> /dev/null; then
+if [ -z "$(ls ./build/*.hex &> /dev/null)" ]; then
   echo -e "\n*** Built targets in ./build:"
   stat -c "%n (%.19y)" ./build/*.hex
 fi

docs/Betaflight 4.3 compatible OSD.md

@@ -0,0 +1,46 @@
+# Betaflight 4.3 compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode")
+
+INAV 6.0 includes a special mode for MSP DisplayPort that supports incomplete implementations of MSP DisplayPort that only support BetaFlight, like the DJI O3 Air Unit.
+
+Different flight controllers have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs.
+
+While there is some overlap between the glyphs in BetaFlight and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the BetaFlight font, a question mark `?` will be displayed.
+
+This mode can be enabled by selecting BF43COMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI:
+
+`set osd_video_system = BF43COMPAT`
+
+## Limitations
+
+* Canvas size is limited to PAL's canvas size.
+* Unsupported Glyphs show up as `?`
+
+## FAQ
+
+### I see a lot of `?` on my OSD.
+
+That is expected, when your INAV OSD widgets use glyphs that don't have a suitable mapping in BetaFlight's font.
+
+### Does it work with the G2 and Original Air Unit/Vista?
+
+Yes.
+
+### Is this a replacement for WTFOS?
+
+Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than BetaFlight compatibility mode, being able to display all INAV's glyphs.
+
+### Can INAV fix DJI's product?
+
+No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767).
+
+### BetaFlight X.Y now has more symbols, can you update INAV?
+
+Maybe. If a future version of BetaFlight includes more Glyphs that can be mapped into INAV it is fairly simple to add the mapping, but the problem with DJI's implemenation persists. Even if we update the mapping, if DJI does not update the fonts on their side the problem will persist.
+
+### Can you replace glyph `X` with text `x description`?
+
+While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround.
+
+### Does DJI support Canvas Mode?
+
+Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD.

docs/Navigation.md

@@ -169,3 +169,5 @@ wp 12 0 0 0 0 0 0 0 0
 ...
 wp 59 0 0 0 0 0 0 0 0
 ```
+### Changing Mission-Index in flight
+The MISSION CHANGE mode allows to switch between multiple stored missions in flight. With mode active the required mission index can be selected by cycling through missions using the WP mode switch. Selected mission is loaded when mission change mode is switched off. Mission index can also be changed through addition of a new Mission Index adjustment function which should be useful for DJI users unable to use the normal OSD mission related fields.

docs/Programming Framework.md

@@ -80,6 +80,12 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 44            | MAX                           | Finds the highest value of `Operand A` and `Operand B` |
 | 45			| FLIGHT_AXIS_ANGLE_OVERRIDE	| Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
 | 46			| FLIGHT_AXIS_RATE_OVERRIDE	    | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
+| 47            | EDGE                          | `Operand A` is activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
+| 48            | DELAY                         | This will return `true` when `Operand A` is true, and the delay time in `Operand B` [ms] has been exceeded. |
+| 49            | TIMER                         | `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
+| 50            | DELTA                         | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater. |
+| 51            | APPROX_EQUAL                  | `true` if `Operand B` is within 1% of `Operand A`. |
+
 ### Operands
 
 | Operand Type  | Name                  | Notes |
@@ -119,37 +125,21 @@ 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` |
-| 40            | RANGEFINDER_RAW               | integer raw distance provided by the rangefinder in `cm` |
-
-#### ACTIVE_WAYPOINT_ACTION
-
-| Action        | Value |
-|---------------|-------|
-| WAYPOINT      | 1     |
-| HOLD_TIME     | 3     |
-| RTH           | 4     |
-| SET_POI       | 5     |
-| JUMP          | 6     |
-| SET_HEAD      | 7     |
-| LAND          | 8     |
-
 
 #### FLIGHT_MODE
 
@@ -167,6 +157,37 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 9             | USER1     |  |
 | 10            | USER2     |  |
 
+#### 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
+
+| Action        | Value |
+|---------------|-------|
+| WAYPOINT      | 1     |
+| HOLD_TIME     | 3     |
+| RTH           | 4     |
+| SET_POI       | 5     |
+| JUMP          | 6     |
+| SET_HEAD      | 7     |
+| LAND          | 8     |
     
 ### Flags
 

docs/Safety.md

@@ -9,7 +9,7 @@ As many can attest, multirotors and RC models in general can be very dangerous,
 Please consult the [Cli](Cli.md), [Controls](Controls.md), [Failsafe](Failsafe.md) and [Modes](Modes.md) 
 pages for further important information.
 
-You are highly advised to use the Receiver tab in the CleanFlight Configurator, making sure your Rx channel 
+You are highly advised to use the Receiver tab in the INAV Configurator, making sure your Rx channel 
 values are centered at 1500 (1520 for Futaba RC) with minimum & maximums of 1000 and 2000 (respectively) 
 are reached when controls are operated.  Failure to configure these ranges properly can create
 problems, such as inability to arm (because you can't reach the endpoints) or immediate activation of

docs/Settings.md

@@ -3802,6 +3802,16 @@ If set to ON, waypoints will be automatically loaded from EEPROM to the FC durin
 
 ---
 
+### nav_wp_max_safe_distance
+
+First waypoint in the mission should be closer than this value [m]. A value of 0 disables this check.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 1500 |
+
+---
+
 ### nav_wp_mission_restart
 
 Sets restart behaviour for a WP mission when interrupted mid mission. START from first WP, RESUME from last active WP or SWITCH between START and RESUME each time WP Mode is reselected ON. SWITCH effectively allows resuming once only from a previous mid mission waypoint after which the mission will restart from the first waypoint.
@@ -3832,16 +3842,6 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within
 
 ---
 
-### nav_wp_safe_distance
-
-First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 10000 |  | 65000 |
-
----
-
 ### opflow_hardware
 
 Selection of OPFLOW hardware.
@@ -4704,7 +4704,7 @@ IMPERIAL, METRIC, UK
 
 ### osd_video_system
 
-Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO` and 'DJIWTF'
+Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, `DJIWTF` and `BF43COMPAT`
 
 | Default | Min | Max |
 | --- | --- | --- |

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

@@ -2,6 +2,10 @@
 
 ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
 
+# 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 Community
 
 * [INAV Discord Server](https://discord.gg/peg2hhbYwN)

src/main/CMakeLists.txt

@@ -476,6 +476,8 @@ main_sources(COMMON_SRC
     io/displayport_max7456.h
     io/displayport_msp.c
     io/displayport_msp.h
+    io/displayport_msp_bf_compat.c
+    io/displayport_msp_bf_compat.h
     io/displayport_oled.c
     io/displayport_oled.h
     io/displayport_msp_osd.c

src/main/build/debug.h

@@ -71,5 +71,6 @@ typedef enum {
     DEBUG_AUTOTUNE,
     DEBUG_RATE_DYNAMICS,
     DEBUG_LANDING,
+    DEBUG_POS_EST,
     DEBUG_COUNT
 } debugType_e;

src/main/cms/cms_menu_navigation.c

@@ -195,7 +195,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
     OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
     OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
     OSD_SETTING_ENTRY("WP ENFORCE ALTITUDE", SETTING_NAV_WP_ENFORCE_ALTITUDE),
-    OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE),
+    OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_MAX_SAFE_DISTANCE),
 #ifdef USE_MULTI_MISSION
     OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX),
 #endif

src/main/cms/cms_menu_osd.c

@@ -215,6 +215,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("3D SPEED", OSD_3D_SPEED),
     OSD_ELEMENT_ENTRY("PLUS CODE", OSD_PLUS_CODE),
     OSD_ELEMENT_ENTRY("AZIMUTH", OSD_AZIMUTH),
+    OSD_ELEMENT_ENTRY("GRD COURSE", OSD_GROUND_COURSE),
+    OSD_ELEMENT_ENTRY("X TRACK ERR", OSD_CROSS_TRACK_ERROR),
 #endif // GPS
     OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
     OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),

src/main/drivers/accgyro/accgyro_mpu.c

@@ -131,7 +131,7 @@ bool mpuTemperatureReadScratchpad(gyroDev_t *gyro, int16_t * data)
 
     if (ctx->lastReadStatus) {
         // Convert to degC*10: degC = raw / 340 + 36.53
-        *data = int16_val(data, 0) / 34 + 365;
+        *data = int16_val(ctx->tempRaw, 0) / 34 + 365;
         return true;
     }
 

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

@@ -47,7 +47,8 @@ typedef enum {
     VIDEO_SYSTEM_PAL,
     VIDEO_SYSTEM_NTSC,
     VIDEO_SYSTEM_HDZERO,
-    VIDEO_SYSTEM_DJIWTF
+    VIDEO_SYSTEM_DJIWTF,
+    VIDEO_SYSTEM_BFCOMPAT
 } videoSystem_e;
 
 typedef enum {

src/main/drivers/osd_symbols.h

@@ -171,9 +171,13 @@
 #define SYM_AH_V_FT_1               0xD7  // 215 mAh/v-ft right
 #define SYM_AH_V_M_0                0xD8  // 216 mAh/v-m left
 #define SYM_AH_V_M_1                0xD9  // 217 mAh/v-m right
+#define SYM_FLIGHT_MINS_REMAINING   0xDA  // 218 Flight time (mins) remaining
+#define SYM_FLIGHT_HOURS_REMAINING  0xDB  // 219 Flight time (hours) remaining
+#define SYM_GROUND_COURSE           0xDC  // 220 Ground course
+#define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
-#define SYM_LOGO_START              0x101 // 257 to 280, INAV logo
+#define SYM_LOGO_START              0x101 // 257 to 297, INAV logo
-#define SYM_LOGO_WIDTH              6
+#define SYM_LOGO_WIDTH              10
 #define SYM_LOGO_HEIGHT             4
 
 #define SYM_AH_LEFT                 0x12C // 300 Arrow left
@@ -220,6 +224,7 @@
 
 #define SYM_HOME_DIST 	            0x165 // 357 DIST
 #define SYM_AH_CH_CENTER            0x166 // 358 Crossair center
+#define SYM_FLIGHT_DIST_REMAINING   0x167 // 359 Flight distance reminaing
 
 #define SYM_AH_CH_TYPE3             0x190 // 400 to 402, crosshair 3
 #define SYM_AH_CH_TYPE4             0x193 // 403 to 405, crosshair 4

src/main/fc/cli.c

@@ -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

@@ -142,6 +142,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 +166,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 +174,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 +334,18 @@ void readEEPROM(void)
     resumeRxSignal();
 }
 
+void processSaveConfigAndNotify(void)
+{
+    writeEEPROM();
+    readEEPROM();
+    beeperConfirmationBeeps(1);
+    osdShowEEPROMSavedNotification();
+}
+
 void writeEEPROM(void)
 {
     suspendRxSignal();
-
     writeConfigToEEPROM();
-
     resumeRxSignal();
 }
 
@@ -350,11 +363,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 +456,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

@@ -122,7 +122,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 +137,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;
@@ -212,10 +200,10 @@ static void updateArmingStatus(void)
         /* CHECK: Throttle */
         if (!armingConfig()->fixed_wing_auto_arm) {
             // Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
-            if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
+            if (throttleStickIsLow()) {
-                ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
-            } else {
                 DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
+            } else {
+                ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
             }
         }
 
@@ -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
@@ -630,13 +597,13 @@ void processRx(timeUs_t currentTimeUs)
 
     failsafeUpdateState();
 
-    const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
+    const bool throttleIsLow = throttleStickIsLow();
 
     // When armed and motors aren't spinning, do beeps periodically
     if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
         static bool armedBeeperOn = false;
 
-        if (throttleStatus == THROTTLE_LOW) {
+        if (throttleIsLow) {
             beeper(BEEPER_ARMED);
             armedBeeperOn = true;
         } else if (armedBeeperOn) {
@@ -645,7 +612,7 @@ void processRx(timeUs_t currentTimeUs)
         }
     }
 
-    processRcStickPositions(throttleStatus);
+    processRcStickPositions(throttleIsLow);
     processAirmode();
     updateActivatedModes();
 
@@ -759,7 +726,7 @@ void processRx(timeUs_t currentTimeUs)
         pidResetErrorAccumulators();
     }
     else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
-        if (throttleStatus == THROTTLE_LOW) {
+        if (throttleIsLow) {
              if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
                  if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
                      ENABLE_STATE(ANTI_WINDUP);
@@ -778,7 +745,7 @@ void processRx(timeUs_t currentTimeUs)
          }
     }
     else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
-        if (throttleStatus == THROTTLE_LOW) {
+        if (throttleIsLow) {
              if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
                  if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
                      ENABLE_STATE(ANTI_WINDUP);
@@ -802,7 +769,7 @@ void processRx(timeUs_t currentTimeUs)
     else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
          DISABLE_STATE(ANTI_WINDUP);
          //This case applies only to MR when Airmode management is throttle threshold activated
-         if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) {
+         if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
              pidResetErrorAccumulators();
          }
      }
@@ -868,8 +835,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         armTime += cycleTime;
         updateAccExtremes();
     }
+
     if (!ARMING_FLAG(ARMED)) {
         armTime = 0;
+        
+        processDelayedSave();
     }
 
     gyroFilter();

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

@@ -1529,6 +1529,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     return true;
 }
 
+#ifdef USE_SAFE_HOME
 static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
 {
     const uint8_t safe_home_no = sbufReadU8(src);    // get the home number
@@ -1542,6 +1543,8 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
          return MSP_RESULT_ERROR;
     }
 }
+#endif
+
 
 static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
     const uint8_t idx = sbufReadU8(src);
@@ -2919,6 +2922,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         return MSP_RESULT_ERROR; // will only be reached if the rollback is not ready
         break;
 #endif
+#ifdef USE_SAFE_HOME
     case MSP2_INAV_SET_SAFEHOME:
         if (dataSize == 10) {
              uint8_t i;
@@ -2932,6 +2936,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             return MSP_RESULT_ERROR;
         }
         break;
+#endif
 
     default:
         return MSP_RESULT_ERROR;
@@ -3377,9 +3382,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         *ret = mspFcLogicConditionCommand(dst, src);
         break;
 #endif
+#ifdef USE_SAFE_HOME
     case MSP2_INAV_SAFEHOME:
         *ret = mspFcSafeHomeOutCommand(dst, src);
         break;
+#endif
 
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:

src/main/fc/rc_adjustments.c

@@ -574,7 +574,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             navigationUsePIDs();
             break;
         case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
-            applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &currentBatteryProfileMutable->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
+            applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
             break;
 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
         case ADJUSTMENT_VTX_POWER_LEVEL:

src/main/fc/rc_controls.c

@@ -111,22 +111,25 @@ bool isRollPitchStickDeflected(uint8_t deadband)
 
 throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
 {
-    int value;
+    int value = rxGetChannelValue(THROTTLE);    // THROTTLE_STATUS_TYPE_RC
-    if (type == THROTTLE_STATUS_TYPE_RC) {
+    if (type == THROTTLE_STATUS_TYPE_COMMAND) {
-        value = rxGetChannelValue(THROTTLE);
-    } else {
         value = rcCommand[THROTTLE];
     }
 
     const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
-    if (feature(FEATURE_REVERSIBLE_MOTORS) && (value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
+    bool midThrottle = value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband);
-        return THROTTLE_LOW;
+    if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) {
-    else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))
         return THROTTLE_LOW;
+    }
 
     return THROTTLE_HIGH;
 }
 
+bool throttleStickIsLow(void)
+{
+    return calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
+}
+
 int16_t throttleStickMixedValue(void)
 {
     int16_t throttleValue;
@@ -181,7 +184,7 @@ static void updateRcStickPositions(void)
     rcStickPositions = tmp;
 }
 
-void processRcStickPositions(throttleStatus_e throttleStatus)
+void processRcStickPositions(bool isThrottleLow)
 {
     static timeMs_t lastTickTimeMs = 0;
     static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@@ -207,11 +210,10 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 
     // 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
-        if (throttleStatus != THROTTLE_LOW) {
+        if (!isThrottleLow) {
             tryArm();
             return;
         }
@@ -221,13 +223,14 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
             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
             if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
                 const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
                 if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
-                    if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
+                    if (armingConfig()->disarm_kill_switch || isThrottleLow) {
                         disarm(DISARM_SWITCH);
                     }
                 }

src/main/fc/rc_controls.h

@@ -87,7 +87,7 @@ typedef struct rcControlsConfig_s {
     uint8_t pos_hold_deadband;              // Deadband for position hold
     uint8_t control_deadband;               // General deadband to check if sticks are deflected, us PWM.
     uint8_t alt_hold_deadband;              // Defines the neutral zone of throttle stick during altitude hold
-    uint16_t mid_throttle_deadband;           // default throttle deadband from MIDRC
+    uint16_t mid_throttle_deadband;         // default throttle deadband from MIDRC
     uint8_t airmodeHandlingType;            // Defaults to ANTI_WINDUP triggered at sticks centered
     uint16_t airmodeThrottleThreshold;      // Throttle threshold for airmode initial activation
 } rcControlsConfig_t;
@@ -95,8 +95,8 @@ typedef struct rcControlsConfig_s {
 PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
 
 typedef struct armingConfig_s {
-    bool fixed_wing_auto_arm;            // Auto-arm fixed wing aircraft on throttle up and never disarm
+    bool fixed_wing_auto_arm;               // Auto-arm fixed wing aircraft on throttle up and never disarm
-    bool disarm_kill_switch;             // allow disarm via AUX switch regardless of throttle value
+    bool disarm_kill_switch;                // allow disarm via AUX switch regardless of throttle value
     uint16_t switchDisarmDelayMs;           // additional delay between ARM box going off and actual disarm
     uint16_t prearmTimeoutMs;               // duration for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout.
 } armingConfig_t;
@@ -112,6 +112,7 @@ bool isRollPitchStickDeflected(uint8_t deadband);
 throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
 int16_t throttleStickMixedValue(void);
 rollPitchStatus_e calculateRollPitchCenterStatus(void);
-void processRcStickPositions(throttleStatus_e throttleStatus);
+void processRcStickPositions(bool isThrottleLow);
+bool throttleStickIsLow(void);
 
 int32_t getRcStickDeflection(int32_t axis);

src/main/fc/runtime_config.c

@@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= {
     "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS",
     "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX",
     "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM",
-    "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER"
+    "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER", "LANDED"
 };
 #endif
 
@@ -178,8 +178,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
 }
 
 #ifdef USE_SIMULATOR
-simulatorData_t simulatorData = { 
+simulatorData_t simulatorData = {
-	flags: 0, 
+	flags: 0,
 	debugIndex: 0
 };
 #endif

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"]
+    values: ["AUTO", "PAL", "NTSC", "HDZERO", "DJIWTF", "BF43COMPAT"]
     enum: videoSystem_e
   - name: osd_telemetry
     values: ["OFF", "ON","TEST"]
@@ -91,7 +91,7 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING"]
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e
@@ -1048,12 +1048,6 @@ groups:
         default_value: 1000
         min: PWM_RANGE_MIN
         max: PWM_RANGE_MAX
-      - name: fw_min_throttle_down_pitch
-        description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
-        default_value: 0
-        field: fwMinThrottleDownPitchAngle
-        min: 0
-        max: 450
       - name: nav_mc_hover_thr
         description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
         default_value: 1500
@@ -1833,11 +1827,6 @@ groups:
         field: fixedWingItermThrowLimit
         min: FW_ITERM_THROW_LIMIT_MIN
         max: FW_ITERM_THROW_LIMIT_MAX
-      - name: fw_loiter_direction
-        description: "Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick."
-        default_value: "RIGHT"
-        field: loiter_direction
-        table: direction
       - name: fw_reference_airspeed
         description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present."
         default_value: 1500
@@ -2354,11 +2343,12 @@ groups:
         field: general.waypoint_enforce_altitude
         min: 0
         max: 2000
-      - name: nav_wp_safe_distance
+      - name: nav_wp_max_safe_distance
-        description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check."
+        description: "First waypoint in the mission should be closer than this value [m]. A value of 0 disables this check."
-        default_value: 10000
+        default_value: 100
         field: general.waypoint_safe_distance
-        max: 65000
+        min: 0
+        max: 1500
       - name: nav_wp_mission_restart
         description: "Sets restart behaviour for a WP mission when interrupted mid mission. START from first WP, RESUME from last active WP or SWITCH between START and RESUME each time WP Mode is reselected ON. SWITCH effectively allows resuming once only from a previous mid mission waypoint after which the mission will restart from the first waypoint."
         default_value: "RESUME"
@@ -2674,6 +2664,12 @@ groups:
         field: fw.pitch_to_throttle_smooth
         min: 0
         max: 9
+      - name: fw_min_throttle_down_pitch
+        description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
+        default_value: 0
+        field: fw.minThrottleDownPitchAngle
+        min: 0
+        max: 450
       - name: nav_fw_pitch2thr_threshold
         description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]"
         default_value: 50
@@ -2686,6 +2682,11 @@ groups:
         field: fw.loiter_radius
         min: 0
         max: 30000
+      - name: fw_loiter_direction
+        description: "Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick."
+        default_value: "RIGHT"
+        field: fw.loiter_direction
+        table: direction
       - name: nav_fw_cruise_speed
         description: "Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s"
         default_value: 0
@@ -3014,7 +3015,7 @@ groups:
         type: uint8_t
         default_value: "OFF"
       - name: osd_video_system
-        description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO` and 'DJIWTF'"
+        description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, `DJIWTF` and `BF43COMPAT`"
         default_value: "AUTO"
         table: osd_video_system
         field: video_system

src/main/fc/stats.c

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

src/main/flight/failsafe.c

@@ -387,7 +387,7 @@ void failsafeUpdateState(void)
             case FAILSAFE_IDLE:
                 if (armed) {
                     // Track throttle command below minimum time
-                    if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
+                    if (!throttleStickIsLow()) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
                     if (!receivingRxDataAndNotFailsafeMode) {

src/main/flight/mixer.c

@@ -626,11 +626,8 @@ motorStatus_e getMotorStatus(void)
     }
 
     const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
-    const bool throttleStickLow =
-        (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW);
-
-    if (throttleStickLow && fixedWingOrAirmodeNotActive) {
 
+    if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) {
         if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) {
             // If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS
             // and either on a plane or on a quad with inactive airmode - stop motor
@@ -652,7 +649,6 @@ motorStatus_e getMotorStatus(void)
                     return MOTOR_STOPPED_USER;
             }
         }
-
     }
 
     return MOTOR_RUNNING;

src/main/flight/pid.c

@@ -171,7 +171,7 @@ static EXTENDED_FASTRAM bool levelingEnabled = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 4);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 5);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -277,7 +277,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT,
         .fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
 
-        .loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
         .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
         .navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT,
         .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT,
@@ -596,7 +595,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) {
 
     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
-        angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, currentBatteryProfile->fwMinThrottleDownPitchAngle);
+        angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
     }
 
     //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming

src/main/flight/pid.h

@@ -132,8 +132,7 @@ typedef struct pidProfile_s {
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
     uint16_t    fixedWingYawItermBankFreeze;       // Freeze yaw Iterm when bank angle is more than this many degrees
-
+    
-    uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
     float       navVelXyDTermLpfHz;
     uint8_t navVelXyDtermAttenuation;       // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
     uint8_t navVelXyDtermAttenuationStart;  // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity

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/io/bf_osd_symbols.h

@@ -0,0 +1,163 @@
+/* @file max7456_symbols.h
+ * @brief max7456 symbols for the mwosd font set
+ *
+ * @author Nathan Tsoi nathan@vertile.com
+ *
+ * Copyright (C) 2016 Nathan Tsoi
+ *
+ * This program 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>
+ */
+
+#pragma once
+
+//Misc
+#define BF_SYM_NONE                    0x00
+#define BF_SYM_END_OF_FONT             0xFF
+#define BF_SYM_BLANK                   0x20
+#define BF_SYM_HYPHEN                  0x2D
+#define BF_SYM_BBLOG                   0x10
+#define BF_SYM_HOMEFLAG                0x11
+//#define SYM_RPM                     0x12
+#define BF_SYM_ROLL                    0x14
+#define BF_SYM_PITCH                   0x15
+#define BF_SYM_TEMPERATURE             0x7A
+
+// GPS and navigation
+#define BF_SYM_LAT                     0x89
+#define BF_SYM_LON                     0x98
+#define BF_SYM_ALTITUDE                0x7F
+#define BF_SYM_TOTAL_DISTANCE          0x71
+#define BF_SYM_OVER_HOME               0x05
+
+// RSSI
+#define BF_SYM_RSSI                    0x01
+#define BF_SYM_LINK_QUALITY            0x7B
+
+// Throttle Position (%)
+#define BF_SYM_THR                     0x04
+
+// Unit Icons (Metric)
+#define BF_SYM_M                       0x0C
+#define BF_SYM_KM                      0x7D
+#define BF_SYM_C                       0x0E
+
+// Unit Icons (Imperial)
+#define BF_SYM_FT                      0x0F
+#define BF_SYM_MILES                   0x7E
+#define BF_SYM_F                       0x0D
+
+// Heading Graphics
+#define BF_SYM_HEADING_N               0x18
+#define BF_SYM_HEADING_S               0x19
+#define BF_SYM_HEADING_E               0x1A
+#define BF_SYM_HEADING_W               0x1B
+#define BF_SYM_HEADING_DIVIDED_LINE    0x1C
+#define BF_SYM_HEADING_LINE            0x1D
+
+// AH Center screen Graphics
+#define BF_SYM_AH_CENTER_LINE          0x72
+#define BF_SYM_AH_CENTER               0x73
+#define BF_SYM_AH_CENTER_LINE_RIGHT    0x74
+#define BF_SYM_AH_RIGHT                0x02
+#define BF_SYM_AH_LEFT                 0x03
+#define BF_SYM_AH_DECORATION           0x13
+
+// Satellite Graphics
+#define BF_SYM_SAT_L                   0x1E
+#define BF_SYM_SAT_R                   0x1F
+
+// Direction arrows
+#define BF_SYM_ARROW_SOUTH             0x60
+#define BF_SYM_ARROW_2                 0x61
+#define BF_SYM_ARROW_3                 0x62
+#define BF_SYM_ARROW_4                 0x63
+#define BF_SYM_ARROW_EAST              0x64
+#define BF_SYM_ARROW_6                 0x65
+#define BF_SYM_ARROW_7                 0x66
+#define BF_SYM_ARROW_8                 0x67
+#define BF_SYM_ARROW_NORTH             0x68
+#define BF_SYM_ARROW_10                0x69
+#define BF_SYM_ARROW_11                0x6A
+#define BF_SYM_ARROW_12                0x6B
+#define BF_SYM_ARROW_WEST              0x6C
+#define BF_SYM_ARROW_14                0x6D
+#define BF_SYM_ARROW_15                0x6E
+#define BF_SYM_ARROW_16                0x6F
+
+#define BF_SYM_ARROW_SMALL_UP          0x75
+#define BF_SYM_ARROW_SMALL_DOWN        0x76
+
+// AH Bars
+#define BF_SYM_AH_BAR9_0               0x80
+#define BF_SYM_AH_BAR9_1               0x81
+#define BF_SYM_AH_BAR9_2               0x82
+#define BF_SYM_AH_BAR9_3               0x83
+#define BF_SYM_AH_BAR9_4               0x84
+#define BF_SYM_AH_BAR9_5               0x85
+#define BF_SYM_AH_BAR9_6               0x86
+#define BF_SYM_AH_BAR9_7               0x87
+#define BF_SYM_AH_BAR9_8               0x88
+
+// Progress bar
+#define BF_SYM_PB_START                0x8A
+#define BF_SYM_PB_FULL                 0x8B
+#define BF_SYM_PB_HALF                 0x8C
+#define BF_SYM_PB_EMPTY                0x8D
+#define BF_SYM_PB_END                  0x8E
+#define BF_SYM_PB_CLOSE                0x8F
+
+// Batt evolution
+#define BF_SYM_BATT_FULL               0x90
+#define BF_SYM_BATT_5                  0x91
+#define BF_SYM_BATT_4                  0x92
+#define BF_SYM_BATT_3                  0x93
+#define BF_SYM_BATT_2                  0x94
+#define BF_SYM_BATT_1                  0x95
+#define BF_SYM_BATT_EMPTY              0x96
+
+// Batt Icons
+#define BF_SYM_MAIN_BATT               0x97
+
+// Voltage and amperage
+#define BF_SYM_VOLT                    0x06
+#define BF_SYM_AMP                     0x9A
+#define BF_SYM_MAH                     0x07
+#define BF_SYM_WATT                    0x57  // 0x57 is 'W'
+
+// Time
+#define BF_SYM_ON_M                    0x9B
+#define BF_SYM_FLY_M                   0x9C
+
+// Speed
+#define BF_SYM_SPEED                   0x70
+#define BF_SYM_KPH                     0x9E
+#define BF_SYM_MPH                     0x9D
+#define BF_SYM_MPS                     0x9F
+#define BF_SYM_FTPS                    0x99
+
+// Menu cursor
+#define BF_SYM_CURSOR                  BF_SYM_AH_LEFT
+
+// Stick overlays
+#define BF_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
+#define BF_SYM_STICK_OVERLAY_SPRITE_MID  0x09
+#define BF_SYM_STICK_OVERLAY_SPRITE_LOW  0x0A
+#define BF_SYM_STICK_OVERLAY_CENTER      0x0B
+#define BF_SYM_STICK_OVERLAY_VERTICAL    0x16
+#define BF_SYM_STICK_OVERLAY_HORIZONTAL  0x17
+
+// GPS degree/minute/second symbols
+#define BF_SYM_GPS_DEGREE              BF_SYM_STICK_OVERLAY_SPRITE_HIGH  // kind of looks like the degree symbol
+#define BF_SYM_GPS_MINUTE              0x27 // '
+#define BF_SYM_GPS_SECOND              0x22 // "

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

@@ -27,4 +27,4 @@
 #define MSP_DP_DRAW_SCREEN  4
 
 struct displayPort_s;
-struct displayPort_s *displayPortMspInit(void);
+struct displayPort_s *displayPortMspInit(void);

src/main/io/displayport_msp_bf_compat.c

@@ -0,0 +1,686 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * 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.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_MSP_DISPLAYPORT
+
+#ifndef DISABLE_MSP_BF_COMPAT
+
+#include "io/displayport_msp_bf_compat.h"
+#include "io/bf_osd_symbols.h"
+#include "drivers/osd_symbols.h"
+
+uint8_t getBfCharacter(uint8_t ch, uint8_t page)
+{
+    uint16_t ech = ch | (page << 8);
+
+    if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
+        return ch;
+    }
+
+    switch (ech) {
+    case SYM_RSSI:
+        return BF_SYM_RSSI;
+
+    case SYM_LQ:
+        return BF_SYM_LINK_QUALITY;
+
+    case SYM_LAT:
+        return BF_SYM_LAT;
+
+    case SYM_LON:
+        return BF_SYM_LON;
+
+    case SYM_SAT_L:
+        return BF_SYM_SAT_L;
+
+    case SYM_SAT_R:
+        return BF_SYM_SAT_R;
+
+    case SYM_HOME_NEAR:
+        return BF_SYM_HOMEFLAG;
+
+    case SYM_DEGREES:
+        return BF_SYM_GPS_DEGREE;
+
+/*
+    case SYM_HEADING:
+        return BF_SYM_HEADING;
+
+    case SYM_SCALE:
+        return BF_SYM_SCALE;
+
+    case SYM_HDP_L:
+        return BF_SYM_HDP_L;
+
+    case SYM_HDP_R:
+        return BF_SYM_HDP_R;
+*/
+    case SYM_HOME:
+        return BF_SYM_HOMEFLAG;
+
+    case SYM_2RSS:
+        return BF_SYM_RSSI;
+
+/*
+    case SYM_DB:
+        return BF_SYM_DB
+
+    case SYM_DBM:
+        return BF_SYM_DBM;
+
+    case SYM_SNR:
+        return BF_SYM_SNR;
+
+    case SYM_AH_DECORATION_UP:
+        return BF_SYM_AH_DECORATION_UP;
+
+    case SYM_AH_DECORATION_DOWN:
+        return BF_SYM_AH_DECORATION_DOWN;
+
+    case SYM_DIRECTION:
+        return BF_SYM_DIRECTION;
+*/
+    case SYM_VOLT:
+        return BF_SYM_VOLT;
+
+    case SYM_MAH:
+        return BF_SYM_MAH;
+
+    case SYM_AH_KM:
+        return BF_SYM_KM;
+
+    case SYM_AH_MI:
+        return BF_SYM_MILES;
+/*
+    case SYM_VTX_POWER:
+        return BF_SYM_VTX_POWER;
+
+    case SYM_AH_NM:
+        return BF_SYM_AH_NM;
+
+    case SYM_MAH_NM_0:
+        return BF_SYM_MAH_NM_0;
+
+    case SYM_MAH_NM_1:
+        return BF_SYM_MAH_NM_1;
+
+    case SYM_MAH_KM_0:
+        return BF_SYM_MAH_KM_0;
+
+    case SYM_MAH_KM_1:
+        return BF_SYM_MAH_KM_1;
+
+    case SYM_MILLIOHM:
+        return BF_SYM_MILLIOHM;
+*/
+    case SYM_BATT_FULL:
+        return BF_SYM_BATT_FULL;
+
+    case SYM_BATT_5:
+        return BF_SYM_BATT_5;
+
+    case SYM_BATT_4:
+        return BF_SYM_BATT_4;
+
+    case SYM_BATT_3:
+        return BF_SYM_BATT_3;
+
+    case SYM_BATT_2:
+        return BF_SYM_BATT_2;
+
+    case SYM_BATT_1:
+        return BF_SYM_BATT_1;
+
+    case SYM_BATT_EMPTY:
+        return BF_SYM_BATT_EMPTY;
+
+    case SYM_AMP:
+        return BF_SYM_AMP;
+/*
+    case SYM_WH:
+        return BF_SYM_WH;
+
+    case SYM_WH_KM:
+        return BF_SYM_WH_KM;
+
+    case SYM_WH_MI:
+        return BF_SYM_WH_MI;
+
+    case SYM_WH_NM:
+        return BF_SYM_WH_NM;
+*/
+    case SYM_WATT:
+        return BF_SYM_WATT;
+/*
+    case SYM_MW:
+        return BF_SYM_MW;
+
+    case SYM_KILOWATT:
+        return BF_SYM_KILOWATT;
+*/
+    case SYM_FT:
+        return BF_SYM_FT;
+/*
+    case SYM_TRIP_DIST:
+        return BF_SYM_TRIP_DIST;
+
+    case SYM_TOTAL:
+        return BF_SYM_TOTAL;
+
+    case SYM_ALT_M:
+        return BF_SYM_ALT_M;
+
+    case SYM_ALT_KM:
+        return BF_SYM_ALT_KM;
+
+    case SYM_ALT_FT:
+        return BF_SYM_ALT_FT;
+
+    case SYM_ALT_KFT:
+        return BF_SYM_ALT_KFT;
+
+    case SYM_DIST_M:
+        return BF_SYM_DIST_M;
+
+    case SYM_DIST_KM:
+        return BF_SYM_DIST_KM;
+
+    case SYM_DIST_FT:
+        return BF_SYM_DIST_FT;
+
+    case SYM_DIST_MI:
+        return BF_SYM_DIST_MI;
+
+    case SYM_DIST_NM:
+        return BF_SYM_DIST_NM;
+*/
+    case SYM_M:
+        return BF_SYM_M;
+
+    case SYM_KM:
+        return BF_SYM_KM;
+
+    case SYM_MI:
+        return BF_SYM_MILES;
+/*
+    case SYM_NM:
+        return BF_SYM_NM;
+
+    case SYM_WIND_HORIZONTAL:
+        return BF_SYM_WIND_HORIZONTAL;
+
+    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_FTS:
+        return BF_SYM_FTPS;
+/*
+    case SYM_100FTM:
+        return BF_SYM_100FTM;
+*/
+    case SYM_MS:
+        return BF_SYM_MPS;
+
+    case SYM_KMH:
+        return BF_SYM_KPH;
+
+    case SYM_MPH:
+        return BF_SYM_MPH;
+/*
+    case SYM_KT:
+        return BF_SYM_KT
+
+    case SYM_MAH_MI_0:
+        return BF_SYM_MAH_MI_0;
+
+    case SYM_MAH_MI_1:
+        return BF_SYM_MAH_MI_1;
+*/
+    case SYM_THR:
+        return BF_SYM_THR;
+
+/*
+    case SYM_TEMP_F:
+        return BF_SYM_TEMP_F;
+
+    case SYM_TEMP_C:
+        return BF_SYM_TEMP_C;
+*/
+    case SYM_BLANK:
+        return BF_SYM_BLANK;
+/*
+    case SYM_ON_H:
+        return BF_SYM_ON_H;
+
+    case SYM_FLY_H:
+        return BF_SYM_FLY_H;
+*/
+    case SYM_ON_M:
+        return BF_SYM_ON_M;
+
+    case SYM_FLY_M:
+        return BF_SYM_FLY_M;
+/*
+    case SYM_GLIDESLOPE:
+        return BF_SYM_GLIDESLOPE;
+
+    case SYM_WAYPOINT:
+        return BF_SYM_WAYPOINT;
+
+    case SYM_CLOCK:
+        return BF_SYM_CLOCK;
+
+    case SYM_ZERO_HALF_TRAILING_DOT:
+        return BF_SYM_ZERO_HALF_TRAILING_DOT;
+
+    case SYM_ZERO_HALF_LEADING_DOT:
+        return BF_SYM_ZERO_HALF_LEADING_DOT;
+
+    case SYM_AUTO_THR0:
+        return BF_SYM_AUTO_THR0;
+
+    case SYM_AUTO_THR1:
+        return BF_SYM_AUTO_THR1;
+
+    case SYM_ROLL_LEFT:
+        return BF_SYM_ROLL_LEFT;
+
+    case SYM_ROLL_LEVEL:
+        return BF_SYM_ROLL_LEVEL;
+
+    case SYM_ROLL_RIGHT:
+        return BF_SYM_ROLL_RIGHT;
+
+    case SYM_PITCH_UP:
+        return BF_SYM_PITCH_UP;
+
+    case SYM_PITCH_DOWN:
+        return BF_SYM_PITCH_DOWN;
+
+    case SYM_GFORCE:
+        return BF_SYM_GFORCE;
+
+    case SYM_GFORCE_X:
+        return BF_SYM_GFORCE_X;
+
+    case SYM_GFORCE_Y:
+        return BF_SYM_GFORCE_Y;
+
+    case SYM_GFORCE_Z:
+        return BF_SYM_GFORCE_Z;
+
+    case SYM_BARO_TEMP:
+        return BF_SYM_BARO_TEMP;
+
+    case SYM_IMU_TEMP:
+        return BF_SYM_IMU_TEMP;
+
+    case SYM_TEMP:
+        return BF_SYM_TEMP;
+
+    case SYM_TEMP_SENSOR_FIRST:
+        return BF_SYM_TEMP_SENSOR_FIRST;
+
+    case SYM_ESC_TEMP:
+        return BF_SYM_ESC_TEMP;
+
+    case SYM_TEMP_SENSOR_LAST:
+        return BF_SYM_TEMP_SENSOR_LAST;
+
+    case TEMP_SENSOR_SYM_COUNT:
+        return BF_TEMP_SENSOR_SYM_COUNT;
+*/
+    case SYM_HEADING_N:
+        return BF_SYM_HEADING_N;
+
+    case SYM_HEADING_S:
+        return BF_SYM_HEADING_S;
+
+    case SYM_HEADING_E:
+        return BF_SYM_HEADING_E;
+
+    case SYM_HEADING_W:
+        return BF_SYM_HEADING_W;
+
+    case SYM_HEADING_DIVIDED_LINE:
+        return BF_SYM_HEADING_DIVIDED_LINE;
+
+    case SYM_HEADING_LINE:
+        return BF_SYM_HEADING_LINE;
+/*
+    case SYM_MAX:
+        return BF_SYM_MAX;
+
+    case SYM_PROFILE:
+        return BF_SYM_PROFILE;
+
+    case SYM_SWITCH_INDICATOR_LOW:
+        return BF_SYM_SWITCH_INDICATOR_LOW;
+
+    case SYM_SWITCH_INDICATOR_MID:
+        return BF_SYM_SWITCH_INDICATOR_MID;
+
+    case SYM_SWITCH_INDICATOR_HIGH:
+        return BF_SYM_SWITCH_INDICATOR_HIGH;
+
+    case SYM_AH:
+        return BF_SYM_AH;
+
+    case SYM_GLIDE_DIST:
+        return BF_SYM_GLIDE_DIST;
+
+    case SYM_GLIDE_MINS:
+        return BF_SYM_GLIDE_MINS;
+
+    case SYM_AH_V_FT_0:
+        return BF_SYM_AH_V_FT_0;
+
+    case SYM_AH_V_FT_1:
+        return BF_SYM_AH_V_FT_1;
+
+    case SYM_AH_V_M_0:
+        return BF_SYM_AH_V_M_0;
+
+    case SYM_AH_V_M_1:
+        return BF_SYM_AH_V_M_1;
+
+    case SYM_FLIGHT_MINS_REMAINING:
+        return BF_SYM_FLIGHT_MINS_REMAINING;
+
+    case SYM_FLIGHT_HOURS_REMAINING:
+        return BF_SYM_FLIGHT_HOURS_REMAINING;
+
+    case SYM_GROUND_COURSE:
+        return BF_SYM_GROUND_COURSE;
+
+    case SYM_CROSS_TRACK_ERROR:
+        return BF_SYM_CROSS_TRACK_ERROR;
+
+    case SYM_LOGO_START:
+        return BF_SYM_LOGO_START;
+
+    case SYM_LOGO_WIDTH:
+        return BF_SYM_LOGO_WIDTH;
+
+    case SYM_LOGO_HEIGHT:
+        return BF_SYM_LOGO_HEIGHT;
+*/
+    case SYM_AH_LEFT:
+        return BF_SYM_AH_LEFT;
+
+    case SYM_AH_RIGHT:
+        return BF_SYM_AH_RIGHT;
+/*
+    case SYM_AH_DECORATION_MIN:
+        return BF_SYM_AH_DECORATION_MIN;
+*/
+    case SYM_AH_DECORATION:
+        return BF_SYM_AH_DECORATION;
+/*
+    case SYM_AH_DECORATION_MAX:
+        return BF_SYM_AH_DECORATION_MAX;
+
+    case SYM_AH_DECORATION_COUNT:
+        return BF_SYM_AH_DECORATION_COUNT;
+*/
+    case SYM_AH_CH_LEFT:
+    case SYM_AH_CH_TYPE3:
+    case SYM_AH_CH_TYPE4:
+    case SYM_AH_CH_TYPE5:
+    case SYM_AH_CH_TYPE6:
+    case SYM_AH_CH_TYPE7:
+    case SYM_AH_CH_TYPE8:
+    case SYM_AH_CH_AIRCRAFT1:
+        return BF_SYM_AH_CENTER_LINE;
+
+    case SYM_AH_CH_RIGHT:
+    case (SYM_AH_CH_TYPE3+2):
+    case (SYM_AH_CH_TYPE4+2):
+    case (SYM_AH_CH_TYPE5+2):
+    case (SYM_AH_CH_TYPE6+2):
+    case (SYM_AH_CH_TYPE7+2):
+    case (SYM_AH_CH_TYPE8+2):
+    case SYM_AH_CH_AIRCRAFT3:
+        return BF_SYM_AH_CENTER_LINE_RIGHT;
+    
+    case SYM_AH_CH_AIRCRAFT0:
+    case SYM_AH_CH_AIRCRAFT4:
+        return ' ';
+
+    case SYM_ARROW_UP:
+        return BF_SYM_ARROW_NORTH;
+
+    case SYM_ARROW_2:
+        return BF_SYM_ARROW_8;
+
+    case SYM_ARROW_3:
+        return BF_SYM_ARROW_7;
+
+    case SYM_ARROW_4:
+        return BF_SYM_ARROW_6;
+
+    case SYM_ARROW_RIGHT:
+        return BF_SYM_ARROW_EAST;
+
+    case SYM_ARROW_6:
+        return BF_SYM_ARROW_4;
+
+    case SYM_ARROW_7:
+        return BF_SYM_ARROW_3;
+
+    case SYM_ARROW_8:
+        return BF_SYM_ARROW_2;
+
+    case SYM_ARROW_DOWN:
+        return BF_SYM_ARROW_SOUTH;
+
+    case SYM_ARROW_10:
+        return BF_SYM_ARROW_16;
+
+    case SYM_ARROW_11:
+        return BF_SYM_ARROW_15;
+
+    case SYM_ARROW_12:
+        return BF_SYM_ARROW_14;
+
+    case SYM_ARROW_LEFT:
+        return BF_SYM_ARROW_WEST;
+
+    case SYM_ARROW_14:
+        return BF_SYM_ARROW_12;
+
+    case SYM_ARROW_15:
+        return BF_SYM_ARROW_11;
+
+    case SYM_ARROW_16:
+        return BF_SYM_ARROW_10;
+
+    case SYM_AH_H_START:
+        return BF_SYM_AH_BAR9_0;
+
+    case (SYM_AH_H_START+1):
+        return BF_SYM_AH_BAR9_1;
+
+    case (SYM_AH_H_START+2):
+        return BF_SYM_AH_BAR9_2;
+
+    case (SYM_AH_H_START+3):
+        return BF_SYM_AH_BAR9_3;
+
+    case (SYM_AH_H_START+4):
+        return BF_SYM_AH_BAR9_4;
+
+    case (SYM_AH_H_START+5):
+        return BF_SYM_AH_BAR9_5;
+
+    case (SYM_AH_H_START+6):
+        return BF_SYM_AH_BAR9_6;
+
+    case (SYM_AH_H_START+7):
+        return BF_SYM_AH_BAR9_7;
+
+    case (SYM_AH_H_START+8):
+        return BF_SYM_AH_BAR9_8;
+
+/*
+    case SYM_AH_V_START:
+        return BF_SYM_AH_V_START;
+
+    case SYM_VARIO_UP_2A:
+        return BF_SYM_VARIO_UP_2A;
+
+    case SYM_VARIO_UP_1A:
+        return BF_SYM_VARIO_UP_1A;
+
+    case SYM_VARIO_DOWN_1A:
+        return BF_SYM_VARIO_DOWN_1A;
+
+    case SYM_VARIO_DOWN_2A:
+        return BF_SYM_VARIO_DOWN_2A;
+*/
+    case SYM_ALT:
+        return BF_SYM_ALTITUDE;
+/*
+    case SYM_HUD_SIGNAL_0:
+        return BF_SYM_HUD_SIGNAL_0;
+
+    case SYM_HUD_SIGNAL_1:
+        return BF_SYM_HUD_SIGNAL_1;
+
+    case SYM_HUD_SIGNAL_2:
+        return BF_SYM_HUD_SIGNAL_2;
+
+    case SYM_HUD_SIGNAL_3:
+        return BF_SYM_HUD_SIGNAL_3;
+
+    case SYM_HUD_SIGNAL_4:
+        return BF_SYM_HUD_SIGNAL_4;
+
+    case SYM_HOME_DIST:
+        return BF_SYM_HOME_DIST;
+*/
+
+    case SYM_AH_CH_CENTER:
+    case (SYM_AH_CH_TYPE3+1):
+    case (SYM_AH_CH_TYPE4+1):
+    case (SYM_AH_CH_TYPE5+1):
+    case (SYM_AH_CH_TYPE6+1):
+    case (SYM_AH_CH_TYPE7+1):
+    case (SYM_AH_CH_TYPE8+1):
+    case SYM_AH_CH_AIRCRAFT2:
+        return BF_SYM_AH_CENTER;
+/*
+    case SYM_FLIGHT_DIST_REMAINING:
+        return BF_SYM_FLIGHT_DIST_REMAINING;
+
+    case SYM_AH_CH_TYPE3:
+        return BF_SYM_AH_CH_TYPE3;
+
+    case SYM_AH_CH_TYPE4:
+        return BF_SYM_AH_CH_TYPE4;
+
+    case SYM_AH_CH_TYPE5:
+        return BF_SYM_AH_CH_TYPE5;
+
+    case SYM_AH_CH_TYPE6:
+        return BF_SYM_AH_CH_TYPE6;
+
+    case SYM_AH_CH_TYPE7:
+        return BF_SYM_AH_CH_TYPE7;
+
+    case SYM_AH_CH_TYPE8:
+        return BF_SYM_AH_CH_TYPE8;
+
+    case SYM_AH_CH_AIRCRAFT0:
+        return BF_SYM_AH_CH_AIRCRAFT0;
+
+    case SYM_AH_CH_AIRCRAFT1:
+        return BF_SYM_AH_CH_AIRCRAFT1;
+
+    case SYM_AH_CH_AIRCRAFT2:
+        return BF_SYM_AH_CH_AIRCRAFT2;
+
+    case SYM_AH_CH_AIRCRAFT3:
+        return BF_SYM_AH_CH_AIRCRAFT3;
+
+    case SYM_AH_CH_AIRCRAFT4:
+        return BF_SYM_AH_CH_AIRCRAFT4;
+
+    case SYM_HUD_ARROWS_L1:
+        return BF_SYM_HUD_ARROWS_L1;
+
+    case SYM_HUD_ARROWS_L2:
+        return BF_SYM_HUD_ARROWS_L2;
+
+    case SYM_HUD_ARROWS_L3:
+        return BF_SYM_HUD_ARROWS_L3;
+
+    case SYM_HUD_ARROWS_R1:
+        return BF_SYM_HUD_ARROWS_R1;
+
+    case SYM_HUD_ARROWS_R2:
+        return BF_SYM_HUD_ARROWS_R2;
+
+    case SYM_HUD_ARROWS_R3:
+        return BF_SYM_HUD_ARROWS_R3;
+
+    case SYM_HUD_ARROWS_U1:
+        return BF_SYM_HUD_ARROWS_U1;
+
+    case SYM_HUD_ARROWS_U2:
+        return BF_SYM_HUD_ARROWS_U2;
+
+    case SYM_HUD_ARROWS_U3:
+        return BF_SYM_HUD_ARROWS_U3;
+
+    case SYM_HUD_ARROWS_D1:
+        return BF_SYM_HUD_ARROWS_D1;
+
+    case SYM_HUD_ARROWS_D2:
+        return BF_SYM_HUD_ARROWS_D2;
+
+    case SYM_HUD_ARROWS_D3:
+        return BF_SYM_HUD_ARROWS_D3;
+*/
+    default:
+        break;
+    }
+
+    return '?'; // Missing/not mapped character
+}
+
+#endif
+
+#endif

src/main/io/displayport_msp_bf_compat.h

@@ -0,0 +1,37 @@
+
+/*
+ * This file is part of INAV Project.
+ *
+ * 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.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_MSP_DISPLAYPORT
+
+#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

@@ -52,12 +52,16 @@ FILE_COMPILE_FOR_SPEED
 
 #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_DRAW_SCREEN  4
-#define MSP_SET_OPTIONS 5
+#define MSP_SET_OPTIONS  5
 
 typedef enum {          // defines are from hdzero code
     SD_3016,
@@ -273,7 +277,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
         uint8_t len = 4;
         do {
             bitArrayClr(dirty, pos);
-            subcmd[len++] = screen[pos++];
+            subcmd[len++] = (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) ? getBfCharacter(screen[pos++], page): screen[pos++];
 
             if (bitArrayGet(dirty, pos)) {
                 next = pos;
@@ -282,7 +286,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
 
         subcmd[1] = row;
         subcmd[2] = col;
-        subcmd[3] = page;
+        subcmd[3] = (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) ? 0 : page;
         output(displayPort, MSP_DISPLAYPORT, subcmd, len);
         updateCount++;
         next = BITARRAY_FIND_FIRST_SET(dirty, pos);
@@ -348,22 +352,21 @@ static bool isReady(displayPort_t *displayPort)
     return vtxActive;
 }
 
-static int grab(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
 static int heartbeat(displayPort_t *displayPort)
 {
-    UNUSED(displayPort);
+    uint8_t subcmd[] = { MSP_HEARTBEAT };
-    return 0;
+    return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
+}
+
+static int grab(displayPort_t *displayPort)
+{
+    return heartbeat(displayPort);
 }
 
 static int release(displayPort_t *displayPort)
 {
-    UNUSED(displayPort);
+    uint8_t subcmd[] = { MSP_RELEASE };
-    return 0;
+    return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 }
 
 static const displayPortVTable_t mspOsdVTable = {
@@ -415,6 +418,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
     if (mspOsdSerialInit()) {
         switch(videoSystem) {
         case VIDEO_SYSTEM_AUTO:
+        case VIDEO_SYSTEM_BFCOMPAT:
         case VIDEO_SYSTEM_PAL:
             currentOsdMode = SD_3016;
             screenRows = PAL_ROWS;
@@ -443,6 +447,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/osd.c

@@ -69,6 +69,7 @@ FILE_COMPILE_FOR_SPEED
 #include "io/osd.h"
 #include "io/osd_common.h"
 #include "io/osd_hud.h"
+#include "io/displayport_msp_bf_compat.h"
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
@@ -148,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
@@ -200,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;
@@ -241,6 +254,7 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
     int decimals = maxDecimals;
     bool negative = false;
     bool scaled = false;
+    bool explicitDecimal = isBfCompatibleVideoSystem(osdConfig());
 
     buff[length] = '\0';
 
@@ -256,6 +270,9 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
 
     int digits = digitCount(integerPart);
     int remaining = length - digits;
+    if (explicitDecimal) {
+        remaining--;
+    }
 
     if (remaining < 0 && scale > 0) {
         // Reduce by scale
@@ -266,6 +283,9 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
         millis = ((centivalue % (100 * scale)) * 10) / scale;
         digits = digitCount(integerPart);
         remaining = length - digits;
+        if (explicitDecimal) {
+            remaining--;
+        }
     }
 
     // 3 decimals at most
@@ -289,8 +309,14 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
     // Now write the digits.
     ui2a(integerPart, 10, 0, ptr);
     ptr += digits;
+
     if (decimals > 0) {
-        *(ptr-1) += SYM_ZERO_HALF_TRAILING_DOT - '0';
+        if (explicitDecimal) {
+            *ptr = '.';
+            ptr++;
+        } else {
+            *(ptr - 1) += SYM_ZERO_HALF_TRAILING_DOT - '0';
+        }
         dec = ptr;
         int factor = 3; // we're getting the decimal part in millis first
         while (decimals < factor) {
@@ -304,7 +330,9 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
             ptr++;
         }
         ui2a(millis, 10, 0, ptr);
-        *dec += SYM_ZERO_HALF_LEADING_DOT - '0';
+        if (!explicitDecimal) {
+            *dec += SYM_ZERO_HALF_LEADING_DOT - '0';
+        }
     }
     return scaled;
 }
@@ -715,10 +743,15 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val)
     // We can show up to 7 digits in decimalPart.
     int32_t decimalPart = abs(val % GPS_DEGREES_DIVIDER);
     STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits);
-    int decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart);
+    int decimalDigits;
-    // Embbed the decimal separator
+    if (!isBfCompatibleVideoSystem(osdConfig())) {
-    buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0';
+        decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart);
-    buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0';
+        // Embbed the decimal separator
+        buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0';
+        buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0';
+    } else {
+        decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart);
+    }
     // Fill up to coordinateLength with zeros
     int total = 1 + integerDigits + decimalDigits;
     while(total < coordinateLength) {
@@ -1389,7 +1422,7 @@ static void osdFormatPidControllerOutput(char *buff, const char *label, const pi
 
 static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t digits, uint8_t decimals)
 {
-    char buff[6];
+    char buff[7];
     textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
 
     osdFormatBatteryChargeSymbol(buff);
@@ -1398,7 +1431,7 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_
     displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    digits = MIN(digits, 4);
+    digits = MIN(digits, 5);
     osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits);
     buff[digits] = SYM_VOLT;
     buff[digits+1] = '\0';
@@ -1574,11 +1607,11 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 
     case OSD_MAIN_BATT_VOLTAGE:
-        osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
+        osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), (isBfCompatibleVideoSystem(osdConfig()) ? 3 : 2) + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
         return true;
 
     case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE:
-        osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
+        osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), (isBfCompatibleVideoSystem(osdConfig()) ? 3 : 2) + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
         return true;
 
     case OSD_CURRENT_DRAW:
@@ -1762,15 +1795,11 @@ static bool osdDrawSingleElement(uint8_t item)
         osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0);
         break;
 
-    case OSD_HEADING:
+    case OSD_GROUND_COURSE:
         {
-            buff[0] = SYM_HEADING;
+            buff[0] = SYM_GROUND_COURSE;
             if (osdIsHeadingValid()) {
-                int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading());
+                tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
-                if (h < 0) {
-                    h += 360;
-                }
-                tfp_sprintf(&buff[1], "%3d", h);
             } else {
                 buff[1] = buff[2] = buff[3] = '-';
             }
@@ -1825,6 +1854,18 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
+    case OSD_CROSS_TRACK_ERROR:
+        {
+            if (isWaypointNavTrackingActive()) {
+                buff[0] = SYM_CROSS_TRACK_ERROR;
+                osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0);
+            } else {
+                displayWrite(osdDisplayPort, elemPosX, elemPosY, "     ");
+                return true;
+            }
+            break;
+        }
+
     case OSD_GPS_HDOP:
         {
             buff[0] = SYM_HDP_L;
@@ -1930,20 +1971,20 @@ static bool osdDrawSingleElement(uint8_t item)
             }
 #endif
             if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
-                buff[0] = SYM_FLY_M;
+                buff[0] = SYM_FLIGHT_MINS_REMAINING;
                 strcpy(buff + 1, "--:--");
 #if defined(USE_ADC) && defined(USE_GPS)
                 updatedTimestamp = 0;
 #endif
             } else if (timeSeconds == -2) {
                 // Wind is too strong to come back with cruise throttle
-                buff[0] = SYM_FLY_M;
+                buff[0] = SYM_FLIGHT_MINS_REMAINING;
                 buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL;
                 buff[3] = ':';
                 buff[6] = '\0';
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             } else {
-                osdFormatTime(buff, timeSeconds, SYM_FLY_M, SYM_FLY_H);
+                osdFormatTime(buff, timeSeconds, SYM_FLIGHT_MINS_REMAINING, SYM_FLIGHT_HOURS_REMAINING);
                 if (timeSeconds == 0)
                     TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
@@ -1964,7 +2005,8 @@ static bool osdDrawSingleElement(uint8_t item)
             updatedTimestamp = currentTimeUs;
         }
 #endif
-        buff[0] = SYM_TRIP_DIST;
+        //buff[0] = SYM_TRIP_DIST;
+        displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING);
         if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
             buff[4] = SYM_BLANK;
             buff[5] = '\0';
@@ -2540,7 +2582,7 @@ static bool osdDrawSingleElement(uint8_t item)
         return true;
 
     case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
-        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)currentBatteryProfile->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)navConfig()->fw.minThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
         return true;
 
     case OSD_FW_ALT_PID_OUTPUTS:
@@ -2659,13 +2701,13 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_MAIN_BATT_CELL_VOLTAGE:
         {
-            osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2);
+            osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), (isBfCompatibleVideoSystem(osdConfig()) ? 4 : 3), 2);
             return true;
         }
 
     case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE:
         {
-            osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), 3, 2);
+            osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), (isBfCompatibleVideoSystem(osdConfig()) ? 4 : 3), 2);
             return true;
         }
 
@@ -2675,6 +2717,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()) {
@@ -2954,7 +3013,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 if (h < 0) {
                     h += 360;
                 }
-                if(h >= 180)
+                if (h >= 180)
                     h = h - 180;
                 else
                     h = h + 180;
@@ -3221,73 +3280,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;
@@ -3298,18 +3386,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,
@@ -3435,8 +3523,10 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
     osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
     osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
+    osdLayoutsConfig->item_pos[0][OSD_GROUND_COURSE] = OSD_POS(12, 3);
     osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2);
     osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2);
+    osdLayoutsConfig->item_pos[0][OSD_CROSS_TRACK_ERROR] = OSD_POS(12, 3);
     osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
     osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
     osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
@@ -3879,6 +3969,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);
 }
 
@@ -4020,6 +4121,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);
 }
 
@@ -4506,6 +4618,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"
@@ -266,6 +268,8 @@ typedef enum {
     OSD_GLIDE_RANGE,
     OSD_CLIMB_EFFICIENCY,
     OSD_NAV_WP_MULTI_MISSION_INDEX,
+    OSD_GROUND_COURSE,      // 140
+    OSD_CROSS_TRACK_ERROR,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -466,6 +470,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

@@ -953,7 +953,7 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
             tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
             break;
         case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
-            tfp_sprintf(buff, "MTDPA %4d", currentBatteryProfileMutable->fwMinThrottleDownPitchAngle);
+            tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle);
             break;
         case ADJUSTMENT_TPA:
             tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);

src/main/navigation/navigation.c

@@ -99,7 +99,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -127,7 +127,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         // General navigation parameters
         .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT,                            // 5 sec
         .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT,                                       // 2m diameter
-        .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT,                         // centimeters - first waypoint should be closer than this
+        .waypoint_safe_distance = SETTING_NAV_WP_MAX_SAFE_DISTANCE_DEFAULT,                         // Metres - first waypoint should be closer than this
 #ifdef USE_MULTI_MISSION
         .waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT,             // mission index selected from multi mission WP entry
 #endif
@@ -185,7 +185,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
         .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
         .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
+        .minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
         .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT,                  // 75m
+        .loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
 
         //Fixed wing landing
         .land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT,              // 2 degrees dive by default
@@ -254,8 +256,8 @@ static void clearJumpCounters(void);
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
-void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
+void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
-static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw);
+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);
 static navigationFSMEvent_t nextForNonGeoStates(void);
@@ -1067,9 +1069,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
 
     resetPositionController();
 
-    posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
+    posControl.cruise.course = posControl.actualState.cog;  // Store the course to follow
-    posControl.cruise.previousYaw = posControl.cruise.yaw;
+    posControl.cruise.previousCourse = posControl.cruise.course;
-    posControl.cruise.lastYawAdjustmentTime = 0;
+    posControl.cruise.lastCourseAdjustmentTime = 0;
 
     return NAV_FSM_EVENT_SUCCESS; // Go to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
 }
@@ -1093,15 +1095,15 @@ 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.lastYawAdjustmentTime;
+        timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
         if (timeDifference > 100) timeDifference = 0;   // if adjustment was called long time ago, reset the time difference.
         float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
         float centidegsPerIteration = rateTarget * MS2S(timeDifference);
-        posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
+        posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
-        DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
+        DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
-        posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
+        posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
-    } else if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000) {
+    } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000)
-        posControl.cruise.previousYaw = posControl.cruise.yaw;
+        posControl.cruise.previousCourse = posControl.cruise.course;
     }
 
     setDesiredPosition(NULL, posControl.cruise.yaw, NAV_POS_UPDATE_HEADING);
@@ -1116,8 +1118,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
 
     // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
     if (posControl.flags.isAdjustingPosition) {
-        posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
+        posControl.cruise.course = posControl.actualState.cog;  //store current course
-        posControl.cruise.lastYawAdjustmentTime = millis();
+        posControl.cruise.lastCourseAdjustmentTime = millis();
         return NAV_FSM_EVENT_NONE;  // reprocess the state
     }
 
@@ -1206,7 +1208,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
                     // Spiral climb centered at xy of RTH activation
                     calculateInitialHoldPosition(&targetHoldPos);
                 } else {
-                    calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f);  // 1km away Linear climb
+                    calculateFarAwayTarget(&targetHoldPos, posControl.actualState.cog, 100000.0f);  // 1km away Linear climb
                 }
             } else {
                 // Multicopter, hover and climb
@@ -1325,7 +1327,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
             return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;    // procede to home after final trackback point
         }
 
-        if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) {
+        if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
             posControl.activeRthTBPointIndex--;
 
             if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
@@ -1361,7 +1363,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
 
         if (isWaypointReached(tmpHomePos, 0)) {
             // Successfully reached position target - update XYZ-position
-            setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+            setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
             return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
         } else {
             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
@@ -1392,13 +1394,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
 
     // If position ok OR within valid timeout - continue
     // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
-    if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
+    if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
         resetLandingDetector();     // force reset landing detector just in case
         updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
         return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
     } else {
         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
-        setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+        setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
         return NAV_FSM_EVENT_NONE;
     }
 }
@@ -1613,7 +1615,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
             case NAV_WP_ACTION_HOLD_TIME:
             case NAV_WP_ACTION_WAYPOINT:
             case NAV_WP_ACTION_LAND:
-                if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) {
+                if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
                 }
                 else {
@@ -1918,7 +1920,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;
@@ -2103,7 +2107,7 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
 /*-----------------------------------------------------------
  * Processes an update to estimated heading
  *-----------------------------------------------------------*/
-void updateActualHeading(bool headingValid, int32_t newHeading)
+void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse)
 {
     /* Update heading. Check if we're acquiring a valid heading for the
      * first time and update home heading accordingly.
@@ -2124,17 +2128,14 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
         // the offset from the fake to the actual yaw and apply the same rotation
         // to the home point.
         int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
+        posControl.rthState.homePosition.heading += fakeToRealYawOffset;
+        posControl.rthState.homePosition.heading = wrap_36000(posControl.rthState.homePosition.heading);
 
-        posControl.rthState.homePosition.yaw += fakeToRealYawOffset;
-        if (posControl.rthState.homePosition.yaw < 0) {
-            posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
-        }
-        if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
-            posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
-        }
         posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
     }
+
     posControl.actualState.yaw = newHeading;
+    posControl.actualState.cog = newGroundCourse;
     posControl.flags.estHeadingStatus = newEstHeading;
 
     /* Precompute sin/cos of yaw angle */
@@ -2189,7 +2190,7 @@ int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, cons
     return calculateBearingFromDelta(deltaX, deltaY);
 }
 
-bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)
+bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)   // NOT USED ANYWHERE
 {
     if (posControl.flags.estPosStatus == EST_NONE ||
         posControl.flags.estHeadingStatus == EST_NONE) {
@@ -2209,7 +2210,7 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3
 static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
 {
     // Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
-    if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) {
+    if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && !isLastMissionWaypoint()) {
         navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action;
 
         if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) {
@@ -2236,9 +2237,9 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
 
 /*-----------------------------------------------------------
  * Check if waypoint is/was reached.
- * waypointYaw stores initial bearing to waypoint
+ * waypointBearing stores initial bearing to waypoint
  *-----------------------------------------------------------*/
-static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw)
+static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
 {
     posControl.wpDistance = calculateDistanceToDestination(waypointPos);
 
@@ -2257,7 +2258,7 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
         // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
         // Same method for turn smoothing option but relative bearing set at 60 degrees
         uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
-        if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > relativeBearing) {
+        if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
             return true;
         }
     }
@@ -2382,7 +2383,7 @@ bool validateRTHSanityChecker(void)
 /*-----------------------------------------------------------
  * Reset home position to current position
  *-----------------------------------------------------------*/
-void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
+void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
 {
     // XY-position
     if ((useMask & NAV_POS_UPDATE_XY) != 0) {
@@ -2408,7 +2409,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
     // Heading
     if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
         // Heading
-        posControl.rthState.homePosition.yaw = yaw;
+        posControl.rthState.homePosition.heading = heading;
         if (homeFlags & NAV_HOME_VALID_HEADING) {
             posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
         } else {
@@ -2641,7 +2642,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;
     }
 
@@ -2659,7 +2660,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
         if (posControl.activeRthTBPointIndex < 0) {
             saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
 
-            previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw);
+            previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
             previousTBTripDist = posControl.totalTripDistance;
         } else {
             // Minimum distance increment between course change track points when GPS course valid - set to 10m
@@ -2790,10 +2791,10 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
     }
 }
 
-void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance)
+void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance)
 {
-    farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
+    farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing));
-    farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
+    farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing));
     farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
 }
 
@@ -2886,10 +2887,19 @@ 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)) {
+            if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
-                posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
+                // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
-                posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500);    // FIXME: calculate sanity limits in a smarter way
+                if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
+                    posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
+                    targetHoldActive = false;
+                } else if (!targetHoldActive) {     // Reset and hold target to actual + climb rate boost until actual catches up
+                    posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
+                    targetHoldActive = true;
+                }
+            } else {
+                targetHoldActive = false;
             }
         }
         else {
@@ -3356,18 +3366,18 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
 
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
 {
-    // Calculate bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
+    // Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints)
     if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
-        posControl.activeWaypoint.yaw = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
+        posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
     } else {
-        posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
+        posControl.activeWaypoint.bearing = calculateBearingToDestination(pos);
     }
     posControl.activeWaypoint.nextTurnAngle = -1;     // no turn angle set (-1), will be set by WP mode as required
 
     posControl.activeWaypoint.pos = *pos;
 
     // Set desired position to next waypoint (XYZ-controller)
-    setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+    setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 }
 
 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
@@ -3385,7 +3395,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
         fpVector3_t posNextWp;
         if (getLocalPosNextWaypoint(&posNextWp)) {
             int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
-            posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.yaw);
+            posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
         }
     }
 }
@@ -3597,8 +3607,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         const bool canActivatePosHold    = canActivatePosHoldMode();
         const bool canActivateNavigation = canActivateNavigationModes();
         const bool isExecutingRTH        = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
+#ifdef USE_SAFE_HOME
         checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
-
+#endif
         // deactivate rth trackback if RTH not active
         if (posControl.flags.rthTrackbackActive) {
             posControl.flags.rthTrackbackActive = isExecutingRTH;
@@ -3823,7 +3834,6 @@ bool navigationPositionEstimateIsHealthy(void)
 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
 {
     const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
-    const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
 
     if (usedBypass) {
         *usedBypass = false;
@@ -3842,13 +3852,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
     }
 
     // Don't allow arming if any of NAV modes is active
-    if (!ARMING_FLAG(ARMED) && navBoxModesEnabled && !navLaunchComboModesEnabled) {
+    if (!ARMING_FLAG(ARMED) && navBoxModesEnabled) {
         return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE;
     }
 
     // Don't allow arming if first waypoint is farther than configured safe distance
     if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
-        if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
+        if (distanceToFirstWP() > METERS_TO_CENTIMETERS(navConfig()->general.waypoint_safe_distance) && !checkStickPosition(YAW_HI)) {
             return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
         }
     }
@@ -4187,7 +4197,9 @@ void activateForcedRTH(void)
 {
     abortFixedWingLaunch();
     posControl.flags.forcedRTHActivated = true;
+#ifdef USE_SAFE_HOME
     checkSafeHomeState(true);
+#endif
     navProcessFSMEvents(selectNavEventFromBoxModeInput());
 }
 
@@ -4196,7 +4208,9 @@ void abortForcedRTH(void)
     // Disable failsafe RTH and make sure we back out of navigation mode to IDLE
     // If any navigation mode was active prior to RTH it will be re-enabled with next RX update
     posControl.flags.forcedRTHActivated = false;
+#ifdef USE_SAFE_HOME
     checkSafeHomeState(false);
+#endif
     navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
 }
 
@@ -4304,13 +4318,12 @@ bool isNavLaunchEnabled(void)
 bool abortLaunchAllowed(void)
 {
     // allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
-    throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
+    return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
-    return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
 }
 
 int32_t navigationGetHomeHeading(void)
 {
-    return posControl.rthState.homePosition.yaw;
+    return posControl.rthState.homePosition.heading;
 }
 
 // returns m/s
@@ -4333,5 +4346,5 @@ bool isAdjustingHeading(void) {
 }
 
 int32_t getCruiseHeadingAdjustment(void) {
-    return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
+    return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
 }

src/main/navigation/navigation.h

@@ -291,11 +291,13 @@ typedef struct navConfig_s {
         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
         uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
-        uint8_t control_smoothness;          // The amount of smoothing to apply to controls for navigation
+        uint8_t  control_smoothness;         // The amount of smoothing to apply to controls for navigation
         uint16_t pitch_to_throttle_smooth;   // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
         uint8_t  pitch_to_throttle_thresh;   // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
+        uint16_t minThrottleDownPitchAngle;  // Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle. [decidegrees]
         uint16_t loiter_radius;              // Loiter radius when executing PH on a fixed wing
-        int8_t land_dive_angle;
+        uint8_t  loiter_direction;           // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
+        int8_t   land_dive_angle;
         uint16_t launch_velocity_thresh;     // Velocity threshold for swing launch detection
         uint16_t launch_accel_thresh;        // Acceleration threshold for launch detection (cm/s/s)
         uint16_t launch_time_thresh;         // Time threshold for launch detection (ms)
@@ -393,11 +395,12 @@ extern radar_pois_t radar_pois[RADAR_MAX_POIS];
 
 typedef struct {
     fpVector3_t pos;
-    int32_t     yaw;                // centidegrees
+    int32_t     heading;            // centidegrees
+    int32_t     bearing;            // centidegrees
     int32_t     nextTurnAngle;      // centidegrees
 } navWaypointPosition_t;
 
-typedef struct navDestinationPath_s {
+typedef struct navDestinationPath_s {   // NOT USED
     uint32_t distance; // meters * 100
     int32_t bearing; // deg * 100
 } navDestinationPath_t;
@@ -468,6 +471,7 @@ typedef struct {
     navSystemStatus_Error_e error;
     navSystemStatus_Flags_e flags;
     uint8_t                 activeWpNumber;
+    uint8_t                 activeWpIndex;
     navWaypointActions_e    activeWpAction;
 } navSystemStatus_t;
 
@@ -568,7 +572,7 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
 geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
 
 /* Distance/bearing calculation */
-bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
+bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);   // NOT USED
 uint32_t distanceToFirstWP(void);
 
 /* Failsafe-forced RTH mode */
@@ -607,6 +611,7 @@ void updateLandingStatus(void);
 const navigationPIDControllers_t* getNavigationPIDControllers(void);
 
 int32_t navigationGetHeadingError(void);
+float navigationGetCrossTrackError(void);
 int32_t getCruiseHeadingAdjustment(void);
 bool isAdjustingPosition(void);
 bool isAdjustingHeading(void);

src/main/navigation/navigation_fixedwing.c

@@ -72,6 +72,7 @@ static bool isYawAdjustmentValid = false;
 static float throttleSpeedAdjustment = 0;
 static bool isAutoThrottleManuallyIncreased = false;
 static int32_t navHeadingError;
+static float navCrossTrackError;
 static int8_t loiterDirYaw = 1;
 bool needToCalculateCircularLoiter;
 
@@ -240,11 +241,11 @@ void resetFixedWingPositionController(void)
 static int8_t loiterDirection(void) {
     int8_t dir = 1; //NAV_LOITER_RIGHT
 
-    if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) {
+    if (navConfig()->fw.loiter_direction == NAV_LOITER_LEFT) {
         dir = -1;
     }
 
-    if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
+    if (navConfig()->fw.loiter_direction == NAV_LOITER_YAW) {
 
         if (rcCommand[YAW] < -250) {
             loiterDirYaw = 1; //RIGHT //yaw is contrariwise
@@ -308,7 +309,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
         // velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time
         if (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) {
             if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) {
-                int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
+                int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.bearing + 18000);
                 loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
                 loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
 
@@ -406,20 +407,20 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
         // courseVirtualCorrection initially used to determine current position relative to course line for later use
-        int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
+        int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
-        float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
+        navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
 
         // tracking only active when certain distance and heading conditions are met
-        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) && distToCourseLine > 200) {
+        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
-            int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
+            int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog);
 
             // captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
             // Closing distance threashold based on speed and an assumed 1 second response time.
-            float captureFactor = distToCourseLine < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
+            float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
 
             // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
             // initial courseCorrectionFactor based on distance to course line
-            float courseCorrectionFactor = constrainf(captureFactor * distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
+            float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
             courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
 
             // course heading alignment factor
@@ -431,7 +432,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
             // final courseVirtualCorrection value
             courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
-            virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
+            virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection);
         }
     }
 
@@ -439,7 +440,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
      * Calculate NAV heading error
      * Units are centidegrees
      */
-    navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
+    navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
 
     // Forced turn direction
     // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
@@ -469,7 +470,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
 
     // Input error in (deg*100), output roll angle (deg*100)
-    float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros),
+    float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros),
                                        -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                         DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                         pidFlags);
@@ -694,12 +695,11 @@ bool isFixedWingLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
     static bool fixAxisCheck = false;
-    const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 
     // Basic condition to start looking for landing
     bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
                           || FLIGHT_MODE(FAILSAFE_MODE)
-                          || (!navigationIsControllingThrottle() && throttleIsLow);
+                          || (!navigationIsControllingThrottle() && throttleStickIsLow());
 
     if (!startCondition || posControl.flags.resetLandingDetector) {
         return fixAxisCheck = posControl.flags.resetLandingDetector = false;
@@ -778,7 +778,7 @@ void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
 
 void resetFixedWingHeadingController(void)
 {
-    updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
+    updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
 }
 
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
@@ -834,3 +834,8 @@ int32_t navigationGetHeadingError(void)
 {
     return navHeadingError;
 }
+
+float navigationGetCrossTrackError(void)
+{
+    return navCrossTrackError;
+}

src/main/navigation/navigation_fw_launch.c

@@ -248,11 +248,6 @@ static void applyThrottleIdleLogic(bool forceMixerIdle)
     }
 }
 
-static inline bool isThrottleLow(void)
-{
-    return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
-}
-
 static inline bool isLaunchMaxAltitudeReached(void)
 {
     return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
@@ -272,7 +267,7 @@ static inline bool isProbablyNotFlying(void)
 
 static void resetPidsIfNeeded(void) {
     // Don't use PID I-term and reset TPA filter until motors are started or until flight is detected
-    if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && isThrottleLow())) {
+    if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && throttleStickIsLow())) {
         pidResetErrorAccumulators();
         pidResetTPAFilter();
     }
@@ -292,7 +287,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
 {
     UNUSED(currentTimeUs);
 
-    if (!isThrottleLow()) {
+    if (!throttleStickIsLow()) {
         if (isThrottleIdleEnabled()) {
             return FW_LAUNCH_EVENT_SUCCESS;
         }
@@ -312,7 +307,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
 
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
 {
-    if (isThrottleLow()) {
+    if (throttleStickIsLow()) {
         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
     }
 
@@ -330,7 +325,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(tim
 
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs)
 {
-    if (isThrottleLow()) {
+    if (throttleStickIsLow()) {
         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
     }
 
@@ -349,7 +344,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
 
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs)
 {
-    if (isThrottleLow()) {
+    if (throttleStickIsLow()) {
         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
     }
 
@@ -427,7 +422,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
 
     if (navConfig()->fw.launch_manual_throttle) {
         // reset timers when throttle is low or until flight detected and abort launch regardless of launch settings
-        if (isThrottleLow()) {
+        if (throttleStickIsLow()) {
             fwLaunch.currentStateTimeUs = currentTimeUs;
             fwLaunch.pitchAngle = 0;
             if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {

src/main/navigation/navigation_multicopter.c

@@ -176,14 +176,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
 
 void setupMulticopterAltitudeController(void)
 {
-    const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
+    const bool throttleIsLow = throttleStickIsLow();
 
     if (navConfig()->general.flags.use_thr_mid_for_althold) {
         altHoldThrottleRCZero = rcLookupThrottleMid();
     }
     else {
-        // If throttle status is THROTTLE_LOW - use Thr Mid anyway
+        // If throttle is LOW - use Thr Mid anyway
-        if (throttleStatus == THROTTLE_LOW) {
+        if (throttleIsLow) {
             altHoldThrottleRCZero = rcLookupThrottleMid();
         }
         else {
@@ -198,7 +198,7 @@ void setupMulticopterAltitudeController(void)
 
     // Force AH controller to initialize althold integral for pending takeoff on reset
     // Signal for that is low throttle _and_ low actual altitude
-    if (throttleStatus == THROTTLE_LOW && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
+    if (throttleIsLow && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
         prepareForTakeoffOnReset = true;
     }
 }
@@ -724,13 +724,12 @@ bool isMulticopterLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
     static timeUs_t landingDetectorStartedAt;
-    const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 
     /* Basic condition to start looking for landing
     *  Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
     bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
                           || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
-                          || (!navigationIsFlyingAutonomousMode() && throttleIsLow);
+                          || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
 
     if (!startCondition || posControl.flags.resetLandingDetector) {
         landingDetectorStartedAt = 0;

src/main/navigation/navigation_pos_estimator.c

@@ -36,6 +36,7 @@
 
 #include "fc/config.h"
 #include "fc/settings.h"
+#include "fc/rc_modes.h"
 
 #include "flight/imu.h"
 
@@ -320,7 +321,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 {
@@ -433,7 +434,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));
                 }
             }
@@ -554,6 +555,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();
 
@@ -624,15 +634,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;
 }
 
@@ -792,9 +793,9 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
     static navigationTimer_t posPublishTimer;
 
     /* IMU operates in decidegrees while INAV operates in deg*100
-     * Use course over ground for fixed wing navigation yaw/"heading" */
+     * Use course over ground when GPS heading valid */
-    int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? posEstimator.est.cog : attitude.values.yaw;
+    int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
-    updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue));
+    updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
 
     /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
     if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
@@ -819,6 +820,23 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
         //Update Blackbox states
         navEPH = posEstimator.est.eph;
         navEPV = posEstimator.est.epv;
+
+        DEBUG_SET(DEBUG_POS_EST, 0, (int32_t) posEstimator.est.pos.x*1000.0F);                // Position estimate X
+        DEBUG_SET(DEBUG_POS_EST, 1, (int32_t) posEstimator.est.pos.y*1000.0F);                // Position estimate Y
+        if (IS_RC_MODE_ACTIVE(BOXSURFACE) && posEstimator.est.aglQual!=SURFACE_QUAL_LOW){
+            // SURFACE (following) MODE
+            DEBUG_SET(DEBUG_POS_EST, 2, (int32_t) posControl.actualState.agl.pos.z*1000.0F);  // Position estimate Z
+            DEBUG_SET(DEBUG_POS_EST, 5, (int32_t) posControl.actualState.agl.vel.z*1000.0F);  // Speed estimate VZ
+        } else {
+            DEBUG_SET(DEBUG_POS_EST, 2, (int32_t) posEstimator.est.pos.z*1000.0F);            // Position estimate Z
+            DEBUG_SET(DEBUG_POS_EST, 5, (int32_t) posEstimator.est.vel.z*1000.0F);            // Speed estimate VZ
+        }
+        DEBUG_SET(DEBUG_POS_EST, 3, (int32_t) posEstimator.est.vel.x*1000.0F);                // Speed estimate VX
+        DEBUG_SET(DEBUG_POS_EST, 4, (int32_t) posEstimator.est.vel.y*1000.0F);                // Speed estimate VY
+        DEBUG_SET(DEBUG_POS_EST, 6, (int32_t) attitude.values.yaw);                           // Yaw estimate (4 bytes still available here)
+        DEBUG_SET(DEBUG_POS_EST, 7, (int32_t) (posEstimator.flags & 0b1111111)<<20 |          // navPositionEstimationFlags fit into 8bits
+                                              (MIN(navEPH, 1000) & 0x3FF)<<10 |
+                                              (MIN(navEPV, 1000) & 0x3FF));                   // Horizontal and vertical uncertainties (max value = 1000, fit into 20bits)
     }
 }
 

src/main/navigation/navigation_private.h

@@ -124,6 +124,7 @@ typedef struct {
     navEstimatedPosVel_t    abs;
     navEstimatedPosVel_t    agl;
     int32_t                 yaw;
+    int32_t                 cog;
 
     // Service values
     float                   sinYaw;
@@ -322,9 +323,9 @@ typedef struct {
 } rthSanityChecker_t;
 
 typedef struct {
-    int32_t                     yaw;
+    int32_t                     course;
-    int32_t                     previousYaw;
+    int32_t                     previousCourse;
-    timeMs_t                    lastYawAdjustmentTime;
+    timeMs_t                    lastCourseAdjustmentTime;
 } navCruise_t;
 
 typedef struct {
@@ -443,10 +444,10 @@ bool isMulticopterFlying(void);
 
 navigationFSMStateFlags_t navGetCurrentStateFlags(void);
 
-void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
+void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
 void setDesiredSurfaceOffset(float surfaceOffset);
-void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);
+void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);   // NOT USED
 void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
 
 bool isNavHoldPositionActive(void);
@@ -454,7 +455,7 @@ bool isLastMissionWaypoint(void);
 float getActiveWaypointSpeed(void);
 bool isWaypointNavTrackingActive(void);
 
-void updateActualHeading(bool headingValid, int32_t newHeading);
+void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);
 

src/main/programming/logic_condition.c

@@ -86,10 +86,11 @@ void pgResetFn_logicConditions(logicCondition_t *instance)
 logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
 
 static int logicConditionCompute(
-    int32_t currentVaue,
+    int32_t currentValue,
     logicOperation_e operation,
     int32_t operandA,
-    int32_t operandB
+    int32_t operandB,
+    uint8_t lcIndex
 ) {
     int temporaryValue;
     vtxDeviceCapability_t vtxDeviceCapability;
@@ -104,6 +105,13 @@ static int logicConditionCompute(
             return operandA == operandB;
             break;
 
+        case LOGIC_CONDITION_APPROX_EQUAL:
+            {
+                uint16_t offest = operandA / 100;
+                return ((operandB >= (operandA - offest)) && (operandB <= (operandA + offest)));
+            }
+            break;
+
         case LOGIC_CONDITION_GREATER_THAN:
             return operandA > operandB;
             break;
@@ -159,7 +167,67 @@ static int logicConditionCompute(
             }
 
             //When both operands are not met, keep current value 
-            return currentVaue;
+            return currentValue;
+            break;
+
+        case LOGIC_CONDITION_EDGE:
+            if (operandA && logicConditionStates[lcIndex].timeout == 0 && !(logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED)) {
+                if (operandB < 100) {
+                    logicConditionStates[lcIndex].timeout = millis();
+                } else {
+                    logicConditionStates[lcIndex].timeout = millis() + operandB;
+                }
+                logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
+                return true;
+            } else if (logicConditionStates[lcIndex].timeout > 0) {
+                if (logicConditionStates[lcIndex].timeout < millis()) {
+                    logicConditionStates[lcIndex].timeout = 0;
+                } else {
+                    return true;
+                }
+            }
+
+            if (!operandA) {
+                logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
+            }
+            return false;
+            break;
+
+        case LOGIC_CONDITION_DELAY:
+            if (operandA) {
+                if (logicConditionStates[lcIndex].timeout == 0) {
+                    logicConditionStates[lcIndex].timeout = millis() + operandB;
+                } else if (millis() > logicConditionStates[lcIndex].timeout ) {
+                    logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
+                    return true;
+                } else if (logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED) {
+                    return true;
+                }
+            } else {
+                logicConditionStates[lcIndex].timeout = 0;
+                logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
+            }
+
+            return false;
+            break;
+
+        case LOGIC_CONDITION_TIMER:
+            if ((logicConditionStates[lcIndex].timeout == 0) || (millis() > logicConditionStates[lcIndex].timeout && !currentValue)) {
+                logicConditionStates[lcIndex].timeout = millis() + operandA;
+                return true;
+            } else if (millis() > logicConditionStates[lcIndex].timeout && currentValue) {
+                logicConditionStates[lcIndex].timeout = millis() + operandB;
+                return false;
+            }
+            return currentValue;
+            break;
+
+        case LOGIC_CONDITION_DELTA:
+            {
+                int difference = logicConditionStates[lcIndex].lastValue - operandA;
+                logicConditionStates[lcIndex].lastValue = operandA;
+                return ABS(difference) >= operandB;
+            }
             break;
 
         case LOGIC_CONDITION_GVAR_SET:
@@ -425,7 +493,8 @@ void logicConditionProcess(uint8_t i) {
                 logicConditionStates[i].value, 
                 logicConditions(i)->operation, 
                 operandAValue, 
-                operandBValue
+                operandBValue,
+                i
             );
         
             logicConditionStates[i].value = newValue;
@@ -442,6 +511,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) {
@@ -555,10 +723,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;
@@ -579,14 +743,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;
@@ -709,7 +865,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;
@@ -739,6 +895,10 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
                 retVal = programmingPidGetOutput(operand);
             }
             break;
+        
+        case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
+            retVal = logicConditionGetWaypointOperandValue(operand);
+            break;
 
         default:
             break;
@@ -785,7 +945,9 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) {
 void logicConditionReset(void) {
     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
         logicConditionStates[i].value = 0;
+        logicConditionStates[i].lastValue = 0;
         logicConditionStates[i].flags = 0;
+        logicConditionStates[i].timeout = 0;
     }
 }
 

src/main/programming/logic_condition.h

@@ -76,7 +76,12 @@ typedef enum {
     LOGIC_CONDITION_MAX                         = 44,
     LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE  = 45,
     LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE   = 46,
-    LOGIC_CONDITION_LAST                        = 47,
+    LOGIC_CONDITION_EDGE                        = 47,
+    LOGIC_CONDITION_DELAY                       = 48,
+    LOGIC_CONDITION_TIMER                       = 49,
+    LOGIC_CONDITION_DELTA                       = 50,
+    LOGIC_CONDITION_APPROX_EQUAL                = 51,
+    LOGIC_CONDITION_LAST                        = 52,
 } logicOperation_e;
 
 typedef enum logicOperandType_s {
@@ -87,6 +92,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;
 
@@ -114,24 +120,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 {
@@ -151,6 +154,23 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4,                              // 13
 } 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),
@@ -166,7 +186,8 @@ typedef enum {
 } logicConditionsGlobalFlags_t;
 
 typedef enum {
-    LOGIC_CONDITION_FLAG_LATCH      = 1 << 0,
+    LOGIC_CONDITION_FLAG_LATCH              = 1 << 0,
+    LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED  = 1 << 1,
 } logicConditionFlags_e;
 
 typedef struct logicOperand_s {
@@ -187,7 +208,9 @@ PG_DECLARE_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions);
 
 typedef struct logicConditionState_s {
     int value;
+    int32_t lastValue;
     uint8_t flags;
+    timeMs_t timeout;
 } logicConditionState_t;
 
 typedef struct rcChannelOverride_s {

src/main/sensors/battery.c

@@ -94,7 +94,7 @@ static int32_t mWhDrawn = 0;                    // energy (milliWatt hours) draw
 batteryState_e batteryState;
 const batteryProfile_t *currentBatteryProfile;
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 1);
+PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 2);
 
 void pgResetFn_batteryProfiles(batteryProfile_t *instance)
 {
@@ -130,8 +130,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
 
             .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT,                                 // default throttle off.
 
-            .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
-
             .nav = {
 
                 .mc = {
@@ -549,7 +547,6 @@ void currentMeterUpdate(timeUs_t timeDelta)
         case CURRENT_SENSOR_VIRTUAL:
             amperage = batteryMetersConfig()->current.offset;
             if (ARMING_FLAG(ARMED)) {
-                throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
                 navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
                 bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE;
                 bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
@@ -558,7 +555,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
                 if (allNav || autoNav) {    // account for motors running in Nav modes with throttle low + motor stop
                     throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
                 } else {
-                    throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
+                    throttleOffset = (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
                 }
                 int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
                 amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;

src/main/sensors/battery_config_structs.h

@@ -106,8 +106,6 @@ typedef struct batteryProfile_s {
 
     uint16_t failsafe_throttle;             // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
 
-    uint16_t fwMinThrottleDownPitchAngle;
-
     struct {
 
         struct {

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/AOCODARCF7DUAL/target.c

@@ -27,24 +27,22 @@
 #include "drivers/sensor.h"
 
 BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE, 0,   DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6000_SPI_BUS,    MPU6500_CS_PIN,     NONE, 0,   DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270,      DEVHW_BMI270,       BMI270_SPI_BUS,     BMI270_CS_PIN,      NONE,  0,   DEVFLAGS_NONE,  IMU_BMI270_ALIGN);
-BUSDEV_REGISTER_SPI_TAG(busdev_bmi270,      DEVHW_BMI270,       BMI270_SPI_BUS,     BMI270_CS_PIN,      NONE,  1,   DEVFLAGS_NONE,  IMU_BMI270_ALIGN);
-
 
 timerHardware_t timerHardware[] = {
     DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0),    //PPM
 
-    DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),        // S1   D(2, 2, 7) 
+    DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),        // S1   D(2, 2, 7)
-    DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),        // S2   D(2, 3, 7) 
+    DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),        // S2   D(2, 3, 7)
-    DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),        // S3   D(2, 4, 7) 
+    DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),        // S3   D(2, 4, 7)
-    DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),        // S4   D(2, 7, 7) 
+    DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),        // S4   D(2, 7, 7)
-    
+
-    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR  |TIM_USE_FW_SERVO, 0, 0),        // S5   D(1, 7, 5)
+    DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MC_MOTOR  |TIM_USE_FW_SERVO, 0, 0),        // S5   D(1, 7, 5)
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR  |TIM_USE_FW_SERVO, 0, 0),        // S6   D(1, 2, 5)  
+    DEF_TIM(TIM8, CH3N, PB1, TIM_USE_MC_MOTOR  |TIM_USE_FW_SERVO, 0, 0),        // S6   D(1, 2, 5)
-    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR  |TIM_USE_FW_SERVO, 0, 0),        // S7   D(1, 0, 2)  
+    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(TIM2, CH1, PA0, TIM_USE_LED, 0, 0),    // LED_TRIP                   
+    DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0),    // LED_TRIP
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/AOCODARCF7DUAL/target.h

@@ -27,7 +27,7 @@
 #define BEEPER                  PC13
 #define BEEPER_INVERTED
 
-// *************** SPI1 Gyro & ACC *******************
+// *************** SPI2 Gyro & ACC *******************
 #define USE_SPI
 #define USE_SPI_DEVICE_2
 #define SPI2_SCK_PIN            PB13
@@ -43,11 +43,6 @@
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI2
 
-#define USE_IMU_MPU6500
-#define IMU_MPU6500_ALIGN       CW270_DEG
-#define MPU6500_CS_PIN          PB12
-#define MPU6500_SPI_BUS         BUS_SPI2
-
 #define USE_IMU_BMI270
 #define IMU_BMI270_ALIGN        CW180_DEG
 #define BMI270_CS_PIN           PA13
@@ -87,7 +82,7 @@
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
 
-// *************** SPI2 OSD ***********************
+// *************** SPI1 OSD ***********************
 #define USE_SPI_DEVICE_1
 #define SPI1_SCK_PIN            PA5
 #define SPI1_MISO_PIN           PA6

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/ATOMRCF405NAVI/target.h

@@ -138,6 +138,8 @@
 #define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
 #define RSSI_ADC_CHANNEL            ADC_CHN_1
 
+#define CURRENT_METER_SCALE         320
+
 /*
  * OSD
  */

src/main/target/FOXEERF722V4/target.h

@@ -148,7 +148,13 @@
 
 /*** Timer/PWM output ***/
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#ifdef FOXEERF722V4_X8
+#define MAX_PWM_OUTPUT_PORTS            8
+#else
 #define MAX_PWM_OUTPUT_PORTS            4
+#endif
+
 #define USE_DSHOT
 #define USE_ESC_SENSOR
 

src/main/target/FOXEERF745AIO/CMakeLists.txt

@@ -1 +1,2 @@
 target_stm32f745xg(FOXEERF745AIO)
+target_stm32f745xg(FOXEERF745AIO_V3)

src/main/target/FOXEERF745AIO/target.h

@@ -25,12 +25,29 @@
 #define BEEPER                  PD2
 #define BEEPER_INVERTED
 
+/*** IMU sensors ***/
+#define USE_EXTI
+
+
+#define USE_MPU_DATA_READY_SIGNAL
+
+#ifdef FOXEERF745AIO_V3
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW90_DEG
+#define ICM42605_SPI_BUS        BUS_SPI3
+#define ICM42605_CS_PIN         PA15
+
+#else
+
 // MPU6000
 #define USE_IMU_MPU6000
 #define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA15
 #define MPU6000_SPI_BUS         BUS_SPI3
 
+#endif
+
 /*** SPI/I2C bus ***/
 #define USE_SPI
 #define USE_SPI_DEVICE_1

src/main/target/KAKUTEH7/target.h

@@ -203,6 +203,7 @@
 #define PINIO1_PIN                  PE13
 #define PINIO2_PIN                  PB11
 #define PINIO1_FLAGS				PINIO_FLAGS_INVERTED
+#define PINIO2_FLAGS				PINIO_FLAGS_INVERTED
 
 #else
 #define PINIO1_PIN                  PE13

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/telemetry/crsf.c

@@ -289,15 +289,25 @@ int16_t     Roll angle ( rad / 10000 )
 int16_t     Yaw angle ( rad / 10000 )
 */
 
-#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
+// convert andgle in decidegree to radians/10000 with reducing angle to +/-180 degree range
+static int16_t decidegrees2Radians10000(int16_t angle_decidegree)
+{
+    while (angle_decidegree > 1800) {
+        angle_decidegree -= 3600;
+    }
+    while (angle_decidegree < -1800) {
+        angle_decidegree += 3600;
+    }
+    return (int16_t)(RAD * 1000.0f * angle_decidegree);
+}
 
 static void crsfFrameAttitude(sbuf_t *dst)
 {
      sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
      crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE);
-     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch));
+     crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.pitch));
-     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll));
+     crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.roll));
-     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw));
+     crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.yaw));
 }
 
 /*
@@ -323,7 +333,7 @@ static void crsfFrameFlightMode(sbuf_t *dst)
         }
         if (FLIGHT_MODE(FAILSAFE_MODE)) {
             flightMode = "!FS!";
-        } else if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) {
+        } else if (IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) {
             flightMode = "HRST";
         } else if (FLIGHT_MODE(MANUAL_MODE)) {
             flightMode = "MANU";
@@ -335,10 +345,10 @@ static void crsfFrameFlightMode(sbuf_t *dst)
             flightMode = "CRUZ";
         } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
             flightMode = "CRSH";
-        } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
-            flightMode = "AH";
         } else if (FLIGHT_MODE(NAV_WP_MODE)) {
             flightMode = "WP";
+        } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
+            flightMode = "AH";
         } else if (FLIGHT_MODE(ANGLE_MODE)) {
             flightMode = "ANGL";
         } else if (FLIGHT_MODE(HORIZON_MODE)) {

src/main/telemetry/frsky_d.c

@@ -195,10 +195,7 @@ static void sendThrottleOrBatterySizeAsRpm(void)
 {
     sendDataHead(ID_RPM);
     if (ARMING_FLAG(ARMED)) {
-        const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
+        uint16_t throttleForRPM = getThrottlePercent() / BLADE_NUMBER_DIVIDER;
-        uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
-        if (throttleStatus == THROTTLE_LOW && 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