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

CMakeLists.txt

@@ -15,6 +15,9 @@ option(SITL "SITL build for host system" OFF)
 set(TOOLCHAIN_OPTIONS none arm-none-eabi host)
 if (SITL)
     set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}")
+    if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+        set(MACOSX TRUE)
+    endif()
 else()
     set(TOOLCHAIN "arm-none-eabi" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}")
 endif()
@@ -52,7 +55,11 @@ project(INAV VERSION 7.0.0)
 
 enable_language(ASM)
 
-set(CMAKE_C_STANDARD 99)
+if(MACOSX AND SITL)
+    set(CMAKE_C_STANDARD 11)
+else()
+    set(CMAKE_C_STANDARD 99)
+endif()
 set(CMAKE_C_EXTENSIONS ON)
 set(CMAKE_C_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_STANDARD 11)

cmake/main.cmake

@@ -9,14 +9,27 @@ set(MAIN_DEFINITIONS
     __REVISION__="${GIT_REV}"
 )
 
-set(MAIN_COMPILE_OPTIONS
+
+# Can't check for OSX yet at this point
+if(SITL)
+    set(MAIN_COMPILE_OPTIONS
+        -Wall
+        -Wextra
+        -Wdouble-promotion
+        -Wstrict-prototypes
+        -Werror=switch
+        #-Wno-unknown-warning-option
+    )
+else()
+    set(MAIN_COMPILE_OPTIONS
         -Wall
         -Wextra
         -Wunsafe-loop-optimizations
         -Wdouble-promotion
         -Wstrict-prototypes
         -Werror=switch
-)
+    )
+endif()
 
 macro(main_sources var) # list-var src-1...src-n
     set(${var} ${ARGN})

cmake/sitl.cmake

@@ -25,41 +25,50 @@ main_sources(SITL_SRC
     target/SITL/sim/xplane.h
 )
 
+
+if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+  set(MACOSX ON)
+endif()
+
 set(SITL_LINK_OPTIONS
-    -lrt
     -Wl,-L${STM32_LINKER_DIR}
-    -Wl,--cref
-    -static-libgcc # Required for windows build under cygwin
 )
 
+if(${WIN32} OR ${CYGWIN})
+    set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-static-libgcc")
+endif()
+
 set(SITL_LINK_LIBRARIS
     -lpthread
     -lm
     -lc
 )
 
+if(NOT MACOSX)
+    set(SITL_LINK_LIBRARIS ${SITL_LINK_LIBRARIS} -lrt)
+endif()
+
 set(SITL_COMPILE_OPTIONS
     -Wno-format #Fixme: Compile for 32bit, but settings.rb has to be adjusted
+    -funsigned-char
+)
+
+if(NOT MACOSX)
+    set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
         -Wno-return-local-addr
         -Wno-error=maybe-uninitialized
         -fsingle-precision-constant
-    -funsigned-char
+    )
-)
+endif()
 
 set(SITL_DEFINITIONS
     SITL_BUILD
 )
 
-function(generate_map_file target)
-    if(CMAKE_VERSION VERSION_LESS 3.15)
-        set(map "$<TARGET_FILE:${target}>.map")
-    else()
-        set(map "$<TARGET_FILE_DIR:${target}>/$<TARGET_FILE_BASE_NAME:${target}>.map")
-    endif()
-    target_link_options(${target} PRIVATE "-Wl,-gc-sections,-Map,${map}")
-endfunction()
-
 function (target_sitl name)
+    if(CMAKE_VERSION VERSION_GREATER 3.22)
+        set(CMAKE_C_STANDARD 17)
+    endif()
 
     if(NOT host STREQUAL TOOLCHAIN)
         return()
@@ -102,14 +111,14 @@ function (target_sitl name)
     target_link_libraries(${exe_target} PRIVATE ${SITL_LINK_LIBRARIS})
     target_link_options(${exe_target} PRIVATE ${SITL_LINK_OPTIONS})
 
-    generate_map_file(${exe_target})
-    
     set(script_path ${MAIN_SRC_DIR}/target/link/sitl.ld)
     if(NOT EXISTS ${script_path})
         message(FATAL_ERROR "linker script ${script_path} doesn't exist")
     endif()
     set_target_properties(${exe_target} PROPERTIES LINK_DEPENDS ${script_path})
+    if(NOT MACOSX)
         target_link_options(${exe_target} PRIVATE -T${script_path})
+    endif()
 
     if(${WIN32} OR ${CYGWIN})
         set(exe_filename ${CMAKE_BINARY_DIR}/${binary_name}.exe)
@@ -118,9 +127,7 @@ function (target_sitl name)
     endif()
 
     add_custom_target(${name} ALL
-        cmake -E env PATH="$ENV{PATH}"
+        cmake -E copy $<TARGET_FILE:${exe_target}> ${exe_filename}
-        ${CMAKE_OBJCOPY} $<TARGET_FILE:${exe_target}> ${exe_filename}
-        BYPRODUCTS ${hex}
     )
 
     setup_firmware_target(${exe_target} ${name} ${ARGN})

cmake/stm32h7.cmake

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

docs/LedStrip.md

@@ -237,6 +237,12 @@ The mapping between modes led placement and colors is currently fixed and cannot
 
 #### Indicator
 
+##### For fixed wing (INAV 6.1 onwards)
+
+This mode flashes LEDs that correspond to the roll stick position. Rolling left will flash any `indicator` LED on the left half of the grid. Rolling right will flash any `indicator` on the right side of the grid.
+
+##### For other platforms (all platforms pre INAV 6.1)
+
 This mode flashes LEDs that correspond to roll and pitch stick positions.  i.e.  they indicate the direction the craft is going to turn.
 
 | Mode | Direction | LED Color |

docs/SITL/SITL.md

@@ -12,6 +12,7 @@ The sensors are replaced by data provided by a simulator.
 Currently supported are
 - RealFlight  https://www.realflight.com/
 - X-Plane https://www.x-plane.com/
+- fl2sim [replay Blackbox Log via SITL](https://github.com/stronnag/bbl2kml/wiki/fl2sitl), uses the X-Plane protocol.
 
 INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the Configurator or the command line options.
 
@@ -30,25 +31,38 @@ The following sensors are emulated:
 Select "FAKE" as type for all mentioned, so that they receive the data from the simulator.
 
 ## Serial ports+
-UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 6761, ...
+UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 5761, ...
-By default, UART1 and UART2 are available as MSP connections.
+By default, UART1 and UART2 are available as MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
-To connect the Configurator to SITL: Select TCP and connect to ```127.0.0.1:5760``` (if SITL is running on the same machine).
+To connect the Configurator to SITL: Select TCP and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
-IPv4 and IPv6 are supported, either raw addresses of hostname lookup.
+IPv4 and IPv6 are supported, either raw addresses or host-name lookup.
 
 The assignment and status of user UART/TCP connections is displayed on the console.
 
-![STL-Output](assets/SITL-UART-TCP-Connecion.png)
+```
+INAV 6.1.0 SITL
+[SYSTEM] Init...
+[SIM] No interface specified. Configurator only.
+[EEPROM] Loaded 'eeprom.bin' (32768 of 32768 bytes)
+[SOCKET] Bind TCP :: port 5760 to UART1
+[SOCKET] Bind TCP :: port 5761 to UART2
+[SOCKET] ::1 connected to UART1
+```
 
 All other interfaces (I2C, SPI, etc.) are not emulated.
 
 ## Remote control
-Joystick (via simulator) or serial receiver via USB/Serial interface are supported.
+MSP_RX (TCP/IP) or joystick (via simulator) or serial receiver via USB/Serial interface are supported.
+
+### MSP_RX
+
+MSP_RX is the default, 18 channels are supported over TCP/IP serial emulation.
 
 ### Joystick interface
 Only 8 channels are supported.
 Select "SIM (SITL)" as the receiver and set up a joystick in the simulator, details of which can be found in the documentation for the individual simulators.
 
 ### Serial Receiver via USB
+
 Connect a serial receiver (e.g. SBUS) to the PC via a UART/USB adapter. Configure the receiver in the Configurator as usual.
 
 The Configurator offers a built-in option for forwarding the serial data to the SITL TCP port, if SITL is started manually the following option can be used:
@@ -65,7 +79,11 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/)
 For SBUS, the command line arguments of the python script are:
 ```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000```
 
-Note: Telemetry via return channel through the receiver is not supported by SITL (yet).
+### Telemtry
+
+LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP.
+
+RX Telemetry via a return channel through the receiver is not yet supported by SITL.
 
 ## OSD
 For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
@@ -74,20 +92,22 @@ For this, activate MSP-Displayport on a UART/TCP port and connect to the corresp
 Note: INAV-Sim-OSD only works if the simulator is in window mode.
 
 ## Command line
-The command line options are only necessary if the SITL executable is started by hand, e.g. when debugging.
+
-For normal use, please use the SITL tab in the configurator.
+The command line options are only necessary if the SITL executable is started by hand.
+
+There is also a SITL tab in the  INAV Configurator (6.1.0 and later).
 
 The following SITL specific command line options are available:
 
 If SITL is started without command line options, only a serial MSP / CLI connection can be used (e.g. Configurator or other application) can be used.
 
-```--path``` Full path and file name to config file, if not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```
+```--path``` Path and file name to config file. If not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```, ```/home/user/sitl-eeproms/test-eeprom.bin```.
 
 ```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
 
-```--simip=[ip]``` IP address of the simulator, if you specify a simulator with "--sim" and omit this option localhost (127.0.0.1) will be used. Example: ```--simip=172.65.21.15```
+```--simip=[ip]``` Hostname or IP address of the simulator, if you specify a simulator with "--sim" and omit this option IPv4 localhost (`127.0.0.1`) will be used. Example: ```--simip=172.65.21.15```, ```--simip acme-sims.org```, ```--sim ::1```.
 
-```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```
+```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```. For the X-Plane protocol, the default port is `49000`.
 
 ```--useimu``` Use IMU sensor data from the simulator instead of using attitude data directly from the simulator. Not recommended, use only for debugging.
 
@@ -100,6 +120,8 @@ Please also read the documentation of the individual simulators.
 
 ```--help``` Displays help for the command line options.
 
+For options that take an argument, either form `--flag=value` or `--flag value` may be used.
+
 ## Running SITL
 It is recommended to start the tools in the following order:
 1. Simulator, aircraft should be ready for take-off
@@ -124,6 +146,8 @@ make
 Compile under cygwin, then as in Linux.
 Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH.
 
+If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
+
 #### Build manager
 
 `ninja` may also be used (parallel builds without `-j $(nproc)`):
@@ -135,12 +159,12 @@ ninja
 
 ### Compiler requirements
 
-* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work.
+* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work. GCC 10 to GCC 13 are known to work.
 * Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
 * Pthreads
 
 ## Supported environments
 
-* Linux on x86_64, Aarch64 (e.g. Rpi4), RISC-V (e.g. VisionFive2)
+* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
 * Windows on x86_64
 * FreeBSD (x86_64 at least).

src/main/cms/cms_menu_osd.c

@@ -190,8 +190,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("TIME (HOUR)", OSD_RTC_TIME),
     OSD_ELEMENT_ENTRY("FLY MODE", OSD_FLYMODE),
     OSD_ELEMENT_ENTRY("NAME", OSD_CRAFT_NAME),
-    OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS),
+    OSD_ELEMENT_ENTRY("THR. ", OSD_THROTTLE_POS),
-    OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR),
+    OSD_ELEMENT_ENTRY("THR. (SCALED)", OSD_SCALED_THROTTLE_POS),
     OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES),
     OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL),
     OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW),

src/main/common/encoding.c

@@ -17,8 +17,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "encoding.h"
 
 /**

src/main/common/filter.c

@@ -22,8 +22,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
@@ -318,7 +316,6 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     filter->y2 = y2;
 }
 
-FUNCTION_COMPILE_FOR_SIZE
 void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
     const float dT = US2S(refreshRate);
 
@@ -335,7 +332,6 @@ void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFr
     }
 }
 
-FUNCTION_COMPILE_FOR_SIZE
 void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn) {
     *applyFn = nullFilterApply;
     if (cutoffFrequency) {

src/main/common/fp_pid.c

@@ -24,8 +24,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include <math.h>
 #include "common/fp_pid.h"
 

src/main/common/maths.c

@@ -29,8 +29,6 @@
 #include "arm_math.h"
 #endif
 
-FILE_COMPILE_FOR_SPEED
-
 // http://lolengine.net/blog/2011/12/21/better-function-approximations
 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
 // Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384

src/main/common/typeconversion.c

@@ -21,7 +21,6 @@
 
 #include "maths.h"
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 

src/main/drivers/barometer/barometer_msp.c

@@ -39,6 +39,10 @@
 #include "drivers/barometer/barometer.h"
 #include "drivers/barometer/barometer_msp.h"
 
+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+#include "fc/runtime_config.h"
+
 #include "msp/msp_protocol_v2_sensor_msg.h"
 
 #define MSP_BARO_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
@@ -47,6 +51,8 @@ static int32_t mspBaroPressure;
 static int32_t mspBaroTemperature;
 static timeMs_t mspBaroLastUpdateMs;
 
+static bool mspBaroStarted = false;
+
 static bool mspBaroStartGet(baroDev_t * baro)
 {
     UNUSED(baro);
@@ -57,7 +63,9 @@ static bool mspBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *tempe
 {
     UNUSED(baro);
 
-    if ((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS) {
+    if (((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS)) {
+        sensorsClear(SENSOR_BARO);
+        mspBaroStarted = false;
         return false;
     }
 
@@ -77,12 +85,21 @@ void mspBaroReceiveNewData(uint8_t * bufferPtr)
     mspBaroPressure = pkt->pressurePa;
     mspBaroTemperature = pkt->temp;
     mspBaroLastUpdateMs = millis();
+    
+    // This should only happen after a reset (!ARMING_FLAG(WAS_EVER_ARMED)) to avoid 
+    // getting calibrations mid-air or on a surface that is above the home position
+    if (mspBaroStarted == false && !ARMING_FLAG(WAS_EVER_ARMED)){
+        baroStartCalibration();
+        mspBaroStarted = true;
+        sensorsSet(SENSOR_BARO);
+    }
 }
 
 bool mspBaroDetect(baroDev_t *baro)
 {
     mspBaroPressure = 101325;    // pressure in Pa (0m MSL)
     mspBaroTemperature = 2500;   // temperature in 0.01 C = 25 deg
+    mspBaroLastUpdateMs = 0;
 
     // these are dummy as temperature is measured as part of pressure
     baro->ut_delay = 10000;

src/main/drivers/bus_spi_at32f43x.c

@@ -87,7 +87,7 @@
         { .dev = NULL },    // No SPI1
     #endif
     #ifdef USE_SPI_DEVICE_2
-        { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_MUX_6, .divisorMap = spiDivisorMapSlow },
+        { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_MUX_5, .divisorMap = spiDivisorMapSlow },
     #else
         { .dev = NULL },    // No SPI2
     #endif

src/main/drivers/max7456.c

@@ -23,8 +23,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #ifdef USE_MAX7456
 
 #if defined(MAX7456_USE_BOUNDS_CHECKS)

src/main/drivers/pwm_output.c

@@ -24,8 +24,6 @@
 
 #if !defined(SITL_BUILD)
 
-FILE_COMPILE_FOR_SPEED
-
 #include "build/debug.h"
 
 #include "common/log.h"

src/main/drivers/serial_tcp.c

@@ -45,7 +45,6 @@
 
 static const struct serialPortVTable tcpVTable[];
 static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
-static bool tcpThreadRunning = false;
 
 static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len )
 {
@@ -73,13 +72,11 @@ static int lookup_address (char *name, int port, int type, struct sockaddr *addr
         fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
         return result;
     } else {
-	int j = 0;
         for(p = servinfo; p != NULL; p = p->ai_next) {
             if(p->ai_family == AF_INET6)
                 p6 = p;
             else if(p->ai_family == AF_INET)
                 p4 = p;
-	    j++;
         }
         if (p6 != NULL)
             p = p6;
@@ -112,16 +109,10 @@ static char *tcpGetAddressString(struct sockaddr *addr)
 static void *tcpReceiveThread(void* arg)
 {
     tcpPort_t *port = (tcpPort_t*)arg;
-
+    while(tcpReceive(port) >= 0)
-    while(tcpThreadRunning) {
+        ;
-        if (tcpReceive(port) < 0) {
-            break;
-        }
-    }
-
     return NULL;
 }
-
 static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
 {
     socklen_t sockaddrlen;
@@ -186,6 +177,7 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
 
 int tcpReceive(tcpPort_t *port)
 {
+
     if (!port->isClientConnected) {
 
         fd_set fds;
@@ -212,9 +204,8 @@ int tcpReceive(tcpPort_t *port)
     uint8_t buffer[TCP_BUFFER_SIZE];
     ssize_t recvSize = recv(port->clientSocketFd, buffer, TCP_BUFFER_SIZE, 0);
 
-    // Disconnect
+    // recv() under cygwin does not recognise the closed connection under certain circumstances, but returns ECONNRESET as an error.
-    if (port->isClientConnected && recvSize == 0)
+    if (port->isClientConnected && (recvSize == 0 || ( recvSize == -1 && errno == ECONNRESET))) {
-    {
        fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
        close(port->clientSocketFd);
        memset(&port->clientAddress, 0, sizeof(port->clientAddress));
@@ -248,7 +239,7 @@ serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
 
 #if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
     uint32_t id = (uintptr_t)USARTx;
-    if (id < SERIAL_PORT_COUNT) {
+    if (id <= SERIAL_PORT_COUNT) {
         port = tcpReConfigure(&tcpPorts[id-1], id);
     }
 #endif
@@ -273,8 +264,6 @@ serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
         fprintf(stderr, "[SOCKET] Unable to create receive thread for UART%d\n", id);
         return NULL;
     }
-    tcpThreadRunning = true;
-
     return (serialPort_t*)port;
 }
 

src/main/fc/cli.c

@@ -3098,7 +3098,9 @@ static void cliSave(char *cmdline)
 
     cliPrint("Saving");
     //copyCurrentProfileToProfileSlot(getConfigProfile();
+    suspendRxSignal();
     writeEEPROM();
+    resumeRxSignal();
     cliReboot();
 }
 
@@ -3108,6 +3110,9 @@ static void cliDefaults(char *cmdline)
 
     cliPrint("Resetting to defaults");
     resetEEPROM();
+    suspendRxSignal();
+    writeEEPROM();
+    resumeRxSignal();
 
 #ifdef USE_CLI_BATCH
     commandBatchError = false;

src/main/fc/config.c

@@ -323,8 +323,6 @@ static void activateConfig(void)
 
 void readEEPROM(void)
 {
-    suspendRxSignal();
-
     // Sanity check, read flash
     if (!loadEEPROM()) {
         failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
@@ -335,14 +333,14 @@ void readEEPROM(void)
 
     validateAndFixConfig();
     activateConfig();
-
-    resumeRxSignal();
 }
 
 void processSaveConfigAndNotify(void)
 {
+    suspendRxSignal();
     writeEEPROM();
     readEEPROM();
+    resumeRxSignal();
     beeperConfirmationBeeps(1);
 #ifdef USE_OSD
     osdShowEEPROMSavedNotification();
@@ -351,15 +349,12 @@ void processSaveConfigAndNotify(void)
 
 void writeEEPROM(void)
 {
-    suspendRxSignal();
     writeConfigToEEPROM();
-    resumeRxSignal();
 }
 
 void resetEEPROM(void)
 {
     resetConfigs();
-    writeEEPROM();
 }
 
 void ensureEEPROMContainsValidData(void)
@@ -368,6 +363,9 @@ void ensureEEPROMContainsValidData(void)
         return;
     }
     resetEEPROM();
+    suspendRxSignal();
+    writeEEPROM();
+    resumeRxSignal();
 }
 
 /*
@@ -400,7 +398,9 @@ void processDelayedSave(void)
         processSaveConfigAndNotify();
         saveState = SAVESTATE_NONE;
     } else if (saveState == SAVESTATE_SAVEONLY) {
+        suspendRxSignal();
         writeEEPROM();
+        resumeRxSignal();
         saveState = SAVESTATE_NONE;
     }
 }
@@ -430,8 +430,10 @@ void setConfigProfileAndWriteEEPROM(uint8_t profileIndex)
 {
     if (setConfigProfile(profileIndex)) {
         // profile has changed, so ensure current values saved before new profile is loaded
+        suspendRxSignal();
         writeEEPROM();
         readEEPROM();
+        resumeRxSignal();
     }
     beeperConfirmationBeeps(profileIndex + 1);
 }
@@ -459,8 +461,10 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex)
 {
     if (setConfigBatteryProfile(profileIndex)) {
         // profile has changed, so ensure current values saved before new profile is loaded
+        suspendRxSignal();
         writeEEPROM();
         readEEPROM();
+        resumeRxSignal();
     }
     beeperConfirmationBeeps(profileIndex + 1);
 }

src/main/fc/fc_core.c

@@ -21,8 +21,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "blackbox/blackbox.h"
 
 #include "build/debug.h"

src/main/fc/fc_init.c

@@ -223,7 +223,9 @@ void init(void)
 
     initEEPROM();
     ensureEEPROMContainsValidData();
+    suspendRxSignal();
     readEEPROM();
+    resumeRxSignal();
 
 #ifdef USE_UNDERCLOCK
     // Re-initialize system clock to their final values (if necessary)

src/main/fc/fc_msp.c

@@ -834,7 +834,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU32(dst, getFlightTime()); // Flight time (seconds)
 
         // Throttle
-        sbufWriteU8(dst, getThrottlePercent()); // Throttle Percent
+        sbufWriteU8(dst, getThrottlePercent(true)); // Throttle Percent
         sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1)
 
         break;
@@ -2349,8 +2349,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_RESET_CONF:
         if (!ARMING_FLAG(ARMED)) {
+            suspendRxSignal();
             resetEEPROM();
+            writeEEPROM();
             readEEPROM();
+            resumeRxSignal();
         } else
             return MSP_RESULT_ERROR;
         break;
@@ -2380,8 +2383,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_EEPROM_WRITE:
         if (!ARMING_FLAG(ARMED)) {
+            suspendRxSignal();
             writeEEPROM();
             readEEPROM();
+            resumeRxSignal();
         } else
             return MSP_RESULT_ERROR;
         break;

src/main/fc/settings.yaml

@@ -25,7 +25,7 @@ tables:
     values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
     enum: rxReceiverType_e
   - name: serial_rx
-    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"]
+    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK", "FBUS"]
   - name: blackbox_device
     values: ["SERIAL", "SPIFLASH", "SDCARD"]
   - name: motor_pwm_protocol

src/main/flight/dynamic_gyro_notch.c

@@ -26,8 +26,6 @@
 
 #ifdef USE_DYNAMIC_FILTERS
 
-FILE_COMPILE_FOR_SPEED
-
 #include <stdint.h>
 #include "dynamic_gyro_notch.h"
 #include "fc/config.h"

src/main/flight/gyroanalyse.c

@@ -26,7 +26,6 @@
 #include <stdint.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #ifdef USE_DYNAMIC_FILTERS
 

src/main/flight/imu.c

@@ -23,8 +23,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "blackbox/blackbox.h"
 
 #include "build/build_config.h"

src/main/flight/kalman.c

@@ -20,8 +20,6 @@
 #include "platform.h"
 #ifdef USE_GYRO_KALMAN
 
-FILE_COMPILE_FOR_SPEED
-
 #include <string.h>
 #if !defined(SITL_BUILD)
 #include "arm_math.h"

src/main/flight/mixer.c

@@ -21,8 +21,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "build/debug.h"
 
 #include "common/axis.h"
@@ -614,9 +612,16 @@ void FAST_CODE mixTable()
     }
 }
 
-int16_t getThrottlePercent(void)
+int16_t getThrottlePercent(bool useScaled)
 {
-    int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - getThrottleIdleValue()) * 100 / (motorConfig()->maxthrottle - getThrottleIdleValue());
+    int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
+    const int idleThrottle = getThrottleIdleValue();
+    
+    if (useScaled) {
+       thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle);
+    } else {
+        thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
+    }
     return thr;
 }
 

src/main/flight/mixer.h

@@ -111,7 +111,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int mixerThrottleCommand;
 
 int getThrottleIdleValue(void);
-int16_t getThrottlePercent(void);
+int16_t getThrottlePercent(bool);
 uint8_t getMotorCount(void);
 float getMotorMixRange(void);
 bool mixerIsOutputSaturated(void);

src/main/flight/pid.c

@@ -21,8 +21,6 @@
 
 #include <platform.h>
 
-FILE_COMPILE_FOR_SPEED
-
 #include "build/build_config.h"
 #include "build/debug.h"
 
@@ -307,7 +305,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
 );
 
-FUNCTION_COMPILE_FOR_SIZE
 bool pidInitFilters(void)
 {
     const uint32_t refreshRate = getLooptime();

src/main/flight/power_limits.c

@@ -20,7 +20,6 @@
 #include <math.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_POWER_LIMITS)
 

src/main/flight/rate_dynamics.c

@@ -20,8 +20,6 @@
 
 #ifdef USE_RATE_DYNAMICS
 
-FILE_COMPILE_FOR_SPEED
-
 #include <stdlib.h>
 #include "rate_dynamics.h"
 #include "fc/controlrate_profile.h"

src/main/flight/secondary_dynamic_gyro_notch.c

@@ -26,8 +26,6 @@
 
 #ifdef USE_DYNAMIC_FILTERS
 
-FILE_COMPILE_FOR_SPEED
-
 #include <stdint.h>
 #include "secondary_dynamic_gyro_notch.h"
 #include "fc/config.h"

src/main/flight/smith_predictor.c

@@ -34,7 +34,6 @@
 #include "flight/smith_predictor.h"
 #include "build/debug.h"
 
-FUNCTION_COMPILE_FOR_SPEED
 float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) {
     UNUSED(axis);
     if (predictor->enabled) {
@@ -54,7 +53,6 @@ float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sampl
     return sample;
 }
 
-FUNCTION_COMPILE_FOR_SIZE
 void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) {
     if (delay > 0.1) {
         predictor->enabled = true;

src/main/io/displayport_msp.c

@@ -139,7 +139,8 @@ static void resync(displayPort_t *displayPort)
 static uint32_t txBytesFree(const displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    return mspSerialTxBytesFree();
+    // Should be fixed if we ever get this thing to work
+    return UINT32_MAX;
 }
 
 static const displayPortVTable_t mspDisplayPortVTable = {

src/main/io/displayport_msp_bf_compat.c

@@ -33,6 +33,10 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
         return ch;
     }
 
+    if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) {
+        return BF_SYM_AH_DECORATION;
+    }
+
     switch (ech) {
     case SYM_RSSI:
         return BF_SYM_RSSI;
@@ -444,16 +448,8 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
 
     case SYM_AH_RIGHT:
         return BF_SYM_AH_RIGHT;
-/*
-    case SYM_AH_DECORATION_MIN:
-        return BF_SYM_AH_DECORATION_MIN;
-*/
-    case SYM_AH_DECORATION:
-        return BF_SYM_AH_DECORATION;
-/*
-    case SYM_AH_DECORATION_MAX:
-        return BF_SYM_AH_DECORATION_MAX;
 
+/*
     case SYM_AH_DECORATION_COUNT:
         return BF_SYM_AH_DECORATION_COUNT;
 */

src/main/io/displayport_msp_osd.c

@@ -28,8 +28,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #if defined(USE_OSD) && defined(USE_MSP_OSD)
 
 #include "common/utils.h"
@@ -344,7 +342,7 @@ static int screenSize(const displayPort_t *displayPort)
 static uint32_t txBytesFree(const displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    return mspSerialTxBytesFree();
+    return mspSerialTxBytesFree(mspPort.port);
 }
 
 static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)

src/main/io/ledstrip.c

@@ -678,7 +678,7 @@ static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer)
     if (updateNow) {
         if (rxIsReceivingSignal()) {
             // calculate update frequency
-            int scale = MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH]));  // 0 - 500
+            int scale = (STATE(AIRPLANE) || STATE(ROVER)) ? ABS(rcCommand[ROLL]) : MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH]));  // 0 - 500
             scale += (50 - INDICATOR_DEADBAND);  // start increasing frequency right after deadband
             *timer += LED_STRIP_HZ(5) * 50 / MAX(50, scale);   // 5 - 50Hz update, 2.5 - 25Hz blink
 
@@ -693,6 +693,15 @@ static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer)
 
     const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color?
 
+    if (STATE(AIRPLANE) || STATE(ROVER)) {
+        for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
+            const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
+            if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) {
+                if (((rcCommand[ROLL] > INDICATOR_DEADBAND) && (ledGetX(ledConfig) >= 8)) || ((rcCommand[ROLL] < -INDICATOR_DEADBAND) && (ledGetX(ledConfig) < 8))) 
+                    setLedHsv(ledIndex, flashColor);
+            }
+        }
+    } else {
         quadrant_e quadrants = 0;
         if (rcCommand[ROLL] > INDICATOR_DEADBAND) {
             quadrants |= QUADRANT_NORTH_EAST | QUADRANT_SOUTH_EAST;
@@ -712,6 +721,7 @@ static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer)
                     setLedHsv(ledIndex, flashColor);
             }
         }
+    }
 }
 
 #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off

src/main/io/osd.c

@@ -31,8 +31,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #ifdef USE_OSD
 
 #include "build/debug.h"
@@ -121,7 +119,12 @@ FILE_COMPILE_FOR_SPEED
 
 #define GFORCE_FILTER_TC 0.2
 
-#define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI))
+#define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18
+#define IS_HI(X)  (rxGetChannelValue(X) > 1750)
+#define IS_LO(X)  (rxGetChannelValue(X) < 1250)
+#define IS_MID(X) (rxGetChannelValue(X) > 1250 && rxGetChannelValue(X) < 1750)
+
+#define OSD_RESUME_UPDATES_STICK_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI))
 #define STATS_PAGE2 (checkStickPosition(ROL_HI))
 #define STATS_PAGE1 (checkStickPosition(ROL_LO))
 
@@ -182,7 +185,6 @@ static bool refreshWaitForResumeCmdRelease;
 static bool fullRedraw = false;
 
 static uint8_t armState;
-static uint8_t statsPagesCheck = 0;
 
 typedef struct osdMapData_s {
     uint32_t scale;
@@ -530,13 +532,23 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
 */
 void osdFormatAltitudeSymbol(char *buff, int32_t alt)
 {
-    int digits;
+    uint8_t digits = 4U;
-    if (alt < 0) {
+    uint8_t symbolIndex = 4U;
-        digits = 4;
+    uint8_t symbolKFt = SYM_ALT_KFT;
-    } else {
+
-        digits = 3;
+    if (alt >= 0) {
+        digits = 3U;
         buff[0] = ' ';
     }
+
+#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values  
+    if (isBfCompatibleVideoSystem(osdConfig())) {
+        digits++;
+        symbolIndex++;
+        symbolKFt = SYM_ALT_FT;
+    }
+#endif
+
     switch ((osd_unit_e)osdConfig()->units) {
         case OSD_UNIT_UK:
             FALLTHROUGH;
@@ -545,12 +557,12 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
         case OSD_UNIT_IMPERIAL:
             if (osdFormatCentiNumber(buff + 4 - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) {
                 // Scaled to kft
-                buff[4] = SYM_ALT_KFT;
+                buff[symbolIndex++] = symbolKFt;
             } else {
                 // Formatted in feet
-                buff[4] = SYM_ALT_FT;
+                buff[symbolIndex++] = SYM_ALT_FT;
             }
-            buff[5] = '\0';
+            buff[symbolIndex] = '\0';
             break;
         case OSD_UNIT_METRIC_MPH:
             FALLTHROUGH;
@@ -558,12 +570,12 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
             // alt is alredy in cm
             if (osdFormatCentiNumber(buff + 4 - digits, alt, 1000, 0, 2, digits)) {
                 // Scaled to km
-                buff[4] = SYM_ALT_KM;
+                buff[symbolIndex++] = SYM_ALT_KM;
             } else {
                 // Formatted in m
-                buff[4] = SYM_ALT_M;
+                buff[symbolIndex++] = SYM_ALT_M;
             }
-            buff[5] = '\0';
+            buff[symbolIndex] = '\0';
             break;
     }
 }
@@ -622,6 +634,31 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
     }
 }
 
+/**
+ * Trim whitespace from string. 
+ * Used in Stats screen on lines with multiple values.
+*/
+char *osdFormatTrimWhiteSpace(char *buff)
+{
+    char *end;
+
+    // Trim leading spaces
+    while(isspace((unsigned char)*buff)) buff++;
+
+    // All spaces?
+    if(*buff == 0)  
+    return buff;
+
+    // Trim trailing spaces
+    end = buff + strlen(buff) - 1;
+    while(end > buff && isspace((unsigned char)*end)) end--;
+
+    // Write new null terminator character
+    end[1] = '\0';
+
+    return buff;
+}
+
 /**
  * Converts RSSI into a % value used by the OSD.
  */
@@ -1052,26 +1089,46 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
 }
 
 /**
- * Formats throttle position prefixed by its symbol.
+ * Check if this OSD layout is using scaled or unscaled throttle.
- * Shows output to motor, not stick position
+ * If both are used, it will default to scaled.
- **/
+ */
-static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr)
+bool osdUsingScaledThrottle() 
 {
+    bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]);
+    bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]);
+
+    if (!usingScaledThrottle && !usingRCThrottle)
+        usingScaledThrottle = true;
+
+    return usingScaledThrottle;
+}
+
+/**
+ * Formats throttle position prefixed by its symbol.
+ * Shows unscaled or scaled (output to motor) throttle percentage
+ **/
+static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr)
+{
+    if (useScaled) {
+        buff[0] = SYM_SCALE;
+    } else {
     buff[0] = SYM_BLANK;
+    }
     buff[1] = SYM_THR;
-    if (autoThr && navigationIsControllingThrottle()) {
+    if (navigationIsControllingThrottle()) {
         buff[0] = SYM_AUTO_THR0;
         buff[1] = SYM_AUTO_THR1;
         if (isFixedWingAutoThrottleManuallyIncreased()) {
             TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
         }
+        useScaled = true;
     }
 #ifdef USE_POWER_LIMITS
     if (powerLimiterIsLimiting()) {
         TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
     }
 #endif
-    tfp_sprintf(buff + 2, "%3d", getThrottlePercent());
+    tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled));
 }
 
 /**
@@ -1956,18 +2013,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_ALTITUDE:
         {
             int32_t alt = osdGetAltitude();
-
-#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it
-            if (isBfCompatibleVideoSystem(osdConfig())) {
-                // Use the same formatting function used for distance, which provides the proper scaling functionality
-                osdFormatDistanceSymbol(buff, alt, 0);
-            } else {
             osdFormatAltitudeSymbol(buff, alt);
-            }
-#else       
-            // BFCOMPAT mode not supported, directly call original altitude formatting function
-            osdFormatAltitudeSymbol(buff, alt);
-#endif
 
             uint16_t alt_alarm = osdConfig()->alt_alarm;
             uint16_t neg_alt_alarm = osdConfig()->neg_alt_alarm;
@@ -1982,17 +2028,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_ALTITUDE_MSL:
         {
             int32_t alt = osdGetAltitudeMsl();
-#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it
-            if (isBfCompatibleVideoSystem(osdConfig())) {
-                // Use the same formatting function used for distance, which provides the proper scaling functionality
-                osdFormatDistanceSymbol(buff, alt, 0);
-            } else {
             osdFormatAltitudeSymbol(buff, alt);
-            }
-#else
-            // BFCOMPAT mode not supported, directly call original altitude formatting function
-            osdFormatAltitudeSymbol(buff, alt);
-#endif
             break;
         }
 
@@ -2868,7 +2904,7 @@ static bool osdDrawSingleElement(uint8_t item)
             return true;
         }
 
-    case OSD_THROTTLE_POS_AUTO_THR:
+    case OSD_SCALED_THROTTLE_POS:
         {
             osdFormatThrottlePosition(buff, true, &elemAttr);
             break;
@@ -3679,7 +3715,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_GLIDESLOPE] = OSD_POS(23, 2);
 
     osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
-    osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
+    osdLayoutsConfig->item_pos[0][OSD_SCALED_THROTTLE_POS] = OSD_POS(6, 2);
     osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
     osdLayoutsConfig->item_pos[0][OSD_GROUND_COURSE] = OSD_POS(12, 3);
     osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2);
@@ -4055,21 +4091,43 @@ static void osdUpdateStats(void)
     stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude());
 }
 
-static void osdShowStatsPage1(void)
+static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 {
     const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
-    uint8_t top = 1;    /* first fully visible line */
+    uint8_t top = 1;  // Start one line down leaving space at the top of the screen. 
+    size_t multiValueLengthOffset = 0;
+
     const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
     const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20;
     char buff[10];
-    statsPagesCheck = 1;
+
+    if (page > 1)
+        page = 0;
 
     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
     displayClearScreen(osdDisplayPort);
 
+    if (isSinglePageStatsCompatible) {
+        displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---");
+    } else if (page == 0) {
     displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---      1/2 ->");
+    } else if (page == 1) {
+        displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---   <- 2/2");
+    }
 
+    if (isSinglePageStatsCompatible || page == 0) {
     if (feature(FEATURE_GPS)) {
+            if (isSinglePageStatsCompatible) {
+                displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED    :");
+                osdFormatVelocityStr(buff, stats.max_3D_speed, true, false);
+                osdLeftAlignString(buff);
+                strcat(osdFormatTrimWhiteSpace(buff),"/");
+                multiValueLengthOffset = strlen(buff);
+                displayWrite(osdDisplayPort, statValuesX, top, buff);            
+                osdGenerateAverageVelocityStr(buff);
+                osdLeftAlignString(buff);
+                displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
+            } else {
         displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED        :");
         osdFormatVelocityStr(buff, stats.max_3D_speed, true, false);
         osdLeftAlignString(buff);
@@ -4079,6 +4137,7 @@ static void osdShowStatsPage1(void)
         osdGenerateAverageVelocityStr(buff);
         osdLeftAlignString(buff);
         displayWrite(osdDisplayPort, statValuesX, top++, buff);
+            }
 
         displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE     :");
         osdFormatDistanceStr(buff, stats.max_distance*100);
@@ -4095,6 +4154,18 @@ static void osdShowStatsPage1(void)
 
     switch (rxConfig()->serialrx_provider) {
         case SERIALRX_CRSF:
+                if (isSinglePageStatsCompatible) {
+                    displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM   :");
+                    itoa(stats.min_rssi, buff, 10);
+                    osdLeftAlignString(buff);
+                    strcat(osdFormatTrimWhiteSpace(buff), "%/");
+                    multiValueLengthOffset = strlen(buff);
+                    displayWrite(osdDisplayPort, statValuesX, top, buff);                
+                    itoa(stats.min_rssi_dbm, buff, 10);
+                    tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
+                    osdLeftAlignString(buff);
+                    displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
+                } else {
             displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %       :");
             itoa(stats.min_rssi, buff, 10);
             strcat(buff, "%");
@@ -4104,6 +4175,7 @@ static void osdShowStatsPage1(void)
             itoa(stats.min_rssi_dbm, buff, 10);
             tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
             displayWrite(osdDisplayPort, statValuesX, top++, buff);
+                } 
 
             displayWrite(osdDisplayPort, statNameX, top, "MIN LQ           :");
             itoa(stats.min_lq, buff, 10);
@@ -4128,33 +4200,9 @@ static void osdShowStatsPage1(void)
 
     displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
     displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
-
-    if (savingSettings == true) {
-        displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS));
-    } else if (notify_settings_saved > 0) {
-        if (millis() > notify_settings_saved) {
-            notify_settings_saved = 0;
-        } else {
-            displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED));
-        }
         }
 
-    displayCommitTransaction(osdDisplayPort);
+    if (isSinglePageStatsCompatible || page == 1) {
-}
-
-static void osdShowStatsPage2(void)
-{
-    uint8_t top = 1;    /* first fully visible line */
-    const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
-    const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20;
-    char buff[10];
-    statsPagesCheck = 1;
-
-    displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
-    displayClearScreen(osdDisplayPort);
-
-    displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---   <- 2/2");
-
     if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) {
         displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
         osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
@@ -4274,12 +4322,18 @@ static void osdShowStatsPage2(void)
     displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
     const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
+        const float acc_extremes_min = acc_extremes[Z].min;
+        const float acc_extremes_max = acc_extremes[Z].max;
     displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
-    osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4);
+        osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4);
-    strcat(buff,"/");
+        osdLeftAlignString(buff);
-    displayWrite(osdDisplayPort, statValuesX - 1, top, buff);
+        strcat(osdFormatTrimWhiteSpace(buff),"/"); 
-    osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3);
+        multiValueLengthOffset = strlen(buff);        
-    displayWrite(osdDisplayPort, statValuesX + 4, top++, buff);
+        displayWrite(osdDisplayPort, statValuesX, top, buff);        
+        osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3);
+        osdLeftAlignString(buff);
+        displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
+    }
 
     if (savingSettings == true) {
         displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS));
@@ -4398,6 +4452,55 @@ static void osdFilterData(timeUs_t currentTimeUs) {
     lastRefresh = currentTimeUs;
 }
 
+// Detect when the user is holding the roll stick to the right
+static bool osdIsPageUpStickCommandHeld(void)
+{
+    static int pageUpHoldCount = 1;
+
+    bool keyHeld = false;
+
+    if (IS_HI(ROLL)) {
+         keyHeld = true;
+    }
+
+    if (!keyHeld) {
+        pageUpHoldCount = 1;
+    } else {
+        ++pageUpHoldCount;
+    }
+
+    if (pageUpHoldCount > 20) {
+        pageUpHoldCount = 1;
+        return true;
+    }
+
+    return false;
+}
+
+// Detect when the user is holding the roll stick to the left
+static bool osdIsPageDownStickCommandHeld(void)
+{
+    static int pageDownHoldCount = 1;
+
+    bool keyHeld = false;
+    if (IS_LO(ROLL)) {
+        keyHeld = true;
+    }
+
+    if (!keyHeld) {
+        pageDownHoldCount = 1;
+    } else {
+        ++pageDownHoldCount;
+    }
+
+    if (pageDownHoldCount > 20) {
+        pageDownHoldCount = 1;
+        return true;
+    }
+
+    return false;
+}
+
 static void osdRefresh(timeUs_t currentTimeUs)
 {
     osdFilterData(currentTimeUs);
@@ -4412,72 +4515,107 @@ static void osdRefresh(timeUs_t currentTimeUs)
       return;
     }
 
-    // detect arm/disarm
+    bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS);
-    static uint8_t statsPageAutoSwapCntl = 2;
+    static uint8_t statsCurrentPage = 0;
+    static bool statsDisplayed = false;
+    static bool statsAutoPagingEnabled = true;
+
+    // Detect arm/disarm
     if (armState != ARMING_FLAG(ARMED)) {
         if (ARMING_FLAG(ARMED)) {
+            // Display the "Arming" screen
+            statsDisplayed = false;
             osdResetStats();
-            statsPageAutoSwapCntl = 2;
+            osdShowArmed();
-            osdShowArmed(); // reset statistic etc
             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
-            statsPagesCheck = 0;
 #if defined(USE_SAFE_HOME)
             if (safehome_distance)
                 delay *= 3;
 #endif
             osdSetNextRefreshIn(delay);
         } else {
-            osdShowStatsPage1(); // show first page of statistics
+            // Display the "Stats" screen
+            statsDisplayed = true;
+            statsCurrentPage = 0;
+            statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
+            osdShowStats(statsSinglePageCompatible, statsCurrentPage);
             osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
-            statsPageAutoSwapCntl = osdConfig()->stats_page_auto_swap_time > 0 ? 0 : 2; // disable swapping pages when time = 0
         }
 
         armState = ARMING_FLAG(ARMED);
     }
 
+    // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens
     if (resumeRefreshAt) {
-        // If we already reached he time for the next refresh,
-        // or THR is high or PITCH is high, resume refreshing.
-        // Clear the screen first to erase other elements which
-        // might have been drawn while the OSD wasn't refreshing.
 
-        // auto swap stats pages when first shown
+        // Handle events only when the "Stats" screen is being displayed.
-        // auto swap cancelled using roll stick
+        if (statsDisplayed) {
-        if (statsPageAutoSwapCntl != 2) {
+
-            if (STATS_PAGE1 || STATS_PAGE2) {
+             // Manual paging stick commands are only applicable to multi-page stats.
-                statsPageAutoSwapCntl = 2;
+             // ******************************
-            } else {
+             // For single-page stats, this effectively disables the ability to cancel the 
+             // automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time
+             // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then 
+             // "Saved Settings" should display if it is active within the refresh interval. 
+             // ******************************
+             // With multi-page stats, "Saved Settings" could also be missed if the user
+             // has canceled automatic paging using the stick commands, because that is only 
+             // updated when osdShowStats() is called. So, in that case, they would only see 
+             // the "Saved Settings" message if they happen to manually change pages using the 
+             // stick commands within the interval the message is displayed. 
+            bool manualPageUpRequested = false;
+            bool manualPageDownRequested = false;       
+            if (!statsSinglePageCompatible) {
+                // These methods ensure the paging stick commands are held for a brief period
+                // Otherwise it can result in a race condition where the stats are 
+                // updated too quickly and can result in partial blanks, etc. 
+                if (osdIsPageUpStickCommandHeld()) {               
+                    manualPageUpRequested = true;
+                    statsAutoPagingEnabled = false;
+                } else if (osdIsPageDownStickCommandHeld()) {
+                    manualPageDownRequested = true;                
+                    statsAutoPagingEnabled = false;
+                }
+            }
+
+            if (statsAutoPagingEnabled) {
+                // Alternate screens for multi-page stats.
+                // Also, refreshes screen at swap interval for single-page stats.
                 if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) {
-                    if (statsPageAutoSwapCntl == 0) {
+                    if (statsCurrentPage == 0) {
-                        osdShowStatsPage1();
+                        osdShowStats(statsSinglePageCompatible, statsCurrentPage);
-                        statsPageAutoSwapCntl = 1;
+                        statsCurrentPage = 1;
                     }
                 } else {
-                    if (statsPageAutoSwapCntl == 1) {
+                    if (statsCurrentPage == 1) {
-                        osdShowStatsPage2();
+                        osdShowStats(statsSinglePageCompatible, statsCurrentPage);
-                        statsPageAutoSwapCntl = 0;
+                        statsCurrentPage = 0;
                     }
                 }
+            } else {
+                // Process manual page change events for multi-page stats.
+                if (manualPageUpRequested) {
+                    osdShowStats(statsSinglePageCompatible, 1);
+                    statsCurrentPage = 1;                   
+                } else if (manualPageDownRequested) {
+                    osdShowStats(statsSinglePageCompatible, 0);
+                    statsCurrentPage = 0;
+            }
         }
         }
 
-        if (!DELAYED_REFRESH_RESUME_COMMAND)
+        // Handle events when either "Splash", "Armed" or "Stats" screens are displayed.
-            refreshWaitForResumeCmdRelease = false;
+        if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { 
-
+            // Time elapsed or canceled by stick commands.
-        if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && DELAYED_REFRESH_RESUME_COMMAND)) {
+            // Exit to normal OSD operation.
             displayClearScreen(osdDisplayPort);
             resumeRefreshAt = 0;
-        } else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE1)) {
+            statsDisplayed = false;
-            if (statsPagesCheck == 1) {
-                osdShowStatsPage1();
-            }
-        } else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE2)) {
-            if (statsPagesCheck == 1) {
-                osdShowStatsPage2();
-            }
         } else {
+            // Continue "Splash", "Armed" or "Stats" screens.
             displayHeartbeat(osdDisplayPort);
         }
+        
         return;
     }
 

src/main/io/osd.h

@@ -162,7 +162,7 @@ typedef enum {
     OSD_MESSAGES,
     OSD_GPS_HDOP,
     OSD_MAIN_BATT_CELL_VOLTAGE,
-    OSD_THROTTLE_POS_AUTO_THR,
+    OSD_SCALED_THROTTLE_POS,
     OSD_HEADING_GRAPH,
     OSD_EFFICIENCY_MAH_PER_KM,
     OSD_WH_DRAWN,
@@ -476,6 +476,8 @@ displayCanvas_t *osdGetDisplayPortCanvas(void);
 int16_t osdGetHeading(void);
 int32_t osdGetAltitude(void);
 
+bool osdUsingScaledThrottle(void);
+
 void osdStartedSaveProcess(void);
 void osdShowEEPROMSavedNotification(void);
 

src/main/io/osd_dji_hd.c

@@ -1110,7 +1110,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst)
             activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE;
         }
 
-        if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS_AUTO_THR])) {
+        if (OSD_VISIBLE(osdLayoutConfig[OSD_SCALED_THROTTLE_POS])) {
             activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR;
         }
 

src/main/io/osd_utils.c

@@ -22,8 +22,6 @@
 #include "drivers/osd_symbols.h"
 #include "io/displayport_msp_bf_compat.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #if defined(USE_OSD) || defined(OSD_UNIT_TEST)
 
 int digitCount(int32_t value)

src/main/io/smartport_master.c

@@ -20,7 +20,6 @@
 #include <string.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_SMARTPORT_MASTER)
 

src/main/msp/msp_serial.c

@@ -550,28 +550,9 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
     return ret; // return the number of bytes written
 }
 
-uint32_t mspSerialTxBytesFree(void)
+uint32_t mspSerialTxBytesFree(serialPort_t *port)
 {
-    uint32_t ret = UINT32_MAX;
+   return serialTxBytesFree(port);
-
-    for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
-        mspPort_t * const mspPort = &mspPorts[portIndex];
-        if (!mspPort->port) {
-            continue;
-        }
-
-        // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now)
-        if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) {
-            continue;
-        }
-
-        const uint32_t bytesFree = serialTxBytesFree(mspPort->port);
-        if (bytesFree < ret) {
-            ret = bytesFree;
-        }
-    }
-
-    return ret;
 }
 
 mspPort_t * mspSerialPortFind(const serialPort_t *serialPort)

src/main/msp/msp_serial.h

@@ -107,5 +107,5 @@ void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
 int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version);
 int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen);
-uint32_t mspSerialTxBytesFree(void);
+uint32_t mspSerialTxBytesFree(serialPort_t *port);
 mspPort_t * mspSerialPortFind(const struct serialPort_s *serialPort);

src/main/navigation/navigation.c

@@ -486,7 +486,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
         .mwState = MW_NAV_STATE_NONE,
         .mwError = MW_NAV_ERROR_NONE,
@@ -988,12 +988,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi
 
     resetGCSFlags();
 
-    // If surface tracking mode changed value - reset altitude controller
+    // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
-    if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
+    if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
         resetAltitudeController(navTerrainFollowingRequested());
-    }
-
-    if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
         setupAltitudeController();
         setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);  // This will reset surface offset
     }
@@ -1021,20 +1018,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
 
     resetGCSFlags();
 
-    if ((prevFlags & NAV_CTL_POS) == 0) {
+    // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
-        resetPositionController();
+    if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
-    }
-
-    if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
         resetAltitudeController(navTerrainFollowingRequested());
         setupAltitudeController();
-    }
-
-    if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
         setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);  // This will reset surface offset
     }
 
-    if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) {
+    // Prepare position controller if idle or current Mode NOT active in position hold state
+    if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME &&
+        previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
+        previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME)
+        {
+        resetPositionController();
+
         fpVector3_t targetHoldPos;
         calculateInitialHoldPosition(&targetHoldPos);
         setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
@@ -1058,18 +1055,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
 
     return NAV_FSM_EVENT_NONE;
 }
-/////////////////
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
 
-    if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
+    if (!STATE(FIXED_WING_LEGACY)) {    // Only on FW for now
+        return NAV_FSM_EVENT_ERROR;
+    }
 
     DEBUG_SET(DEBUG_CRUISE, 0, 1);
+    // Switch to IDLE if we do not have an healty position. Try the next iteration.
     if (checkForPositionSensorTimeout()) {
         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
-    }  // Switch to IDLE if we do not have an healty position. Try the next iteration.
+    }
 
     resetPositionController();
 
@@ -1086,9 +1085,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
 
     const timeMs_t currentTimeMs = millis();
 
+    // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
     if (checkForPositionSensorTimeout()) {
         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
-    } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
+    }
 
     DEBUG_SET(DEBUG_CRUISE, 0, 2);
     DEBUG_SET(DEBUG_CRUISE, 2, 0);
@@ -1157,7 +1157,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(naviga
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
 {
-    navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
+    UNUSED(previousState);
 
     if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
         // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
@@ -1173,17 +1173,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
 
     // If we have valid position sensor or configured to ignore it's loss at initial stage - continue
     if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
-        // Reset altitude and position controllers if necessary
+        // Prepare controllers
-        if ((prevFlags & NAV_CTL_POS) == 0) {
         resetPositionController();
-        }
+        resetAltitudeController(false);     // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
-
-        // Reset altitude controller if it was not enabled or if we are in terrain follow mode
-        if ((prevFlags & NAV_CTL_ALT) == 0 || posControl.flags.isTerrainFollowEnabled) {
-            // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
-            resetAltitudeController(false);
         setupAltitudeController();
-        }
 
         // If close to home - reset home position and land
         if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
@@ -1519,21 +1512,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
 {
     UNUSED(previousState);
 
-    if (posControl.waypointCount == 0 || !posControl.waypointListValid) {
+    if (!posControl.waypointCount || !posControl.waypointListValid) {
         return NAV_FSM_EVENT_ERROR;
     }
-    else {
+
     // Prepare controllers
     resetPositionController();
+    resetAltitudeController(false);     // Make sure surface tracking is not enabled - WP uses global altitude, not AGL
 
-        // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
-        resetAltitudeController(false);
-        setupAltitudeController();
-/*
-  Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
-  Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
-*/
     if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
+        /* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
+        Using p3 minimises the risk of saving an invalid counter if a mission is aborted */
         setupJumpCounters();
         posControl.activeWaypointIndex = posControl.startWpIndex;
         wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
@@ -1546,7 +1535,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
     }
 
     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
-    }
 }
 
 static navigationFSMEvent_t nextForNonGeoStates(void)
@@ -1575,8 +1563,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
             posControl.wpAltitudeReached = false;
             return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
 
-                // We use p3 as the volatile jump counter (p2 is the static value)
         case NAV_WP_ACTION_JUMP:
+            // We use p3 as the volatile jump counter (p2 is the static value)
             if (posControl.waypointList[posControl.activeWaypointIndex].p3 != -1) {
                 if (posControl.waypointList[posControl.activeWaypointIndex].p3 == 0) {
                     resetJumpCounter();

src/main/navigation/navigation_pos_estimator.c

@@ -336,6 +336,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
     else {
         posEstimator.baro.alt = 0;
         posEstimator.baro.lastUpdateTime = 0;
+        posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv;
     }
 }
 #endif
@@ -564,14 +565,7 @@ static void estimationPredict(estimationContext_t * ctx)
 
 static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 {   
-    DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
+    bool correctionCalculated = false;
-    DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt);        // Baro altitude
-    DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
-    DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
-    DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
-    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
-    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
-    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
     if (ctx->newFlags & EST_BARO_VALID) {
         timeUs_t currentTimeUs = micros();
@@ -617,9 +611,12 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
         }
 
-        return true;
+        correctionCalculated = true;
+    } else {
+        pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f);
     }
-    else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
+    
+    if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
         // If baro is not available - use GPS Z for correction on a plane
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
@@ -640,10 +637,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
         }
 
-        return true;
+        correctionCalculated = true;
     }
 
-    return false;
+    // DEBUG_ALTITUDE will be always available
+    DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
+    DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt);        // Baro altitude
+    DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
+    DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
+    DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
+    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
+    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
+    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
+
+    return correctionCalculated;
 }
 
 static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
@@ -830,6 +837,7 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
         navEPH = posEstimator.est.eph;
         navEPV = posEstimator.est.epv;
 
+
         DEBUG_SET(DEBUG_POS_EST, 0, (int32_t) posEstimator.est.pos.x*1000.0F);                // Position estimate X
         DEBUG_SET(DEBUG_POS_EST, 1, (int32_t) posEstimator.est.pos.y*1000.0F);                // Position estimate Y
         if (IS_RC_MODE_ACTIVE(BOXSURFACE) && posEstimator.est.aglQual!=SURFACE_QUAL_LOW){

src/main/navigation/navigation_rover_boat.c

@@ -24,8 +24,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SIZE
-
 #include "build/debug.h"
 
 #include "common/utils.h"

src/main/programming/global_variables.c

@@ -24,8 +24,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SIZE
-
 #ifdef USE_PROGRAMMING_FRAMEWORK
 
 #include <stdint.h>

src/main/programming/pid.c

@@ -24,8 +24,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SIZE
-
 #ifdef USE_PROGRAMMING_FRAMEWORK
 
 #include "common/utils.h"

src/main/programming/programming_task.c

@@ -24,8 +24,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SIZE
-
 #include "programming/logic_condition.h"
 #include "programming/pid.h"
 

src/main/rx/crsf.c

@@ -21,7 +21,6 @@
 #include <string.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 #ifdef USE_SERIALRX_CRSF
 
 #include "build/build_config.h"

src/main/rx/fport.c

@@ -21,7 +21,6 @@
 #include <string.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_SERIAL_RX)
 

src/main/rx/fport2.c

@@ -61,6 +61,7 @@
 #define FPORT2_FC_COMMON_ID 0x1B
 #define FPORT2_FC_MSP_ID 0x0D
 #define FPORT2_BAUDRATE 115200
+#define FBUS_BAUDRATE 460800
 #define FPORT2_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
 #define FPORT2_RX_TIMEOUT 120 // µs
 #define FPORT2_CONTROL_FRAME_LENGTH 24
@@ -629,7 +630,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
 }
 
 
-bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, bool isFBUS)
 {
     static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
     rxRuntimeConfig->channelData = sbusChannelData;
@@ -644,11 +645,13 @@ bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
         return false;
     }
 
+    uint32_t baudRate = (isFBUS) ? FBUS_BAUDRATE : FPORT2_BAUDRATE;
+
     fportPort = openSerialPort(portConfig->identifier,
         FUNCTION_RX_SERIAL,
         fportDataReceive,
         NULL,
-        FPORT2_BAUDRATE,
+        baudRate,
         MODE_RXTX,
         FPORT2_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (tristateWithDefaultOnIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
     );

src/main/rx/fport2.h

@@ -22,6 +22,6 @@
 
 #ifdef USE_SERIALRX_FPORT2
 
-bool fport2RxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+bool fport2RxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, bool isFBUS);
 
 #endif

src/main/rx/frsky_crc.c

@@ -19,8 +19,6 @@
 #include <stdint.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
-
 #include "rx/frsky_crc.h"
 
 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)

src/main/rx/mavlink.c

@@ -21,7 +21,6 @@
 #include <string.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 #ifdef USE_SERIALRX_MAVLINK
 
 #include "build/debug.h"

src/main/rx/rx.c

@@ -90,14 +90,10 @@ static uint8_t rxChannelCount;
 
 static timeUs_t rxNextUpdateAtUs = 0;
 static timeUs_t needRxSignalBefore = 0;
-static timeUs_t suspendRxSignalUntil = 0;
+static bool isRxSuspended = false;
-static uint8_t skipRxSamples = 0;
 
 static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 
-#define SKIP_RC_ON_SUSPEND_PERIOD 1500000           // 1.5 second period in usec (call frequency independent)
-#define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements (timing independent)
-
 rxLinkStatistics_t rxLinkStatistics;
 rxRuntimeConfig_t rxRuntimeConfig;
 static uint8_t rcSampleIndex = 0;
@@ -234,7 +230,10 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
 #endif
 #ifdef USE_SERIALRX_FPORT2
     case SERIALRX_FPORT2:
-        enabled = fport2RxInit(rxConfig, rxRuntimeConfig);
+        enabled = fport2RxInit(rxConfig, rxRuntimeConfig, false);
+        break;
+    case SERIALRX_FBUS:
+        enabled = fport2RxInit(rxConfig, rxRuntimeConfig, true);
         break;
 #endif
 #ifdef USE_SERIALRX_GHST
@@ -387,14 +386,12 @@ bool rxAreFlightChannelsValid(void)
 void suspendRxSignal(void)
 {
     failsafeOnRxSuspend();
-    suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
+    isRxSuspended = true;
-    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 }
 
 void resumeRxSignal(void)
 {
-    suspendRxSignalUntil = micros();
+    isRxSuspended = false;
-    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
     failsafeOnRxResume();
 }
 
@@ -463,12 +460,8 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
     rxDataProcessingRequired = false;
     rxNextUpdateAtUs = currentTimeUs + DELAY_10_HZ;
 
-    // only proceed when no more samples to skip and suspend period is over
+    // If RX is suspended, do not process any data
-    if (skipRxSamples) {
+    if (isRxSuspended) {
-        if (currentTimeUs > suspendRxSignalUntil) {
-            skipRxSamples--;
-        }
-
         return true;
     }
 

src/main/rx/rx.h

@@ -80,6 +80,7 @@ typedef enum {
     SERIALRX_SRXL2,
     SERIALRX_GHST,
     SERIALRX_MAVLINK,
+    SERIALRX_FBUS,
 } rxSerialReceiverType_e;
 
 #define MAX_SUPPORTED_RC_CHANNEL_COUNT              18
@@ -136,12 +137,6 @@ PG_DECLARE(rxConfig_t, rxConfig);
 
 #define REMAPPABLE_CHANNEL_COUNT ARRAYLEN(((rxConfig_t *)0)->rcmap)
 
-typedef struct rxRuntimeConfig_s rxRuntimeConfig_t;
-typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
-typedef uint8_t (*rcFrameStatusFnPtr)(rxRuntimeConfig_t *rxRuntimeConfig);
-typedef bool (*rcProcessFrameFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
-typedef uint16_t (*rcGetLinkQualityPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
-
 typedef struct rxLinkQualityTracker_s {
     timeMs_t lastUpdatedMs;
     uint32_t lqAccumulator;
@@ -149,6 +144,15 @@ typedef struct rxLinkQualityTracker_s {
     uint32_t lqValue;
 } rxLinkQualityTracker_e;
 
+
+struct rxRuntimeConfig_s;
+typedef struct rxRuntimeConfig_s rxRuntimeConfig_t;
+
+typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
+typedef uint8_t (*rcFrameStatusFnPtr)(rxRuntimeConfig_t *rxRuntimeConfig);
+typedef bool (*rcProcessFrameFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
+typedef uint16_t (*rcGetLinkQualityPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
+
 typedef struct rxRuntimeConfig_s {
     uint8_t channelCount;                  // number of rc channels as reported by current input driver
     timeUs_t rxSignalTimeout;
@@ -184,6 +188,11 @@ typedef struct rxLinkStatistics_s {
     uint8_t activeAntenna;
 } rxLinkStatistics_t;
 
+typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
+typedef uint8_t (*rcFrameStatusFnPtr)(rxRuntimeConfig_t *rxRuntimeConfig);
+typedef bool (*rcProcessFrameFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
+typedef uint16_t (*rcGetLinkQualityPtr)(const rxRuntimeConfig_t *rxRuntimeConfig);
+
 extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
 extern rxLinkStatistics_t rxLinkStatistics;
 void lqTrackerReset(rxLinkQualityTracker_e * lqTracker);

src/main/rx/sbus.c

@@ -21,7 +21,6 @@
 #include <string.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #ifdef USE_SERIAL_RX
 

src/main/scheduler/scheduler.c

@@ -21,8 +21,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "scheduler.h"
 
 #include "build/build_config.h"

src/main/sensors/acceleration.c

@@ -22,8 +22,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "build/debug.h"
 
 #include "common/axis.h"

src/main/sensors/barometer.c

@@ -342,7 +342,7 @@ int16_t baroGetTemperature(void)
 
 bool baroIsHealthy(void)
 {
-    return true;
+    return sensors(SENSOR_BARO);
 }
 
 #endif /* BARO */

src/main/sensors/boardalignment.c

@@ -22,8 +22,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #include "common/maths.h"
 #include "common/vector.h"
 #include "common/axis.h"

src/main/sensors/gyro.c

@@ -22,8 +22,6 @@
 
 #include "platform.h"
 
-//FILE_COMPILE_FOR_SPEED
-
 #include "build/build_config.h"
 #include "build/debug.h"
 

src/main/sensors/initialisation.c

@@ -39,6 +39,7 @@
 #include "sensors/sensors.h"
 #include "sensors/temperature.h"
 #include "sensors/temperature.h"
+#include "rx/rx.h"
 
 uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
@@ -108,7 +109,9 @@ bool sensorsAutodetect(void)
 #endif
 
     if (eepromUpdatePending) {
+        suspendRxSignal();
         writeEEPROM();
+        resumeRxSignal();
     }
 
     return true;

src/main/sensors/pitotmeter.c

@@ -215,17 +215,14 @@ STATIC_PROTOTHREAD(pitotThread)
 
         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
 #ifdef USE_SIMULATOR
-        float airSpeed;
     	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
-             airSpeed = simulatorData.airSpeed;
+            pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
-#if defined(USE_PITOT_FAKE)
+        }
-        } else if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
+#endif
-        	airSpeed = fakePitotGetAirspeed();
+#if defined(USE_PITOT_FAKE)
-#endif
+        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
-    	} else {
+            pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
-            airSpeed = 0;
     	} 
-        pitotPressureTmp = sq(airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
 #endif
         ptYield();
 
@@ -253,12 +250,11 @@ STATIC_PROTOTHREAD(pitotThread)
 #ifdef USE_SIMULATOR
     	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
             pitot.airSpeed = simulatorData.airSpeed;
-#if defined(USE_PITOT_FAKE)
+        }
-        } else if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
-            pitot.airSpeed = fakePitotGetAirspeed();
 #endif
-        } else {
+#if defined(USE_PITOT_FAKE)
-            pitot.airSpeed = 0;
+        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
+            pitot.airSpeed = fakePitotGetAirspeed();
     	}
 #endif
     }

src/main/target/AOCODARCF4V2/target.c

@@ -29,8 +29,8 @@ timerHardware_t timerHardware[] = {
 
     DEF_TIM(TIM8,   CH1,  PC6,    TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,  0, 1 ),     // S1 
     DEF_TIM(TIM8,   CH2,  PC7,    TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,  0, 1 ),     // S2
-    DEF_TIM(TIM8,   CH3,  PC8,    TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 1 ),     // S3
+    DEF_TIM(TIM8,   CH3,  PC8,    TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,  0, 1 ),     // S3
-    DEF_TIM(TIM8,   CH4,  PC9,    TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 0 ),     // S4
+    DEF_TIM(TIM8,   CH4,  PC9,    TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,  0, 0 ),     // S4
 
     DEF_TIM(TIM2,   CH1,  PA15,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 0 ),     // S5
     DEF_TIM(TIM1,   CH1,  PA8,    TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 1 ),     // S6

src/main/target/AOCODARCF4V2/target.h

@@ -60,9 +60,12 @@
 #define USE_MAG
 #define MAG_I2C_BUS                     DEFAULT_I2C_BUS
 
+#define USE_MAG_AK8975
 #define USE_MAG_HMC5883
 #define USE_MAG_QMC5883
-#define USE_MAG_LIS3MDL
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
 
 // ******* SERIAL ********
 #define USE_VCP

src/main/target/HAKRCF405V2/CMakeLists.txt

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

src/main/target/HAKRCF405V2/config.c

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

src/main/target/HAKRCF405V2/target.c

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

src/main/target/HAKRCF405V2/target.h

@@ -0,0 +1,182 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "HK4V2"
+#define USBD_PRODUCT_STRING  "HAKRCF405V2"
+
+#define USE_TARGET_CONFIG
+
+/*** Indicators ***/
+#define LED0                    PC15
+#define LED1                    PC14
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+/*** SPI/I2C bus ***/
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_NSS_PIN            PA4
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_NSS_PIN            PB12
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_NSS_PIN            PA15
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PB5
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8      
+#define I2C1_SDA                PB9 
+
+
+/*** IMU sensors ***/
+
+// MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
+#define MPU6000_SPI_BUS         BUS_SPI1
+#define MPU6000_CS_PIN          SPI1_NSS_PIN
+
+
+// ICM42605/ICM42688P
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN       CW90_DEG
+#define ICM42605_SPI_BUS         BUS_SPI1
+#define ICM42605_CS_PIN          SPI1_NSS_PIN
+
+//BMI270
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW90_DEG
+#define BMI270_SPI_BUS          BUS_SPI1
+#define BMI270_CS_PIN           SPI1_NSS_PIN
+
+
+/*** OSD ***/
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          SPI2_NSS_PIN
+
+/*** Onboard flash ***/
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           SPI3_NSS_PIN
+#define M25P16_SPI_BUS          BUS_SPI3
+
+// *** PINIO ***
+
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN              PC2
+#define PINIO2_PIN              PC5
+
+/*** Serial ports ***/
+#define USE_VCP
+#define USE_UART_INVERTER
+
+#define USE_UART1
+#define UART1_RX_PIN            PB7
+#define UART1_TX_PIN            PB6
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+#define INVERTER_PIN_UART2_RX   PC0
+
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART5
+#define UART5_RX_PIN            PD2
+#define UART5_TX_PIN            PC12
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       7
+
+/*** BARO & MAG ***/
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_SPL06
+#define USE_BARO_DPS310
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+/*** ADC ***/
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN               PC1
+#define ADC_CHANNEL_2_PIN               PC3
+
+
+#define VBAT_ADC_CHANNEL                ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL       ADC_CHN_2
+
+
+/*** LED STRIP ***/
+#define USE_LED_STRIP
+#define WS2811_PIN                      PB3
+ 
+/*** Default settings ***/
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define DEFAULT_RX_TYPE                 RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER               SERIALRX_SBUS
+#define CURRENT_METER_SCALE             250
+
+
+#define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL )
+
+/*** Timer/PWM output ***/
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define MAX_PWM_OUTPUT_PORTS            8
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
+// ********** Optical Flow and Lidar **************
+#define USE_RANGEFINDER
+#define USE_RANGEFINDER_MSP
+#define USE_OPFLOW
+#define USE_OPFLOW_MSP
+
+/*** Used pins ***/
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         (BIT(2))

src/main/target/HAKRCF722V2/CMakeLists.txt

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

src/main/target/HAKRCF722V2/config.c

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

src/main/target/HAKRCF722V2/target.c

@@ -0,0 +1,49 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <platform.h>
+#include "drivers/io.h"
+#include "drivers/bus.h"
+#include "drivers/timer.h"
+#include "drivers/sensor.h"
+#include "drivers/pwm_mapping.h"
+
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, GYRO_SPI_BUS, GYRO1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2, DEVHW_MPU6000, GYRO_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
+
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, GYRO_SPI_BUS, GYRO1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, GYRO_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
+
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, GYRO_SPI_BUS, GYRO1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, GYRO_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
+    DEF_TIM(TIM1, CH2, PA9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8
+
+    DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), // 2812LED
+
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/HAKRCF722V2/target.h

@@ -0,0 +1,184 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "HK7V2"
+#define USBD_PRODUCT_STRING  "HAKRCF722V2"
+
+#define USE_TARGET_CONFIG
+
+/*** Indicators ***/
+#define LED0                    PC14
+#define LED1                    PC15
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *** SPI & I2C ***
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_NSS1_PIN           PA4
+#define SPI1_NSS2_PIN           PB2
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6 
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_SPI_DEVICE_2 
+#define SPI2_NSS_PIN            PB12
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_NSS_PIN            PA15
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PB5
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+// *** IMU sensors ***
+#define USE_MPU_DATA_READY_SIGNAL
+#define ENSURE_MPU_DATA_READY_IS_LOW
+
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_DUAL_GYRO
+
+#define GYRO_SPI_BUS         BUS_SPI1
+#define GYRO1_CS_PIN          SPI1_NSS1_PIN
+#define GYRO2_CS_PIN          SPI1_NSS2_PIN
+
+// MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
+
+// ICM42605/ICM42688P
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN       CW90_DEG
+
+//BMI270
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW90_DEG
+
+// *** SPI2 OSD ***
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          SPI2_NSS_PIN
+
+// *** SPI3 FLASH ***
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           SPI3_NSS_PIN
+#define M25P16_SPI_BUS          BUS_SPI3
+
+// *** UART ***
+#define USE_VCP
+
+#define USE_UART1
+#define USE_UART2
+#define USE_UART3
+#define USE_UART4
+#define USE_UART5
+#define USE_UART6
+
+#define UART1_TX_PIN            PB6
+#define UART1_RX_PIN            PB7
+
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+
+#define UART3_TX_PIN            NONE
+#define UART3_RX_PIN            PB11
+
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1
+
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            PD2
+
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
+
+#define SERIAL_PORT_COUNT       7
+
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *** I2C/Baro/Mag/Pitot ***
+#define DEFAULT_I2C_BUS         BUS_I2C1
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_SPL06
+#define USE_BARO_DPS310
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+// *** PINIO ***
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN              PC0
+#define PINIO2_PIN              PC2
+
+// *** ADC ***
+#define USE_ADC
+//#define ADC_INSTANCE                ADC3
+//#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC1
+#define ADC_CHANNEL_2_PIN           PC3
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+
+
+// *** LED2812 ***
+#define USE_LED_STRIP
+#define WS2811_PIN                      PB3
+
+// *** OTHERS ***
+#define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL )
+
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         (BIT(2))
+
+#define MAX_PWM_OUTPUT_PORTS    8

src/main/target/SAGEATF4/target.h

@@ -100,6 +100,7 @@
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C3
 #define USE_BARO_BMP280
+#define USE_BARO_DPS310
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C3

src/main/target/SITL/sim/xplane.c

@@ -468,13 +468,11 @@ static int lookup_address (char *name, int port, int type, struct sockaddr *addr
         fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
         return result;
     } else {
-	int j = 0;
 	for(p = servinfo; p != NULL; p = p->ai_next) {
 	    if(p->ai_family == AF_INET6)
 		p6 = p;
 	    else if(p->ai_family == AF_INET)
 		p4 = p;
-	    j++;
 	}
 
 	if (p6 != NULL)

src/main/target/SITL/target.c

@@ -74,7 +74,7 @@ void systemInit(void) {
     clock_gettime(CLOCK_MONOTONIC, &start_time);
     fprintf(stderr, "[SYSTEM] Init...\n");
 
-#if !defined(__FreeBSD__)   // maybe also || !defined(__APPLE__)
+#if !defined(__FreeBSD__)  && !defined(__APPLE__)
     pthread_attr_t thAttr;
     int policy = 0;
 

src/main/target/SKYSTARSF722HD/CMakeLists.txt

@@ -1 +1,2 @@
 target_stm32f722xe(SKYSTARSF722HD)
+target_stm32f722xe(SKYSTARSF722MINIHD)

src/main/target/SKYSTARSF722HD/target.h

@@ -17,9 +17,13 @@
 
 #pragma once
 
+#ifdef SKYSTARSF7MINIHD
+#define TARGET_BOARD_IDENTIFIER "SS7M"
+#define USBD_PRODUCT_STRING  "SkystarsF722MiniHD"
+#else
 #define TARGET_BOARD_IDENTIFIER "SS7D"
-
 #define USBD_PRODUCT_STRING  "SkystarsF722HD"
+#endif
 
 #define LED0                    PC14 // green
 #define LED1                    PC15 // blue
@@ -35,11 +39,19 @@
 #define SPI1_MISO_PIN	        PA6
 #define SPI1_MOSI_PIN	        PA7
 
+#ifdef SKYSTARSF722MINIHD
+#define MPU6000_CS_PIN          PA4
+#define MPU6000_SPI_BUS         BUS_SPI1
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+#else
 #define BMI270_CS_PIN           PA4
 #define BMI270_SPI_BUS          BUS_SPI1
 
 #define USE_IMU_BMI270
 #define IMU_BMI270_ALIGN        CW90_DEG_FLIP
+#endif
 
 // *************** M25P256 flash ********************
 #define USE_FLASHFS

src/main/target/SKYSTARSH743HD/CMakeLists.txt

@@ -0,0 +1 @@
+target_stm32h743xi(SKYSTARSH743HD)

src/main/target/SKYSTARSH743HD/config.c

@@ -0,0 +1,66 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "common/axis.h"
+
+#include "config/config_master.h"
+#include "config/feature.h"
+
+#include "drivers/sensor.h"
+#include "drivers/pwm_esc_detect.h"
+#include "drivers/pwm_output.h"
+#include "drivers/serial.h"
+
+#include "fc/rc_controls.h"
+
+#include "flight/failsafe.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "rx/rx.h"
+
+#include "io/serial.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+#include "telemetry/telemetry.h"
+
+#include "io/piniobox.h"
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
+    pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3;
+
+    /*
+     * UART1 is SerialRX
+     */
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL;
+    
+    /*
+     * Enable MSP at 115200 at UART4
+     */
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
+}

src/main/target/SKYSTARSH743HD/target.c

@@ -0,0 +1,50 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/pinio.h"
+#include "drivers/sensor.h"
+
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_1,  DEVHW_BMI270,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2,  DEVHW_BMI270,  IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU2_ALIGN);
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S1
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),   // S2
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2),   // S3
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3),   // S4
+
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4),   // S5  
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5),   // S6
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6),   // S7
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7),   // S8
+
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S9
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
+
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/SKYSTARSH743HD/target.h

@@ -0,0 +1,201 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "SSH7"
+#define USBD_PRODUCT_STRING     "SKYSTARSH743HD"
+
+#define USE_TARGET_CONFIG
+
+#define LED0                    PE3
+#define LED1                    PE4
+
+#define BEEPER                  PB3
+#define BEEPER_INVERTED
+
+// ************* DEVICES-SPI : IMU/OSD/FLASH***********
+
+// ************************** IMU **************
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_IMU_BMI270
+// *****IMU1 BMI270 ON  SPI4 **************
+#define IMU1_ALIGN              CW0_DEG
+#define IMU1_SPI_BUS            BUS_SPI4
+#define IMU1_CS_PIN             PE11
+// *****IMU2 BMI270 ON  SPI1 **************
+#define IMU2_ALIGN              CW90_DEG
+#define IMU2_SPI_BUS            BUS_SPI1
+#define IMU2_CS_PIN             PC15
+
+// ***************  OSD MAX7456 ON SPI2************
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PB12
+
+// *************** FLASH BLACKBOX ON SPI3*******************
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA15
+#define M25P16_SPI_BUS          BUS_SPI3
+
+#define W25N01G_SPI_BUS         BUS_SPI3
+#define W25N01G_CS_PIN          PA15
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** DEVICES-I2C :Baro/Mag/Temp ************
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C2
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+#define USE_BARO_SPL06
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+// *************** SPI BUS *******************************
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PD7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN            PE12
+#define SPI4_MISO_PIN           PE13
+#define SPI4_MOSI_PIN           PE14
+
+
+// *************** I2C BUS *******************************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL                PB10
+#define I2C2_SDA                PB11
+
+
+// *************** UART ***********************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PD5
+#define UART2_RX_PIN            PD6
+
+#define USE_UART3
+#define UART3_TX_PIN            PD8
+#define UART3_RX_PIN            PD9
+
+#define USE_UART4
+#define UART4_TX_PIN            PD1
+#define UART4_RX_PIN            PD0
+
+#define USE_UART5
+#define UART5_TX_PIN            PB6
+#define UART5_RX_PIN            PB5
+
+#define USE_UART6
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
+
+#define USE_UART7
+#define UART7_TX_PIN            PE8
+#define UART7_RX_PIN            PE7
+
+#define USE_UART8
+#define UART8_TX_PIN            PE1
+#define UART8_RX_PIN            PE0
+
+#define SERIAL_PORT_COUNT       9
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART1
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+
+#define ADC_CHANNEL_1_PIN           PC0  //ADC123 VBAT1
+#define ADC_CHANNEL_2_PIN           PC1  //ADC123 CURR1
+#define ADC_CHANNEL_3_PIN           PC3  //ADC12  RSSI
+#define ADC_CHANNEL_4_PIN           PC4  //ADC12  AirS
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+#define AIRSPEED_ADC_CHANNEL        ADC_CHN_4
+
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PC5  // VTX power switcher
+#define PINIO2_PIN                  PC2  // 2xCamera switcher
+#define PINIO3_PIN                  PD11
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES            (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
+#define CURRENT_METER_SCALE         290
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTE 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        10
+#define USE_DSHOT
+#define USE_ESC_SENSOR

src/main/target/common_post.h

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

src/main/telemetry/crsf.c

@@ -20,7 +20,6 @@
 #include <string.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
 

src/main/telemetry/ibus_shared.c

@@ -33,6 +33,7 @@
 #include "fc/runtime_config.h"
 #include "scheduler/scheduler.h"
 
+#include "io/osd.h"
 #include "io/serial.h"
 
 #include "sensors/barometer.h"
@@ -147,7 +148,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
         if (!temp_valid || (temperature < -400)) temperature = -400; // Minimum reported temperature is -40°C
         return sendIbusMeasurement2(address, (uint16_t)(temperature  + IBUS_TEMPERATURE_OFFSET));
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_RPM) {
-        return sendIbusMeasurement2(address, (uint16_t)getThrottlePercent() );
+        return sendIbusMeasurement2(address, (uint16_t)getThrottlePercent(osdUsingScaledThrottle()) );
     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE) { //VBAT
         if (telemetryConfig()->report_cell_voltage) {
             return sendIbusMeasurement2(address, getBatteryAverageCellVoltage());

src/main/telemetry/mavlink.c

@@ -653,7 +653,7 @@ void mavlinkSendHUDAndHeartbeat(void)
     mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
     mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
 
-    int16_t thr = getThrottlePercent();
+    int16_t thr = getThrottlePercent(osdUsingScaledThrottle());
     mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
         // airspeed Current airspeed in m/s
         mavAirSpeed,

src/main/telemetry/smartport.c

@@ -9,7 +9,6 @@
 #include <math.h>
 
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
 

src/test/unit/platform.h

@@ -84,9 +84,3 @@ extern SysTick_Type *SysTick;
 #define FAST_CODE 
 #define NOINLINE
 #define EXTENDED_FASTRAM
-#define FUNCTION_COMPILE_FOR_SIZE
-#define FUNCTION_COMPILE_NORMAL
-#define FUNCTION_COMPILE_FOR_SPEED
-#define FILE_COMPILE_FOR_SIZE
-#define FILE_COMPILE_NORMAL
-#define FILE_COMPILE_FOR_SPEED