Merge branch 'master' of https://github.com/dragnea/inav into dragnea_autolaunch_refactor

# Conflicts:
#	src/main/io/beeper.c
#	src/main/io/beeper.h
#	src/main/navigation/navigation_fw_launch.c

docs/Controls.md

@@ -2,9 +2,9 @@
 
 ## Arming
 
-When armed, the aircraft is ready to fly and the motors will spin when throttle is applied.  The motors will spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons, that is not recommended).
+When armed, the aircraft is ready to fly and the motors will spin when throttle is applied.  With multirotors, the motors will spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons, that is not recommended).
 
-By default, arming and disarming is done using stick positions.  (NOTE: this feature is disabled when using a switch to arm.)
+Arming and disarming is done using a switch, set up on the modes page.  (NOTE: Stick arming was removed in iNav 2.2)
 
 ## Stick Positions
 

docs/Programming Framework.md

@@ -116,6 +116,9 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 28            | STABILIZED_YAW        | Yaw PID controller output `[-500:500]`    |
 | 29            | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` |
 | 30            | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
+| 31            | 3D HOME_DISTANCE      | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
+| 32            | CROSSFIRE LQ          | Crossfire Link quality as returned by the CRSF protocol   | 
+| 33            | CROSSFIRE SNR          | Crossfire SNR as returned by the CRSF protocol   | 
 
 ##### ACTIVE_WAYPOINT_ACTION
 
@@ -143,6 +146,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 6             | ANGLE         |                                       |
 | 7             | HORIZON       |                                       |
 | 8             | AIR           |                                       |
+| 9             | USER1         |                                       |
+| 10            | USER2         |                                       |
 
     
 ### Flags

docs/Rx.md

@@ -33,6 +33,8 @@ http://www.frsky-rc.com/product/pro.php?pro_id=21
 
 ### Spektrum
 
+This section describes the legacy Spektrum satellite capability; the newer SRXL2 protocol is described [later in this document](#srxl2) .
+
 8 channels via serial currently supported.
 
 These receivers are reported working:
@@ -47,10 +49,10 @@ As of iNav 1.6, a pseudo RSSI, based on satellite fade count is supported and re
 
 * Bind the satellite receiver using a physical RX; the bind function provided by the flight controller is not sufficient.
 * The CLI variable `rssi_channel` is set to channel 9:
-````
+```
 set rssi_channel = 9
-````
-This pseudo-RSSI should work on all makes of Spektrum satellite RX; it is tested as working on Lemon RX satellites http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=109 and http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=135 (recommended).
+```
+This pseudo-RSSI should work on all makes of Spektrum satellite RX; it is tested as working on [Lemon RX satellites](http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=109 and http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=135) (recommended).
 
 ### S.BUS
 
@@ -170,6 +172,46 @@ After flash "10ch Timer Mod i6 Updater", it is passible to get RSSI signal on se
 
 It is possible to use IBUS RX and IBUS telemetry on only one port of the hardware UART. More information in Telemetry.md.obj/inav_2.2.2_SPRACINGF3EVO.hex
 
+### SRXL2
+
+SRXL2 is a newer Spektrum protocol that provides a bidirectional link between the FC and the receiver, allowing the user to get FC telemetry data and basic settings on Spektrum Gen 2 airware TX. SRXL2 is supported in inav 2.6 and later. It offers improved performance and features compared to earlier Spektrum RX.
+
+#### Wiring
+
+Signal pin on receiver (labeled "S") must be wired to a **UART TX** pin on the FC. Voltage can be 3.3V (4.0V for SPM4651T) to 8.4V. On some F4 FCs, the TX pin may have a signal inverter (such as for S.Port). Make sure this isn't the case for the pin you intend to use.
+
+#### Configuration
+
+Selection of SXRL2 is provided in the inav 2.6 and later configurators. It is necessary to complete the configuration via the CLI; the following settings are recommended:
+
+```
+feature TELEMETRY
+feature -RSSI_ADC
+map TAER
+set receiver_type = SERIAL
+set serialrx_provider = SRXL2
+set serialrx_inverted = OFF
+set srxl2_unit_id = 1
+set srxl2_baud_fast = ON
+set rssi_source = PROTOCOL
+set rssi_channel = 0
+```
+
+#### Notes:
+
+* RSSI_ADC is disabled, as this would override the value provided through SRXL2
+* `rssi_channel = 0` is required, unlike earlier Spektrum devices (e.g. SPM4649T).
+
+Setting these values differently may have an adverse effects on RSSI readings.
+
+#### CLI Bind Command
+
+This command will put the receiver into bind mode without the need to reboot the FC as it was required with the older `spektrum_sat_bind` command.
+
+```
+bind_rx
+```
+
 ## MultiWii serial protocol (MSP)
 
 Allows you to use MSP commands as the RC input.  Only 8 channel support to maintain compatibility with MSP.
@@ -215,21 +257,20 @@ To setup spectrum in the GUI:
 2. Move to the "Configuration" page and in the upper lefthand corner choose Serial RX as the receiver type.
 3. Below that choose the type of serial receiver that you are using.  Save and reboot.
 
-Using CLI:
-For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as follows.
+#### Using CLI:
 
-| Serial RX Provider | Value |
-| ------------------ | ----- |
-| SPEKTRUM1024       | 0     |
-| SPEKTRUM2048       | 1     |
-| SBUS               | 2     |
-| SUMD               | 3     |
-| SUMH               | 4     |
-| XBUS_MODE_B        | 5     |
-| XBUS_MODE_B_RJ01   | 6     |
-| SERIALRX_IBUS      | 7     |
-| SERIALRX_JETIEXBUS | 8     |
-| SERIALRX_CRSF      | 9     |
+For Serial RX set the `receiver_type` and `serialrx_provider` setting as appropriate for your RX.
+
+```
+# get rec
+receiver_type = SERIAL
+Allowed values: NONE, PPM, SERIAL, MSP, SPI, UIB
+
+# get serialrx
+serialrx_provider = SBUS
+Allowed values: SPEK1024, SPEK2048, SBUS, SUMD, SUMH, XB-B, XB-B-RJ01, IBUS, JETIEXBUS, CRSF, FPORT, SBUS_FAST, FPORT2, SRXL2
+
+```
 
 ### PPM/PWM input filtering.
 

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

@@ -41,6 +41,18 @@ Mount MS windows C drive and clone iNav
 You are ready!
 You now have a folder called inav in the root of C drive that you can edit in windows
 
+### If you get a cloning error
+
+On some installations, you may see the following error:
+```
+Cloning into 'inav'...
+error: chmod on /mnt/c/inav/.git/config.lock failed: Operation not permitted
+fatal: could not set 'core.filemode' to 'false'
+```
+
+You can fix this with by remounting the drive using the following commands
+1. `sudo umount /mnt/c`
+2. `sudo mount -t drvfs C: /mnt/c -o metadata`
 
 ## Building (example):
 
@@ -84,7 +96,9 @@ make[1]: *** [CMakeFiles/Makefile2:33290: src/main/target/MATEKF722SE/CMakeFiles
 make: *** [Makefile:13703: MATEKF722SE] Error 2
 ```
 
-This error can be triggered by a Windows PATHs included in the Linux Subsystem. The solution is to:
+This error can be triggered by a Windows PATHs included in the Linux Subsystem. The solution is:
+
+#### For WSL V1 - Flags set as 7 by default
 
 1. Open Windows RegEdit tool
 1. Find `HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersion\Lxss\{GUID}\Flags`
@@ -93,3 +107,28 @@ This error can be triggered by a Windows PATHs included in the Linux Subsystem.
 1. `cd build`
 1. `cmake ..`
 1. `make {TARGET}` should be working again 
+
+#### For WSL V2 - Flags set as 0x0000000f (15) by default
+1. Open Windows RegEdit tool
+1. Find `HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersion\Lxss\{GUID}\Flags`
+1. Change `Flags` from `f` to `d`, it is stored as Base Hexadecimal
+1. Restart WSL and Windows preferably
+1. `cd build`
+1. `cmake ..`
+1. `make {TARGET}` should be working again 
+
+#### Or, for either version
+1. In the Linux Subsystem, `cd /etc/`
+2. Create a new file with `sudo nano wsl.conf`
+3. Enter the following in to the new file:
+```
+[Interop]
+appendWindowsPath=false
+```
+4. Save the file by holding `Ctrl` and pressing `o`
+5. Press `Enter` to confirm the wsl.conf filename.
+6. Hit `Ctrl`+`x` to exit nano
+7. Restart WSL and Windows preferably
+8. `cd build`
+9. `cmake ..`
+9. `make {TARGET}` should be working again 

docs/development/IDE - Visual Studio Code with Windows 10.md

@@ -104,13 +104,13 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard
         }
         ,
         {
-            "label": "CMAKE Update",
+            "label": "Install/Update CMAKE",
             "type": "shell",
-            "command": "cmake ..",
+            "command": "mkdir -p build && cd build && cmake ..",
             "group": "build",
             "problemMatcher": [],
             "options": {
-                "cwd": "${workspaceFolder}/build"
+                "cwd": "${workspaceFolder}"
             }
         }
     ]

src/main/CMakeLists.txt

@@ -118,6 +118,8 @@ main_sources(COMMON_SRC
     drivers/barometer/barometer_ms56xx.h
     drivers/barometer/barometer_spl06.c
     drivers/barometer/barometer_spl06.h
+    drivers/barometer/barometer_msp.c
+    drivers/barometer/barometer_msp.h
 
     drivers/buf_writer.c
     drivers/buf_writer.h
@@ -148,6 +150,8 @@ main_sources(COMMON_SRC
     drivers/compass/compass_mpu9250.h
     drivers/compass/compass_qmc5883l.c
     drivers/compass/compass_qmc5883l.h
+    drivers/compass/compass_msp.c
+    drivers/compass/compass_msp.h
 
     drivers/display.c
     drivers/display.h
@@ -195,12 +199,14 @@ main_sources(COMMON_SRC
     drivers/osd.h
     drivers/persistent.c
     drivers/persistent.h
-    drivers/pitotmeter_adc.c
-    drivers/pitotmeter_adc.h
-    drivers/pitotmeter_ms4525.c
-    drivers/pitotmeter_ms4525.h
-    drivers/pitotmeter_virtual.c
-    drivers/pitotmeter_virtual.h
+    drivers/pitotmeter/pitotmeter_adc.c
+    drivers/pitotmeter/pitotmeter_adc.h
+    drivers/pitotmeter/pitotmeter_ms4525.c
+    drivers/pitotmeter/pitotmeter_ms4525.h
+    drivers/pitotmeter/pitotmeter_msp.c
+    drivers/pitotmeter/pitotmeter_msp.h
+    drivers/pitotmeter/pitotmeter_virtual.c
+    drivers/pitotmeter/pitotmeter_virtual.h
     drivers/pwm_esc_detect.c
     drivers/pwm_esc_detect.h
     drivers/pwm_mapping.c
@@ -401,6 +407,8 @@ main_sources(COMMON_SRC
     rx/sbus_channels.h
     rx/spektrum.c
     rx/spektrum.h
+    rx/srxl2.c
+    rx/srxl2.h
     rx/sumd.c
     rx/sumd.h
     rx/sumh.c
@@ -486,6 +494,8 @@ main_sources(COMMON_SRC
     io/displayport_msp.h
     io/displayport_oled.c
     io/displayport_oled.h
+    io/displayport_srxl.c
+    io/displayport_srxl.h
     io/displayport_hott.c
     io/displayport_hott.h
     io/flashfs.c
@@ -495,6 +505,7 @@ main_sources(COMMON_SRC
     io/gps_ublox.c
     io/gps_nmea.c
     io/gps_naza.c
+    io/gps_msp.c
     io/gps_private.h
     io/ledstrip.c
     io/ledstrip.h
@@ -547,6 +558,8 @@ main_sources(COMMON_SRC
 
     telemetry/crsf.c
     telemetry/crsf.h
+    telemetry/srxl.c
+    telemetry/srxl.h
     telemetry/frsky.c
     telemetry/frsky.h
     telemetry/frsky_d.c

src/main/cms/cms_menu_osd.c

@@ -143,8 +143,9 @@ static const OSD_Entry menuCrsfRxEntries[]=
 {
     OSD_LABEL_ENTRY("-- CRSF RX --"),
 
-    OSD_SETTING_ENTRY("CRSF SNR LEVEL", SETTING_OSD_SNR_ALARM),
-    OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_RSSI_ALARM),
+    OSD_SETTING_ENTRY("LQ FORMAT", SETTING_OSD_CRSF_LQ_FORMAT),
+    OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM),
+    OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM),
     OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM),
     OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ),
     OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB),

src/main/common/filter.c

@@ -135,6 +135,17 @@ void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t s
     biquadFilterInit(filter, filterFreq, samplingIntervalUs, BIQUAD_Q, FILTER_LPF);
 }
 
+
+static void biquadFilterSetupPassthrough(biquadFilter_t *filter)
+{
+    // By default set as passthrough
+    filter->b0 = 1.0f;
+    filter->b1 = 0.0f;
+    filter->b2 = 0.0f;
+    filter->a1 = 0.0f;
+    filter->a2 = 0.0f;
+}
+
 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType)
 {
     // Check for Nyquist frequency and if it's not possible to initialize filter as requested - set to no filtering at all
@@ -158,6 +169,9 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
                 b1 = -2 * cs;
                 b2 = 1;
                 break;
+            default:
+                biquadFilterSetupPassthrough(filter);
+                return;
         }
         const float a0 =  1 + alpha;
         const float a1 = -2 * cs;
@@ -169,14 +183,8 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
         filter->b2 = b2 / a0;
         filter->a1 = a1 / a0;
         filter->a2 = a2 / a0;
-    }
-    else {
-        // Not possible to filter frequencies above Nyquist frequency - passthrough
-        filter->b0 = 1.0f;
-        filter->b1 = 0.0f;
-        filter->b2 = 0.0f;
-        filter->a1 = 0.0f;
-        filter->a2 = 0.0f;
+    } else {
+        biquadFilterSetupPassthrough(filter);
     }
 
     // zero initial samples

src/main/common/log.h

@@ -30,6 +30,7 @@ typedef enum {
     LOG_TOPIC_POS_ESTIMATOR,    // 8, mask = 256
     LOG_TOPIC_VTX,              // 9, mask = 512
     LOG_TOPIC_OSD,              // 10, mask = 1024
+    LOG_TOPIC_GPS,              // 11, mask = 2048
 
     LOG_TOPIC_COUNT,
 } logTopic_e;

src/main/config/parameter_group_ids.h

@@ -114,7 +114,8 @@
 #define PG_SMARTPORT_MASTER_CONFIG 1024
 #define PG_OSD_LAYOUTS_CONFIG 1025
 #define PG_SAFE_HOME_CONFIG 1026
-#define PG_INAV_END 1026
+#define PG_DJI_OSD_CONFIG 1027
+#define PG_INAV_END 1027
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/barometer/barometer_msp.c

@@ -0,0 +1,102 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#if defined(USE_BARO_MSP)
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/time.h"
+#include "drivers/barometer/barometer.h"
+#include "drivers/barometer/barometer_msp.h"
+
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
+#define MSP_BARO_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
+
+static int32_t mspBaroPressure;
+static int32_t mspBaroTemperature;
+static timeMs_t mspBaroLastUpdateMs;
+
+static bool mspBaroStartGet(baroDev_t * baro)
+{
+    UNUSED(baro);
+    return true;
+}
+
+static bool mspBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *temperature)
+{
+    UNUSED(baro);
+
+    if ((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS) {
+        return false;
+    }
+
+    if (pressure)
+        *pressure = mspBaroPressure;
+
+    if (temperature)
+        *temperature = mspBaroTemperature;
+
+    return true;
+}
+
+void mspBaroReceiveNewData(uint8_t * bufferPtr)
+{
+    const mspSensorBaroDataMessage_t * pkt = (const mspSensorBaroDataMessage_t *)bufferPtr;
+
+    mspBaroPressure = pkt->pressurePa;
+    mspBaroTemperature = pkt->temp;
+    mspBaroLastUpdateMs = millis();
+}
+
+bool mspBaroDetect(baroDev_t *baro)
+{
+    mspBaroPressure = 101325;    // pressure in Pa (0m MSL)
+    mspBaroTemperature = 2500;   // temperature in 0.01 C = 25 deg
+
+    // these are dummy as temperature is measured as part of pressure
+    baro->ut_delay = 10000;
+    baro->get_ut = mspBaroStartGet;
+    baro->start_ut = mspBaroStartGet;
+
+    // only _up part is executed, and gets both temperature and pressure
+    baro->up_delay = 10000;
+    baro->start_up = mspBaroStartGet;
+    baro->get_up = mspBaroStartGet;
+
+    baro->calculate = mspBaroCalculate;
+
+    return true;
+}
+
+#endif

src/main/drivers/barometer/barometer_msp.h

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

src/main/drivers/compass/compass_msp.c

@@ -0,0 +1,96 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#if defined(USE_MAG_MSP)
+
+#include "build/build_config.h"
+
+#include "common/axis.h"
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/time.h"
+#include "drivers/compass/compass.h"
+#include "drivers/compass/compass_msp.h"
+
+#include "sensors/boardalignment.h"
+
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
+#define MSP_MAG_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
+
+static int32_t mspMagData[XYZ_AXIS_COUNT];
+static timeMs_t mspMagLastUpdateMs;
+
+static bool mspMagInit(magDev_t *magDev)
+{
+    UNUSED(magDev);
+    mspMagData[X] = 0;
+    mspMagData[Y] = 0;
+    mspMagData[Z] = 0;
+    mspMagLastUpdateMs = 0;
+    return true;
+}
+
+void mspMagReceiveNewData(uint8_t * bufferPtr)
+{
+    const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr;
+
+    mspMagData[X] = pkt->magX;
+    mspMagData[Y] = pkt->magY;
+    mspMagData[Z] = pkt->magZ;
+
+    applySensorAlignment(mspMagData, mspMagData, CW90_DEG_FLIP);
+
+    mspMagLastUpdateMs = millis();
+}
+
+static bool mspMagRead(magDev_t *magDev)
+{
+    UNUSED(magDev);
+
+    if ((millis() - mspMagLastUpdateMs) > MSP_MAG_TIMEOUT_MS) {
+        return false;
+    }
+
+    magDev->magADCRaw[X] = mspMagData[X];
+    magDev->magADCRaw[Y] = mspMagData[Y];
+    magDev->magADCRaw[Z] = mspMagData[Z];
+
+    return true;
+}
+
+bool mspMagDetect(magDev_t *mag)
+{
+    mag->init = mspMagInit;
+    mag->read = mspMagRead;
+    return true;
+}
+
+#endif

src/main/drivers/compass/compass_msp.h

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

src/main/drivers/pitotmeter/pitotmeter_adc.c

@@ -24,9 +24,9 @@
 
 #include "common/utils.h"
 
-#include "pitotmeter.h"
-#include "pitotmeter_adc.h"
-#include "adc.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_adc.h"
+#include "drivers/adc.h"
 
 #if defined(USE_ADC) && defined(USE_PITOT_ADC)
 

src/main/drivers/pitotmeter/pitotmeter_ms4525.c

@@ -24,8 +24,8 @@
 #include "common/utils.h"
 #include "common/maths.h"
 #include "drivers/bus_i2c.h"
-#include "drivers/pitotmeter.h"
 #include "drivers/time.h"
+#include "drivers/pitotmeter/pitotmeter.h"
 
 // MS4525, Standard address 0x28
 #define MS4525_ADDR                 0x28

src/main/drivers/pitotmeter/pitotmeter_msp.c

@@ -0,0 +1,97 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#if defined(USE_PITOT_MSP)
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/time.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_msp.h"
+
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
+#define MSP_PITOT_TIMEOUT_MS      500     // Less than 2Hz updates is considered a failure
+
+static int32_t mspPitotPressure;
+static int32_t mspPitotTemperature;
+static timeMs_t mspPitotLastUpdateMs;
+
+static bool mspPitotStart(pitotDev_t *pitot)
+{
+    UNUSED(pitot);
+    return true;
+}
+
+static bool mspPitotRead(pitotDev_t *pitot)
+{
+    UNUSED(pitot);
+    return true;
+}
+
+static void mspPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature)
+{
+    UNUSED(pitot);
+
+    if (pressure) {
+        *pressure = mspPitotPressure;
+    }
+
+    if (temperature) {
+        *temperature = (mspPitotTemperature - 27315) / 100.0f;  // Pitot expects temp in Kelvin
+    }
+}
+
+void mspPitotmeterReceiveNewData(uint8_t * bufferPtr)
+{
+    const mspSensorAirspeedDataMessage_t * pkt = (const mspSensorAirspeedDataMessage_t *)bufferPtr;
+
+    mspPitotPressure = pkt->diffPressurePa;
+    mspPitotTemperature = pkt->temp;
+    mspPitotLastUpdateMs = millis();
+}
+
+bool mspPitotmeterDetect(pitotDev_t *pitot)
+{
+    mspPitotPressure = 0;
+    mspPitotTemperature = 27315;    // 0 deg/c
+
+    pitot->delay = 10000;
+    pitot->start = mspPitotStart;
+    pitot->get = mspPitotRead;
+    pitot->calculate = mspPitotCalculate;
+
+    return true;
+}
+
+#endif

src/main/drivers/pitotmeter/pitotmeter_msp.h

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

src/main/drivers/pitotmeter/pitotmeter_virtual.c

@@ -42,8 +42,8 @@
 
 #include "sensors/pitotmeter.h"
 
-#include "pitotmeter.h"
-#include "pitotmeter_virtual.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_virtual.h"
 
 #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) 
 

src/main/drivers/sdcard/sdmmc_sdio_f7xx.c

@@ -1000,7 +1000,7 @@ SD_Error_t SD_GetStatus(void)
         }
     }
     else {
-        ErrorState = SD_CARD_ERROR;
+        ErrorState = SD_ERROR;
     }
 
     return ErrorState;

src/main/fc/cli.c

@@ -103,6 +103,7 @@ extern uint8_t __config_end;
 #include "rx/rx.h"
 #include "rx/spektrum.h"
 #include "rx/eleres.h"
+#include "rx/srxl2.h"
 
 #include "scheduler/scheduler.h"
 
@@ -2578,6 +2579,35 @@ static void cliEleresBind(char *cmdline)
 }
 #endif // USE_RX_ELERES
 
+#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
+void cliRxBind(char *cmdline){
+    UNUSED(cmdline);
+    if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
+        switch (rxConfig()->serialrx_provider) {
+        default:
+            cliPrint("Not supported.");
+            break;
+#if defined(USE_SERIALRX_SRXL2)
+        case SERIALRX_SRXL2:
+            srxl2Bind();
+            cliPrint("Binding SRXL2 receiver...");
+            break;
+#endif
+        }
+    } 
+#if defined(USE_RX_SPI)
+    else if (rxConfig()->receiverType == RX_TYPE_SPI) {
+        switch (rxConfig()->rx_spi_protocol) {
+        default:
+            cliPrint("Not supported.");
+            break;
+        }
+    
+    }
+#endif
+}
+#endif
+
 static void cliExit(char *cmdline)
 {
     UNUSED(cmdline);
@@ -3465,6 +3495,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
             "\t<+|->[name]", cliBeeper),
 #endif
+#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
+    CLI_COMMAND_DEF("bind_rx", "initiate binding for RX SPI or SRXL2", NULL, cliRxBind),
+#endif
 #if defined(USE_BOOTLOG)
     CLI_COMMAND_DEF("bootlog", "show boot events", NULL, cliBootlog),
 #endif

src/main/fc/fc_init.c

@@ -107,6 +107,7 @@
 #include "io/displayport_frsky_osd.h"
 #include "io/displayport_msp.h"
 #include "io/displayport_max7456.h"
+#include "io/displayport_srxl.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
@@ -573,6 +574,11 @@ void init(void)
     uavInterconnectBusInit();
 #endif
 
+#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
+    // Register the srxl Textgen telemetry sensor as a displayport device
+    cmsDisplayPortRegister(displayPortSrxlInit());
+#endif
+
 #ifdef USE_GPS
     if (feature(FEATURE_GPS)) {
         gpsInit();

src/main/fc/fc_msp.c

@@ -40,8 +40,11 @@
 #include "config/parameter_group_ids.h"
 
 #include "drivers/accgyro/accgyro.h"
-#include "drivers/bus_i2c.h"
 #include "drivers/compass/compass.h"
+#include "drivers/compass/compass_msp.h"
+#include "drivers/barometer/barometer_msp.h"
+#include "drivers/pitotmeter/pitotmeter_msp.h"
+#include "drivers/bus_i2c.h"
 #include "drivers/display.h"
 #include "drivers/flash.h"
 #include "drivers/osd.h"
@@ -3215,6 +3218,30 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
             mspOpflowReceiveNewData(sbufPtr(src));
             break;
 #endif
+
+#if defined(USE_GPS_PROTO_MSP)
+        case MSP2_SENSOR_GPS:
+            mspGPSReceiveNewData(sbufPtr(src));
+            break;
+#endif
+
+#if defined(USE_MAG_MSP)
+        case MSP2_SENSOR_COMPASS:
+            mspMagReceiveNewData(sbufPtr(src));
+            break;
+#endif
+
+#if defined(USE_BARO_MSP)
+        case MSP2_SENSOR_BAROMETER:
+            mspBaroReceiveNewData(sbufPtr(src));
+            break;
+#endif
+
+#if defined(USE_PITOT_MSP)
+        case MSP2_SENSOR_AIRSPEED:
+            mspPitotmeterReceiveNewData(sbufPtr(src));
+            break;
+#endif
     }
 
     return MSP_RESULT_NO_REPLY;

src/main/fc/settings.yaml

@@ -10,22 +10,22 @@ tables:
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"]
     enum: rangefinderType_e
   - name: mag_hardware
-    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "FAKE"]
+    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"]
     enum: magSensor_e
   - name: opflow_hardware
     values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"]
     enum: opticalFlowSensor_e
   - name: baro_hardware
-    values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "FAKE"]
+    values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"]
     enum: baroSensor_e
   - name: pitot_hardware
-    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"]
+    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
     enum: pitotSensor_e
   - name: receiver_type
     values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
     enum: rxReceiverType_e
   - name: serial_rx
-    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2"]
+    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2"]
   - name: rx_spi_protocol
     values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
     enum: rx_spi_protocol_e
@@ -44,7 +44,7 @@ tables:
     values: ["NONE", "ADC", "ESC"]
     enum: voltageSensor_e
   - name: gps_provider
-    values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK"]
+    values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK", "MSP"]
     enum: gpsProvider_e
   - name: gps_sbas_mode
     values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"]
@@ -144,6 +144,17 @@ tables:
   - name: tristate
     enum: tristate_e
     values: ["AUTO", "ON", "OFF"]
+  - name: osd_crsf_lq_format
+    enum: osd_crsf_lq_format_e
+    values: ["TYPE1", "TYPE2"]
+  - name: off_on
+    values: ["OFF", "ON"]
+  - name: djiOsdTempSource
+    values: ["ESC", "IMU", "BARO"]
+    enum: djiOsdTempSource_e
+  - name: nav_overrides_motor_stop
+    enum: navOverridesMotorStop_e
+    values: ["OFF", "AUTO_ONLY", "ALL_NAV"]
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -567,6 +578,13 @@ groups:
         condition: USE_SPEKTRUM_BIND
         min: SPEKTRUM_SAT_BIND_DISABLED
         max: SPEKTRUM_SAT_BIND_MAX
+      - name: srxl2_unit_id
+        condition: USE_SERIALRX_SRXL2
+        min: 0
+        max: 15
+      - name: srxl2_baud_fast
+        condition: USE_SERIALRX_SRXL2
+        table: off_on
       - name: rx_min_usec
         description: "Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc."
         default_value: "885"
@@ -1539,6 +1557,12 @@ groups:
         field: fixedWingCoordinatedYawGain
         min: 0
         max: 2
+      - name: fw_turn_assist_pitch_gain
+        description: "Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter"
+        default_value: "1"
+        field: fixedWingCoordinatedPitchGain
+        min: 0
+        max: 2
       - name: fw_iterm_limit_stick_position
         description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side"
         default_value: "0.5"
@@ -2083,10 +2107,10 @@ groups:
         min: 0
         max: 5000
       - name: nav_overrides_motor_stop
-        description: "Setting to OFF combined with MOTOR_STOP feature will allow user to stop motor when in autonomous modes. On most places this setting is likely to cause a stall."
-        default_value: "ON"
-        field: general.flags.auto_overrides_motor_stop
-        type: bool
+        description: "When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV"
+        default_value: "NOMS_ALL_NAV"
+        field: general.flags.nav_overrides_motor_stop
+        table: nav_overrides_motor_stop
       - name: nav_rth_climb_first
         description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way."
         default_value: "ON"
@@ -2630,7 +2654,7 @@ groups:
         type: uint8_t
 
       - name: osd_rssi_alarm
-        description: "Value bellow which to make the OSD RSSI indicator blink"
+        description: "Value below which to make the OSD RSSI indicator blink"
         default_value: "20"
         field: rssi_alarm
         min: 0
@@ -2654,7 +2678,7 @@ groups:
         min: 0
         max: 50000
       - name: osd_neg_alt_alarm
-        description: "Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters)"
+        description: "Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters)"
         default_value: "5"
         field: neg_alt_alarm
         min: 0
@@ -2723,10 +2747,16 @@ groups:
         max: 1250
       - name: osd_snr_alarm
         description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
-        default_value: "5"
+        default_value: "4"
         field: snr_alarm
         min: -12
         max: 8
+      - name: osd_link_quality_alarm
+        description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%"
+        default_value: "70"
+        field: link_quality_alarm
+        min: 0
+        max: 100
       - name: osd_temp_label_align
         description: "Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT`"
         default_value: "LEFT"
@@ -2750,6 +2780,12 @@ groups:
         field: crosshairs_style
         table: osd_crosshairs_style
         type: uint8_t
+      - name: osd_crsf_lq_format
+        description: "To select LQ format"
+        default_value: "TYPE1"
+        field: crsf_lq_format
+        table: osd_crsf_lq_format
+        type: uint8_t
       - name: osd_horizon_offset
         description: "To vertically adjust the whole OSD and AHI and scrolling bars"
         default_value: "0"
@@ -3102,6 +3138,7 @@ groups:
           max: UINT32_MAX
           description: "Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage."
           default_value: "0"
+
   - name: PG_ESC_SENSOR_CONFIG
     type: escSensorConfig_t
     headers: ["sensors/esc_sensor.h"]
@@ -3122,3 +3159,23 @@ groups:
       - name: smartport_master_inverted
         field: inverted
         type: bool
+
+  - name: PG_DJI_OSD_CONFIG
+    type: djiOsdConfig_t
+    headers: ["io/osd_dji_hd.h"]
+    condition: USE_DJI_HD_OSD
+    members:
+        - name: dji_workarounds
+          description: "Enables workarounds for different versions of MSP protocol used"
+          field: proto_workarounds
+        - name: dji_use_name_for_messages
+          description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance"
+          default_value: "ON"
+          field: use_name_for_messages
+          type: bool
+        - name: dji_esc_temp_source
+          description: "Re-purpose the ESC temperature field for IMU/BARO temperature"
+          default_value: "ESC"
+          field: esc_temperature_source
+          table: djiOsdTempSource
+          type: uint8_t

src/main/flight/imu.c

@@ -77,7 +77,7 @@ FILE_COMPILE_FOR_SPEED
 // http://gentlenav.googlecode.com/files/fastRotations.pdf
 
 #define SPIN_RATE_LIMIT             20
-#define MAX_ACC_SQ_NEARNESS         25      // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
+#define MAX_ACC_NEARNESS            0.33    // 33% or G error soft-accepted (0.67-1.33G)
 #define IMU_CENTRIFUGAL_LPF         1       // Hz
 
 FASTRAM fpVector3_t imuMeasuredAccelBF;
@@ -472,16 +472,12 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 
 static float imuCalculateAccelerometerWeight(const float dT)
 {
-    // If centrifugal test passed - do the usual "nearness" style check
     float accMagnitudeSq = 0;
-
     for (int axis = 0; axis < 3; axis++) {
         accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis];
     }
 
-    // Magnitude^2 in percent of G^2
-    const float nearness = ABS(100 - (accMagnitudeSq * 100));
-    const float accWeight_Nearness = (nearness > MAX_ACC_SQ_NEARNESS) ? 0.0f : 1.0f;
+    const float accWeight_Nearness = bellCurve(sqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
 
     // Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we 
     // should not use measured accel for AHRS comp

src/main/flight/mixer.c

@@ -579,7 +579,9 @@ motorStatus_e getMotorStatus(void)
     }
 
     if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
-        if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
+        navOverridesMotorStop_e motorStopOverride = navConfig()->general.flags.nav_overrides_motor_stop;
+        if (!failsafeIsActive() && (STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) &&
+            ((motorStopOverride == NOMS_OFF) || ((motorStopOverride == NOMS_ALL_NAV) && !navigationIsControllingThrottle()) || ((motorStopOverride == NOMS_AUTO_ONLY) && !navigationIsFlyingAutonomousMode()))) {
             return MOTOR_STOPPED_USER;
         }
     }

src/main/flight/pid.c

@@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 15);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -256,6 +256,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT,
         .fixedWingReferenceAirspeed = 1000,
         .fixedWingCoordinatedYawGain = 1.0f,
+        .fixedWingCoordinatedPitchGain = 1.0f,
         .fixedWingItermLimitOnStickPosition = 0.5f,
 
         .loiter_direction = NAV_LOITER_RIGHT,
@@ -889,7 +890,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
 
     // Add in roll and pitch
     pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f);
-    pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
+    pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
 
     // Replace YAW on quads - add it in on airplanes
     if (STATE(AIRPLANE)) {

src/main/flight/pid.h

@@ -126,6 +126,7 @@ typedef struct pidProfile_s {
     uint16_t    fixedWingItermThrowLimit;
     float       fixedWingReferenceAirspeed;     // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
+    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
 
     uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)

src/main/io/asyncfatfs/asyncfatfs.c

@@ -1694,6 +1694,8 @@ static afatfsOperationStatus_e afatfs_appendSuperclusterContinue(afatfsFile_t *f
             // Update the fileSize/firstCluster in the directory entry for the file
             status = afatfs_saveDirectoryEntry(file, AFATFS_SAVE_DIRECTORY_NORMAL);
         break;
+        default:
+            status = AFATFS_OPERATION_FAILURE;
     }
 
     if ((status == AFATFS_OPERATION_FAILURE || status == AFATFS_OPERATION_SUCCESS) && file->operation.operation == AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER) {
@@ -2487,6 +2489,8 @@ static afatfsOperationStatus_e afatfs_ftruncateContinue(afatfsFilePtr_t file, bo
 
             return AFATFS_OPERATION_SUCCESS;
         break;
+        default:
+            status = AFATFS_OPERATION_FAILURE;
     }
 
     if (status == AFATFS_OPERATION_FAILURE && file->operation.operation == AFATFS_FILE_OPERATION_TRUNCATE) {

src/main/io/beeper.c

@@ -189,11 +189,11 @@ typedef struct beeperTableEntry_s {
     { BEEPER_ENTRY(BEEPER_USB,                      18, NULL,                           "ON_USB") },
     { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED,      19, beep_launchModeBeep,            "LAUNCH_MODE") },
     { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_LOW_THROTTLE, 20, beep_launchModeLowThrottleBeep, "LAUNCH_MODE_LOW_THROTTLE") },
-    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN,      20, beep_camOpenBeep,               "CAM_CONNECTION_OPEN") },
-    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE,     21, beep_camCloseBeep,              "CAM_CONNECTION_CLOSED") },
+    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN,      21, beep_camOpenBeep,               "CAM_CONNECTION_OPEN") },
+    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE,     22, beep_camCloseBeep,              "CAM_CONNECTION_CLOSED") },
 
-    { BEEPER_ENTRY(BEEPER_ALL,                      22, NULL,                           "ALL") },
-    { BEEPER_ENTRY(BEEPER_PREFERENCE,               23, NULL,                           "PREFERED") },
+    { BEEPER_ENTRY(BEEPER_ALL,                      23, NULL,                           "ALL") },
+    { BEEPER_ENTRY(BEEPER_PREFERENCE,               24, NULL,                           "PREFERED") },
 };
 
 static const beeperTableEntry_t *currentBeeperEntry = NULL;

src/main/io/beeper.h

@@ -43,7 +43,7 @@ typedef enum {
     BEEPER_SYSTEM_INIT,                 // Initialisation beeps when board is powered on
     BEEPER_USB,                         // Some boards have beeper powered USB connected
     BEEPER_LAUNCH_MODE_ENABLED,         // Fixed-wing launch mode enabled
-    BEEPER_LAUNCH_MODE_LOW_THROTTLE,// Fixed-wing launch mode enabled, but throttle is low
+    BEEPER_LAUNCH_MODE_LOW_THROTTLE,    // Fixed-wing launch mode enabled, but throttle is low
     BEEPER_CAM_CONNECTION_OPEN,         // When the 5 key simulation stated
     BEEPER_CAM_CONNECTION_CLOSE,        // When the 5 key simulation stop
 

src/main/io/displayport_srxl.c

@@ -0,0 +1,152 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL)
+
+#include "common/utils.h"
+
+#include "drivers/display.h"
+#include "cms/cms.h"
+
+#include "telemetry/srxl.h"
+
+displayPort_t srxlDisplayPort;
+
+static int srxlDrawScreen(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int srxlScreenSize(const displayPort_t *displayPort)
+{
+    return displayPort->rows * displayPort->cols;
+}
+
+static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
+{
+    return (spektrumTmTextGenPutChar(col, row, c));
+    UNUSED(displayPort);
+    UNUSED(attr);
+}
+
+
+static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s, textAttributes_t attr)
+{
+    while (*s) {
+        srxlWriteChar(displayPort, col++, row, *(s++), attr);
+    }
+    return 0;
+}
+
+static int srxlClearScreen(displayPort_t *displayPort)
+{
+    textAttributes_t attr = 0;
+    for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
+        for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
+            srxlWriteChar(displayPort, col, row, ' ', attr);
+        }
+    }
+    srxlWriteString(displayPort, 1, 0, "INAV", attr);
+
+    if (displayPort->grabCount == 0) {
+        srxlWriteString(displayPort, 0, 2, CMS_STARTUP_HELP_TEXT1, attr);
+        srxlWriteString(displayPort, 2, 3, CMS_STARTUP_HELP_TEXT2, attr);
+        srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT3, attr);
+    }
+    return 0;
+}
+
+static bool srxlIsTransferInProgress(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return false;
+}
+
+/*
+static bool srxlIsSynced(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return true;
+}
+*/
+
+static int srxlHeartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static void srxlResync(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+}
+
+static uint32_t srxlTxBytesFree(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return UINT32_MAX;
+}
+
+static int srxlGrab(displayPort_t *displayPort)
+{
+    return displayPort->grabCount = 1;
+}
+
+static int srxlRelease(displayPort_t *displayPort)
+{
+    int cnt = displayPort->grabCount = 0;
+    srxlClearScreen(displayPort);
+    return cnt;
+}
+
+static const displayPortVTable_t srxlVTable = {
+    .grab = srxlGrab,
+    .release = srxlRelease,
+    .clearScreen = srxlClearScreen,
+    .drawScreen = srxlDrawScreen,
+    .screenSize = srxlScreenSize,
+    .writeString = srxlWriteString,
+    .writeChar = srxlWriteChar,
+    .isTransferInProgress = srxlIsTransferInProgress,
+    .heartbeat = srxlHeartbeat,
+    .resync = srxlResync,
+    //.isSynced = srxlIsSynced,
+    .txBytesFree = srxlTxBytesFree,
+    //.layerSupported = NULL,
+    //.layerSelect = NULL,
+    //.layerCopy = NULL,
+};
+
+displayPort_t *displayPortSrxlInit(void)
+{
+    srxlDisplayPort.device = NULL;
+    displayInit(&srxlDisplayPort, &srxlVTable);
+    srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
+    srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
+    return &srxlDisplayPort;
+}
+
+#endif

src/main/io/displayport_srxl.h

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

src/main/io/gps.c

@@ -60,6 +60,7 @@
 #include "fc/runtime_config.h"
 
 typedef struct {
+    bool                isDriverBased;
     portMode_t          portMode;           // Port mode RX/TX (only for serial based)
     bool                hasCompass;         // Has a compass (NAZA)
     void                (*restart)(void);   // Restart protocol driver thread
@@ -77,42 +78,48 @@ baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600,
 static gpsProviderDescriptor_t  gpsProviders[GPS_PROVIDER_COUNT] = {
     /* NMEA GPS */
 #ifdef USE_GPS_PROTO_NMEA
-    { MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA },
+    { false, MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA },
 #else
-    { 0, false,  NULL, NULL },
+    { false, 0, false,  NULL, NULL },
 #endif
 
     /* UBLOX binary */
 #ifdef USE_GPS_PROTO_UBLOX
-    { MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
+    { false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
 #else
-    { 0, false,  NULL, NULL },
+    { false, 0, false,  NULL, NULL },
 #endif
 
     /* Stub */
-    { 0, false,  NULL, NULL },
+    { false, 0, false,  NULL, NULL },
 
     /* NAZA GPS module */
 #ifdef USE_GPS_PROTO_NAZA
-    { MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA },
+    { false, MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA },
 #else
-    { 0, false,  NULL, NULL },
+    { false, 0, false,  NULL, NULL },
 #endif
 
     /* UBLOX7PLUS binary */
 #ifdef USE_GPS_PROTO_UBLOX
-    { MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
+    { false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX },
 #else
-    { 0, false,  NULL, NULL },
+    { false, 0, false,  NULL, NULL },
 #endif
 
     /* MTK GPS */
 #ifdef USE_GPS_PROTO_MTK
-    { MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK },
+    { false, MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK },
 #else
-    { 0, false,  NULL, NULL },
+    { false, 0, false,  NULL, NULL },
 #endif
 
+    /* MSP GPS */
+#ifdef USE_GPS_PROTO_MSP
+    { true, 0, false, &gpsRestartMSP, &gpsHandleMSP },
+#else
+    { false, 0, false,  NULL, NULL },
+#endif
 };
 
 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
@@ -135,7 +142,7 @@ void gpsSetState(gpsState_e state)
 
 static void gpsUpdateTime(void)
 {
-    if (!rtcHasTime() && gpsSol.flags.validTime) {
+    if (!rtcHasTime() && gpsSol.flags.validTime && gpsSol.time.year != 0) {
         rtcSetDateTime(&gpsSol.time);
     }
 }
@@ -220,6 +227,12 @@ void gpsInit(void)
         return;
     }
 
+    // Shortcut for driver-based GPS (MSP)
+    if (gpsProviders[gpsState.gpsConfig->provider].isDriverBased) {
+        gpsSetState(GPS_INITIALIZING);
+        return;
+    }
+
     serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
     if (!gpsPortConfig) {
         featureClear(FEATURE_GPS);

src/main/io/gps.h

@@ -38,6 +38,7 @@ typedef enum {
     GPS_NAZA,
     GPS_UBLOX7PLUS,
     GPS_MTK,
+    GPS_MSP,
     GPS_PROVIDER_COUNT
 } gpsProvider_e;
 
@@ -164,3 +165,4 @@ bool isGPSHealthy(void);
 bool isGPSHeadingValid(void);
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
+void mspGPSReceiveNewData(const uint8_t * bufferPtr);

src/main/io/gps_msp.c

@@ -0,0 +1,113 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <ctype.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+#include "build/build_config.h"
+
+
+#if defined(USE_GPS_PROTO_MSP)
+
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/gps_conversion.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/serial.h"
+#include "drivers/time.h"
+
+#include "fc/config.h"
+#include "fc/runtime_config.h"
+
+#include "io/gps.h"
+#include "io/gps_private.h"
+#include "io/serial.h"
+
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
+static bool newDataReady;
+
+void gpsRestartMSP(void)
+{
+    // NOP
+}
+
+void gpsHandleMSP(void)
+{
+    if (newDataReady) {
+        gpsProcessNewSolutionData();
+        newDataReady = false;
+    }
+}
+
+static uint8_t gpsMapFixType(uint8_t mspFixType)
+{
+    if (mspFixType == 2)
+        return GPS_FIX_2D;
+    if (mspFixType >= 3)
+        return GPS_FIX_3D;
+    return GPS_NO_FIX;
+}
+
+void mspGPSReceiveNewData(const uint8_t * bufferPtr)
+{
+    const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
+
+    gpsSol.fixType   = gpsMapFixType(pkt->fixType);
+    gpsSol.numSat    = pkt->satellitesInView;
+    gpsSol.llh.lon   = pkt->longitude;
+    gpsSol.llh.lat   = pkt->latitude;
+    gpsSol.llh.alt   = pkt->mslAltitude;
+    gpsSol.velNED[X] = pkt->nedVelNorth;
+    gpsSol.velNED[Y] = pkt->nedVelEast;
+    gpsSol.velNED[Z] = pkt->nedVelDown;
+    gpsSol.groundSpeed = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
+    gpsSol.groundCourse = pkt->groundCourse / 10;   // in deg * 10
+    gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
+    gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
+    gpsSol.hdop = gpsConstrainHDOP(pkt->hdop);
+    gpsSol.flags.validVelNE = 1;
+    gpsSol.flags.validVelD = 1;
+    gpsSol.flags.validEPE = 1;
+
+    gpsSol.time.year   = pkt->year;
+    gpsSol.time.month  = pkt->month;
+    gpsSol.time.day    = pkt->day;
+    gpsSol.time.hours  = pkt->hour;
+    gpsSol.time.minutes = pkt->min;
+    gpsSol.time.seconds = pkt->sec;
+    gpsSol.time.millis  = 0;
+
+    gpsSol.flags.validTime = (pkt->fixType >= 3);
+
+    newDataReady = true;
+}
+#endif

src/main/io/gps_private.h

@@ -78,4 +78,7 @@ extern void gpsHandleMTK(void);
 extern void gpsRestartNAZA(void);
 extern void gpsHandleNAZA(void);
 
+extern void gpsRestartMSP(void);
+extern void gpsHandleMSP(void);
+
 #endif

src/main/io/gps_ublox.c

@@ -35,6 +35,7 @@
 #include "common/gps_conversion.h"
 #include "common/maths.h"
 #include "common/utils.h"
+#include "common/log.h"
 
 #include "drivers/serial.h"
 #include "drivers/time.h"
@@ -274,6 +275,8 @@ enum {
     MSG_VELNED = 0x12,
     MSG_TIMEUTC = 0x21,
     MSG_SVINFO = 0x30,
+    MSG_NAV_SAT = 0x35,
+    MSG_NAV_SIG = 0x43,
     MSG_CFG_PRT = 0x00,
     MSG_CFG_RATE = 0x08,
     MSG_CFG_SET_RATE = 0x01,
@@ -701,6 +704,7 @@ STATIC_PROTOTHREAD(gpsConfigure)
     gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
 
     // Set dynamic model
+    LOG_D(GPS, "UBLOX: Configuring NAV5");
     switch (gpsState.gpsConfig->dynModel) {
         case GPS_DYNMODEL_PEDESTRIAN:
             configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
@@ -718,6 +722,8 @@ STATIC_PROTOTHREAD(gpsConfigure)
     // Disable NMEA messages
     gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
 
+    LOG_D(GPS, "UBLOX: Disable NMEA messages");
+
     configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0);
     ptWait(_ack_state == UBX_ACK_GOT_ACK);
 
@@ -739,7 +745,43 @@ STATIC_PROTOTHREAD(gpsConfigure)
     // Configure UBX binary messages
     gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
 
+    // u-Blox 9 
+    // M9N does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence
+    if (gpsState.hwVersion >= 190000) {
+        LOG_D(GPS, "UBLOX: Configuring UBX-9 messages");
+
+        configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+        configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+        configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+        configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+        configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+        configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+        configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+        // u-Blox 9 receivers such as M9N can do 10Hz as well, but the number of used satellites will be restricted to 16.
+        // Not mentioned in the datasheet
+        configureRATE(200);
+        ptWait(_ack_state == UBX_ACK_GOT_ACK);
+    }
+    else {
+        // u-Blox 6/7/8
+        // u-Blox 6 doesn't support PVT, use legacy config
         if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) {
+            LOG_D(GPS, "UBLOX: Configuring UBX-6 messages");
+
             configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
             ptWait(_ack_state == UBX_ACK_GOT_ACK);
 
@@ -758,15 +800,25 @@ STATIC_PROTOTHREAD(gpsConfigure)
             // This may fail on old UBLOX units, advance forward on both ACK and NAK
             configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
             ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
+
+            configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
+            ptWait(_ack_state == UBX_ACK_GOT_ACK);
+
+            // Configure data rate to 5HZ
+            configureRATE(200);
+            ptWait(_ack_state == UBX_ACK_GOT_ACK);
         }
+        // u-Blox 7-8 support PVT
         else {
+            LOG_D(GPS, "UBLOX: Configuring UBX-7 messages");
+
             configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
             ptWait(_ack_state == UBX_ACK_GOT_ACK);
 
             configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
             ptWait(_ack_state == UBX_ACK_GOT_ACK);
 
-        configureMSG(MSG_CLASS_UBX, MSG_SOL, 0);
+            configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
             ptWait(_ack_state == UBX_ACK_GOT_ACK);
 
             configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
@@ -777,13 +829,10 @@ STATIC_PROTOTHREAD(gpsConfigure)
 
             configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
             ptWait(_ack_state == UBX_ACK_GOT_ACK);
-    }
 
             configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
             ptWait(_ack_state == UBX_ACK_GOT_ACK);
 
-    // Configure data rate
-    gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
             if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= 70000)) {
                 configureRATE(100); // 10Hz
             }
@@ -791,6 +840,8 @@ STATIC_PROTOTHREAD(gpsConfigure)
                 configureRATE(200); // 5Hz
             }
             ptWait(_ack_state == UBX_ACK_GOT_ACK);
+        }
+    }
 
     // Configure SBAS
     // If particular SBAS setting is not supported by the hardware we'll get a NAK,
@@ -905,6 +956,14 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
             ptWaitTimeout((gpsState.hwVersion != 0), GPS_CFG_CMD_TIMEOUT_MS);
         } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == 0);
 
+        if (gpsState.hwVersion == 0) {
+            LOG_E(GPS, "UBLOX: version detection timeout");
+        }
+        else {
+            LOG_I(GPS, "UBLOX: detected HW: %u", (unsigned)gpsState.hwVersion);
+        }
+
+
         // Configure GPS
         ptSpawn(gpsConfigure);
     }

src/main/io/opflow_msp.c

@@ -37,15 +37,13 @@
 #include "io/serial.h"
 
 #if defined(USE_OPFLOW_MSP)
+
 #include "drivers/opflow/opflow_virtual.h"
 #include "drivers/time.h"
 #include "io/opflow.h"
 
-typedef struct __attribute__((packed)) {
-    uint8_t quality;    // [0;255]
-    int32_t motionX;
-    int32_t motionY;
-} mspOpflowSensor_t;
+#include "msp/msp_protocol_v2_sensor_msg.h"
+
 
 static bool hasNewData = false;
 static timeUs_t updatedTimeUs = 0;
@@ -76,7 +74,7 @@ static bool mspOpflowUpdate(opflowData_t * data)
 void mspOpflowReceiveNewData(uint8_t * bufferPtr)
 {
     const timeUs_t currentTimeUs = micros();
-    const mspOpflowSensor_t * pkt = (const mspOpflowSensor_t *)bufferPtr;
+    const mspSensorOpflowDataMessage_t * pkt = (const mspSensorOpflowDataMessage_t *)bufferPtr;
 
     sensorData.deltaTime = currentTimeUs - updatedTimeUs;
     sensorData.flowRateRaw[0] = pkt->motionX;

src/main/io/osd.c

@@ -417,6 +417,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
             centivalue = (ws * 224) / 100;
             suffix = SYM_MPH;
             break;
+        default:
         case OSD_UNIT_METRIC:
             centivalue = (ws * 36) / 10;
             suffix = SYM_KMH;
@@ -1011,6 +1012,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
             break;
         case OSD_UNIT_UK:
             FALLTHROUGH;
+        default:
         case OSD_UNIT_METRIC:
             initialScale = 10; // 10m as initial scale
             break;
@@ -1660,17 +1662,32 @@ static bool osdDrawSingleElement(uint8_t item)
             }
             break;
 
-    case OSD_CRSF_LQ:
+#if defined(USE_SERIALRX_CRSF)
+        case OSD_CRSF_LQ: {
         buff[0] = SYM_BLANK;
+        int16_t statsLQ = rxLinkStatistics.uplinkLQ;
+        int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
+            if (rxLinkStatistics.rfMode == 2) {
+                if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
+                    tfp_sprintf(buff, "%5d%s", scaledLQ, "%");
+                } else {
                     tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
+                }
+            } else {
+                if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
+                    tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%");
+            } else {
+                    tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
+            }
+            }
             if (!failsafeIsReceivingRxData()){
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-        } else if (rxLinkStatistics.uplinkLQ < osdConfig()->rssi_alarm) {
+            } else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) {
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
             break;
+        }
 
-#if defined(USE_SERIALRX_CRSF)
     case OSD_CRSF_SNR_DB: {
         const char* showsnr = "-12";
         const char* hidesnr = "     ";
@@ -1829,6 +1846,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     value = CENTIMETERS_TO_CENTIFEET(value);
                     sym = SYM_FTS;
                     break;
+                default:
                 case OSD_UNIT_METRIC:
                     // Already in cm/s
                     sym = SYM_MS;
@@ -2310,6 +2328,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 break;
             case OSD_UNIT_UK:
                 FALLTHROUGH;
+            default:
             case OSD_UNIT_METRIC:
                 scaleToUnit = 100; // scale to cm for osdFormatCentiNumber()
                 scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m
@@ -2509,7 +2528,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .baro_temp_alarm_max = 600,
 #endif
 #ifdef USE_SERIALRX_CRSF
-    .snr_alarm = 5,
+    .snr_alarm = 4,
+    .crsf_lq_format = OSD_CRSF_LQ_TYPE1,
+    .link_quality_alarm = 70,
 #endif
 #ifdef USE_TEMPERATURE_SENSOR
     .temp_label_align = OSD_ALIGN_LEFT,
@@ -2598,10 +2619,10 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
 
 #ifdef USE_SERIALRX_CRSF
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(22, 11);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(23, 9);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(24, 12);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(24, 11);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(25, 9);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(25, 10);
 #endif
 
     osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);

src/main/io/osd.h

@@ -260,6 +260,11 @@ typedef enum {
     OSD_AHI_STYLE_LINE,
 } osd_ahi_style_e;
 
+typedef enum {
+    OSD_CRSF_LQ_TYPE1,
+    OSD_CRSF_LQ_TYPE2,
+} osd_crsf_lq_format_e;
+
 typedef struct osdLayoutsConfig_s {
     // Layouts
     uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT];
@@ -284,6 +289,7 @@ typedef struct osdConfig_s {
     float gforce_axis_alarm_max;
 #ifdef USE_SERIALRX_CRSF
     int16_t snr_alarm; //CRSF SNR alarm in dB
+    int8_t link_quality_alarm;
 #endif
 #ifdef USE_BARO
     int16_t baro_temp_alarm_min;
@@ -337,6 +343,8 @@ typedef struct osdConfig_s {
     uint8_t left_sidebar_scroll_step;   // How many units each sidebar step represents. 0 means the default value for the scroll type.
     uint8_t right_sidebar_scroll_step;  // Same as left_sidebar_scroll_step, but for the right sidebar.
 
+    uint8_t crsf_lq_format;
+
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

src/main/io/osd_dji_hd.c

@@ -39,12 +39,16 @@
 #include "common/time.h"
 #include "common/crc.h"
 
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "fc/fc_core.h"
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/fc_msp.h"
 #include "fc/fc_msp_box.h"
 #include "fc/runtime_config.h"
+#include "fc/settings.h"
 
 #include "flight/imu.h"
 #include "flight/pid.h"
@@ -63,14 +67,21 @@
 #include "sensors/rangefinder.h"
 #include "sensors/acceleration.h"
 #include "sensors/esc_sensor.h"
+#include "sensors/temperature.h"
 
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
 
+#include "common/string_light.h"
 #include "navigation/navigation.h"
-
+#include "navigation/navigation_private.h"
+#include "common/constants.h"
 #include "scheduler/scheduler.h"
+#include "common/printf.h"
+#include <stdlib.h>
+#include "rx/rx.h"
+#include "fc/rc_controls.h"
 
 #if defined(USE_DJI_HD_OSD)
 
@@ -80,6 +91,24 @@
 #define DJI_OSD_WARNING_COUNT               16
 #define DJI_OSD_TIMER_COUNT                 2
 #define DJI_OSD_FLAGS_OSD_FEATURE           (1 << 0)
+#define EFFICIENCY_UPDATE_INTERVAL          (5 * 1000)
+
+#define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
+
+// Adjust OSD_MESSAGE's default position when
+// changing OSD_MESSAGE_LENGTH
+#define OSD_MESSAGE_LENGTH 28
+
+#define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices)
+#define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0'
+
+// Wrap all string constants intenteded for display as messages with
+// this macro to ensure compile time length validation.
+#define OSD_MESSAGE_STR(x) ({ \
+    STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \
+    x; \
+})
+
 
 /* 
  * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0
@@ -89,6 +118,13 @@
  * but reuse the packet decoder to minimize code duplication
  */
 
+PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 1);
+PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
+    .use_name_for_messages  = true,
+    .esc_temperature_source = DJI_OSD_TEMP_ESC,
+    .proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA,
+);
+
 // External dependency on looptime
 extern timeDelta_t cycleTime;
 
@@ -145,7 +181,7 @@ const djiOsdMapping_t djiOSDItemIndexMap[] = {
     { OSD_HEADING,                            0 }, // DJI: OSD_NUMERICAL_HEADING
     { OSD_VARIO_NUM,                          0 }, // DJI: OSD_NUMERICAL_VARIO
     { -1,                                     0 }, // DJI: OSD_COMPASS_BAR
-    { -1,                                     0 }, // DJI: OSD_ESC_TMP
+    { OSD_ESC_TEMPERATURE,                    0 }, // DJI: OSD_ESC_TEMPERATURE
     { OSD_ESC_RPM,                            0 }, // DJI: OSD_ESC_RPM
     { OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,   FEATURE_CURRENT_METER }, // DJI: OSD_REMAINING_TIME_ESTIMATE
     { OSD_RTC_TIME,                           0 }, // DJI: OSD_RTC_DATETIME
@@ -374,8 +410,493 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
     //sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width
     //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
 }
+
+static const char * osdArmingDisabledReasonMessage(void)
+{
+    switch (isArmingDisabledReason()) {
+        case ARMING_DISABLED_FAILSAFE_SYSTEM:
+            // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
+            if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) {
+                if (failsafeIsReceivingRxData()) {
+                    // If we're not using sticks, it means the ARM switch
+                    // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
+                    // yet
+                    return OSD_MESSAGE_STR("DISARM!");
+                }
+                // Not receiving RX data
+                return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
+            }
+            return OSD_MESSAGE_STR("FAILSAFE");
+        case ARMING_DISABLED_NOT_LEVEL:
+            return OSD_MESSAGE_STR("!LEVEL");
+        case ARMING_DISABLED_SENSORS_CALIBRATING:
+            return OSD_MESSAGE_STR("CALIBRATING");
+        case ARMING_DISABLED_SYSTEM_OVERLOADED:
+            return OSD_MESSAGE_STR("OVERLOAD");
+        case ARMING_DISABLED_NAVIGATION_UNSAFE:
+            // Check the exact reason
+            switch (navigationIsBlockingArming(NULL)) {
+                case NAV_ARMING_BLOCKER_NONE:
+                    break;
+                case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
+                    return OSD_MESSAGE_STR("NO GPS FIX");
+                case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
+                    return OSD_MESSAGE_STR("DISABLE NAV");
+                case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
+                    return OSD_MESSAGE_STR("1ST WYP TOO FAR");
+                case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
+                    return OSD_MESSAGE_STR("WYP MISCONFIGURED");
+            }
+            break;
+        case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
+            return OSD_MESSAGE_STR("COMPS CALIB");
+        case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED:
+            return OSD_MESSAGE_STR("ACC CALIB");
+        case ARMING_DISABLED_ARM_SWITCH:
+            return OSD_MESSAGE_STR("DISARM!");
+        case ARMING_DISABLED_HARDWARE_FAILURE:
+            return OSD_MESSAGE_STR("ERR HW!");
+        //     {
+        //         if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) {
+        //             return OSD_MESSAGE_STR("GYRO FAILURE");
+        //         }
+        //         if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) {
+        //             return OSD_MESSAGE_STR("ACCELEROMETER FAILURE");
+        //         }
+        //         if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) {
+        //             return OSD_MESSAGE_STR("COMPASS FAILURE");
+        //         }
+        //         if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) {
+        //             return OSD_MESSAGE_STR("BAROMETER FAILURE");
+        //         }
+        //         if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) {
+        //             return OSD_MESSAGE_STR("GPS FAILURE");
+        //         }
+        //         if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) {
+        //             return OSD_MESSAGE_STR("RANGE FINDER FAILURE");
+        //         }
+        //         if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) {
+        //             return OSD_MESSAGE_STR("PITOT METER FAILURE");
+        //         }
+        //     }
+        //     return OSD_MESSAGE_STR("HARDWARE FAILURE");
+        case ARMING_DISABLED_BOXFAILSAFE:
+            return OSD_MESSAGE_STR("FAILSAFE ENABLED");
+        case ARMING_DISABLED_BOXKILLSWITCH:
+            return OSD_MESSAGE_STR("KILLSWITCH ENABLED");
+        case ARMING_DISABLED_RC_LINK:
+            return OSD_MESSAGE_STR("NO RC LINK");
+        case ARMING_DISABLED_THROTTLE:
+            return OSD_MESSAGE_STR("THROTTLE!");
+        case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED:
+            return OSD_MESSAGE_STR("ROLLPITCH!");
+        case ARMING_DISABLED_SERVO_AUTOTRIM:
+            return OSD_MESSAGE_STR("AUTOTRIM!");
+        case ARMING_DISABLED_OOM:
+            return OSD_MESSAGE_STR("MEM LOW");
+        case ARMING_DISABLED_INVALID_SETTING:
+            return OSD_MESSAGE_STR("ERR SETTING");
+        case ARMING_DISABLED_CLI:
+            return OSD_MESSAGE_STR("CLI");
+        case ARMING_DISABLED_PWM_OUTPUT_ERROR:
+            return OSD_MESSAGE_STR("PWM ERR");
+            // Cases without message
+        case ARMING_DISABLED_CMS_MENU:
+            FALLTHROUGH;
+        case ARMING_DISABLED_OSD_MENU:
+            FALLTHROUGH;
+        case ARMING_DISABLED_ALL_FLAGS:
+            FALLTHROUGH;
+        case ARMED:
+            FALLTHROUGH;
+        case WAS_EVER_ARMED:
+            break;
+    }
+
+    return NULL;
+}
+
+static const char * osdFailsafePhaseMessage(void)
+{
+    // See failsafe.h for each phase explanation
+    switch (failsafePhase()) {
+        case FAILSAFE_RETURN_TO_HOME:
+            // XXX: Keep this in sync with OSD_FLYMODE.
+            return OSD_MESSAGE_STR("(RTH)");
+        case FAILSAFE_LANDING:
+            // This should be considered an emergengy landing
+            return OSD_MESSAGE_STR("(EMRGY LANDING)");
+        case FAILSAFE_RX_LOSS_MONITORING:
+            // Only reachable from FAILSAFE_LANDED, which performs
+            // a disarm. Since aircraft has been disarmed, we no
+            // longer show failsafe details.
+            FALLTHROUGH;
+        case FAILSAFE_LANDED:
+            // Very brief, disarms and transitions into
+            // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents
+            // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM,
+            // so we'll show the user how to re-arm in when
+            // that flag is the reason to prevent arming.
+            FALLTHROUGH;
+        case FAILSAFE_RX_LOSS_IDLE:
+            // This only happens when user has chosen NONE as FS
+            // procedure. The recovery messages should be enough.
+            FALLTHROUGH;
+        case FAILSAFE_IDLE:
+            // Failsafe not active
+            FALLTHROUGH;
+        case FAILSAFE_RX_LOSS_DETECTED:
+            // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED
+            // or the FS procedure immediately.
+            FALLTHROUGH;
+        case FAILSAFE_RX_LOSS_RECOVERED:
+            // Exiting failsafe
+            break;
+    }
+
+    return NULL;
+}
+
+static const char * osdFailsafeInfoMessage(void)
+{
+    if (failsafeIsReceivingRxData()) {
+        // User must move sticks to exit FS mode
+        return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!");
+    }
+
+    return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
+}
+
+static const char * navigationStateMessage(void)
+{
+    switch (NAV_Status.state) {
+        case MW_NAV_STATE_NONE:
+            break;
+        case MW_NAV_STATE_RTH_START:
+            return OSD_MESSAGE_STR("STARTING RTH");
+        case MW_NAV_STATE_RTH_ENROUTE:
+            // TODO: Break this up between climb and head home
+            return OSD_MESSAGE_STR("EN ROUTE TO HOME");
+        case MW_NAV_STATE_HOLD_INFINIT:
+            // Used by HOLD flight modes. No information to add.
+            break;
+        case MW_NAV_STATE_HOLD_TIMED:
+            // TODO: Maybe we can display a count down
+            return OSD_MESSAGE_STR("HOLDING WAYPOINT");
+            break;
+        case MW_NAV_STATE_WP_ENROUTE:
+            // TODO: Show WP number
+            return OSD_MESSAGE_STR("TO WP");
+        case MW_NAV_STATE_PROCESS_NEXT:
+            return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
+        case MW_NAV_STATE_DO_JUMP:
+            // Not used
+            break;
+        case MW_NAV_STATE_LAND_START:
+            // Not used
+            break;
+        case MW_NAV_STATE_EMERGENCY_LANDING:
+            return OSD_MESSAGE_STR("EMRGY LANDING");
+        case MW_NAV_STATE_LAND_IN_PROGRESS:
+            return OSD_MESSAGE_STR("LANDING");
+        case MW_NAV_STATE_HOVER_ABOVE_HOME:
+            if (STATE(FIXED_WING_LEGACY)) {
+                return OSD_MESSAGE_STR("LOITERING AROUND HOME");
+            }
+            return OSD_MESSAGE_STR("HOVERING");
+        case MW_NAV_STATE_LANDED:
+            return OSD_MESSAGE_STR("LANDED");
+        case MW_NAV_STATE_LAND_SETTLE:
+            return OSD_MESSAGE_STR("PREPARING TO LAND");
+        case MW_NAV_STATE_LAND_START_DESCENT:
+            // Not used
+            break;
+    }
+    return NULL;
+}
+
+
+/**
+ * Converts velocity based on the current unit system (kmh or mph).
+ * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second)
+ */
+static int32_t osdConvertVelocityToUnit(int32_t vel)
+{
+    switch (osdConfig()->units) {
+        case OSD_UNIT_UK:
+            FALLTHROUGH;
+
+        case OSD_UNIT_IMPERIAL:
+            return (vel * 224) / 10000; // Convert to mph
+
+        case OSD_UNIT_METRIC:
+            return (vel * 36) / 1000;   // Convert to kmh
+    }
+
+    // Unreachable
+    return -1;
+}
+
+static int16_t osdDJIGet3DSpeed(void)
+{
+    int16_t vert_speed = getEstimatedActualVelocity(Z);
+    int16_t hor_speed = gpsSol.groundSpeed;
+    return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
+}
+
+/**
+ * Converts velocity into a string based on the current unit system.
+ * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
+ */
+void osdDJIFormatVelocityStr(char* buff, int32_t vel )
+{
+    switch (osdConfig()->units) {
+        case OSD_UNIT_UK:
+            FALLTHROUGH;
+        case OSD_UNIT_IMPERIAL:
+            tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH");
+            break;
+        case OSD_UNIT_METRIC:
+            tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH");
+            break;
+    }
+}
+static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
+{
+    int16_t thr = rxGetChannelValue(THROTTLE);
+    if (autoThr && navigationIsControllingThrottle()) {
+        thr = rcCommand[THROTTLE];
+    }
+
+    tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
+}
+
+/**
+ * Converts distance into a string based on the current unit system.
+ * @param dist Distance in centimeters
+ */
+static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
+{
+    int32_t centifeet;
+
+    switch (osdConfig()->units) {
+        case OSD_UNIT_IMPERIAL:
+            centifeet = CENTIMETERS_TO_CENTIFEET(dist);
+            if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
+                // Show feet when dist < 0.5mi
+                tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
+            } 
+            else {
+                // Show miles when dist >= 0.5mi
+                tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)),
+                (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi");
+            }
+            break;
+        case OSD_UNIT_UK:
+            FALLTHROUGH;
+        case OSD_UNIT_METRIC:
+            if (abs(dist) < METERS_PER_KILOMETER * 100) {
+                // Show meters when dist < 1km
+                tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M");
+            }
+            else {
+                // Show kilometers when dist >= 1km
+                tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)),
+                    (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM");
+            }
+            break;
+    }
+}
+
+static void osdDJIEfficiencyMahPerKM(char *buff)
+{
+    // amperage is in centi amps, speed is in cms/s. We want
+    // mah/km. Values over 999 are considered useless and
+    // displayed as "---""
+    static pt1Filter_t eFilterState;
+    static timeUs_t efficiencyUpdated = 0;
+    int32_t value = 0;
+    timeUs_t currentTimeUs = micros();
+    timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
+
+    if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
+        if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
+            value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
+                1, efficiencyTimeDelta * 1e-6f);
+
+            efficiencyUpdated = currentTimeUs;
+        }
+        else {
+            value = eFilterState.state;
+        }
+    }
+
+    if (value > 0 && value <= 999) {
+        tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM");
+    }
+    else {
+        tfp_sprintf(buff, "%s", "---mAhKM");
+    }
+}
+
+static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
+{
+    // :W T S E D
+    //  | | | | Distance Trip
+    //  | | | Efficiency mA/KM
+    //  | | S 3dSpeed
+    //  | Throttle
+    //  Warnings
+    const char *message = " ";
+    const char *enabledElements = name + 1;
+    char djibuf[24];
+
+    // clear name from chars : and leading W
+    if (enabledElements[0] == 'W') {
+        enabledElements += 1;
+    }
+    
+    int elemLen = strlen(enabledElements);
+
+    if (elemLen > 0) {
+        switch (enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen)]){
+            case 'T':
+                osdDJIFormatThrottlePosition(djibuf,true);
+                break;
+            case 'S':
+                osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed());
+                break;
+            case 'E':
+                osdDJIEfficiencyMahPerKM(djibuf);
+                break;
+            case 'D':
+                osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
+                break;
+            case 'W':
+                tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST");
+                break;
+            default:
+                tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM");
+                break;
+        }
+
+        if (djibuf[0] != '\0') {
+            message = djibuf;
+        }
+    }
+
+    if (name[1] == 'W') {
+        char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
+        if (ARMING_FLAG(ARMED)) {
+            // Aircraft is armed. We might have up to 5
+            // messages to show.
+            const char *messages[5];
+            unsigned messageCount = 0;
+
+            if (FLIGHT_MODE(FAILSAFE_MODE)) {
+                // In FS mode while being armed too
+                const char *failsafePhaseMessage = osdFailsafePhaseMessage();
+                const char *failsafeInfoMessage = osdFailsafeInfoMessage();
+                const char *navStateFSMessage = navigationStateMessage();
+
+                if (failsafePhaseMessage) {
+                    messages[messageCount++] = failsafePhaseMessage;
+                }
+
+                if (failsafeInfoMessage) {
+                    messages[messageCount++] = failsafeInfoMessage;
+                }
+
+                if (navStateFSMessage) {
+                    messages[messageCount++] = navStateFSMessage;
+                }
+
+                if (messageCount > 0) {
+                    message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
+                    if (message == failsafeInfoMessage) {
+                        // failsafeInfoMessage is not useful for recovering
+                        // a lost model, but might help avoiding a crash.
+                        // Blink to grab user attention.
+                    //doesnt work   TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+                    }
+                    // We're shoing either failsafePhaseMessage or
+                    // navStateFSMessage. Don't BLINK here since
+                    // having this text available might be crucial
+                    // during a lost aircraft recovery and blinking
+                    // will cause it to be missing from some frames.
+                }
+            } 
+            else {
+                if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
+                    const char *navStateMessage = navigationStateMessage();
+                    if (navStateMessage) {
+                        messages[messageCount++] = navStateMessage;
+                    }
+                } 
+                else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
+                    messages[messageCount++] = "AUTOLAUNCH";
+                }
+                else {
+                    if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
+                        // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
+                        // when it doesn't require ANGLE mode (required only in FW
+                        // right now). If if requires ANGLE, its display is handled
+                        // by OSD_FLYMODE.
+                        messages[messageCount++] = "(ALT HOLD)";
+                    }
+                    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
+                        messages[messageCount++] = "(AUTOTRIM)";
+                    }
+                    if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
+                        messages[messageCount++] = "(AUTOTUNE)";
+                    }
+                    if (FLIGHT_MODE(HEADFREE_MODE)) {
+                        messages[messageCount++] = "(HEADFREE)";
+                    }
+                }
+                // Pick one of the available messages. Each message lasts
+                // a second.
+                if (messageCount > 0) {
+                    message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
+                }
+            }
+        }
+        else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
+            unsigned invalidIndex;
+            // Check if we're unable to arm for some reason
+            if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
+                if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
+                    const setting_t *setting = settingGet(invalidIndex);
+                    settingGetName(setting, messageBuf);
+                    for (int ii = 0; messageBuf[ii]; ii++) {
+                        messageBuf[ii] = sl_toupper(messageBuf[ii]);
+                    }
+                    message = messageBuf;
+                }
+                else {
+                    message = "ERR SETTING";
+                    // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
+                }
+            }
+            else {
+                if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
+                    message = "CANT ARM";
+                    // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
+                } else {
+                    // Show the reason for not arming
+                    message = osdArmingDisabledReasonMessage();
+                }
+            }
+        }
+    }
+
+    if (message[0] != '\0') {
+        sbufWriteData(dst, message, strlen(message));
+    }
+}
 #endif
 
+
 static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 {
     UNUSED(mspPostProcessFn);
@@ -410,9 +931,25 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
         case DJI_MSP_NAME:
             {
                 const char * name = systemConfig()->name;
+
+#if defined(USE_OSD)
+                if (djiOsdConfig()->use_name_for_messages)  {
+                    if (name[0] == ':') {
+                        // If craft name starts with a semicolon - use it as a template for what we want to show
+                        djiSerializeCraftNameOverride(dst, name);
+                    }
+                    else {
+                        // Otherwise fall back to just warnings
+                        djiSerializeCraftNameOverride(dst, ":W");
+                    }
+                }
+                else
+#endif
+                {
                     int len = strlen(name);
-                if (len > 12) len = 12;
-                   sbufWriteData(dst, name, len);
+                    sbufWriteData(dst, name, MAX(len, 12));
+                    break;
+                }
             }
             break;
 
@@ -537,21 +1074,93 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             }
             break;
 
-#if defined(USE_ESC_SENSOR)
         case DJI_MSP_ESC_SENSOR_DATA:
-            if (STATE(ESC_SENSOR_ENABLED)) {
-                sbufWriteU8(dst, getMotorCount());
+            if (djiOsdConfig()->proto_workarounds & DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA) {
+                // Version 1.00.06 of DJI firmware is not using the standard MSP_ESC_SENSOR_DATA
+                uint16_t protoRpm = 0;
+                int16_t protoTemp = 0;
+
+#if defined(USE_ESC_SENSOR)
+                if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
+                    uint32_t motorRpmAcc = 0;
+                    int32_t motorTempAcc = 0;
+
                     for (int i = 0; i < getMotorCount(); i++) {
                         const escSensorData_t * escSensor = getEscTelemetry(i);
-                    sbufWriteU8(dst, escSensor->temperature);
-                    sbufWriteU16(dst, escSensor->rpm);
+                        motorRpmAcc += escSensor->rpm;
+                        motorTempAcc += escSensor->temperature;
                     }
+
+                    protoRpm = motorRpmAcc / getMotorCount();
+                    protoTemp = motorTempAcc / getMotorCount();
+                }
+#endif
+
+                switch (djiOsdConfig()->esc_temperature_source) {
+                    // This is ESC temperature (as intended)
+                    case DJI_OSD_TEMP_ESC:
+                        // No-op, temperature is already set to ESC
+                        break;
+
+                    // Re-purpose the field for core temperature
+                    case DJI_OSD_TEMP_CORE:
+                        getIMUTemperature(&protoTemp);
+                        protoTemp = protoTemp / 10;
+                        break;
+
+                    // Re-purpose the field for baro temperature
+                    case DJI_OSD_TEMP_BARO:
+                        getBaroTemperature(&protoTemp);
+                        protoTemp = protoTemp / 10;
+                        break;
+                }
+
+                // No motor count, just raw temp and RPM data
+                sbufWriteU8(dst, protoTemp);
+                sbufWriteU16(dst, protoRpm);
             }
             else {
-                reply->result = MSP_RESULT_ERROR;
+                // Use standard MSP_ESC_SENSOR_DATA message
+                sbufWriteU8(dst, getMotorCount());
+                for (int i = 0; i < getMotorCount(); i++) {
+                    uint16_t motorRpm = 0;
+                    int16_t motorTemp = 0;
+
+                    // If ESC_SENSOR is enabled, pull the telemetry data and get motor RPM
+#if defined(USE_ESC_SENSOR)
+                    if (STATE(ESC_SENSOR_ENABLED)) {
+                        const escSensorData_t * escSensor = getEscTelemetry(i);
+                        motorRpm = escSensor->rpm;
+                        motorTemp = escSensor->temperature;
+                    }
+#endif
+
+                    // Now populate temperature field (which we may override for different purposes)
+                    switch (djiOsdConfig()->esc_temperature_source) {
+                        // This is ESC temperature (as intended)
+                        case DJI_OSD_TEMP_ESC:
+                            // No-op, temperature is already set to ESC
+                            break;
+
+                        // Re-purpose the field for core temperature
+                        case DJI_OSD_TEMP_CORE:
+                            getIMUTemperature(&motorTemp);
+                            motorTemp = motorTemp / 10;
+                            break;
+
+                        // Re-purpose the field for baro temperature
+                        case DJI_OSD_TEMP_BARO:
+                            getBaroTemperature(&motorTemp);
+                            motorTemp = motorTemp / 10;
+                            break;
+                    }
+
+                    // Add data for this motor to the packet
+                    sbufWriteU8(dst, motorTemp);
+                    sbufWriteU16(dst, motorRpm);
+                }
             }
             break;
-#endif
 
         case DJI_MSP_OSD_CONFIG:
 #if defined(USE_OSD)

src/main/io/osd_dji_hd.h

@@ -29,6 +29,8 @@
 #include "msp/msp.h"
 #include "msp/msp_serial.h"
 
+#include "config/parameter_group.h"
+
 #if defined(USE_DJI_HD_OSD)
 
 #define DJI_API_VERSION_MAJOR           1
@@ -60,6 +62,24 @@
 #define DJI_MSP_SET_PID                 202
 #define DJI_MSP_SET_RC_TUNING           204
 
+enum djiOsdTempSource_e {
+    DJI_OSD_TEMP_ESC    = 0,
+    DJI_OSD_TEMP_CORE   = 1,
+    DJI_OSD_TEMP_BARO   = 2
+};
+
+enum djiOsdProtoWorkarounds_e {
+    DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA    = 1 << 0,
+};
+
+typedef struct djiOsdConfig_s {
+    uint8_t use_name_for_messages;
+    uint8_t esc_temperature_source;
+    uint8_t proto_workarounds;
+} djiOsdConfig_t;
+
+PG_DECLARE(djiOsdConfig_t, djiOsdConfig);
+
 void djiOsdSerialInit(void);
 void djiOsdSerialProcess(void);
 

src/main/io/osd_grid.c

@@ -225,7 +225,7 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
     // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX.
     // Zero scrolling should draw SYM_AH_DECORATION.
     uint8_t decoration = SYM_AH_DECORATION;
-    int offset;
+    int offset = 0;
     int steps;
     switch (scroll) {
         case OSD_SIDEBAR_SCROLL_NONE:
@@ -240,8 +240,6 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
         case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
 #if defined(USE_GPS)
             offset = gpsSol.groundSpeed;
-#else
-            offset = 0;
 #endif
             // Move 1 char for every 20 cm/s
             steps = offset / 20;
@@ -249,8 +247,6 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side
         case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
 #if defined(USE_GPS)
             offset = GPS_distanceToHome;
-#else
-            offset = 0;
 #endif
             // Move 1 char for every 5m
             steps = offset / 5;

src/main/io/rangefinder_benewake.c

@@ -55,15 +55,20 @@ typedef struct __attribute__((packed)) {
 
 #define BENEWAKE_PACKET_SIZE    sizeof(tfminiPacket_t)
 #define BENEWAKE_MIN_QUALITY    0
+#define BENEWAKE_TIMEOUT_MS     200 // 200ms
 
 static serialPort_t * serialPort = NULL;
 static serialPortConfig_t * portConfig;
 static uint8_t  buffer[BENEWAKE_PACKET_SIZE];
 static unsigned bufferPtr;
+static timeMs_t lastProtocolActivityMs;
 
 static bool hasNewData = false;
 static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
 
+// TFmini command to initiate 100Hz sampling
+static const uint8_t initCmd100Hz[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
+
 static bool benewakeRangefinderDetect(void)
 {
     portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
@@ -80,16 +85,27 @@ static void benewakeRangefinderInit(void)
         return;
     }
 
-    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, baudRates[BAUD_115200], MODE_RX, SERIAL_NOT_INVERTED);
+    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
     if (!serialPort) {
         return;
     }
 
+    lastProtocolActivityMs = 0;
     bufferPtr = 0;
 }
 
 static void benewakeRangefinderUpdate(void)
 {
+    // Initialize the sensor
+    if (lastProtocolActivityMs == 0 || (millis() - lastProtocolActivityMs) > BENEWAKE_TIMEOUT_MS) {
+        // Initialize the sensor to do 100Hz sampling to make sure we get the most recent data always
+        serialWriteBuf(serialPort, initCmd100Hz, sizeof(initCmd100Hz));
+
+        // Send the init command every BENEWAKE_TIMEOUT_MS
+        lastProtocolActivityMs = millis();
+    }
+
+    // Process incoming bytes
     tfminiPacket_t *tfminiPacket = (tfminiPacket_t *)buffer;
     while (serialRxBytesWaiting(serialPort) > 0) {
         uint8_t c = serialRead(serialPort);
@@ -117,6 +133,7 @@ static void benewakeRangefinderUpdate(void)
                 // Valid packet
                 hasNewData = true;
                 sensorData = (tfminiPacket->distL << 0) | (tfminiPacket->distH << 8);
+                lastProtocolActivityMs = millis();
 
                 uint16_t qual = (tfminiPacket->strengthL << 0) | (tfminiPacket->strengthH << 8);
 

src/main/io/rangefinder_msp.c

@@ -37,14 +37,12 @@
 #include "io/serial.h"
 
 #if defined(USE_RANGEFINDER_MSP)
+
 #include "drivers/rangefinder/rangefinder_virtual.h"
 #include "drivers/time.h"
 #include "io/rangefinder.h"
+#include "msp/msp_protocol_v2_sensor_msg.h"
 
-typedef struct __attribute__((packed)) {
-    uint8_t quality;    // [0;255]
-    int32_t distanceMm; // Negative value for out of range
-} mspRangefinderSensor_t;
 
 static bool hasNewData = false;
 static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
@@ -76,7 +74,7 @@ static int32_t mspRangefinderGetDistance(void)
 
 void mspRangefinderReceiveNewData(uint8_t * bufferPtr)
 {
-    const mspRangefinderSensor_t * pkt = (const mspRangefinderSensor_t *)bufferPtr;
+    const mspSensorRangefinderDataMessage_t * pkt = (const mspSensorRangefinderDataMessage_t *)bufferPtr;
 
     sensorData = pkt->distanceMm / 10;
     hasNewData = true;

src/main/io/serial_4way.c

@@ -72,7 +72,7 @@
 // *** change to adapt Revision
 #define SERIAL_4WAY_VER_MAIN 20
 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0
-#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03
+#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 05
 
 #define SERIAL_4WAY_PROTOCOL_VER 107
 // *** end
@@ -326,14 +326,13 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
 #define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \
         (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B))
 
-#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \
+#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] == 0xF330) || \
         (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \
         (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \
         (pDeviceInfo->words[0] == 0xE8B2))
 
-#define ARM_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x1F06) || \
-        (pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506) || \
-        (pDeviceInfo->words[0] == 0x2B06) || (pDeviceInfo->words[0] == 0x4706))
+// BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06
+#define ARM_DEVICE_MATCH ((pDeviceInfo->bytes[1] > 0x00) && (pDeviceInfo->bytes[1] < 0x90) && (pDeviceInfo->bytes[0] == 0x06))
 
 static uint8_t CurrentInterfaceMode;
 

src/main/io/smartport_master.c

@@ -300,6 +300,9 @@ static void smartportMasterPoll(void)
                 break;
             }
 
+            default: // should not happen
+                return;
+
         }
 
     }

src/main/msp/msp_protocol_v2_sensor.h

@@ -19,3 +19,7 @@
 
 #define MSP2_SENSOR_RANGEFINDER     0x1F01
 #define MSP2_SENSOR_OPTIC_FLOW      0x1F02
+#define MSP2_SENSOR_GPS             0x1F03
+#define MSP2_SENSOR_COMPASS         0x1F04
+#define MSP2_SENSOR_BAROMETER       0x1F05
+#define MSP2_SENSOR_AIRSPEED        0x1F06

src/main/msp/msp_protocol_v2_sensor_msg.h

@@ -0,0 +1,84 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+typedef struct __attribute__((packed)) {
+    uint8_t quality;    // [0;255]
+    int32_t motionX;
+    int32_t motionY;
+} mspSensorOpflowDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t quality;    // [0;255]
+    int32_t distanceMm; // Negative value for out of range
+} mspSensorRangefinderDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;                  // sensor instance number to support multi-sensor setups
+    uint16_t gpsWeek;                   // GPS week, 0xFFFF if not available
+    uint32_t msTOW;
+    uint8_t  fixType;
+    uint8_t  satellitesInView;
+    uint16_t horizontalPosAccuracy;     // [cm]
+    uint16_t verticalPosAccuracy;       // [cm]
+    uint16_t horizontalVelAccuracy;     // [cm/s]
+    uint16_t hdop;
+    int32_t  longitude;
+    int32_t  latitude;
+    int32_t  mslAltitude;       // cm
+    int32_t  nedVelNorth;       // cm/s
+    int32_t  nedVelEast;
+    int32_t  nedVelDown;
+    uint16_t groundCourse;      // deg * 100, 0..36000
+    uint16_t trueYaw;           // deg * 100, values of 0..36000 are valid. 65535 = no data available
+    uint16_t year;
+    uint8_t  month;
+    uint8_t  day;
+    uint8_t  hour;
+    uint8_t  min;
+    uint8_t  sec;
+} mspSensorGpsDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;
+    uint32_t timeMs;
+    float    pressurePa;
+    int16_t  temp; // centi-degrees C
+} mspSensorBaroDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;
+    uint32_t timeMs;
+    float    diffPressurePa;
+    int16_t  temp;              // centi-degrees C
+} mspSensorAirspeedDataMessage_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  instance;
+    uint32_t timeMs;
+    int16_t  magX; // mGauss, front
+    int16_t  magY; // mGauss, right
+    int16_t  magZ; // mGauss, down
+} mspSensorCompassDataMessage_t;

src/main/navigation/navigation.c

@@ -83,11 +83,15 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO
 #endif
 
 #if defined(USE_NAV)
+
+// waypoint 254, 255 are special waypoints
+STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range);
+
 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -102,7 +106,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
             .rth_tail_first = 0,
             .disarm_on_landing = 0,
             .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS,
-            .auto_overrides_motor_stop = 1,
+            .nav_overrides_motor_stop = NOMS_ALL_NAV,
         },
 
         // General navigation parameters
@@ -2793,6 +2797,20 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
         wpData->lon = wpLLH.lon;
         wpData->alt = wpLLH.alt;
     }
+    // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in 3D-guided mode 
+    else if (wpNumber == 254) {
+        navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
+
+        if ((posControl.gpsOrigin.valid) && (navStateFlags & NAV_CTL_ALT) && (navStateFlags & NAV_CTL_POS)) {
+            gpsLocation_t wpLLH;
+
+            geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
+            
+            wpData->lat = wpLLH.lat;
+            wpData->lon = wpLLH.lon;
+            wpData->alt = wpLLH.alt;
+        }
+    }
     // WP #1 - #60 - common waypoints - pre-programmed mission
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
         if (wpNumber <= posControl.waypointCount) {

src/main/navigation/navigation.h

@@ -124,6 +124,12 @@ typedef enum {
     NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR = 4,
 } navArmingBlocker_e;
 
+typedef enum {
+    NOMS_OFF,
+    NOMS_AUTO_ONLY,
+    NOMS_ALL_NAV
+} navOverridesMotorStop_e;
+
 typedef struct positionEstimationConfig_s {
     uint8_t automatic_mag_declination;
     uint8_t reset_altitude_type; // from nav_reset_type_e
@@ -175,7 +181,7 @@ typedef struct navConfig_s {
             uint8_t disarm_on_landing;          //
             uint8_t rth_allow_landing;          // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e.
             uint8_t rth_climb_ignore_emerg;     // Option to ignore GPS loss on initial climb stage of RTH
-            uint8_t auto_overrides_motor_stop;  // Autonomous modes override motor_stop setting and user command to stop motor
+            uint8_t nav_overrides_motor_stop;  // Autonomous modes override motor_stop setting and user command to stop motor
         } flags;
 
         uint8_t  pos_failure_timeout;           // Time to wait before switching to emergency landing (0 - disable)

src/main/programming/logic_condition.c

@@ -36,6 +36,7 @@
 #include "fc/fc_core.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
+#include "fc/rc_modes.h"
 #include "navigation/navigation.h"
 #include "sensors/battery.h"
 #include "sensors/pitotmeter.h"
@@ -478,6 +479,26 @@ static int logicConditionGetFlightOperandValue(int operand) {
             return NAV_Status.activeWpAction;
             break;
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
+            return constrain(sqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX);
+            break;
+
+        case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:
+        #ifdef USE_SERIALRX_CRSF
+            return rxLinkStatistics.uplinkLQ;
+        #else
+            return 0;
+        #endif
+            break;
+
+        case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR:
+        #ifdef USE_SERIALRX_CRSF
+            return rxLinkStatistics.uplinkSNR;
+        #else
+            return 0;
+        #endif
+            break;
+
         default:
             return 0;
             break;
@@ -524,6 +545,14 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
             return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
             break;
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
+            return IS_RC_MODE_ACTIVE(BOXUSER1);
+            break;
+
+        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2:
+            return IS_RC_MODE_ACTIVE(BOXUSER2);
+            break;
+
         default:
             return 0;
             break;

src/main/programming/logic_condition.h

@@ -112,18 +112,24 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 28
     LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX,                          // 29
     LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION,                         // 30
+    LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE,                        // 31
+    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ,                                 // 32
+    LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR,                                // 33
+
 } logicFlightOperands_e;
 
 typedef enum {
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON,
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR,
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE,                           // 0
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL,                             // 1
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH,                                // 2
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD,                            // 3
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE,                             // 4
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD,                            // 5
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE,                              // 6
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON,                            // 7
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR,                                // 8
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1,                              // 9
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2,                              // 10
 } logicFlightModeOperands_e;
 
 typedef enum {

src/main/rx/rx.c

@@ -61,6 +61,7 @@
 #include "rx/rx_spi.h"
 #include "rx/sbus.h"
 #include "rx/spektrum.h"
+#include "rx/srxl2.h"
 #include "rx/sumd.h"
 #include "rx/sumh.h"
 #include "rx/uib_rx.h"
@@ -91,6 +92,7 @@ static bool mspOverrideDataProcessingRequired = false;
 static bool rxSignalReceived = false;
 static bool rxFlightChannelsValid = false;
 static bool rxIsInFailsafeMode = true;
+static uint8_t rxChannelCount;
 
 static timeUs_t rxNextUpdateAtUs = 0;
 static timeUs_t needRxSignalBefore = 0;
@@ -141,6 +143,8 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
     .mspOverrideChannels = 15,
 #endif
     .rssi_source = RSSI_SOURCE_AUTO,
+    .srxl2_unit_id = 1,
+    .srxl2_baud_fast = 1,
 );
 
 void resetAllRxChannelRangeConfigurations(void)
@@ -187,6 +191,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
 {
     bool enabled = false;
     switch (rxConfig->serialrx_provider) {
+#ifdef USE_SERIALRX_SRXL2
+    case SERIALRX_SRXL2:
+        enabled = srxl2RxInit(rxConfig, rxRuntimeConfig);
+        break;
+#endif
 #ifdef USE_SERIALRX_SPEKTRUM
     case SERIALRX_SPEKTRUM1024:
     case SERIALRX_SPEKTRUM2048:
@@ -347,6 +356,8 @@ void rxInit(void)
         mspOverrideInit();
     }
 #endif
+
+    rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
 }
 
 void rxUpdateRSSISource(void)
@@ -516,7 +527,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
     rxFlightChannelsValid = true;
 
     // Read and process channel data
-    for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+    for (int channel = 0; channel < rxChannelCount; channel++) {
         const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
 
         // sample the channel
@@ -549,11 +560,11 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
     // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained
     if (rxFlightChannelsValid && rxSignalReceived) {
         if (rxRuntimeConfig.requireFiltering) {
-            for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+            for (int channel = 0; channel < rxChannelCount; channel++) {
                 rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]);
             }
         } else {
-            for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+            for (int channel = 0; channel < rxChannelCount; channel++) {
                 rcChannels[channel].data = rcStaging[channel];
             }
         }

src/main/rx/rx.h

@@ -82,6 +82,7 @@ typedef enum {
     SERIALRX_FPORT = 10,
     SERIALRX_SBUS_FAST = 11,
     SERIALRX_FPORT2 = 12,
+    SERIALRX_SRXL2 = 13,
 } rxSerialReceiverType_e;
 
 #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT          16
@@ -128,6 +129,8 @@ typedef struct rxConfig_s {
     uint8_t rcFilterFrequency;              // RC filter cutoff frequency (smoothness vs response sharpness)
     uint16_t mspOverrideChannels;           // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active
     uint8_t rssi_source;
+    uint8_t srxl2_unit_id;
+    uint8_t srxl2_baud_fast;
 } rxConfig_t;
 
 PG_DECLARE(rxConfig_t, rxConfig);

src/main/rx/spektrum.c

@@ -15,10 +15,13 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
 
+#include "common/maths.h"
+
 #include "platform.h"
 
 #ifdef USE_SERIALRX_SPEKTRUM
@@ -44,19 +47,13 @@
 #include "rx/spektrum.h"
 
 // driver for spektrum satellite receiver / sbus
-
-#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
-#define SPEKTRUM_2048_CHANNEL_COUNT 12
-#define SPEKTRUM_1024_CHANNEL_COUNT 7
-
-#define SPEK_FRAME_SIZE 16
-#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
-
-#define SPEKTRUM_BAUDRATE 115200
+#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame
 
 #define SPEKTRUM_MAX_FADE_PER_SEC 40
 #define SPEKTRUM_FADE_REPORTS_PER_SEC 2
 
+bool srxlEnabled = false;
+
 static uint8_t spek_chan_shift;
 static uint8_t spek_chan_mask;
 static bool rcFrameComplete = false;
@@ -81,6 +78,10 @@ static IO_t BindPin = DEFIO_IO(NONE);
 static IO_t BindPlug = DEFIO_IO(NONE);
 #endif
 
+#if defined(USE_TELEMETRY_SRXL)
+static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
+static uint8_t telemetryBufLen = 0;
+#endif
 
 // Receive ISR callback
 static void spektrumDataReceive(uint16_t c, void *rxCallbackData)
@@ -116,10 +117,15 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
 
-    if (!rcFrameComplete) {
-        return RX_FRAME_PENDING;
-    }
+#if defined(USE_TELEMETRY_SRXL)
+    static timeUs_t telemetryFrameRequestedUs = 0;
 
+    timeUs_t currentTimeUs = micros();
+#endif
+
+    uint8_t result = RX_FRAME_PENDING;
+
+    if (rcFrameComplete) {
         rcFrameComplete = false;
 
         // Fetch the fade count
@@ -155,7 +161,23 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
             }
         }
 
-    return RX_FRAME_COMPLETE;
+#if defined(USE_TELEMETRY_SRXL)
+        if (srxlEnabled && (spekFrame[2] & 0x80) == 0) {
+                    telemetryFrameRequestedUs = currentTimeUs;
+        }
+#endif
+
+        result = RX_FRAME_COMPLETE;
+    }
+#if defined(USE_TELEMETRY_SRXL)
+    if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) {
+        telemetryFrameRequestedUs = 0;
+
+        result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
+    }
+#endif
+
+    return result;
 }
 
 static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
@@ -251,6 +273,25 @@ void spektrumBind(rxConfig_t *rxConfig)
 }
 #endif // SPEKTRUM_BIND
 
+#if defined(USE_TELEMETRY_SRXL)
+
+bool srxlTelemetryBufferEmpty(void)
+{
+  if (telemetryBufLen == 0) {
+      return true;
+  } else {
+      return false;
+  }
+}
+
+void srxlRxWriteTelemetryData(const void *data, int len)
+{
+    len = MIN(len, (int)sizeof(telemetryBuf));
+    memcpy(telemetryBuf, data, len);
+    telemetryBufLen = len;
+}
+#endif
+
 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
     rxRuntimeConfigPtr = rxRuntimeConfig;
@@ -310,4 +351,10 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
 
     return serialPort != NULL;
 }
+
+bool srxlRxIsActive(void)
+{
+    return serialPort != NULL;
+}
+
 #endif // USE_SERIALRX_SPEKTRUM

src/main/rx/spektrum.h

@@ -17,8 +17,26 @@
 
 #pragma once
 
+#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
+#define SPEKTRUM_2048_CHANNEL_COUNT          12
+#define SPEKTRUM_1024_CHANNEL_COUNT          7
+
 #define SPEKTRUM_SAT_BIND_DISABLED           0
 #define SPEKTRUM_SAT_BIND_MAX                10
 
+#define SPEK_FRAME_SIZE                      16
+#define SRXL_FRAME_OVERHEAD                   5
+#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
+
+#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
+
+#define SPEKTRUM_BAUDRATE 115200
+
+extern bool srxlEnabled;
+
 void spektrumBind(rxConfig_t *rxConfig);
 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+
+bool srxlTelemetryBufferEmpty(void);
+void srxlRxWriteTelemetryData(const void *data, int len);
+bool srxlRxIsActive(void);

src/main/rx/srxl2.c

@@ -0,0 +1,580 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef USE_SERIALRX_SRXL2
+
+#include "common/crc.h"
+#include "common/maths.h"
+#include "common/streambuf.h"
+
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+
+#include "io/serial.h"
+
+#include "rx/srxl2.h"
+#include "rx/srxl2_types.h"
+
+#ifndef SRXL2_DEBUG
+#define SRXL2_DEBUG 0
+#endif
+
+#if SRXL2_DEBUG
+//void cliPrintf(const char *format, ...);
+//#define DEBUG_PRINTF(format, ...) cliPrintf(format, __VA_ARGS__)
+#define DEBUG_PRINTF(...) //Temporary until a better debug printf can be included
+#else
+#define DEBUG_PRINTF(...)
+#endif
+
+
+
+#define SRXL2_MAX_CHANNELS             32
+#define SRXL2_FRAME_PERIOD_US   11000 // 5500 for DSMR
+#define SRXL2_CHANNEL_SHIFT            5
+#define SRXL2_CHANNEL_CENTER           0x8000
+
+#define SRXL2_PORT_BAUDRATE_DEFAULT    115200
+#define SRXL2_PORT_BAUDRATE_HIGH       400000
+#define SRXL2_PORT_OPTIONS             (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR)
+#define SRXL2_PORT_MODE                MODE_RXTX
+
+#define SRXL2_REPLY_QUIESCENCE         (2 * 10 * 1000000 / SRXL2_PORT_BAUDRATE_DEFAULT) // 2 * (lastIdleTimestamp - lastReceiveTimestamp). Time taken to send 2 bytes
+
+#define SRXL2_ID                       0xA6
+#define SRXL2_MAX_PACKET_LENGTH        80
+#define SRXL2_DEVICE_ID_BROADCAST      0xFF
+
+#define SRXL2_FRAME_TIMEOUT_US         50000
+
+#define SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US 50000
+#define SRXL2_SEND_HANDSHAKE_TIMEOUT_US 50000
+#define SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US 200000
+
+#define SPEKTRUM_PULSE_OFFSET          988 // Offset value to convert digital data into RC pulse
+
+typedef union {
+        uint8_t raw[SRXL2_MAX_PACKET_LENGTH];
+        Srxl2Header header;
+} Srxl2Frame;
+
+struct rxBuf {
+    volatile unsigned len;
+    Srxl2Frame packet;
+};
+
+static uint8_t unitId = 0;
+static uint8_t baudRate = 0;
+
+static Srxl2State state = Disabled;
+static uint32_t timeoutTimestamp = 0;
+static uint32_t fullTimeoutTimestamp = 0;
+static uint32_t lastValidPacketTimestamp = 0;
+static volatile uint32_t lastReceiveTimestamp = 0;
+static volatile uint32_t lastIdleTimestamp = 0;
+
+struct rxBuf readBuffer[2];
+struct rxBuf* readBufferPtr = &readBuffer[0];
+struct rxBuf* processBufferPtr = &readBuffer[1];
+static volatile unsigned readBufferIdx = 0;
+static volatile bool transmittingTelemetry = false;
+static uint8_t writeBuffer[SRXL2_MAX_PACKET_LENGTH];
+static unsigned writeBufferIdx = 0;
+
+static serialPort_t *serialPort;
+
+static uint8_t busMasterDeviceId = 0xFF;
+static bool telemetryRequested = false;
+
+static uint8_t telemetryFrame[22];
+
+uint8_t globalResult = 0;
+
+/* handshake protocol
+    1. listen for 50ms for serial activity and go to State::Running if found, autobaud may be necessary
+    2. if srxl2_unitId = 0:
+            send a Handshake with destinationDeviceId = 0 every 50ms for at least 200ms
+        else:
+            listen for Handshake for at least 200ms
+    3.  respond to Handshake as currently implemented in process if rePst received
+    4.  respond to broadcast Handshake
+*/
+
+// if 50ms with not activity, go to default baudrate and to step 1
+
+bool srxl2ProcessHandshake(const Srxl2Header* header)
+{
+    const Srxl2HandshakeSubHeader* handshake = (Srxl2HandshakeSubHeader*)(header + 1);
+    if (handshake->destinationDeviceId == Broadcast) {
+        DEBUG_PRINTF("broadcast handshake from %x\r\n", handshake->sourceDeviceId);
+        busMasterDeviceId = handshake->sourceDeviceId;
+
+        if (handshake->baudSupported == 1) {
+            serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
+            DEBUG_PRINTF("switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_HIGH);
+        }
+
+        state = Running;
+
+        return true;
+    }
+
+
+    if (handshake->destinationDeviceId != ((FlightController << 4) | unitId)) {
+        return true;
+    }
+
+    DEBUG_PRINTF("FC handshake from %x\r\n", handshake->sourceDeviceId);
+
+    Srxl2HandshakeFrame response = {
+        .header = *header,
+        .payload = {
+            handshake->destinationDeviceId,
+            handshake->sourceDeviceId,
+            /* priority */ 10,
+            /* baudSupported*/ baudRate,
+            /* info */ 0,
+            // U_ID_2
+        }
+    };
+
+    srxl2RxWriteData(&response, sizeof(response));
+
+    return true;
+}
+
+void srxl2ProcessChannelData(const Srxl2ChannelDataHeader* channelData, rxRuntimeConfig_t *rxRuntimeConfig) {
+    globalResult = RX_FRAME_COMPLETE;
+
+    if (channelData->rssi >= 0) {
+        const int rssiPercent = channelData->rssi;
+        lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiPercent, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
+        //setRssi(scaleRange(rssiPercent, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL);
+    }
+
+    //If receiver is in a connected state, and a packet is missed, the channel mask will be 0.
+    if (!channelData->channelMask.u32) {
+        globalResult |= RX_FRAME_FAILSAFE;
+        return;
+    }
+
+    const uint16_t *frameChannels = (const uint16_t *) (channelData + 1);
+    uint32_t channelMask = channelData->channelMask.u32;
+    while (channelMask) {
+        unsigned idx = __builtin_ctz (channelMask);
+        uint32_t mask = 1 << idx;
+        rxRuntimeConfig->channelData[idx] = *frameChannels++;
+        channelMask &= ~mask;
+    }
+
+     DEBUG_PRINTF("channel data: %d %d %x\r\n", channelData_header->rssi, channelData_header->frameLosses, channelData_header->channelMask.u32);
+}
+
+bool srxl2ProcessControlData(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    const Srxl2ControlDataSubHeader* controlData = (Srxl2ControlDataSubHeader*)(header + 1);
+    const uint8_t ownId = (FlightController << 4) | unitId;
+    if (controlData->replyId == ownId) {
+        telemetryRequested = true;
+        DEBUG_PRINTF("command: %x replyId: %x ownId: %x\r\n", controlData->command, controlData->replyId, ownId);
+    }
+
+    switch (controlData->command) {
+    case ChannelData:
+        srxl2ProcessChannelData((const Srxl2ChannelDataHeader *) (controlData + 1), rxRuntimeConfig);
+        break;
+
+    case FailsafeChannelData: {
+        globalResult |= RX_FRAME_FAILSAFE;
+        //setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
+        lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
+        // DEBUG_PRINTF("fs channel data\r\n");
+    } break;
+
+    case VTXData: {
+#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+        Srxl2VtxData *vtxData = (Srxl2VtxData*)(controlData + 1);
+        DEBUG_PRINTF("vtx data\r\n");
+        DEBUG_PRINTF("vtx band: %x\r\n", vtxData->band);
+        DEBUG_PRINTF("vtx channel: %x\r\n", vtxData->channel);
+        DEBUG_PRINTF("vtx pit: %x\r\n", vtxData->pit);
+        DEBUG_PRINTF("vtx power: %x\r\n", vtxData->power);
+        DEBUG_PRINTF("vtx powerDec: %x\r\n", vtxData->powerDec);
+        DEBUG_PRINTF("vtx region: %x\r\n", vtxData->region);
+        // Pack data as it was used before srxl2 to use existing functions.
+        // Get the VTX control bytes in a frame
+        uint32_t vtxControl =   (0xE0 << 24) | (0xE0 << 8) |
+                                ((vtxData->band & 0x07) << 21) |
+                                ((vtxData->channel & 0x0F) << 16) |
+                                ((vtxData->pit & 0x01) << 4) |
+                                ((vtxData->region & 0x01) << 3) |
+                                ((vtxData->power & 0x07));
+        spektrumHandleVtxControl(vtxControl);
+#endif
+    } break;
+    }
+
+    return true;
+}
+
+bool srxl2ProcessPacket(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    switch (header->packetType) {
+    case Handshake: 
+        return srxl2ProcessHandshake(header);
+    case ControlData: 
+        return srxl2ProcessControlData(header, rxRuntimeConfig);
+    default: 
+        DEBUG_PRINTF("Other packet type, ID: %x \r\n", header->packetType);
+        break;
+    }
+
+    return false;
+}
+
+// @note assumes packet is fully there
+void srxl2Process(rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    if (processBufferPtr->packet.header.id != SRXL2_ID || processBufferPtr->len != processBufferPtr->packet.header.length) {
+        DEBUG_PRINTF("invalid header id: %x, or length: %x received vs %x expected \r\n", processBufferPtr->packet.header.id, processBufferPtr->len, processBufferPtr->packet.header.length);
+        globalResult = RX_FRAME_DROPPED;
+        return;
+    }
+
+    const uint16_t calculatedCrc = crc16_ccitt_update(0, processBufferPtr->packet.raw, processBufferPtr->packet.header.length);
+
+    //Invalid if crc non-zero
+    if (calculatedCrc) {
+        globalResult = RX_FRAME_DROPPED;
+        DEBUG_PRINTF("crc mismatch %x\r\n", calculatedCrc);
+        return;
+    }
+
+    //Packet is valid only after ID and CRC check out
+    lastValidPacketTimestamp = micros();
+
+    if (srxl2ProcessPacket(&processBufferPtr->packet.header, rxRuntimeConfig)) {
+        return;
+    }
+
+    DEBUG_PRINTF("could not parse packet: %x\r\n", processBufferPtr->packet.header.packetType);
+    globalResult = RX_FRAME_DROPPED;
+}
+
+
+static void srxl2DataReceive(uint16_t character, void *data)
+{
+    UNUSED(data);
+
+    lastReceiveTimestamp = microsISR();
+
+    //If the buffer len is not reset for whatever reason, disable reception
+    if (readBufferPtr->len > 0 || readBufferIdx >= SRXL2_MAX_PACKET_LENGTH) {
+        readBufferIdx = 0;
+        globalResult = RX_FRAME_DROPPED;
+    }
+    else {
+        readBufferPtr->packet.raw[readBufferIdx] = character;
+        readBufferIdx++;
+    }
+}
+
+static void srxl2Idle(void)
+{
+    if(transmittingTelemetry) { // Transmitting telemetry triggers idle interrupt as well. We dont want to change buffers then
+        transmittingTelemetry = false;
+    }
+    else if(readBufferIdx == 0) { // Packet was invalid
+        readBufferPtr->len = 0;
+    }
+    else {
+        lastIdleTimestamp = microsISR();
+        //Swap read and process buffer pointers
+        if(processBufferPtr == &readBuffer[0]) {
+            processBufferPtr = &readBuffer[1];
+            readBufferPtr = &readBuffer[0];
+        } else {
+            processBufferPtr = &readBuffer[0];
+            readBufferPtr = &readBuffer[1];
+        }
+        processBufferPtr->len = readBufferIdx;
+    }
+
+    readBufferIdx = 0;
+}
+
+static uint8_t srxl2FrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+    if(serialIsIdle(serialPort))
+    {
+        srxl2Idle();
+    }
+
+    globalResult = RX_FRAME_PENDING;
+
+    // len should only be set after an idle interrupt (packet reception complete)
+    if (processBufferPtr != NULL && processBufferPtr->len) {
+        srxl2Process(rxRuntimeConfig);
+        processBufferPtr->len = 0;
+    }
+
+    uint8_t result = globalResult;
+
+    const uint32_t now = micros();
+
+    switch (state) {
+    case Disabled: break;
+
+    case ListenForActivity: {
+        // activity detected
+        if (lastValidPacketTimestamp != 0) {
+            // as ListenForActivity is done at default baud-rate, we don't need to change anything
+            // @todo if there were non-handshake packets - go to running,
+            // if there were - go to either Send Handshake or Listen For Handshake
+            state = Running;
+        } else if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
+            if (baudRate != 0) {
+                uint32_t currentBaud = serialGetBaudRate(serialPort);
+
+                if(currentBaud == SRXL2_PORT_BAUDRATE_DEFAULT)
+                    serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
+                else
+                    serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+            }
+        } else if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
+            // @todo if there was activity - detect baudrate and ListenForHandshake
+
+            if (unitId == 0) {
+                state = SendHandshake;
+                timeoutTimestamp = now + SRXL2_SEND_HANDSHAKE_TIMEOUT_US;
+                fullTimeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
+            } else {
+                state = ListenForHandshake;
+                timeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
+            }
+        }
+    } break;
+
+    case SendHandshake: {
+        if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
+            // @todo set another timeout for 50ms tries
+            // fill write buffer with handshake frame
+            result |= RX_FRAME_PROCESSING_REQUIRED;
+        }
+
+        if (cmpTimeUs(now, fullTimeoutTimestamp) >= 0) {
+            serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+            DEBUG_PRINTF("case SendHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
+            timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+            result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
+
+            state = ListenForActivity;
+            lastReceiveTimestamp = 0;
+        }
+    } break;
+
+    case ListenForHandshake: {
+        if (cmpTimeUs(now, timeoutTimestamp) >= 0)  {
+            serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+            DEBUG_PRINTF("case ListenForHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
+            timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+            result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
+
+            state = ListenForActivity;
+            lastReceiveTimestamp = 0;
+        }
+    } break;
+
+    case Running: {
+        // frame timed out, reset state
+        if (cmpTimeUs(now, lastValidPacketTimestamp) >= SRXL2_FRAME_TIMEOUT_US) {
+            serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+            DEBUG_PRINTF("case Running: switching to %d baud: %d %d\r\n", SRXL2_PORT_BAUDRATE_DEFAULT, now, lastValidPacketTimestamp);
+            timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+            result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
+
+            state = ListenForActivity;
+            lastReceiveTimestamp = 0;
+            lastValidPacketTimestamp = 0;
+        }
+    } break;
+    };
+
+    if (writeBufferIdx) {
+        result |= RX_FRAME_PROCESSING_REQUIRED;
+    }
+
+    return result;
+}
+
+static bool srxl2ProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+    if (writeBufferIdx == 0) {
+        return true;
+    }
+
+    const uint32_t now = micros();
+
+    if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
+        // time sufficient for at least 2 characters has passed
+        if (cmpTimeUs(now, lastReceiveTimestamp) > SRXL2_REPLY_QUIESCENCE) {
+            transmittingTelemetry = true;
+            serialWriteBuf(serialPort, writeBuffer, writeBufferIdx);
+            writeBufferIdx = 0;
+        } else {
+            DEBUG_PRINTF("not enough time to send 2 characters passed yet, %d us since last receive, %d required\r\n", now - lastReceiveTimestamp, SRXL2_REPLY_QUIESCENCE);
+        }
+    } else {
+        DEBUG_PRINTF("still receiving a frame, %d %d\r\n", lastIdleTimestamp, lastReceiveTimestamp);
+    }
+
+    return true;
+}
+
+static uint16_t srxl2ReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channelIdx)
+{
+    if (channelIdx >= rxRuntimeConfig->channelCount) {
+        return 0;
+    }
+
+    return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeConfig->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1);
+}
+
+void srxl2RxWriteData(const void *data, int len)
+{
+    const uint16_t crc = crc16_ccitt_update(0, (uint8_t*)data, len - 2);
+    ((uint8_t*)data)[len-2] = ((uint8_t *) &crc)[1] & 0xFF;
+    ((uint8_t*)data)[len-1] = ((uint8_t *) &crc)[0] & 0xFF;
+    
+    len = MIN(len, (int)sizeof(writeBuffer));
+    memcpy(writeBuffer, data, len);
+    writeBufferIdx = len;
+}
+
+bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    static uint16_t channelData[SRXL2_MAX_CHANNELS];
+    for (size_t i = 0; i < SRXL2_MAX_CHANNELS; ++i) {
+        channelData[i] = SRXL2_CHANNEL_CENTER;
+    }
+
+    unitId = rxConfig->srxl2_unit_id;
+    baudRate = rxConfig->srxl2_baud_fast;
+
+    rxRuntimeConfig->channelData = channelData;
+    rxRuntimeConfig->channelCount = SRXL2_MAX_CHANNELS;
+    rxRuntimeConfig->rxRefreshRate = SRXL2_FRAME_PERIOD_US;
+
+    rxRuntimeConfig->rcReadRawFn = srxl2ReadRawRC;
+    rxRuntimeConfig->rcFrameStatusFn = srxl2FrameStatus;
+    rxRuntimeConfig->rcProcessFrameFn = srxl2ProcessFrame;
+
+    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
+    if (!portConfig) {
+        return false;
+    }
+
+    portOptions_t options = SRXL2_PORT_OPTIONS;
+    if (rxConfig->serialrx_inverted) {
+        options |= SERIAL_INVERTED;
+    }
+    if (rxConfig->halfDuplex) {
+        options |= SERIAL_BIDIR;
+    }
+
+    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive,
+        NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options);
+
+    if (!serialPort) {
+        return false;
+    }
+
+    state = ListenForActivity;
+    timeoutTimestamp = micros() + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+
+    //Looks like this needs to be set in cli config
+    //if (rssiSource == RSSI_SOURCE_NONE) {
+    //    rssiSource = RSSI_SOURCE_RX_PROTOCOL;
+    //}
+
+    return (bool)serialPort;
+}
+
+bool srxl2RxIsActive(void)
+{
+    return serialPort;
+}
+
+bool srxl2TelemetryRequested(void)
+{
+    return telemetryRequested;
+}
+
+void srxl2InitializeFrame(sbuf_t *dst)
+{
+    dst->ptr = telemetryFrame;
+    dst->end = ARRAYEND(telemetryFrame);
+
+    sbufWriteU8(dst, SRXL2_ID);
+    sbufWriteU8(dst, TelemetrySensorData);
+    sbufWriteU8(dst, ARRAYLEN(telemetryFrame));
+    sbufWriteU8(dst, busMasterDeviceId);
+}
+
+void srxl2FinalizeFrame(sbuf_t *dst)
+{
+  sbufSwitchToReader(dst, telemetryFrame);
+  // Include 2 additional bytes of length since we're letting the srxl2RxWriteData function add the CRC in
+  srxl2RxWriteData(sbufPtr(dst), sbufBytesRemaining(dst) + 2);
+  telemetryRequested = false;
+}
+
+void srxl2Bind(void)
+{
+    const size_t length = sizeof(Srxl2BindInfoFrame);
+
+    Srxl2BindInfoFrame bind = {
+        .header = {
+            .id = SRXL2_ID,
+            .packetType = BindInfo,
+            .length = length
+        },
+        .payload = {
+            .request = EnterBindMode,
+            .deviceId = busMasterDeviceId,
+            .bindType = DMSX_11ms,
+            .options = SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE,
+        }
+    };
+
+    srxl2RxWriteData(&bind, length);
+}
+
+#endif

src/main/rx/srxl2.h

@@ -0,0 +1,15 @@
+#pragma once
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "rx/rx.h"
+
+struct sbuf_s;
+
+bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+bool srxl2RxIsActive(void);
+void srxl2RxWriteData(const void *data, int len);
+bool srxl2TelemetryRequested(void);
+void srxl2InitializeFrame(struct sbuf_s *dst);
+void srxl2FinalizeFrame(struct sbuf_s *dst);
+void srxl2Bind(void);

src/main/rx/srxl2_types.h

@@ -0,0 +1,138 @@
+#pragma once
+
+#define PACKED __attribute__((packed))
+
+typedef enum {
+    Disabled,
+    ListenForActivity,
+    SendHandshake,
+    ListenForHandshake,
+    Running
+} Srxl2State;
+
+typedef enum {
+    Handshake = 0x21,
+    BindInfo = 0x41,
+    ParameterConfiguration = 0x50,
+    SignalQuality = 0x55,
+    TelemetrySensorData = 0x80,
+    ControlData = 0xCD,
+} Srxl2PacketType;
+
+typedef struct {
+    uint8_t id;
+    uint8_t packetType;
+    uint8_t length;
+} PACKED Srxl2Header;
+
+typedef struct {
+    uint8_t sourceDeviceId;
+    uint8_t destinationDeviceId;
+    uint8_t priority;
+    uint8_t baudSupported;
+    uint8_t info;
+    uint32_t uniqueId;
+} PACKED Srxl2HandshakeSubHeader;
+
+typedef struct {
+    uint8_t command;
+    uint8_t replyId;
+} PACKED Srxl2ControlDataSubHeader;
+
+typedef enum {
+    ChannelData = 0x00,
+    FailsafeChannelData = 0x01,
+    VTXData = 0x02,
+} Srxl2ControlDataCommand;
+
+typedef struct {
+    int8_t rssi;
+    uint16_t frameLosses;
+    union {
+        //struct {
+        //    uint8_t channels_0_7;
+        //    uint8_t channels_8_15;
+        //    uint8_t channels_16_23;
+        //    uint8_t channels_24_31;
+        //} u8;
+        uint8_t u8[4];
+        uint32_t u32;
+    } channelMask;
+} PACKED Srxl2ChannelDataHeader;
+
+typedef enum {
+    NoDevice = 0,
+    RemoteReceiver = 1,
+    Receiver = 2,
+    FlightController = 3,
+    ESC = 4,
+    Reserved = 5,
+    SRXLServo = 6,
+    SRXLServo_2 = 7,
+    VTX = 8,
+} Srxl2DeviceType;
+
+typedef enum {
+    FlightControllerDefault = 0x30,
+    FlightControllerMax = 0x3F,
+    Broadcast = 0xFF,
+} Srxl2DeviceId;
+
+typedef struct {
+    Srxl2Header header;
+    Srxl2HandshakeSubHeader payload;
+    uint8_t crcHigh;
+    uint8_t crcLow;
+} PACKED Srxl2HandshakeFrame;
+
+typedef enum {
+    EnterBindMode = 0xEB,
+    RequestBindStatus = 0xB5,
+    BoundDataReport = 0xDB,
+    SetBindInfo = 0x5B,
+} Srxl2BindRequest;
+
+typedef enum {
+    NotBound = 0x0,
+    DSM2_1024_22ms = 0x01,
+    DSM2_1024_MC24 = 0x02,
+    DMS2_2048_11ms = 0x12,
+    DMSX_22ms = 0xA2,
+    DMSX_11ms = 0xB2,
+    Surface_DSM2_16_5ms = 0x63,
+    DSMR_11ms_22ms = 0xE2,
+    DSMR_5_5ms = 0xE4,
+} Srxl2BindType;
+
+// Bit masks for Options byte
+#define SRXL_BIND_OPT_NONE              (0x00)
+#define SRXL_BIND_OPT_TELEM_TX_ENABLE   (0x01)  // Set if this device should be enabled as the current telemetry device to tx over RF
+#define SRXL_BIND_OPT_BIND_TX_ENABLE    (0x02)  // Set if this device should reply to a bind request with a Discover packet over RF
+
+typedef struct {
+  uint8_t request;
+  uint8_t deviceId;
+  uint8_t bindType;
+  uint8_t options;
+  uint64_t guid;
+  uint32_t uid;
+} PACKED Srxl2BindInfoPayload;
+
+typedef struct {
+  Srxl2Header header;
+  Srxl2BindInfoPayload payload;
+  uint8_t crcHigh;
+  uint8_t crcLow;
+} PACKED Srxl2BindInfoFrame;
+
+// VTX Data
+typedef struct {
+  uint8_t   band;     // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A)
+  uint8_t   channel;  // VTX Channel (0-7)
+  uint8_t   pit;      // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode.
+  uint8_t   power;    // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
+  uint16_t  powerDec; // VTX Power as a decimal 1mw/unit
+  uint8_t   region;   // Region (0 = USA, 1 = EU)
+} PACKED Srxl2VtxData;
+
+#undef PACKED

src/main/sensors/barometer.c

@@ -40,6 +40,7 @@
 #include "drivers/barometer/barometer_ms56xx.h"
 #include "drivers/barometer/barometer_spl06.h"
 #include "drivers/barometer/barometer_dps310.h"
+#include "drivers/barometer/barometer_msp.h"
 #include "drivers/time.h"
 
 #include "fc/runtime_config.h"
@@ -55,7 +56,7 @@
 
 baro_t baro;                        // barometer access functions
 
-PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
 
 #ifdef USE_BARO
 #define BARO_HARDWARE_DEFAULT    BARO_AUTODETECT
@@ -64,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER
 #endif
 PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
     .baro_hardware = BARO_HARDWARE_DEFAULT,
-    .use_median_filtering = 1,
+    .use_median_filtering = 0,
     .baro_calibration_tolerance = 150
 );
 
@@ -187,6 +188,20 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
         }
         FALLTHROUGH;
 
+    case BARO_MSP:
+#ifdef USE_BARO_MSP
+        // Skip autodetection for MSP baro, only allow manual config
+        if (baroHardwareToUse != BARO_AUTODETECT && mspBaroDetect(dev)) {
+            baroHardware = BARO_MSP;
+            break;
+        }
+#endif
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (baroHardwareToUse != BARO_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+
     case BARO_FAKE:
 #ifdef USE_FAKE_BARO
         if (fakeBaroDetect(dev)) {

src/main/sensors/barometer.h

@@ -32,7 +32,8 @@ typedef enum {
     BARO_SPL06  = 7,
     BARO_BMP388 = 8,
     BARO_DPS310 = 9,
-    BARO_FAKE   = 10,
+    BARO_MSP    = 10,
+    BARO_FAKE   = 11,
     BARO_MAX = BARO_FAKE
 } baroSensor_e;
 

src/main/sensors/compass.c

@@ -40,6 +40,7 @@
 #include "drivers/compass/compass_qmc5883l.h"
 #include "drivers/compass/compass_mpu9250.h"
 #include "drivers/compass/compass_lis3mdl.h"
+#include "drivers/compass/compass_msp.h"
 #include "drivers/io.h"
 #include "drivers/light_led.h"
 #include "drivers/time.h"
@@ -249,6 +250,20 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
         }
         FALLTHROUGH;
 
+    case MAG_MSP:
+#ifdef USE_MAG_MSP
+        // Skip autodetection for MSP mag
+        if (magHardwareToUse != MAG_AUTODETECT && mspMagDetect(dev)) {
+            magHardware = MAG_MSP;
+            break;
+        }
+#endif
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (magHardwareToUse != MAG_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+
     case MAG_FAKE:
 #ifdef USE_FAKE_MAG
         if (fakeMagDetect(dev)) {

src/main/sensors/compass.h

@@ -41,7 +41,8 @@ typedef enum {
     MAG_MPU9250 = 9,
     MAG_IST8308 = 10,
     MAG_LIS3MDL = 11,
-    MAG_FAKE = 12,
+    MAG_MSP = 12,
+    MAG_FAKE = 13,
     MAG_MAX = MAG_FAKE
 } magSensor_e;
 

src/main/sensors/pitotmeter.c

@@ -29,10 +29,11 @@
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
-#include "drivers/pitotmeter.h"
-#include "drivers/pitotmeter_ms4525.h"
-#include "drivers/pitotmeter_adc.h"
-#include "drivers/pitotmeter_virtual.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_ms4525.h"
+#include "drivers/pitotmeter/pitotmeter_adc.h"
+#include "drivers/pitotmeter/pitotmeter_msp.h"
+#include "drivers/pitotmeter/pitotmeter_virtual.h"
 #include "drivers/time.h"
 
 #include "fc/config.h"
@@ -108,6 +109,20 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             }
             FALLTHROUGH;
 
+        case PITOT_MSP:
+#ifdef USE_PITOT_MSP
+            // Skip autodetection for MSP baro, only allow manual config
+            if (pitotHardwareToUse != PITOT_AUTODETECT && mspPitotmeterDetect(dev)) {
+                pitotHardware = PITOT_MSP;
+                break;
+            }
+#endif
+            /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+            if (pitotHardwareToUse != PITOT_AUTODETECT) {
+                break;
+            }
+            FALLTHROUGH;
+
         case PITOT_FAKE:
 #ifdef USE_PITOT_FAKE
             if (fakePitotDetect(dev)) {

src/main/sensors/pitotmeter.h

@@ -21,7 +21,7 @@
 #include "common/filter.h"
 #include "common/calibration.h"
 
-#include "drivers/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter.h"
 
 typedef enum {
     PITOT_NONE = 0,
@@ -30,6 +30,7 @@ typedef enum {
     PITOT_ADC = 3,
     PITOT_VIRTUAL = 4,
     PITOT_FAKE = 5,
+    PITOT_MSP = 6,
 } pitotSensor_e;
 
 #define PITOT_MAX  PITOT_FAKE

src/main/target/AIRHEROF3/target.h

@@ -76,6 +76,10 @@
 #define VBAT_ADC_CHANNEL                ADC_CHN_1
 #define AIRSPEED_ADC_CHANNEL            ADC_CHN_2
 
+#define USE_DJI_HD_OSD
+#define USE_OSD
+#undef USE_CMS
+#undef CMS_MENU_OSD
 
 /*
 #define USE_LED_STRIP

src/main/target/MATEKF722PX/CMakeLists.txt

@@ -1 +1,2 @@
 target_stm32f722xe(MATEKF722PX)
+target_stm32f722xe(MATEKF722WPX)

src/main/target/MATEKF722PX/target.h

@@ -78,11 +78,21 @@
 #define SPI2_MISO_PIN           PB14
 #define SPI2_MOSI_PIN           PC3
 
-#define USE_FLASHFS
-#define USE_FLASH_M25P16
-#define M25P16_SPI_BUS          BUS_SPI2
-#define M25P16_CS_PIN           PB12
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+//F722-PX,F722-HD
+#if defined(MATEKF722PX)
+ #define USE_FLASHFS
+ #define USE_FLASH_M25P16
+ #define M25P16_SPI_BUS          BUS_SPI2
+ #define M25P16_CS_PIN           PB12
+ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#else
+//F722-WPX
+ #define USE_SDCARD
+ #define USE_SDCARD_SPI
+ #define SDCARD_SPI_BUS          BUS_SPI2
+ #define SDCARD_CS_PIN           PC15
+ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+#endif
 
 // *************** UART *****************************
 #define USE_VCP
@@ -163,4 +173,3 @@
 #define USE_DSHOT
 #define USE_SERIALSHOT
 #define USE_ESC_SENSOR
-

src/main/target/MATEKF765/target.h

@@ -144,13 +144,13 @@
 #define UART8_TX_PIN            PE1
 #define UART8_RX_PIN            PE0
 
-/*
-#define USE_SOFTSERIAL1
-#define SOFTSERIAL_1_TX_PIN      PA2
-#define SOFTSERIAL_1_RX_PIN      PA2
-*/
 
-#define SERIAL_PORT_COUNT       8
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN      PC6  //TX6 pad
+#define SOFTSERIAL_1_RX_PIN      PC6  //TX6 pad
+
+
+#define SERIAL_PORT_COUNT       9
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
@@ -171,6 +171,8 @@
 #define ADC_CHANNEL_2_PIN           PC3
 #define ADC_CHANNEL_3_PIN           PC1
 #define ADC_CHANNEL_4_PIN           PC0
+#define ADC_CHANNEL_5_PIN           PA4 //VBAT2
+#define ADC_CHANNEL_6_PIN           PC5 //CURR2
 
 #define VBAT_ADC_CHANNEL            ADC_CHN_1
 #define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2

src/main/target/SPRACINGF3/target.h

@@ -93,6 +93,11 @@
 #define CURRENT_METER_ADC_CHANNEL       ADC_CHN_2
 #define RSSI_ADC_CHANNEL                ADC_CHN_3
 
+#define USE_DJI_HD_OSD
+#define USE_OSD
+#undef USE_CMS
+#undef CMS_MENU_OSD
+
 #define USE_LED_STRIP
 #define WS2811_PIN                      PA8
 

src/main/target/common.h

@@ -54,6 +54,7 @@
 #define USE_BLACKBOX
 #define USE_GPS
 #define USE_GPS_PROTO_UBLOX
+#define USE_GPS_PROTO_MSP
 #define USE_NAV
 #define USE_TELEMETRY
 #define USE_TELEMETRY_LTM
@@ -93,8 +94,10 @@
 #define USE_OPFLOW_CXOF
 #define USE_OPFLOW_MSP
 
+// Allow default airspeed sensors
 #define USE_PITOT
 #define USE_PITOT_MS4525
+#define USE_PITOT_MSP
 
 #define USE_1WIRE
 #define USE_1WIRE_DS2482
@@ -124,6 +127,14 @@
 
 #define USE_I2C_IO_EXPANDER
 
+#define USE_SERIALRX_SRXL2     // Spektrum SRXL2 protocol
+#define USE_TELEMETRY_SRXL
+#define USE_SPEKTRUM_CMS_TELEMETRY
+//#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented
+#define USE_SPEKTRUM_VTX_TELEMETRY
+
+#define USE_VTX_COMMON
+
 #else // FLASH_SIZE < 256
 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
 #endif

src/main/target/common_post.h

@@ -35,6 +35,15 @@
 #define USE_CANVAS
 #endif
 
+// Enable MSP BARO & MAG drivers if BARO and MAG sensors are compiled in
+#if defined(USE_MAG)
+#define USE_MAG_MSP
+#endif
+
+#if defined(USE_BARO)
+#define USE_BARO_MSP
+#endif
+
 #ifdef USE_ESC_SENSOR
     #define USE_RPM_FILTER
 #endif

src/main/target/link/stm32_flash_F303_128k.ld

@@ -1,30 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash.ld
-**
-**  Abstract    : Linker script for STM32F30x Device with
-**                128KByte FLASH and 40KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Specify the memory areas. */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 126K
-    FLASH_CONFIG (r)  : ORIGIN = 0x0801F800, LENGTH = 2K
-
-    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
-    CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F303_256k.ld

@@ -1,30 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash.ld
-**
-**  Abstract    : Linker script for STM32F30x Device with
-**                256KByte FLASH and 40KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Specify the memory areas. */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 250K
-    FLASH_CONFIG (r)  : ORIGIN = 0x0803E800, LENGTH = 6K
-
-    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
-    CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F405.ld

@@ -1,40 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f405.ld
-**
-**  Abstract    : Linker script for STM32F405RG Device with
-**                1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x08100000 1024K full flash,
-0x08000000 to 0x080DFFFF 896K firmware,
-0x080E0000 to 0x08100000 128K config,       // FLASH_Sector_11
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 896K
-    FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
-
-    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
-    CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
-    BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F405_bl.ld

@@ -1,43 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f405.ld
-**
-**  Abstract    : Linker script for STM32F405RG Device with
-**                1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x08100000 1024K full flash,
-0x08000000 to 0x080DFFFF 896K firmware,
-0x080E0000 to 0x08100000 128K config,       // FLASH_Sector_11
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 32K
-    FIRMWARE (rx)     : ORIGIN = 0x08008000, LENGTH = 864K
-    FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
-
-    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
-    CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
-    BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-__firmware_start = ORIGIN(FIRMWARE);
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F405_for_bl.ld

@@ -1,42 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f405.ld
-**
-**  Abstract    : Linker script for STM32F405RG Device with
-**                1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x08100000 1024K full flash,
-0x08000000 to 0x080DFFFF 896K firmware,
-0x080E0000 to 0x08100000 128K config,       // FLASH_Sector_11
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08008000, LENGTH = 864K
-    FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
-
-    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
-    CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
-    BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-__firmware_start = ORIGIN(FLASH);
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F405_opbl.ld

@@ -1,40 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f405.ld
-**
-**  Abstract    : Linker script for STM32F405RG Device with
-**                1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x08100000 1024K full flash,
-0x08000000 to 0x08004000   16K OPBL,
-0x08004000 to 0x080DFFFF  880K firmware,
-0x080E0000 to 0x08100000  128K config,       // FLASH_Sector_11
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)      : ORIGIN = 0x08004000, LENGTH = 880K
-    FLASH_CONFIG (r): ORIGIN = 0x080E0000, LENGTH = 128K
-
-    RAM (rwx)       : ORIGIN = 0x20000000, LENGTH = 128K
-    CCM (rwx)       : ORIGIN = 0x10000000, LENGTH = 64K
-    MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F411.ld

@@ -1,40 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f411.ld
-**
-**  Abstract    : Linker script for STM32F11 Device with
-**                512KByte FLASH, 128KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x0807FFFF  512K full flash,
-0x08000000 to 0x08003FFF   16K isr vector, startup code,
-0x08004000 to 0x08007FFF   16K config,                  // FLASH_Sector_1
-0x08008000 to 0x0807FFFF  480K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 16K
-    FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
-    FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
-
-    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", RAM)
-REGION_ALIAS("FASTRAM", RAM)
-
-INCLUDE "stm32_flash_split.ld"

src/main/target/link/stm32_flash_F411_opbl.ld

@@ -1,40 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f411.ld
-**
-**  Abstract    : Linker script for STM32F411 Device with
-**                512KByte FLASH, 128KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x08080000 512K full flash,
-0x08000000 to 0x08004000  16K OPBL,
-0x08004000 to 0x0805FFFF 368K firmware,
-0x08060000 to 0x08080000 128K config,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08004000, LENGTH = 368K
-    FLASH_CONFIG (r)  : ORIGIN = 0x08060000, LENGTH = 128K
-
-    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
-    CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 64K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F427.ld

@@ -1,59 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash.ld
-**
-**  Abstract    : Linker script for STM32F427 Device with
-**                2048 KByte FLASH, 192KByte RAM
-**
-**                Set heap size, stack size and stack location according
-**                to application requirements.
-**
-**                Set memory bank area and size if external memory is used.
-**
-**  Target      : STMicroelectronics STM32
-**
-**  Environment : Atollic TrueSTUDIO(R)
-**
-**  Distribution: The file is distributed as is, without any warranty
-**                of any kind.
-**
-**  (c)Copyright Atollic AB.
-**  You may use this file as-is or modify it according to the needs of your
-**  project. Distribution of this file (unmodified or modified) is not
-**  permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the
-**  rights to distribute the assembled, compiled & linked contents of this
-**  file as part of an application binary file, provided that it is built
-**  using the Atollic TrueSTUDIO(R) toolchain.
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x08100000 1024K full flash,
-0x08000000 to 0x080DFFFF 896K firmware,
-0x080E0000 to 0x08100000 128K config,       // FLASH_Sector_11
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 896K
-    FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
-
-    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
-    CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", CCM)
-REGION_ALIAS("FASTRAM", CCM)
-
-INCLUDE "stm32_flash.ld"

src/main/target/link/stm32_flash_F446.ld

@@ -1,40 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f411.ld
-**
-**  Abstract    : Linker script for STM32F11 Device with
-**                512KByte FLASH, 128KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x0807FFFF  512K full flash,
-0x08000000 to 0x08003FFF   16K isr vector, startup code,
-0x08004000 to 0x08007FFF   16K config,                  // FLASH_Sector_1
-0x08008000 to 0x0807FFFF  480K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 16K
-    FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
-    FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
-
-    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", RAM)
-REGION_ALIAS("FASTRAM", RAM)
-
-INCLUDE "stm32_flash_split.ld"

src/main/target/link/stm32_flash_F746.ld

@@ -1,47 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f746.ld
-**
-**  Abstract    : Linker script for STM32F746VGTx Device with
-**                1024KByte FLASH, 320KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x080FFFFF 1024K full flash,
-0x08000000 to 0x08007FFF   32K isr vector, startup code,
-0x08008000 to 0x0800FFFF   32K config,                  // FLASH_Sector_1
-0x08010000 to 0x080FFFFF  960K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rx)	        : ORIGIN = 0x00000000, LENGTH = 16K
-    ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
-    ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)	        : ORIGIN = 0x00210000, LENGTH = 960K
-
-    FLASH (rx)                  : ORIGIN = 0x08000000, LENGTH = 32K
-    FLASH_CONFIG (r)            : ORIGIN = 0x08008000, LENGTH = 32K
-    FLASH1 (rx)                 : ORIGIN = 0x08010000, LENGTH = 960K
-
-    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
-    MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
-}
-/* note CCM could be used for stack */
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
-
-INCLUDE "stm32_flash_f7_split.ld"

src/main/target/link/stm32_flash_F7x2.ld

@@ -1,48 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f722.ld
-**
-**  Abstract    : Linker script for STM32F722RETx Device with
-**                512KByte FLASH, 256KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x0807FFFF  512K full flash,
-0x08000000 to 0x08003FFF   16K isr vector, startup code,
-0x08004000 to 0x08007FFF   16K config,                  // FLASH_Sector_1
-0x08008000 to 0x0807FFFF  480K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rwx)           : ORIGIN = 0x00000000, LENGTH = 16K
-
-    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 16K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */
-    ITCM_FLASH_CONFIG (r)   : ORIGIN = 0x00204000, LENGTH = 16K
-    ITCM_FLASH1 (rx)        : ORIGIN = 0x00208000, LENGTH = 480K
-
-    FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 16K
-    FLASH_CONFIG (r)   : ORIGIN = 0x08004000, LENGTH = 16K
-    FLASH1 (rx)        : ORIGIN = 0x08008000, LENGTH = 480K
-
-    TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
-    MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
-
-INCLUDE "stm32_flash_f7_split.ld"

src/main/target/link/stm32_flash_F7x2_bl.ld

@@ -1,50 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f722.ld
-**
-**  Abstract    : Linker script for STM32F722RETx Device with
-**                512KByte FLASH, 256KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x0807FFFF  512K full flash,
-0x08000000 to 0x08003FFF   16K isr vector, startup code,
-0x08004000 to 0x08007FFF   16K config,                  // FLASH_Sector_1
-0x08008000 to 0x0807FFFF  480K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rwx)           : ORIGIN = 0x00000000, LENGTH = 16K
-
-    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 16K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */
-    ITCM_FLASH_CONFIG (r)   : ORIGIN = 0x00204000, LENGTH = 16K
-    ITCM_FLASH1 (rx)        : ORIGIN = 0x00208000, LENGTH = 480K
-
-    FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 32K
-    FIRMWARE (rx)      : ORIGIN = 0x08008000, LENGTH = 16K
-    FLASH_CONFIG (r)   : ORIGIN = 0x0800c000, LENGTH = 16K
-
-    TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
-    MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
-
-__firmware_start = ORIGIN(FIRMWARE);
-
-INCLUDE "stm32_flash_f7.ld"

src/main/target/link/stm32_flash_F7x2_for_bl.ld

@@ -1,50 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f722.ld
-**
-**  Abstract    : Linker script for STM32F722RETx Device with
-**                512KByte FLASH, 256KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x08000000 to 0x0807FFFF  512K full flash,
-0x08000000 to 0x08003FFF   16K isr vector, startup code,
-0x08004000 to 0x08007FFF   16K config,                  // FLASH_Sector_1
-0x08008000 to 0x0807FFFF  480K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rwx)           : ORIGIN = 0x00000000, LENGTH = 16K
-
-    /*ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 16K*/
-    /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */
-    /*ITCM_FLASH_CONFIG (r)   : ORIGIN = 0x00204000, LENGTH = 16K*/
-    /*ITCM_FLASH1 (rx)        : ORIGIN = 0x00208000, LENGTH = 480K*/
-
-    FLASH (rx)         : ORIGIN = 0x08008000, LENGTH = 16K
-    FLASH_CONFIG (r)   : ORIGIN = 0x0800c000, LENGTH = 16K
-    FLASH1 (rx)        : ORIGIN = 0x08010000, LENGTH = 448K
-
-    TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
-    MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
-
-__firmware_start = ORIGIN(FLASH);
-
-INCLUDE "stm32_flash_f7_split.ld"

src/main/target/link/stm32_flash_F7x5xG.ld

@@ -1,48 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f745.ld
-**
-**  Abstract    : Linker script for STM32F745VGTx Device with
-**                1024KByte FLASH, 320KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x00000000 to 0x00003FFF   16K ITCM RAM,
-0x08000000 to 0x080FFFFF 1024K full flash,
-0x08000000 to 0x08007FFF   32K isr vector, startup code,
-0x08008000 to 0x0800FFFF   32K config,                  // FLASH_Sector_1
-0x08010000 to 0x080FFFFF  960K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rx)			: ORIGIN = 0x00000000, LENGTH = 16K
-    ITCM_FLASH (rx)        	: ORIGIN = 0x00200000, LENGTH = 32K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
-    ITCM_FLASH_CONFIG (r)  	: ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)       	: ORIGIN = 0x00210000, LENGTH = 960K
-
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 32K
-    FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
-    FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
-
-    TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 256K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
-}
-/* note CCM could be used for stack */
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
-
-INCLUDE "stm32_flash_f7_split.ld"

src/main/target/link/stm32_flash_F7x5xG_bl.ld

@@ -1,50 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f745.ld
-**
-**  Abstract    : Linker script for STM32F745VGTx Device with
-**                1024KByte FLASH, 320KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x00000000 to 0x00003FFF   16K ITCM RAM,
-0x08000000 to 0x080FFFFF 1024K full flash,
-0x08000000 to 0x08007FFF   32K isr vector, startup code,
-0x08008000 to 0x0800FFFF   32K config,                  // FLASH_Sector_1
-0x08010000 to 0x080FFFFF  960K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rx)               : ORIGIN = 0x00000000, LENGTH = 16K
-    ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
-    ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)            : ORIGIN = 0x00210000, LENGTH = 928K
-
-    FLASH (rx)                  : ORIGIN = 0x08000000, LENGTH = 32K
-    FIRMWARE (rx)               : ORIGIN = 0x08008000, LENGTH = 32K
-    FLASH_CONFIG (r)            : ORIGIN = 0x08010000, LENGTH = 32K
-
-    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
-    MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
-}
-/* note CCM could be used for stack */
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
-
-__firmware_start = ORIGIN(FIRMWARE);
-
-INCLUDE "stm32_flash_f7.ld"

src/main/target/link/stm32_flash_F7x5xG_for_bl.ld

@@ -1,50 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f745.ld
-**
-**  Abstract    : Linker script for STM32F745VGTx Device with
-**                1024KByte FLASH, 320KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x00000000 to 0x00003FFF   16K ITCM RAM,
-0x08000000 to 0x080FFFFF 1024K full flash,
-0x08000000 to 0x08007FFF   32K isr vector, startup code,
-0x08008000 to 0x0800FFFF   32K config,                  // FLASH_Sector_1
-0x08010000 to 0x080FFFFF  960K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rx)               : ORIGIN = 0x00000000, LENGTH = 16K
-    ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
-    ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)            : ORIGIN = 0x00210000, LENGTH = 960K
-
-    FLASH (rx)                  : ORIGIN = 0x08008000, LENGTH = 32K
-    FLASH_CONFIG (r)            : ORIGIN = 0x08010000, LENGTH = 32K
-    FLASH1 (rx)                 : ORIGIN = 0x08018000, LENGTH = 928K
-
-    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
-    MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
-}
-/* note CCM could be used for stack */
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
-
-__firmware_start = ORIGIN(FLASH);
-
-INCLUDE "stm32_flash_f7_split.ld"

src/main/telemetry/srxl.c

@@ -0,0 +1,812 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#if defined(USE_TELEMETRY_SRXL)
+
+#include "build/version.h"
+
+#include "cms/cms.h"
+#include "io/displayport_srxl.h"
+
+#include "common/crc.h"
+#include "common/streambuf.h"
+#include "common/utils.h"
+
+#include "config/feature.h"
+
+#include "io/gps.h"
+#include "io/serial.h"
+
+//#include "config/config.h"
+#include "fc/config.h"
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+#include "flight/mixer.h"
+
+#include "io/gps.h"
+
+//#include "pg/motor.h"
+
+#include "rx/rx.h"
+#include "rx/spektrum.h"
+#include "rx/srxl2.h"
+
+#include "sensors/battery.h"
+//#include "sensors/adcinternal.h"
+#include "sensors/esc_sensor.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/srxl.h"
+
+#include "drivers/vtx_common.h"
+//#include "drivers/dshot.h"
+
+#include "io/vtx_tramp.h"
+#include "io/vtx_smartaudio.h"
+
+#define SRXL_ADDRESS_FIRST          0xA5
+#define SRXL_ADDRESS_SECOND         0x80
+#define SRXL_PACKET_LENGTH          0x15
+
+#define SRXL_FRAMETYPE_TELE_QOS     0x7F
+#define SRXL_FRAMETYPE_TELE_RPM     0x7E
+#define SRXL_FRAMETYPE_POWERBOX     0x0A
+#define SRXL_FRAMETYPE_TELE_FP_MAH  0x34
+#define TELE_DEVICE_VTX             0x0D   // Video Transmitter Status
+#define SRXL_FRAMETYPE_SID          0x00
+#define SRXL_FRAMETYPE_GPS_LOC      0x16   // GPS Location Data (Eagle Tree)
+#define SRXL_FRAMETYPE_GPS_STAT     0x17
+
+static bool srxlTelemetryEnabled;
+static bool srxl2 = false;
+static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX];
+
+static void srxlInitializeFrame(sbuf_t *dst)
+{
+    if (srxl2) {
+#if defined(USE_SERIALRX_SRXL2)
+      srxl2InitializeFrame(dst);
+#endif
+    } else {
+        dst->ptr = srxlFrame;
+        dst->end = ARRAYEND(srxlFrame);
+
+        sbufWriteU8(dst, SRXL_ADDRESS_FIRST);
+        sbufWriteU8(dst, SRXL_ADDRESS_SECOND);
+        sbufWriteU8(dst, SRXL_PACKET_LENGTH);
+    }
+}
+
+static void srxlFinalize(sbuf_t *dst)
+{
+    if (srxl2) {
+#if defined(USE_SERIALRX_SRXL2)
+      srxl2FinalizeFrame(dst);
+#endif
+    } else {
+        crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length
+        sbufSwitchToReader(dst, srxlFrame);
+        // write the telemetry frame to the receiver.
+        srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
+    }
+}
+
+/*
+SRXL frame has the structure:
+<0xA5><0x80><Length><16-byte telemetry packet><2 Byte CRC of payload>
+The <Length> shall be 0x15 (length of the 16-byte telemetry packet + overhead).
+*/
+
+/*
+typedef struct
+{
+    UINT8 identifier; // Source device = 0x7F
+    UINT8 sID; // Secondary ID
+    UINT16 A;
+    UINT16 B;
+    UINT16 L;
+    UINT16 R;
+    UINT16 F;
+    UINT16 H;
+    UINT16 rxVoltage; // Volts, 0.01V increments
+} STRU_TELE_QOS;
+*/
+
+#define STRU_TELE_QOS_EMPTY_FIELDS_COUNT 14
+#define STRU_TELE_QOS_EMPTY_FIELDS_VALUE 0xff
+
+bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS);
+    sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+
+    sbufFill(dst, STRU_TELE_QOS_EMPTY_FIELDS_VALUE, STRU_TELE_QOS_EMPTY_FIELDS_COUNT); // Clear remainder
+
+    // Mandatory frame, send it unconditionally.
+    return true;
+}
+
+
+/*
+typedef struct
+{
+    UINT8 identifier; // Source device = 0x7E
+    UINT8 sID; // Secondary ID
+    UINT16 microseconds; // microseconds between pulse leading edges
+    UINT16 volts; // 0.01V increments
+    INT16 temperature; // degrees F
+    INT8 dBm_A, // Average signal for A antenna in dBm
+    INT8 dBm_B; // Average signal for B antenna in dBm.
+    // If only 1 antenna, set B = A
+} STRU_TELE_RPM;
+*/
+
+#define STRU_TELE_RPM_EMPTY_FIELDS_COUNT 8
+#define STRU_TELE_RPM_EMPTY_FIELDS_VALUE 0xff
+
+#define SPEKTRUM_RPM_UNUSED 0xffff
+#define SPEKTRUM_TEMP_UNUSED 0x7fff
+#define MICROSEC_PER_MINUTE 60000000
+
+//Original range of 1 - 65534 uSec gives an RPM range of 915 - 60000000rpm, 60MegaRPM
+#define SPEKTRUM_MIN_RPM 999      // Min RPM to show the user, indicating RPM is really below 999
+#define SPEKTRUM_MAX_RPM 60000000
+
+uint16_t getMotorAveragePeriod(void)
+{
+
+#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY)
+    uint32_t rpm = 0;
+    uint16_t period_us = SPEKTRUM_RPM_UNUSED;
+
+#if defined( USE_ESC_SENSOR_TELEMETRY)
+    escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
+    if (escData != NULL) {
+        rpm = escData->rpm;
+    }
+#endif
+
+#if defined(USE_DSHOT_TELEMETRY)
+    if (useDshotTelemetry) {
+        uint16_t motors = getMotorCount();
+
+        if (motors > 0) {
+            for (int motor = 0; motor < motors; motor++) {
+                rpm += getDshotTelemetry(motor);
+            }
+            rpm = 100.0f / (motorConfig()->motorPoleCount / 2.0f) * rpm;  // convert erpm freq to RPM.
+            rpm /= motors;           // Average combined rpm
+        }
+    }
+#endif
+
+    if (rpm > SPEKTRUM_MIN_RPM && rpm < SPEKTRUM_MAX_RPM) {
+        period_us = MICROSEC_PER_MINUTE / rpm; // revs/minute -> microSeconds
+    } else {
+        period_us = MICROSEC_PER_MINUTE / SPEKTRUM_MIN_RPM;
+    }
+
+    return period_us;
+#else
+    return SPEKTRUM_RPM_UNUSED;
+#endif
+}
+
+bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    int16_t coreTemp = SPEKTRUM_TEMP_UNUSED;
+#if defined(USE_ADC_INTERNAL)
+    coreTemp = getCoreTemperatureCelsius();
+    coreTemp = coreTemp * 9 / 5 + 32; // C -> F
+#endif
+
+    UNUSED(currentTimeUs);
+
+    sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
+    sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+    sbufWriteU16BigEndian(dst, getMotorAveragePeriod());    // pulse leading edges
+    if (telemetryConfig()->report_cell_voltage) {
+        sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V
+    } else {
+        sbufWriteU16BigEndian(dst, getBatteryVoltage());   // vbat is in units of 0.01V
+    }
+    sbufWriteU16BigEndian(dst, coreTemp);                   // temperature
+    sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT);
+
+    // Mandatory frame, send it unconditionally.
+    return true;
+}
+
+#if defined(USE_GPS)
+
+// From Frsky implementation
+static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
+{
+    int32_t absgps, deg, min;
+    absgps = ABS(mwiigps);
+    deg = absgps / GPS_DEGREES_DIVIDER;
+    absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60;     // absgps = Minutes left * 10^7
+    min = absgps / GPS_DEGREES_DIVIDER;                     // minutes left
+    result->dddmm = deg * 100 + min;
+    result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
+}
+
+// BCD conversion
+static uint32_t dec2bcd(uint16_t dec)
+{
+    uint32_t result = 0;
+    uint8_t counter = 0;
+
+    while (dec) {
+        result |= (dec % 10) << counter * 4;
+        counter++;
+        dec /= 10;
+    }
+    return result;
+}
+
+/*
+typedef struct
+{
+    UINT8    identifier;    // Source device = 0x16
+    UINT8    sID;           // Secondary ID
+    UINT16   altitudeLow;   // BCD, meters, format 3.1 (Low order of altitude)
+    UINT32   latitude;      // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees
+    UINT32   longitude;     // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees
+    UINT16   course;        // BCD, 3.1
+    UINT8    HDOP;          // BCD, format 1.1
+    UINT8    GPSflags;      // see definitions below
+} STRU_TELE_GPS_LOC;
+*/
+
+// GPS flags definitions
+#define GPS_FLAGS_IS_NORTH_BIT              0x01
+#define GPS_FLAGS_IS_EAST_BIT               0x02
+#define GPS_FLAGS_LONGITUDE_GREATER_99_BIT  0x04
+#define GPS_FLAGS_GPS_FIX_VALID_BIT         0x08
+#define GPS_FLAGS_GPS_DATA_RECEIVED_BIT     0x10
+#define GPS_FLAGS_3D_FIX_BIT                0x20
+#define GPS_FLAGS_NEGATIVE_ALT_BIT          0x80
+
+bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    gpsCoordinateDDDMMmmmm_t coordinate;
+    uint32_t latitudeBcd, longitudeBcd, altitudeLo;
+    uint16_t altitudeLoBcd, groundCourseBcd, hdop;
+    uint8_t hdopBcd, gpsFlags;
+
+    if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
+        return false;
+    }
+
+    // lattitude
+    GPStoDDDMM_MMMM(gpsSol.llh.lat, &coordinate);
+    latitudeBcd  = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
+
+    // longitude
+    GPStoDDDMM_MMMM(gpsSol.llh.lon, &coordinate);
+    longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
+
+    // altitude (low order)
+    altitudeLo = ABS(gpsSol.llh.alt) / 10;
+    altitudeLoBcd = dec2bcd(altitudeLo % 100000);
+
+    // Ground course
+    groundCourseBcd = dec2bcd(gpsSol.groundCourse);
+
+    // HDOP
+    hdop = gpsSol.hdop / 10;
+    hdop = (hdop > 99) ? 99 : hdop;
+    hdopBcd = dec2bcd(hdop);
+
+    // flags
+    gpsFlags = GPS_FLAGS_GPS_DATA_RECEIVED_BIT | GPS_FLAGS_GPS_FIX_VALID_BIT | GPS_FLAGS_3D_FIX_BIT;
+    gpsFlags |= (gpsSol.llh.lat > 0) ? GPS_FLAGS_IS_NORTH_BIT : 0;
+    gpsFlags |= (gpsSol.llh.lon > 0) ? GPS_FLAGS_IS_EAST_BIT : 0;
+    gpsFlags |= (gpsSol.llh.alt < 0) ? GPS_FLAGS_NEGATIVE_ALT_BIT : 0;
+    gpsFlags |= (gpsSol.llh.lon / GPS_DEGREES_DIVIDER > 99) ? GPS_FLAGS_LONGITUDE_GREATER_99_BIT : 0;
+
+    // SRXL frame
+    sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_LOC);
+    sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+    sbufWriteU16(dst, altitudeLoBcd);
+    sbufWriteU32(dst, latitudeBcd);
+    sbufWriteU32(dst, longitudeBcd);
+    sbufWriteU16(dst, groundCourseBcd);
+    sbufWriteU8(dst, hdopBcd);
+    sbufWriteU8(dst, gpsFlags);
+
+    return true;
+}
+
+/*
+typedef struct
+{
+   UINT8   identifier;                      // Source device = 0x17
+   UINT8   sID;                             // Secondary ID
+   UINT16  speed;                           // BCD, knots, format 3.1
+   UINT32  UTC;                             // BCD, format HH:MM:SS.S, format 6.1
+   UINT8   numSats;                         // BCD, 0-99
+   UINT8   altitudeHigh;                    // BCD, meters, format 2.0 (High bits alt)
+} STRU_TELE_GPS_STAT;
+*/
+
+#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE 0xff
+#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT 6
+#define SPEKTRUM_TIME_UNKNOWN 0xFFFFFFFF
+
+bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    uint32_t timeBcd;
+    uint16_t speedKnotsBcd, speedTmp;
+    uint8_t numSatBcd, altitudeHighBcd;
+    bool timeProvided = false;
+
+    if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
+        return false;
+    }
+
+    // Number of sats and altitude (high bits)
+    numSatBcd = (gpsSol.numSat > 99) ? dec2bcd(99) : dec2bcd(gpsSol.numSat);
+    altitudeHighBcd = dec2bcd(gpsSol.llh.alt / 100000);
+
+    // Speed (knots)
+    speedTmp = gpsSol.groundSpeed * 1944 / 1000;
+    speedKnotsBcd = (speedTmp > 9999) ? dec2bcd(9999) : dec2bcd(speedTmp);
+
+#ifdef USE_RTC_TIME
+    dateTime_t dt;
+    // RTC
+    if (rtcHasTime()) {
+        rtcGetDateTime(&dt);
+        timeBcd = dec2bcd(dt.hours);
+        timeBcd = timeBcd << 8;
+        timeBcd = timeBcd | dec2bcd(dt.minutes);
+        timeBcd = timeBcd << 8;
+        timeBcd = timeBcd | dec2bcd(dt.seconds);
+        timeBcd = timeBcd << 4;
+        timeBcd = timeBcd | dec2bcd(dt.millis / 100);
+        timeProvided = true;
+    }
+#endif
+    timeBcd = (timeProvided) ? timeBcd : SPEKTRUM_TIME_UNKNOWN;
+
+    // SRXL frame
+    sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_STAT);
+    sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+    sbufWriteU16(dst, speedKnotsBcd);
+    sbufWriteU32(dst, timeBcd);
+    sbufWriteU8(dst, numSatBcd);
+    sbufWriteU8(dst, altitudeHighBcd);
+    sbufFill(dst, STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT);
+
+    return true;
+}
+
+#endif
+
+/*
+typedef struct
+{
+    UINT8   identifier;     // Source device = 0x34
+    UINT8   sID;            // Secondary ID
+    INT16   current_A;      // Instantaneous current, 0.1A (0-3276.8A)
+    INT16   chargeUsed_A;   // Integrated mAh used, 1mAh (0-32.766Ah)
+    UINT16  temp_A;         // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+    INT16   current_B;      // Instantaneous current, 0.1A (0-3276.8A)
+    INT16   chargeUsed_B;   // Integrated mAh used, 1mAh (0-32.766Ah)
+    UINT16  temp_B;         // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+    UINT16  spare;          // Not used
+} STRU_TELE_FP_MAH;
+*/
+#define STRU_TELE_FP_EMPTY_FIELDS_COUNT 2
+#define STRU_TELE_FP_EMPTY_FIELDS_VALUE 0xff
+
+#define SPEKTRUM_AMPS_UNUSED 0x7fff
+#define SPEKTRUM_AMPH_UNUSED 0x7fff
+
+#define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s
+
+bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    uint16_t amps = getAmperage() / 10;
+    uint16_t mah  = getMAhDrawn();
+    static uint16_t sentAmps;
+    static uint16_t sentMah;
+    static timeUs_t lastTimeSentFPmAh = 0;
+
+    timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh;
+
+    if ( amps != sentAmps ||
+         mah != sentMah ||
+         keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) {
+
+        sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
+        sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+        sbufWriteU16(dst, amps);
+        sbufWriteU16(dst, mah);
+        sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED);            // temp A
+        sbufWriteU16(dst, SPEKTRUM_AMPS_UNUSED);            // Amps B
+        sbufWriteU16(dst, SPEKTRUM_AMPH_UNUSED);            // mAH B
+        sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED);            // temp B
+
+        sbufFill(dst, STRU_TELE_FP_EMPTY_FIELDS_VALUE, STRU_TELE_FP_EMPTY_FIELDS_COUNT);
+
+        sentAmps = amps;
+        sentMah = mah;
+        lastTimeSentFPmAh = currentTimeUs;
+        return true;
+    }
+    return false;
+}
+
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+
+// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display.
+
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C)     // Text Generator
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9)   // Text Generator ROWS
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13)  // Text Generator COLS
+
+/*
+typedef struct
+{
+    UINT8       identifier;
+    UINT8       sID;               // Secondary ID
+    UINT8       lineNumber;        // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen)
+    char        text[13];          // 0-terminated text when < 13 chars
+} STRU_SPEKTRUM_SRXL_TEXTGEN;
+*/
+
+#if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS )
+static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS];
+static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS];
+#else
+static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS];
+static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS];
+#endif
+
+//**************************************************************************
+// API Running in external client task context. E.g. in the CMS task
+int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c)
+{
+    if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) {
+      // Only update and force a tm transmision if something has actually changed.
+        if (srxlTextBuff[row][col] != c) {
+          srxlTextBuff[row][col] = c;
+          lineSent[row] = false;
+        }
+    }
+    return 0;
+}
+//**************************************************************************
+
+bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    static uint8_t lineNo = 0;
+    int lineCount = 0;
+
+    // Skip already sent lines...
+    while (lineSent[lineNo] &&
+           lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) {
+        lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
+        lineCount++;
+    }
+
+    sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN);
+    sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+    sbufWriteU8(dst, lineNo);
+    sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS);
+
+    lineSent[lineNo] = true;
+    lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
+
+    // Always send something, Always one user frame after the two mandatory frames
+    // I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true
+    // too keep the "Waltz" sequence intact.
+    return true;
+}
+#endif
+
+#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+
+static uint8_t vtxDeviceType;
+
+static void collectVtxTmData(spektrumVtx_t * vtx)
+{
+    const vtxDevice_t *vtxDevice = vtxCommonDevice();
+    vtxDeviceType = vtxCommonGetDeviceType(vtxDevice);
+
+    // Collect all data from VTX, if VTX is ready
+    unsigned vtxStatus;
+    if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) &&
+           vtxCommonGetStatus(vtxDevice, &vtxStatus) &&
+           vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) )
+        {
+            vtx->band    = 0;
+            vtx->channel = 0;
+            vtx->power   = 0;
+            vtx->pitMode = 0;
+        } else {
+            vtx->pitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0;
+        }
+
+    vtx->powerValue = 0;
+#ifdef USE_SPEKTRUM_REGION_CODES
+    vtx->region = SpektrumRegion;
+#else
+    vtx->region = SPEKTRUM_VTX_REGION_NONE;
+#endif
+}
+
+// Reverse lookup, device power index to Spektrum power range index.
+static void convertVtxPower(spektrumVtx_t * vtx)
+    {
+        uint8_t const * powerIndexTable = NULL;
+
+        vtxCommonLookupPowerValue(vtxCommonDevice(), vtx->power, &vtx->powerValue);
+        switch (vtxDeviceType) {
+
+#if defined(USE_VTX_TRAMP)
+        case VTXDEV_TRAMP:
+            powerIndexTable = vtxTrampPi;
+            break;
+#endif
+#if defined(USE_VTX_SMARTAUDIO)
+        case VTXDEV_SMARTAUDIO:
+            powerIndexTable = vtxSaPi;
+            break;
+#endif
+#if defined(USE_VTX_RTC6705)
+        case VTXDEV_RTC6705:
+            powerIndexTable = vtxRTC6705Pi;
+            break;
+#endif
+
+        case VTXDEV_UNKNOWN:
+        case VTXDEV_UNSUPPORTED:
+        default:
+          break;
+
+        }
+
+        if (powerIndexTable != NULL) {
+            for (int i = 0; i < SPEKTRUM_VTX_POWER_COUNT; i++)
+                if (powerIndexTable[i] >= vtx->power) {
+                    vtx->power = i;                                    // Translate device power index to Spektrum power index.
+                    break;
+                }
+        }
+    }
+
+static void convertVtxTmData(spektrumVtx_t * vtx)
+{
+    // Convert from internal band indexes to Spektrum indexes
+    for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) {
+        if (spek2commonBand[i] == vtx->band) {
+            vtx->band = i;
+            break;
+        }
+    }
+
+    // De-bump channel no 1 based interally, 0-based in Spektrum.
+    vtx->channel--;
+
+    // Convert Power index to Spektrum ranges, different per brand.
+    convertVtxPower(vtx);
+}
+
+/*
+typedef struct
+{
+    UINT8		identifier;
+    UINT8		sID;	  // Secondary ID
+    UINT8		band;	  // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A, 5-7 = Reserved)
+    UINT8		channel;  // VTX Channel (0-7)
+    UINT8		pit;	  // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode. When PIT is set, it overrides all other power settings
+    UINT8		power;	  // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
+    UINT16		powerDec; // VTX Power as a decimal 1mw/unit
+    UINT8		region;	  // Region (0 = USA, 1 = EU, 0xFF = N/A)
+    UINT8		rfu[7];	  // reserved
+} STRU_TELE_VTX;
+*/
+
+#define STRU_TELE_VTX_EMPTY_COUNT 7
+#define STRU_TELE_VTX_EMPTY_VALUE 0xff
+
+#define VTX_KEEPALIVE_TIME_OUT 2000000 // uS
+
+static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+    static timeUs_t lastTimeSentVtx = 0;
+    static spektrumVtx_t vtxSent;
+
+    spektrumVtx_t vtx;
+    collectVtxTmData(&vtx);
+
+    if ((vtxDeviceType != VTXDEV_UNKNOWN) && vtxDeviceType != VTXDEV_UNSUPPORTED) {
+        convertVtxTmData(&vtx);
+
+        if ((memcmp(&vtxSent, &vtx, sizeof(spektrumVtx_t)) != 0) ||
+            ((currentTimeUs - lastTimeSentVtx) > VTX_KEEPALIVE_TIME_OUT) ) {
+            // Fill in the VTX tm structure
+            sbufWriteU8(dst, TELE_DEVICE_VTX);
+            sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+            sbufWriteU8(dst,  vtx.band);
+            sbufWriteU8(dst,  vtx.channel);
+            sbufWriteU8(dst,  vtx.pitMode);
+            sbufWriteU8(dst,  vtx.power);
+            sbufWriteU16(dst, vtx.powerValue);
+            sbufWriteU8(dst,  vtx.region);
+
+            sbufFill(dst, STRU_TELE_VTX_EMPTY_VALUE, STRU_TELE_VTX_EMPTY_COUNT);
+
+            memcpy(&vtxSent, &vtx, sizeof(spektrumVtx_t));
+            lastTimeSentVtx = currentTimeUs;
+            return true;
+        }
+    }
+    return false;
+}
+#endif // USE_SPEKTRUM_VTX_TELEMETRY && USE_SPEKTRUM_VTX_CONTROL && USE_VTX_COMMON
+
+
+// Schedule array to decide how often each type of frame is sent
+// The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame.
+// The user frame type is cycled for each set.
+// Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc
+
+#define SRXL_SCHEDULE_MANDATORY_COUNT  2 // Mandatory QOS and RPM sensors
+
+#define SRXL_FP_MAH_COUNT   1
+
+#if defined(USE_GPS)
+#define SRXL_GPS_LOC_COUNT  1
+#define SRXL_GPS_STAT_COUNT 1
+#else
+#define SRXL_GPS_LOC_COUNT  0
+#define SRXL_GPS_STAT_COUNT 0
+#endif
+
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+#define SRXL_SCHEDULE_CMS_COUNT  1
+#else
+#define SRXL_SCHEDULE_CMS_COUNT  0
+#endif
+
+#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+#define SRXL_VTX_TM_COUNT        1
+#else
+#define SRXL_VTX_TM_COUNT        0
+#endif
+
+#define SRXL_SCHEDULE_USER_COUNT (SRXL_FP_MAH_COUNT + SRXL_SCHEDULE_CMS_COUNT + SRXL_VTX_TM_COUNT + SRXL_GPS_LOC_COUNT + SRXL_GPS_STAT_COUNT)
+#define SRXL_SCHEDULE_COUNT_MAX  (SRXL_SCHEDULE_MANDATORY_COUNT + 1)
+#define SRXL_TOTAL_COUNT         (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT)
+
+typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs);
+
+const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = {
+    /* must send srxlFrameQos, Rpm and then alternating items of our own */
+    srxlFrameQos,
+    srxlFrameRpm,
+    srxlFrameFlightPackCurrent,
+#if defined(USE_GPS)
+    srxlFrameGpsStat,
+    srxlFrameGpsLoc,
+#endif
+#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+    srxlFrameVTX,
+#endif
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+    srxlFrameText,
+#endif
+};
+
+
+static void processSrxl(timeUs_t currentTimeUs)
+{
+    static uint8_t srxlScheduleIndex = 0;
+    static uint8_t srxlScheduleUserIndex = 0;
+
+    sbuf_t srxlPayloadBuf;
+    sbuf_t *dst = &srxlPayloadBuf;
+    srxlScheduleFnPtr srxlFnPtr;
+
+    if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) {
+        srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
+    } else {
+        srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex];
+        srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT;
+
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+        // Boost CMS performance by sending nothing else but CMS Text frames when in a CMS menu.
+        // Sideeffect, all other reports are still not sent if user leaves CMS without a proper EXIT.
+        if (cmsInMenu &&
+            (cmsDisplayPortGetCurrent() == &srxlDisplayPort)) {
+            srxlFnPtr = srxlFrameText;
+        }
+#endif
+
+    }
+
+    if (srxlFnPtr) {
+        srxlInitializeFrame(dst);
+        if (srxlFnPtr(dst, currentTimeUs)) {
+            srxlFinalize(dst);
+        }
+    }
+    srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;
+}
+
+void initSrxlTelemetry(void)
+{
+    // check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX)
+    // and feature is enabled, if so, set SRXL telemetry enabled
+  if (srxlRxIsActive()) {
+    srxlTelemetryEnabled = true;
+    srxl2 = false; 
+#if defined(USE_SERIALRX_SRXL2)
+  } else if (srxl2RxIsActive()) {
+    srxlTelemetryEnabled = true;
+    srxl2 = true;
+#endif
+  } else {
+    srxlTelemetryEnabled = false;
+    srxl2 = false;
+  }
+ }
+
+bool checkSrxlTelemetryState(void)
+{
+    return srxlTelemetryEnabled;
+}
+
+/*
+ * Called periodically by the scheduler
+ */
+void handleSrxlTelemetry(timeUs_t currentTimeUs)
+{
+  if (srxl2) {
+#if defined(USE_SERIALRX_SRXL2)
+      if (srxl2TelemetryRequested()) {
+          processSrxl(currentTimeUs);
+      }
+#endif
+  } else {
+      if (srxlTelemetryBufferEmpty()) {
+          processSrxl(currentTimeUs);
+      }
+  }
+}
+#endif

src/main/telemetry/srxl.h

@@ -0,0 +1,35 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+void srxlCollectTelemetryNow(void);
+void initSrxlTelemetry(void);
+bool checkSrxlTelemetryState(void);
+void handleSrxlTelemetry(timeUs_t currentTimeUs);
+
+#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS 9
+#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 12 // Airware 1.20
+//#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 13 // Airware 1.21
+#define SPEKTRUM_SRXL_TEXTGEN_CLEAR_SCREEN 255
+
+int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c);

src/main/telemetry/telemetry.c

@@ -48,6 +48,7 @@
 #include "telemetry/jetiexbus.h"
 #include "telemetry/ibus.h"
 #include "telemetry/crsf.h"
+#include "telemetry/srxl.h"
 #include "telemetry/sim.h"
 
 
@@ -124,6 +125,10 @@ void telemetryInit(void)
     initCrsfTelemetry();
 #endif
 
+#ifdef USE_TELEMETRY_SRXL
+    initSrxlTelemetry();
+#endif
+
     telemetryCheckState();
 }
 
@@ -185,6 +190,10 @@ void telemetryCheckState(void)
 #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
     checkCrsfTelemetryState();
 #endif
+
+#ifdef USE_TELEMETRY_SRXL
+    checkSrxlTelemetryState();
+#endif
 }
 
 void telemetryProcess(timeUs_t currentTimeUs)
@@ -226,6 +235,10 @@ void telemetryProcess(timeUs_t currentTimeUs)
 #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
     handleCrsfTelemetry(currentTimeUs);
 #endif
+
+#ifdef USE_TELEMETRY_SRXL
+    handleSrxlTelemetry(currentTimeUs);
+#endif
 }
 
 #endif