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

.gitattributes

@@ -10,4 +10,4 @@
 Makefile text
 *.bat eol=crlf
 *.txt text
-*.sh text
+*.sh text eol=lf

.gitignore

@@ -8,6 +8,7 @@
 .project
 .settings
 .cproject
+.cache/
 __pycache__
 startup_stm32f10x_md_gcc.s
 .vagrant/

AUTHORS

@@ -35,6 +35,7 @@ Gaël James
 Gregor Ottmann
 Google LLC
 Hyon Lim
+Hubert Jozwiak
 James Harrison
 Jan Staal
 Jeremy Waters

cmake/stm32h7.cmake

@@ -143,9 +143,9 @@ main_sources(STM32H7_SRC
 
     config/config_streamer_stm32h7.c
 
-#    drivers/adc_stm32h7xx.c
+    drivers/adc_stm32h7xx.c
     drivers/bus_i2c_hal.c
-#    drivers/dma_stm32h7xx.c
+    drivers/dma_stm32h7xx.c
     drivers/bus_spi_hal.c
     drivers/memprot.h
     drivers/memprot_hal.c

docs/Board - Matek F411-WSE.md

@@ -6,7 +6,7 @@
 
 * STM32F411 CPU
 * OSD
-* BMP280 barometer
+* BMP280 barometer (DSP310 with new FCs from around June 2021)
 * Integrated PDB for 2 motors
 * 2 UART ports
 * 6 servos

docs/Programming Framework.md

@@ -81,7 +81,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | Operand Type  | Name      | Notes                                 |
 |----           |----       |----                                   |
 | 0             | VALUE     | Value derived from `value` field      |
-| 1             | RC_CHANNEL    | `value` points to RC channel number, indexed from 1   |
+| 1             | GET_RC_CHANNEL    | `value` points to RC channel number, indexed from 1   |
 | 2             | FLIGHT        | `value` points to flight parameter table              |
 | 3             | FLIGHT_MODE   | `value` points to flight modes table                  |
 | 4             | LC            | `value` points to other logic condition ID            |

docs/development/Building in Docker.md

@@ -23,12 +23,14 @@ Where `<TARGET>` must be replaced with the name of the target that you want to b
 ## Windows 10
 
 Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` .
+If you are getting error "standard_init_linux.go:219: exec user process caused: no such file or directory", make sure `\cmake\docker.sh` has lf (not crlf) line endings.
 
 You'll have to manually execute the same steps that the build script does:
 
 1. `docker build -t inav-build .`
    + This step is only needed the first time.
-2. `docker run --rm -it -v <PATH_TO_REPO>:/src inav-build <TARGET>`
+2. `docker run --rm -it -u root -v <PATH_TO_REPO>:/src inav-build <TARGET>`
    + Where `<PATH_TO_REPO>` must be replaced with the absolute path of where you cloned this repo (see above), and `<TARGET>` with the name of the target that you want to build.
+   + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above, or by removing "USER inav" line from the .\DockerFile before building image.   
 
 Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details.

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

@@ -0,0 +1,80 @@
+# General Info
+
+This is a guide on how to use Windows MSYS2 distribution and building platform to build iNav firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com
+
+Some of those benefits are described here:
+
+https://xpack.github.io/arm-none-eabi-gcc/
+
+## Setting up build environment
+
+Download MSYS2 for your architecture (most likely 64-bit)
+
+https://www.msys2.org/wiki/MSYS2-installation/
+
+Click on 64-bit, scroll all the way down for the latest release
+
+pacman is the package manager which makes it a lot easier to install and maintain all the dependencies
+
+## Installing dependencies
+
+Once MSYS2 is installed, you can open up a new terminal window by running:
+
+"C:\msys64\mingw64.exe"
+
+You can also make a shortcut of this somewhere on your taskbar, desktop, etc, or even setup a shortcut key to open it up every time you need to get a terminal window. If you right click on the window you can customize the font and layout to make it more comfortable to work with. This is very similar to cygwin or any other terminal program you might have used before
+
+This is the best part:
+```
+pacman -S git ruby make cmake gcc mingw-w64-x86_64-libwinpthread-git unzip wget
+```
+
+Now, each release needs a different version of arm toolchain. To setup the xPack ARM toolchain, use the following process:
+
+First, setup your work path, get the release you want to build or master if you want the latest/greatest
+```
+mkdir /c/Workspace
+cd /c/Workspace
+# you can also check out your own fork here which makes contributing easier
+git clone https://github.com/iNavFlight/inav
+cd inav
+# switch to release you want or skip next 2 lines if you want latest
+git fetch origin
+git checkout -b release_2.6.1 origin/release_2.6.1
+```
+Now create the build and xpack directories and get the toolkit version you need for your inav version
+```
+mkdir build
+cd build
+mkdir /c/Workspace/xpack
+cd /c/Workspace/xpack
+cat /c/Workspace/inav/cmake/arm-none-eabi-checks.cmake | grep "set(arm_none_eabi_gcc_version" | cut -d\" -f2
+```
+This will give you the version you need for any given release or master branch. You can get to all the releases here and find the version you need
+
+https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/
+```
+# for version 2.6.1, version needed is 9.2.1
+wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v9.2.1-1.1/xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
+unzip xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
+```
+This is important, put the toolkit first before your path so that it is  picked up ahead of any other versions that may be present on your system
+```
+export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-9.2.1-1.1/bin:$PATH
+cd /c/Workspace/inav/build
+```
+You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake
+```
+# while inside the build directory
+cmake ..
+```
+Once that's done you can compile the firmware for your controller
+```
+make DALRCF405
+```
+To get a list of available targets in iNav, see the target src folder
+https://github.com/tednv/inav/tree/master/src/main/target
+
+The generated hex file will be in /c/Workspace/inav/build folder
+
+At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building iNav 2.1 and 2.2, and now I'm getting back into it so figured I would share my method

src/main/CMakeLists.txt

@@ -280,6 +280,7 @@ main_sources(COMMON_SRC
     fc/config.h
     fc/controlrate_profile.c
     fc/controlrate_profile.h
+    fc/controlrate_profile_config_struct.h
     fc/fc_core.c
     fc/fc_core.h
     fc/fc_init.c
@@ -440,6 +441,7 @@ main_sources(COMMON_SRC
     sensors/acceleration.h
     sensors/battery.c
     sensors/battery.h
+    sensors/battery_config_structs.h
     sensors/boardalignment.c
     sensors/boardalignment.h
     sensors/compass.c

src/main/build/build_config.h

@@ -51,5 +51,11 @@
 #define EXTENDED_FASTRAM
 #endif
 
+#ifdef STM32H7
+#define DMA_RAM __attribute__ ((section(".DMA_RAM")))
+#else
+#define DMA_RAM
+#endif
+
 #define STATIC_FASTRAM              static FASTRAM
 #define STATIC_FASTRAM_UNIT_TESTED  STATIC_UNIT_TESTED FASTRAM

src/main/cms/cms_menu_navigation.c

@@ -182,6 +182,30 @@ static const CMS_Menu cmsx_menuFWSettings = {
     .entries = cmsx_menuFWSettingsEntries
 };
 
+static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
+{
+    OSD_LABEL_ENTRY("-- MISSIONS --"),
+
+    OSD_SETTING_ENTRY("MC WP SLOWDOWN", SETTING_NAV_MC_WP_SLOWDOWN),
+    OSD_SETTING_ENTRY("MISSION FAILSAFE", SETTING_FAILSAFE_MISSION),
+    OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
+    OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
+    OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE),
+
+    OSD_BACK_AND_END_ENTRY,
+ };
+
+static const CMS_Menu cmsx_menuMissionSettings = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "MENUMISSIONSETTINGS",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = NULL,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = cmsx_menuMissionSettingsEntries
+};
+
 static const OSD_Entry cmsx_menuNavigationEntries[] =
 {
     OSD_LABEL_ENTRY("-- NAVIGATION --"),
@@ -189,6 +213,7 @@ static const OSD_Entry cmsx_menuNavigationEntries[] =
     OSD_SUBMENU_ENTRY("BASIC SETTINGS", &cmsx_menuNavSettings),
     OSD_SUBMENU_ENTRY("RTH", &cmsx_menuRTH),
     OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFWSettings),
+    OSD_SUBMENU_ENTRY("MISSIONS", &cmsx_menuMissionSettings),
 
     OSD_BACK_AND_END_ENTRY,
 };

src/main/common/maths.c

@@ -25,6 +25,10 @@
 #include "quaternion.h"
 #include "platform.h"
 
+#ifdef USE_ARM_MATH
+#include "arm_math.h"
+#endif 
+
 FILE_COMPILE_FOR_SPEED
 
 // http://lolengine.net/blog/2011/12/21/better-function-approximations
@@ -95,7 +99,7 @@ float atan2_approx(float y, float x)
 float acos_approx(float x)
 {
     float xa = fabsf(x);
-    float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
+    float result = fast_fsqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
     if (x < 0.0f)
         return M_PIf - result;
     else
@@ -200,7 +204,7 @@ float devVariance(stdev_t *dev)
 
 float devStandardDeviation(stdev_t *dev)
 {
-    return sqrtf(devVariance(dev));
+    return fast_fsqrtf(devVariance(dev));
 }
 
 float degreesToRadians(int16_t degrees)
@@ -508,7 +512,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
     sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
 
     for (int i = 0; i < 3; i++) {
-        result[i] = sqrtf(beta[i]);
+        result[i] = fast_fsqrtf(beta[i]);
     }
 
     return sensorCalibrationValidateResult(result);
@@ -518,3 +522,13 @@ float bellCurve(const float x, const float curveWidth)
 {
     return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
 }
+
+float fast_fsqrtf(const double value) {
+#ifdef USE_ARM_MATH
+    float squirt;
+    arm_sqrt_f32(value, &squirt);
+    return squirt;
+#else 
+    return sqrtf(value);
+#endif
+}

src/main/common/maths.h

@@ -176,3 +176,4 @@ float acos_approx(float x);
 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 
 float bellCurve(const float x, const float curveWidth);
+float fast_fsqrtf(const double value);

src/main/common/quaternion.h

@@ -142,7 +142,7 @@ static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, cons
 
 static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q)
 {
-    float mod = sqrtf(quaternionNormSqared(q));
+    float mod = fast_fsqrtf(quaternionNormSqared(q));
     if (mod < 1e-6f) {
         // Length is too small - re-initialize to zero rotation
         result->q0 = 1;

src/main/common/vector.h

@@ -67,7 +67,7 @@ static inline float vectorNormSquared(const fpVector3_t * v)
 
 static inline fpVector3_t * vectorNormalize(fpVector3_t * result, const fpVector3_t * v)
 {
-    float length = sqrtf(vectorNormSquared(v));
+    float length = fast_fsqrtf(vectorNormSquared(v));
     if (length != 0) {
         result->x = v->x / length;
         result->y = v->y / length;

src/main/drivers/accgyro/accgyro.c

@@ -98,7 +98,7 @@ void gyroIntExtiInit(gyroDev_t *gyro)
     }
 #endif
 
-#if defined (STM32F7)
+#if defined (STM32F7) || defined (STM32H7)
     IOInit(gyro->busDev->irqPin, OWNER_MPU, RESOURCE_EXTI, 0);
 
     EXTIHandlerInit(&gyro->exti, gyroIntExtiHandler);

src/main/drivers/adc.c

@@ -55,9 +55,9 @@ static uint8_t activeChannelCount[ADCDEV_COUNT] = {0};
 
 static int adcFunctionMap[ADC_FUNCTION_COUNT];
 adc_config_t adcConfig[ADC_CHN_COUNT];  // index 0 is dummy for ADC_CHN_NONE
-volatile uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES];
+volatile ADC_VALUES_ALIGNMENT(uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]);
 
-uint8_t adcChannelByTag(ioTag_t ioTag)
+uint32_t adcChannelByTag(ioTag_t ioTag)
 {
     for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) {
         if (ioTag == adcTagMap[i].tag)

src/main/drivers/adc_impl.h

@@ -22,19 +22,27 @@
 
 #if defined(STM32F4) || defined(STM32F7)
 #define ADC_TAG_MAP_COUNT 16
+#elif defined(STM32H7)
+#define ADC_TAG_MAP_COUNT 28
 #elif defined(STM32F3)
 #define ADC_TAG_MAP_COUNT 39
 #else
 #define ADC_TAG_MAP_COUNT 10
 #endif
 
+#if defined(STM32H7)
+#define ADC_VALUES_ALIGNMENT(def) DMA_RAM def __attribute__ ((aligned (32)))
+#else
+#define ADC_VALUES_ALIGNMENT(def) def 
+#endif
+
 typedef enum ADCDevice {
     ADCINVALID = -1,
     ADCDEV_1   = 0,
 #if defined(STM32F3)
     ADCDEV_2,
     ADCDEV_MAX = ADCDEV_2,
-#elif defined(STM32F4) || defined(STM32F7)
+#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
     ADCDEV_2,
     ADCDEV_3,
     ADCDEV_MAX = ADCDEV_3,
@@ -46,20 +54,20 @@ typedef enum ADCDevice {
 
 typedef struct adcTagMap_s {
     ioTag_t tag;
-    uint8_t channel;
+    uint32_t channel;
 } adcTagMap_t;
 
 typedef struct adcDevice_s {
     ADC_TypeDef* ADCx;
     rccPeriphTag_t rccADC;
     rccPeriphTag_t rccDMA;
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
     DMA_Stream_TypeDef* DMAy_Streamx;
     uint32_t channel;
 #else
     DMA_Channel_TypeDef* DMAy_Channelx;
 #endif
-#if defined(STM32F7)
+#if defined(STM32F7) || defined(STM32H7)
     ADC_HandleTypeDef ADCHandle;
     DMA_HandleTypeDef DmaHandle;
 #endif
@@ -70,7 +78,7 @@ typedef struct adcDevice_s {
 typedef struct adc_config_s {
     ioTag_t tag;
     ADCDevice adcDevice;
-    uint8_t adcChannel;         // ADC1_INxx channel number
+    uint32_t adcChannel;         // ADC1_INxx channel number
     uint8_t dmaIndex;           // index into DMA buffer in case of sparse channels
     bool enabled;
     uint8_t sampleTime;
@@ -78,8 +86,8 @@ typedef struct adc_config_s {
 
 extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT];
 extern adc_config_t adcConfig[ADC_CHN_COUNT];
-extern volatile uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES];
+extern volatile ADC_VALUES_ALIGNMENT(uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]);
 
 void adcHardwareInit(drv_adc_config_t *init);
 ADCDevice adcDeviceByInstance(ADC_TypeDef *instance);
-uint8_t adcChannelByTag(ioTag_t ioTag);
+uint32_t adcChannelByTag(ioTag_t ioTag);

src/main/drivers/adc_stm32h7xx.c

@@ -0,0 +1,230 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "platform.h"
+#include "drivers/time.h"
+
+#include "drivers/io.h"
+#include "io_impl.h"
+#include "rcc.h"
+#include "dma.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro.h"
+
+#include "adc.h"
+#include "adc_impl.h"
+
+
+static adcDevice_t adcHardware[ADCDEV_COUNT] = {
+    {
+        .ADCx = ADC1,
+        .rccADC = RCC_AHB1(ADC12),
+        .rccDMA = RCC_AHB1(DMA2), 
+        .DMAy_Streamx = DMA2_Stream0,
+        .channel = DMA_REQUEST_ADC1,
+        .enabled = false, 
+        .usedChannelCount = 0
+    },
+    /* currently not used
+    { 
+        .ADCx = ADC2,
+        .rccADC = RCC_AHB1(ADC12),
+        .rccDMA = RCC_AHB1(DMA2), 
+        .DMAy_Streamx = DMA2_Stream1,
+        .channel = DMA_REQUEST_ADC2,
+        .enabled = false, 
+        .usedChannelCount = 0
+    }
+    */
+};
+
+adcDevice_t adcDevice[ADCDEV_COUNT];
+
+/* note these could be packed up for saving space */
+const adcTagMap_t adcTagMap[] = {
+    { DEFIO_TAG_E__PC0, ADC_CHANNEL_10 },
+    { DEFIO_TAG_E__PC1, ADC_CHANNEL_11 },
+    { DEFIO_TAG_E__PC2, ADC_CHANNEL_0  },
+    { DEFIO_TAG_E__PC3, ADC_CHANNEL_1  },
+    { DEFIO_TAG_E__PC4, ADC_CHANNEL_4  },
+    { DEFIO_TAG_E__PC5, ADC_CHANNEL_8  },
+    { DEFIO_TAG_E__PB0, ADC_CHANNEL_9  },
+    { DEFIO_TAG_E__PB1, ADC_CHANNEL_5  },
+    { DEFIO_TAG_E__PA0, ADC_CHANNEL_16 },
+    { DEFIO_TAG_E__PA1, ADC_CHANNEL_17 },
+    { DEFIO_TAG_E__PA2, ADC_CHANNEL_14 },
+    { DEFIO_TAG_E__PA3, ADC_CHANNEL_15 },
+    { DEFIO_TAG_E__PA4, ADC_CHANNEL_18 },
+    { DEFIO_TAG_E__PA5, ADC_CHANNEL_19 },
+    { DEFIO_TAG_E__PA6, ADC_CHANNEL_3  },
+    { DEFIO_TAG_E__PA7, ADC_CHANNEL_7  },
+};
+
+// Translate rank number x to ADC_REGULAR_RANK_x (Note that array index is 0-origin)
+static const uint32_t adcRegularRankMap[] = {
+    ADC_REGULAR_RANK_1,
+    ADC_REGULAR_RANK_2,
+    ADC_REGULAR_RANK_3,
+    ADC_REGULAR_RANK_4,
+    ADC_REGULAR_RANK_5,
+    ADC_REGULAR_RANK_6,
+    ADC_REGULAR_RANK_7,
+    ADC_REGULAR_RANK_8,
+    ADC_REGULAR_RANK_9,
+    ADC_REGULAR_RANK_10,
+    ADC_REGULAR_RANK_11,
+    ADC_REGULAR_RANK_12,
+    ADC_REGULAR_RANK_13,
+    ADC_REGULAR_RANK_14,
+    ADC_REGULAR_RANK_15,
+    ADC_REGULAR_RANK_16,
+};
+
+ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
+{
+    if (instance == ADC1)
+        return ADCDEV_1;
+
+    if (instance == ADC2)
+        return ADCDEV_2;
+
+    return ADCINVALID;
+}
+
+static void adcInstanceInit(ADCDevice adcDevice)
+{
+    adcDevice_t * adc = &adcHardware[adcDevice];
+
+    RCC_ClockCmd(adc->rccDMA, ENABLE);
+    RCC_ClockCmd(adc->rccADC, ENABLE);
+
+    adc->ADCHandle.Instance = adc->ADCx;
+
+    adc->ADCHandle.Init.ClockPrescaler        = ADC_CLOCK_SYNC_PCLK_DIV4;
+    adc->ADCHandle.Init.Resolution            = ADC_RESOLUTION_12B;
+    adc->ADCHandle.Init.ContinuousConvMode    = ENABLE;
+    adc->ADCHandle.Init.ExternalTrigConv      = ADC_EXTERNALTRIG_T1_CC1;
+    adc->ADCHandle.Init.ExternalTrigConvEdge  = ADC_EXTERNALTRIGCONVEDGE_NONE;
+    adc->ADCHandle.Init.NbrOfConversion       = adc->usedChannelCount;
+    adc->ADCHandle.Init.ScanConvMode          = adc->usedChannelCount > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
+    adc->ADCHandle.Init.DiscontinuousConvMode = DISABLE;
+    adc->ADCHandle.Init.NbrOfDiscConversion   = 0;
+    adc->ADCHandle.Init.EOCSelection          = DISABLE;
+    adc->ADCHandle.Init.LowPowerAutoWait      = DISABLE;
+
+    // Enable circular DMA.
+    adc->ADCHandle.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_CIRCULAR;
+
+    adc->ADCHandle.Init.Overrun                  = ADC_OVR_DATA_OVERWRITTEN;
+    adc->ADCHandle.Init.OversamplingMode         = DISABLE;
+
+    if (HAL_ADC_Init(&adc->ADCHandle) != HAL_OK) {
+        return;
+    }
+
+    if (HAL_ADCEx_Calibration_Start(&adc->ADCHandle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) != HAL_OK) {
+      return;
+    }
+
+    adc->DmaHandle.Init.Request = adc->channel;
+    adc->DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    adc->DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+    adc->DmaHandle.Init.MemInc = ((adc->usedChannelCount > 1) || (ADC_AVERAGE_N_SAMPLES > 1)) ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;
+    adc->DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+    adc->DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+    adc->DmaHandle.Init.Mode = DMA_CIRCULAR;
+    adc->DmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
+    adc->DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    adc->DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+    adc->DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+    adc->DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+    adc->DmaHandle.Instance = adc->DMAy_Streamx;
+
+    if (HAL_DMA_Init(&adc->DmaHandle) != HAL_OK) {
+        return;
+    }
+
+    __HAL_LINKDMA(&adc->ADCHandle, DMA_Handle, adc->DmaHandle);
+
+    uint8_t rank = 0;
+    for (int i = ADC_CHN_1; i < ADC_CHN_COUNT; i++) {
+        if (!adcConfig[i].enabled || adcConfig[i].adcDevice != adcDevice) {
+            continue;
+        }
+
+        ADC_ChannelConfTypeDef sConfig;
+        sConfig.Channel      = adcConfig[i].adcChannel;
+        sConfig.Rank         = adcRegularRankMap[rank++];
+        sConfig.SamplingTime = adcConfig[i].sampleTime;
+        sConfig.SingleDiff   = ADC_SINGLE_ENDED;
+        sConfig.OffsetNumber = ADC_OFFSET_NONE;
+        sConfig.Offset       = 0;
+
+        if (HAL_ADC_ConfigChannel(&adc->ADCHandle, &sConfig) != HAL_OK) {
+            return;
+        }
+    }
+
+    if (HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t*)&adcValues[adcDevice], adc->usedChannelCount * ADC_AVERAGE_N_SAMPLES) != HAL_OK) {
+        return;
+    }
+}
+
+void adcHardwareInit(drv_adc_config_t *init)
+{
+    UNUSED(init);
+    int configuredAdcChannels = 0;
+
+    for (int i = ADC_CHN_1; i < ADC_CHN_COUNT; i++) {
+        if (!adcConfig[i].tag)
+            continue;
+
+        adcDevice_t * adc = &adcHardware[adcConfig[i].adcDevice];
+
+        IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_CH1 + (i - ADC_CHN_1), 0);
+        IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_NOPULL));
+
+        adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
+        adcConfig[i].dmaIndex = adc->usedChannelCount++;
+        adcConfig[i].sampleTime = ADC_SAMPLETIME_387CYCLES_5;
+        adcConfig[i].enabled = true;
+
+        adc->enabled = true;
+        configuredAdcChannels++;
+    }
+
+    if (configuredAdcChannels == 0)
+        return;
+
+    for (int i = 0; i < ADCDEV_COUNT; i++) {
+        if (adcHardware[i].enabled) {
+            adcInstanceInit(i);
+        }
+    }
+}

src/main/drivers/dma_stm32h7xx.c

@@ -0,0 +1,128 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform.h>
+
+#include "common/utils.h"
+#include "drivers/nvic.h"
+#include "drivers/dma.h"
+#include "drivers/rcc.h"
+
+/*
+ * DMA descriptors.
+ */
+static dmaChannelDescriptor_t dmaDescriptors[] = {
+    [0]  = DEFINE_DMA_CHANNEL(1, 0,  0), // DMA1_ST0
+    [1]  = DEFINE_DMA_CHANNEL(1, 1,  6), // DMA1_ST1
+    [2]  = DEFINE_DMA_CHANNEL(1, 2, 16), // DMA1_ST2
+    [3]  = DEFINE_DMA_CHANNEL(1, 3, 22), // DMA1_ST3
+    [4]  = DEFINE_DMA_CHANNEL(1, 4, 32), // DMA1_ST4
+    [5]  = DEFINE_DMA_CHANNEL(1, 5, 38), // DMA1_ST5
+    [6]  = DEFINE_DMA_CHANNEL(1, 6, 48), // DMA1_ST6
+    [7]  = DEFINE_DMA_CHANNEL(1, 7, 54), // DMA1_ST7
+
+    [8]  = DEFINE_DMA_CHANNEL(2, 0,  0), // DMA2_ST0
+    [9]  = DEFINE_DMA_CHANNEL(2, 1,  6), // DMA2_ST1
+    [10] = DEFINE_DMA_CHANNEL(2, 2, 16), // DMA2_ST2
+    [11] = DEFINE_DMA_CHANNEL(2, 3, 22), // DMA2_ST3
+    [12] = DEFINE_DMA_CHANNEL(2, 4, 32), // DMA2_ST4
+    [13] = DEFINE_DMA_CHANNEL(2, 5, 38), // DMA2_ST5
+    [14] = DEFINE_DMA_CHANNEL(2, 6, 48), // DMA2_ST6
+    [15] = DEFINE_DMA_CHANNEL(2, 7, 54), // DMA2_ST7
+};
+
+/*
+ * DMA IRQ Handlers
+ */
+DEFINE_DMA_IRQ_HANDLER(1, 0, 0) // DMA1_ST0
+DEFINE_DMA_IRQ_HANDLER(1, 1, 1) // DMA1_ST1
+DEFINE_DMA_IRQ_HANDLER(1, 2, 2) // DMA1_ST2
+DEFINE_DMA_IRQ_HANDLER(1, 3, 3) // DMA1_ST3
+DEFINE_DMA_IRQ_HANDLER(1, 4, 4) // DMA1_ST4
+DEFINE_DMA_IRQ_HANDLER(1, 5, 5) // DMA1_ST5
+DEFINE_DMA_IRQ_HANDLER(1, 6, 6) // DMA1_ST6
+DEFINE_DMA_IRQ_HANDLER(1, 7, 7) // DMA1_ST7
+DEFINE_DMA_IRQ_HANDLER(2, 0, 8) // DMA2_ST0
+DEFINE_DMA_IRQ_HANDLER(2, 1, 9) // DMA2_ST1
+DEFINE_DMA_IRQ_HANDLER(2, 2, 10) // DMA2_ST2
+DEFINE_DMA_IRQ_HANDLER(2, 3, 11) // DMA2_ST3
+DEFINE_DMA_IRQ_HANDLER(2, 4, 12) // DMA2_ST4
+DEFINE_DMA_IRQ_HANDLER(2, 5, 13) // DMA2_ST5
+DEFINE_DMA_IRQ_HANDLER(2, 6, 14) // DMA2_ST6
+DEFINE_DMA_IRQ_HANDLER(2, 7, 15) // DMA2_ST7
+
+DMA_t dmaGetByTag(dmaTag_t tag)
+{
+    for (unsigned i = 0; i < ARRAYLEN(dmaDescriptors); i++) {
+        if (DMATAG_GET_DMA(dmaDescriptors[i].tag) == DMATAG_GET_DMA(tag) && DMATAG_GET_STREAM(dmaDescriptors[i].tag) == DMATAG_GET_STREAM(tag)) {
+            return (DMA_t)&dmaDescriptors[i];
+        }
+    }
+
+    return (DMA_t) NULL;
+}
+
+void dmaEnableClock(DMA_t dma)
+{
+    if (dma->dma == DMA1) {
+        RCC_ClockCmd(RCC_AHB1(DMA1), ENABLE);
+    }
+    else {
+        RCC_ClockCmd(RCC_AHB1(DMA2), ENABLE);
+    }
+}
+
+resourceOwner_e dmaGetOwner(DMA_t dma)
+{
+    return dma->owner;
+}
+
+void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
+{
+    dmaEnableClock(dma);
+    dma->owner = owner;
+    dma->resourceIndex = resourceIndex;
+}
+
+void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
+{
+    dmaEnableClock(dma);
+
+    dma->irqHandlerCallback = callback;
+    dma->userParam = userParam;
+
+    HAL_NVIC_SetPriority(dma->irqNumber, priority, 0);
+    HAL_NVIC_EnableIRQ(dma->irqNumber);
+}
+
+DMA_t dmaGetByRef(const DMA_Stream_TypeDef* ref)
+{
+    for (unsigned i = 0; i < (sizeof(dmaDescriptors) / sizeof(dmaDescriptors[0])); i++) {
+        if (ref == dmaDescriptors[i].ref) {
+            return &dmaDescriptors[i];
+        }
+    }
+
+    return NULL;
+}

src/main/drivers/exti.c

@@ -24,7 +24,7 @@ extiChannelRec_t extiChannelRecs[16];
 static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 };
 static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS];
 
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
 static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
     EXTI0_IRQn,
     EXTI1_IRQn,
@@ -48,6 +48,15 @@ static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
 # warning "Unknown CPU"
 #endif
 
+// Absorb the difference in IMR and PR assignments to registers
+#if defined(STM32H7)
+#define EXTI_REG_IMR (EXTI_D1->IMR1)
+#define EXTI_REG_PR  (EXTI_D1->PR1)
+#else
+#define EXTI_REG_IMR (EXTI->IMR)
+#define EXTI_REG_PR  (EXTI->PR)
+#endif
+
 
 void EXTIInit(void)
 {
@@ -64,7 +73,7 @@ void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn)
     self->fn = fn;
 }
 
-#if defined(STM32F7)
+#if defined(STM32F7) || defined(STM32H7)
 void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config)
 {
     (void)config;
@@ -156,23 +165,23 @@ void EXTIRelease(IO_t io)
 
 void EXTIEnable(IO_t io, bool enable)
 {
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
     uint32_t extiLine = IO_EXTI_Line(io);
     if (!extiLine)
         return;
     if (enable)
-        EXTI->IMR |= extiLine;
+        EXTI_REG_IMR |= extiLine;
     else
-        EXTI->IMR &= ~extiLine;
+        EXTI_REG_IMR &= ~extiLine;
 #elif defined(STM32F303xC)
     int extiLine = IO_EXTI_Line(io);
     if (extiLine < 0)
         return;
     // assume extiLine < 32 (valid for all EXTI pins)
     if (enable)
-        EXTI->IMR |= 1 << extiLine;
+        EXTI_REG_IMR |= 1 << extiLine;
     else
-        EXTI->IMR &= ~(1 << extiLine);
+        EXTI_REG_IMR &= ~(1 << extiLine);
 #else
 # error "Unsupported target"
 #endif
@@ -180,13 +189,13 @@ void EXTIEnable(IO_t io, bool enable)
 
 void EXTI_IRQHandler(void)
 {
-    uint32_t exti_active = EXTI->IMR & EXTI->PR;
+    uint32_t exti_active = EXTI_REG_IMR & EXTI_REG_PR;
 
     while (exti_active) {
         unsigned idx = 31 - __builtin_clz(exti_active);
         uint32_t mask = 1 << idx;
         extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler);
-        EXTI->PR = mask;  // clear pending mask (by writing 1)
+        EXTI_REG_PR = mask;  // clear pending mask (by writing 1)
         exti_active &= ~mask;
     }
 }
@@ -201,7 +210,7 @@ void EXTI_IRQHandler(void)
 
 _EXTI_IRQ_HANDLER(EXTI0_IRQHandler);
 _EXTI_IRQ_HANDLER(EXTI1_IRQHandler);
-#if defined(STM32F7)
+#if defined(STM32F7) || defined(STM32H7)
 _EXTI_IRQ_HANDLER(EXTI2_IRQHandler);
 #elif defined(STM32F3) || defined(STM32F4)
 _EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler);

src/main/drivers/osd_symbols.h

@@ -109,7 +109,7 @@
 
 #define SYM_AH_CH_CENTER          0x7E // 126 Crossair center
 
-//                                0x7F // 127 -
+#define SYM_GLIDESLOPE            0x7F // 127 Glideslope
 
 #define SYM_AH_H_START            0x80 // 128 to 136 Horizontal AHI
 

src/main/drivers/pwm_output.c

@@ -91,7 +91,7 @@ typedef struct {
     bool                requestTelemetry;
 } pwmOutputMotor_t;
 
-static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
+static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
 
 static pwmOutputMotor_t        motors[MAX_MOTORS];
 static motorPwmProtocolTypes_e initMotorProtocol;

src/main/drivers/serial_uart_stm32h7xx.c

@@ -331,6 +331,10 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
 
     s->Handle.Instance = uart->dev;
 
+    if (uart->rcc) {
+        RCC_ClockCmd(uart->rcc, ENABLE);
+    }
+
     IO_t tx = IOGetByTag(uart->tx);
     IO_t rx = IOGetByTag(uart->rx);
 

src/main/fc/cli.c

@@ -1460,7 +1460,7 @@ static void cliWaypoints(char *cmdline)
 
             if (!(validArgumentCount == 6 || validArgumentCount == 8)) {
                 cliShowParseError();
-            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
+            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST || flag == NAV_WP_FLAG_HOME)) {
                 cliShowParseError();
             } else {
                 posControl.waypointList[i].action = action;

src/main/fc/controlrate_profile.h

@@ -21,35 +21,10 @@
 
 #include "config/parameter_group.h"
 
-#define MAX_CONTROL_RATE_PROFILE_COUNT 3
+#include "fc/controlrate_profile_config_struct.h"
+#include "fc/settings.h"
 
-typedef struct controlRateConfig_s {
-
-    struct {
-        uint8_t rcMid8;
-        uint8_t rcExpo8;
-        uint8_t dynPID;
-        uint16_t pa_breakpoint;                // Breakpoint where TPA is activated
-        uint16_t fixedWingTauMs;               // Time constant of airplane TPA PT1-filter
-    } throttle;
-
-    struct {
-        uint8_t rcExpo8;
-        uint8_t rcYawExpo8;
-        uint8_t rates[3];
-    } stabilized;
-
-    struct {
-        uint8_t rcExpo8;
-        uint8_t rcYawExpo8;
-        uint8_t rates[3];
-    } manual;
-
-    struct {
-        uint8_t fpvCamAngleDegrees;             // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis)
-    } misc;
-
-} controlRateConfig_t;
+#define MAX_CONTROL_RATE_PROFILE_COUNT SETTING_CONSTANT_MAX_CONTROL_RATE_PROFILE_COUNT
 
 PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);
 

src/main/fc/controlrate_profile_config_struct.h

@@ -0,0 +1,52 @@
+/*
+ * This file is part of iNav
+ *
+ * iNav free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav distributed in the hope that it
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct controlRateConfig_s {
+
+    struct {
+        uint8_t rcMid8;
+        uint8_t rcExpo8;
+        uint8_t dynPID;
+        uint16_t pa_breakpoint;                // Breakpoint where TPA is activated
+        uint16_t fixedWingTauMs;               // Time constant of airplane TPA PT1-filter
+    } throttle;
+
+    struct {
+        uint8_t rcExpo8;
+        uint8_t rcYawExpo8;
+        uint8_t rates[3];
+    } stabilized;
+
+    struct {
+        uint8_t rcExpo8;
+        uint8_t rcYawExpo8;
+        uint8_t rates[3];
+    } manual;
+
+    struct {
+        uint8_t fpvCamAngleDegrees;             // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis)
+    } misc;
+
+} controlRateConfig_t;

src/main/fc/fc_core.c

@@ -45,6 +45,7 @@ FILE_COMPILE_FOR_SPEED
 #include "sensors/boardalignment.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/compass.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
@@ -135,7 +136,7 @@ static emergencyArmingState_t emergencyArming;
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;
 
-bool isCalibrating(void)
+bool areSensorsCalibrating(void)
 {
 #ifdef USE_BARO
     if (sensors(SENSOR_BARO) && !baroIsCalibrationComplete()) {
@@ -143,6 +144,12 @@ bool isCalibrating(void)
     }
 #endif
 
+#ifdef USE_MAG
+    if (sensors(SENSOR_MAG) && !compassIsCalibrationComplete()) {
+        return true;
+    }
+#endif
+
 #ifdef USE_PITOT
     if (sensors(SENSOR_PITOT) && !pitotIsCalibrationComplete()) {
         return true;
@@ -183,7 +190,7 @@ static void updateArmingStatus(void)
     } else {
         /* CHECK: Run-time calibration */
         static bool calibratingFinishedBeep = false;
-        if (isCalibrating()) {
+        if (areSensorsCalibrating()) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_SENSORS_CALIBRATING);
             calibratingFinishedBeep = false;
         }
@@ -216,7 +223,7 @@ static void updateArmingStatus(void)
 
 	/* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */
 	if (isNavLaunchEnabled()) {
-	  if (areSticksDeflectedMoreThanPosHoldDeadband()) {
+	  if (areSticksDeflected()) {
 	    ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
 	  } else {
 	    DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
@@ -711,7 +718,7 @@ void processRx(timeUs_t currentTimeUs)
     // Handle passthrough mode
     if (STATE(FIXED_WING_LEGACY)) {
         if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) ||    // Normal activation of passthrough
-            (!ARMING_FLAG(ARMED) && isCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating
+            (!ARMING_FLAG(ARMED) && areSensorsCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating
             ENABLE_FLIGHT_MODE(MANUAL_MODE);
         } else {
             DISABLE_FLIGHT_MODE(MANUAL_MODE);

src/main/fc/fc_core.h

@@ -43,7 +43,7 @@ disarmReason_t getDisarmReason(void);
 
 void emergencyArmingUpdate(bool armingSwitchIsOn);
 
-bool isCalibrating(void);
+bool areSensorsCalibrating(void);
 float getFlightTime(void);
 float getArmTime(void);
 

src/main/fc/fc_msp.c

@@ -774,7 +774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, motorConfig()->maxthrottle);
         sbufWriteU16(dst, motorConfig()->mincommand);
 
-        sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
+        sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
 
 #ifdef USE_GPS
         sbufWriteU8(dst, gpsConfig()->provider); // gps_type
@@ -815,7 +815,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, motorConfig()->maxthrottle);
         sbufWriteU16(dst, motorConfig()->mincommand);
 
-        sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
+        sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
 
 #ifdef USE_GPS
         sbufWriteU8(dst, gpsConfig()->provider); // gps_type
@@ -1041,7 +1041,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP_FAILSAFE_CONFIG:
         sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
         sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
-        sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
+        sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
         sbufWriteU8(dst, 0);    // was failsafe_kill_switch
         sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
         sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
@@ -1322,7 +1322,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
         sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
         sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold);
-        sbufWriteU16(dst, navConfig()->mc.hover_throttle);
+        sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
         break;
 
     case MSP_RTH_AND_LAND_CONFIG:
@@ -1342,13 +1342,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
     case MSP_FW_CONFIG:
-        sbufWriteU16(dst, navConfig()->fw.cruise_throttle);
-        sbufWriteU16(dst, navConfig()->fw.min_throttle);
-        sbufWriteU16(dst, navConfig()->fw.max_throttle);
+        sbufWriteU16(dst, currentBatteryProfile->nav.fw.cruise_throttle);
+        sbufWriteU16(dst, currentBatteryProfile->nav.fw.min_throttle);
+        sbufWriteU16(dst, currentBatteryProfile->nav.fw.max_throttle);
         sbufWriteU8(dst, navConfig()->fw.max_bank_angle);
         sbufWriteU8(dst, navConfig()->fw.max_climb_angle);
         sbufWriteU8(dst, navConfig()->fw.max_dive_angle);
-        sbufWriteU8(dst, navConfig()->fw.pitch_to_throttle);
+        sbufWriteU8(dst, currentBatteryProfile->nav.fw.pitch_to_throttle);
         sbufWriteU16(dst, navConfig()->fw.loiter_radius);
         break;
 #endif
@@ -1664,7 +1664,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 {
     uint8_t tmp_u8;
     uint16_t tmp_u16;
-    batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile;
 
     const unsigned int dataSize = sbufBytesRemaining(src);
 
@@ -1867,7 +1866,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
         motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
 
-        failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
+        currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
 
 #ifdef USE_GPS
         gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
@@ -1915,7 +1914,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
             motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
 
-            failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
+            currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
 
 #ifdef USE_GPS
             gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
@@ -2293,7 +2292,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
             navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
             navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
-            navConfigMutable()->mc.hover_throttle = sbufReadU16(src);
+            currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
         } else
             return MSP_RESULT_ERROR;
         break;
@@ -2319,13 +2318,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_FW_CONFIG:
         if (dataSize >= 12) {
-            navConfigMutable()->fw.cruise_throttle = sbufReadU16(src);
-            navConfigMutable()->fw.min_throttle = sbufReadU16(src);
-            navConfigMutable()->fw.max_throttle = sbufReadU16(src);
+            currentBatteryProfileMutable->nav.fw.cruise_throttle = sbufReadU16(src);
+            currentBatteryProfileMutable->nav.fw.min_throttle = sbufReadU16(src);
+            currentBatteryProfileMutable->nav.fw.max_throttle = sbufReadU16(src);
             navConfigMutable()->fw.max_bank_angle = sbufReadU8(src);
             navConfigMutable()->fw.max_climb_angle = sbufReadU8(src);
             navConfigMutable()->fw.max_dive_angle = sbufReadU8(src);
-            navConfigMutable()->fw.pitch_to_throttle = sbufReadU8(src);
+            currentBatteryProfileMutable->nav.fw.pitch_to_throttle = sbufReadU8(src);
             navConfigMutable()->fw.loiter_radius = sbufReadU16(src);
         } else
             return MSP_RESULT_ERROR;
@@ -2695,7 +2694,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         if (dataSize >= 20) {
             failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
             failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
-            failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
+            currentBatteryProfileMutable->failsafe_throttle = sbufReadU16(src);
             sbufReadU8(src); // was failsafe_kill_switch
             failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
             failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);

src/main/fc/fc_msp_box.c

@@ -326,7 +326,7 @@ void initActiveBoxIds(void)
 #endif
 }
 
-#define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
+#define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1)
 #define CHECK_ACTIVE_BOX(condition, index)    do { if (IS_ENABLED(condition)) { activeBoxes[index] = 1; } } while(0)
 
 void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)

src/main/fc/rc_adjustments.c

@@ -49,6 +49,7 @@
 #include "io/beeper.h"
 #include "io/vtx.h"
 
+#include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 
 #include "rx/rx.h"
@@ -500,10 +501,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             schedulePidGainsUpdate();
             break;
         case ADJUSTMENT_NAV_FW_CRUISE_THR:
-            applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
+            applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &currentBatteryProfileMutable->nav.fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
             break;
         case ADJUSTMENT_NAV_FW_PITCH2THR:
-            applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
+            applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &currentBatteryProfileMutable->nav.fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
             break;
         case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
             updateBoardAlignment(delta, 0);
@@ -578,7 +579,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             navigationUsePIDs();
             break;
         case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
-            applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
+            applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &currentBatteryProfileMutable->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
             break;
 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
         case ADJUSTMENT_VTX_POWER_LEVEL:
@@ -722,3 +723,14 @@ bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction) {
     }
     return false;
 }
+
+uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions) {
+    uint8_t adjustmentCount = 0;
+    for (uint8_t i = 0; i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; i++) {
+        if (adjustmentStates[i].config) {
+            adjustmentCount++;
+            adjustmentFunctions[i] = adjustmentStates[i].config->adjustmentFunction;
+        }
+    }
+    return adjustmentCount;
+}

src/main/fc/rc_adjustments.h

@@ -144,3 +144,4 @@ void updateAdjustmentStates(bool canUseRxData);
 struct controlRateConfig_s;
 void processRcAdjustments(struct controlRateConfig_s *controlRateConfig, bool canUseRxData);
 bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction);
+uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions);

src/main/fc/rc_controls.c

@@ -71,12 +71,13 @@ stickPositions_e rcStickPositions;
 
 FASTRAM int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
-PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 3);
 
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
     .deadband = SETTING_DEADBAND_DEFAULT,
     .yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT,
     .pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT,
+    .control_deadband = SETTING_CONTROL_DEADBAND_DEFAULT,
     .alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT,
     .mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT,
     .airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT,
@@ -97,9 +98,14 @@ bool areSticksInApModePosition(uint16_t ap_mode)
     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
 }
 
-bool areSticksDeflectedMoreThanPosHoldDeadband(void)
+bool areSticksDeflected(void)
 {
-    return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
+    return (ABS(rcCommand[ROLL]) > rcControlsConfig()->control_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->control_deadband) || (ABS(rcCommand[YAW]) > rcControlsConfig()->control_deadband);
+}
+
+bool isRollPitchStickDeflected(void)
+{
+    return (ABS(rcCommand[ROLL]) > rcControlsConfig()->control_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->control_deadband);
 }
 
 throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)

src/main/fc/rc_controls.h

@@ -84,7 +84,8 @@ extern int16_t rcCommand[4];
 typedef struct rcControlsConfig_s {
     uint8_t deadband;                       // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
     uint8_t yaw_deadband;                   // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
-    uint8_t pos_hold_deadband;              // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
+    uint8_t pos_hold_deadband;              // Deadband for position hold
+    uint8_t control_deadband;               // General deadband to check if sticks are deflected, us PWM.
     uint8_t alt_hold_deadband;              // Defines the neutral zone of throttle stick during altitude hold
     uint16_t mid_throttle_deadband;           // default throttle deadband from MIDRC
     uint8_t airmodeHandlingType;            // Defaults to ANTI_WINDUP triggered at sticks centered
@@ -106,7 +107,8 @@ stickPositions_e getRcStickPositions(void);
 bool checkStickPosition(stickPositions_e stickPos);
 
 bool areSticksInApModePosition(uint16_t ap_mode);
-bool areSticksDeflectedMoreThanPosHoldDeadband(void);
+bool areSticksDeflected(void);
+bool isRollPitchStickDeflected(void);
 throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
 rollPitchStatus_e calculateRollPitchCenterStatus(void);
 void processRcStickPositions(throttleStatus_e throttleStatus);

src/main/fc/settings.h

@@ -67,9 +67,9 @@ typedef struct {
 
 } __attribute__((packed)) setting_t;
 
-static inline setting_type_e SETTING_TYPE(const setting_t *s) { return s->type &  SETTING_TYPE_MASK; }
-static inline setting_section_e SETTING_SECTION(const setting_t *s) { return s->type & SETTING_SECTION_MASK; }
-static inline setting_mode_e SETTING_MODE(const setting_t *s) { return s->type & SETTING_MODE_MASK; }
+static inline setting_type_e SETTING_TYPE(const setting_t *s) { return (setting_type_e)(s->type &  SETTING_TYPE_MASK); }
+static inline setting_section_e SETTING_SECTION(const setting_t *s) { return (setting_section_e)(s->type & SETTING_SECTION_MASK); }
+static inline setting_mode_e SETTING_MODE(const setting_t *s) { return (setting_mode_e)(s->type & SETTING_MODE_MASK); }
 
 void settingGetName(const setting_t *val, char *buf);
 bool settingNameContains(const setting_t *val, char *buf, const char *cmdline);

src/main/fc/settings.yaml

@@ -182,17 +182,25 @@ tables:
   - name: nav_wp_mission_restart
     enum: navMissionRestart_e
     values: ["START", "RESUME", "SWITCH"]
+  - name: djiRssiSource
+    values: ["RSSI", "CRSF_LQ"]
+    enum: djiRssiSource_e
+
 
 constants:
   RPYL_PID_MIN: 0
-  RPYL_PID_MAX: 200
+  RPYL_PID_MAX: 255
 
   MANUAL_RATE_MIN: 0
   MANUAL_RATE_MAX: 100
 
-  ROLL_PITCH_RATE_MIN: 6
+  ROLL_PITCH_RATE_MIN: 4
   ROLL_PITCH_RATE_MAX: 180
 
+  MAX_CONTROL_RATE_PROFILE_COUNT: 3
+  MAX_BATTERY_PROFILE_COUNT: 3
+
+
 groups:
   - name: PG_GYRO_CONFIG
     type: gyroConfig_t
@@ -217,7 +225,7 @@ groups:
         description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
         default_value: 250
         field: gyro_anti_aliasing_lpf_hz
-        max: 255
+        max: 1000
       - name: gyro_anti_aliasing_lpf_type
         description: "Specifies the type of the software LPF of the gyro signals."
         default_value: "PT1"
@@ -323,6 +331,19 @@ groups:
         condition: USE_ALPHA_BETA_GAMMA_FILTER
         min: 0
         max: 10
+      - name: setpoint_kalman_enabled
+        description: "Enable Kalman filter on the gyro data"
+        default_value: OFF
+        condition: USE_GYRO_KALMAN
+        field: kalmanEnabled
+        type: bool
+      - name: setpoint_kalman_q
+        description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds"
+        default_value: 100
+        field: kalman_q
+        condition: USE_GYRO_KALMAN
+        min: 1
+        max: 16000
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t
@@ -860,31 +881,12 @@ groups:
         default_value: "ONESHOT125"
         field: motorPwmProtocol
         table: motor_pwm_protocol
-      - name: throttle_scale
-        description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%"
-        default_value: 1.0
-        field: throttleScale
-        min: 0
-        max: 1
-      - name: throttle_idle
-        description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle."
-        default_value: 15
-        field: throttleIdle
-        min: 0
-        max: 30
       - name: motor_poles
         field: motorPoleCount
         description: "The number of motor poles. Required to compute motor RPM"
         min: 4
         max: 255
         default_value: 14
-      - name: turtle_mode_power_factor
-        field: turtleModePowerFactor
-        default_value: 55
-        description: "Turtle mode power factor"
-        condition: "USE_DSHOT"
-        min: 0
-        max: 100
 
   - name: PG_FAILSAFE_CONFIG
     type: failsafeConfig_t
@@ -905,11 +907,6 @@ groups:
         default_value: 200
         min: 0
         max: 200
-      - name: failsafe_throttle
-        description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)."
-        default_value: 1000
-        min: PWM_RANGE_MIN
-        max: PWM_RANGE_MAX
       - name: failsafe_throttle_low_delay
         description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout"
         default_value: 0
@@ -1002,7 +999,7 @@ groups:
 
   - name: PG_BATTERY_METERS_CONFIG
     type: batteryMetersConfig_t
-    headers: ["sensors/battery.h"]
+    headers: ["sensors/battery_config_structs.h"]
     members:
       - name: vbat_meter_type
         description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running"
@@ -1016,8 +1013,8 @@ groups:
         default_value: :target
         field: voltage.scale
         condition: USE_ADC
-        min: VBAT_SCALE_MIN
-        max: VBAT_SCALE_MAX
+        min: 0
+        max: 65535
       - name: current_meter_scale
         description: "This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt."
         default_value: :target
@@ -1068,7 +1065,7 @@ groups:
 
   - name: PG_BATTERY_PROFILES
     type: batteryProfile_t
-    headers: ["sensors/battery.h"]
+    headers: ["sensors/battery_config_structs.h"]
     value_type: BATTERY_CONFIG_VALUE
     members:
       - name: bat_cells
@@ -1130,6 +1127,133 @@ groups:
         field: capacity.unit
         table: bat_capacity_unit
         type: uint8_t
+      - name: controlrate_profile
+        description: "Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile"
+        default_value: 0
+        field: controlRateProfile
+        min: 0
+        max: MAX_CONTROL_RATE_PROFILE_COUNT
+
+      - name: throttle_scale
+        description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%"
+        default_value: 1.0
+        field: motor.throttleScale
+        min: 0
+        max: 1
+      - name: throttle_idle
+        description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle."
+        default_value: 15
+        field: motor.throttleIdle
+        min: 0
+        max: 30
+      - name: turtle_mode_power_factor
+        field: motor.turtleModePowerFactor
+        default_value: 55
+        description: "Turtle mode power factor"
+        condition: "USE_DSHOT"
+        min: 0
+        max: 100
+      - name: failsafe_throttle
+        description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)."
+        default_value: 1000
+        min: PWM_RANGE_MIN
+        max: PWM_RANGE_MAX
+      - name: fw_min_throttle_down_pitch
+        description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
+        default_value: 0
+        field: fwMinThrottleDownPitchAngle
+        min: 0
+        max: 450
+      - name: nav_mc_hover_thr
+        description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
+        default_value: 1500
+        field: nav.mc.hover_throttle
+        min: 1000
+        max: 2000
+      - name: nav_fw_cruise_thr
+        description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
+        default_value: 1400
+        field: nav.fw.cruise_throttle
+        min: 1000
+        max: 2000
+      - name: nav_fw_min_thr
+        description: "Minimum throttle for flying wing in GPS assisted modes"
+        default_value: 1200
+        field: nav.fw.min_throttle
+        min: 1000
+        max: 2000
+      - name: nav_fw_max_thr
+        description: "Maximum throttle for flying wing in GPS assisted modes"
+        default_value: 1700
+        field: nav.fw.max_throttle
+        min: 1000
+        max: 2000
+      - name: nav_fw_pitch2thr
+        description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)"
+        default_value: 10
+        field: nav.fw.pitch_to_throttle
+        min: 0
+        max: 100
+      - name: nav_fw_launch_thr
+        description: "Launch throttle - throttle to be set during launch sequence (pwm units)"
+        default_value: 1700
+        field: nav.fw.launch_throttle
+        min: 1000
+        max: 2000
+      - name: nav_fw_launch_idle_thr
+        description: "Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position)"
+        default_value: 1000
+        field: nav.fw.launch_idle_throttle
+        min: 1000
+        max: 2000
+      - name: limit_cont_current
+        description: "Continous current limit (dA), set to 0 to disable"
+        condition: USE_POWER_LIMITS
+        default_value: 0
+        field: powerLimits.continuousCurrent
+        max: 4000
+      - name: limit_burst_current
+        description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
+        condition: USE_POWER_LIMITS
+        default_value: 0
+        field: powerLimits.burstCurrent
+        max: 4000
+      - name: limit_burst_current_time
+        description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
+        condition: USE_POWER_LIMITS
+        default_value: 0
+        field: powerLimits.burstCurrentTime
+        max: 3000
+      - name: limit_burst_current_falldown_time
+        description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
+        condition: USE_POWER_LIMITS
+        default_value: 0
+        field: powerLimits.burstCurrentFalldownTime
+        max: 3000
+      - name: limit_cont_power
+        description: "Continous power limit (dW), set to 0 to disable"
+        condition: USE_POWER_LIMITS && USE_ADC
+        default_value: 0
+        field: powerLimits.continuousPower
+        max: 40000
+      - name: limit_burst_power
+        description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
+        condition: USE_POWER_LIMITS && USE_ADC
+        default_value: 0
+        field: powerLimits.burstPower
+        max: 40000
+      - name: limit_burst_power_time
+        description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
+        condition: USE_POWER_LIMITS && USE_ADC
+        default_value: 0
+        field: powerLimits.burstPowerTime
+        max: 3000
+      - name: limit_burst_power_falldown_time
+        description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
+        condition: USE_POWER_LIMITS && USE_ADC
+        default_value: 0
+        field: powerLimits.burstPowerFalldownTime
+        max: 3000
 
   - name: PG_MIXER_CONFIG
     type: mixerConfig_t
@@ -1156,12 +1280,6 @@ groups:
         field: appliedMixerPreset
         min: -1
         max: INT16_MAX
-      - name: fw_min_throttle_down_pitch
-        description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
-        default_value: 0
-        field: fwMinThrottleDownPitchAngle
-        min: 0
-        max: 450
 
   - name: PG_REVERSIBLE_MOTORS_CONFIG
     type: reversibleMotorsConfig_t
@@ -1229,7 +1347,7 @@ groups:
 
   - name: PG_CONTROL_RATE_PROFILES
     type: controlRateConfig_t
-    headers: ["fc/controlrate_profile.h"]
+    headers: ["fc/controlrate_profile_config_struct.h"]
     value_type: CONTROL_RATE_VALUE
     members:
       - name: thr_mid
@@ -1292,7 +1410,7 @@ groups:
         description: "Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure."
         default_value: 20
         field: stabilized.rates[FD_YAW]
-        min: 2
+        min: 1
         max: 180
       - name: manual_rc_expo
         description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]"
@@ -1508,7 +1626,12 @@ groups:
         min: 0
         max: 100
       - name: pos_hold_deadband
-        description: "Stick deadband in [r/c points], applied after r/c deadband and expo"
+        description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used for adjustments in navigation modes."
+        default_value: 10
+        min: 2
+        max: 250
+      - name: control_deadband
+        description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used to check if sticks are centered."
         default_value: 10
         min: 2
         max: 250
@@ -2061,33 +2184,6 @@ groups:
         field: controlDerivativeLpfHz
         min: 0
         max: 200
-      - name: setpoint_kalman_enabled
-        description: "Enable Kalman filter on the PID controller setpoint"
-        default_value: OFF
-        condition: USE_GYRO_KALMAN
-        field: kalmanEnabled
-        type: bool
-      - name: setpoint_kalman_q
-        description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds"
-        default_value: 100
-        field: kalman_q
-        condition: USE_GYRO_KALMAN
-        min: 1
-        max: 16000
-      - name: setpoint_kalman_w
-        description: "Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response"
-        default_value: 4
-        field: kalman_w
-        condition: USE_GYRO_KALMAN
-        min: 1
-        max: 40
-      - name: setpoint_kalman_sharpness
-        description: "Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets"
-        default_value: 100
-        field: kalman_sharpness
-        condition: USE_GYRO_KALMAN
-        min: 1
-        max: 16000
       - name: fw_level_pitch_trim
         description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level"
         default_value: 0
@@ -2121,12 +2217,6 @@ groups:
         field: fixedWingLevelTrimGain
         min: 0
         max: 20
-      - name: fw_level_pitch_deadband
-        description: "Deadband for automatic leveling when AUTOLEVEL mode is used."
-        default_value: 5
-        field: fixedWingLevelTrimDeadband
-        min: 0
-        max: 20
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
@@ -2164,7 +2254,7 @@ groups:
         type: uint8_t
       - name: fw_autotune_max_rate_deflection
         description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`."
-        default_value: 80
+        default_value: 90
         field: fw_max_rate_deflection
         min: 50
         max: 100
@@ -2491,12 +2581,6 @@ groups:
         field: mc.max_bank_angle
         min: 15
         max: 45
-      - name: nav_mc_hover_thr
-        description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
-        default_value: 1500
-        field: mc.hover_throttle
-        min: 1000
-        max: 2000
       - name: nav_mc_auto_disarm_delay
         description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)"
         default_value: 2000
@@ -2578,24 +2662,6 @@ groups:
         default_value: ON
         field: mc.slowDownForTurning
         type: bool
-      - name: nav_fw_cruise_thr
-        description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
-        default_value: 1400
-        field: fw.cruise_throttle
-        min: 1000
-        max: 2000
-      - name: nav_fw_min_thr
-        description: "Minimum throttle for flying wing in GPS assisted modes"
-        default_value: 1200
-        field: fw.min_throttle
-        min: 1000
-        max: 2000
-      - name: nav_fw_max_thr
-        description: "Maximum throttle for flying wing in GPS assisted modes"
-        default_value: 1700
-        field: fw.max_throttle
-        min: 1000
-        max: 2000
       - name: nav_fw_bank_angle
         description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll"
         default_value: 35
@@ -2614,12 +2680,6 @@ groups:
         field: fw.max_dive_angle
         min: 5
         max: 80
-      - name: nav_fw_pitch2thr
-        description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)"
-        default_value: 10
-        field: fw.pitch_to_throttle
-        min: 0
-        max: 100
       - name: nav_fw_pitch2thr_smoothing
         description:  "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
         default_value: 6
@@ -2683,18 +2743,12 @@ groups:
         field: fw.launch_time_thresh
         min: 10
         max: 1000
-      - name: nav_fw_launch_thr
-        description: "Launch throttle - throttle to be set during launch sequence (pwm units)"
-        default_value: 1700
-        field: fw.launch_throttle
-        min: 1000
-        max: 2000
-      - name: nav_fw_launch_idle_thr
-        description: "Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position)"
-        default_value: 1000
-        field: fw.launch_idle_throttle
-        min: 1000
-        max: 2000
+      - name: nav_fw_launch_idle_motor_delay
+        description: "Delay between raising throttle and motor starting at idle throttle (ms)"
+        default_value: 0
+        field: fw.launch_idle_motor_timer
+        min: 0
+        max: 60000
       - name: nav_fw_launch_motor_delay
         description: "Delay between detected launch and launch sequence start and throttling up (ms)"
         default_value: 500
@@ -3134,6 +3188,13 @@ groups:
         field: link_quality_alarm
         min: 0
         max: 100
+      - name: osd_rssi_dbm_alarm
+        condition: USE_SERIALRX_CRSF
+        description: "RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm"
+        default_value: 0
+        field: rssi_dbm_alarm
+        min: -130
+        max: 0
       - name: osd_temp_label_align
         description: "Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT`"
         default_value: "LEFT"
@@ -3172,11 +3233,16 @@ groups:
         min: -2
         max: 2
       - name: osd_camera_uptilt
-        description: "Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal"
+        description: "Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal. Used for correct display of HUD items and AHI (when enabled)."
         default_value: 0
         field: camera_uptilt
         min: -40
         max: 80
+      - name: osd_ahi_camera_uptilt_comp
+        description: "When set to `ON`, the AHI position is adjusted by `osd_camera_uptilt`. For example, with a cammera uptilt of 30 degrees, the AHI will appear in the middle of the OSD when the aircraft is pitched forward 30 degrees. When set to `OFF`, the AHI will appear in the center of the OSD regardless of camera angle, but can still be shifted up and down using `osd_horizon_offset` (`osd_ahi_vertical_offset` for pixel-OSD)."
+        default_value: OFF
+        field: ahi_camera_uptilt_comp
+        type: bool
       - name: osd_camera_fov_h
         description: "Horizontal field of view for the camera in degres"
         default_value: 135
@@ -3625,7 +3691,7 @@ groups:
           max: UINT8_MAX
           default_value: 1
         - name: dji_use_name_for_messages
-          description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance"
+          description: "Re-purpose the craft name field for messages."
           default_value: ON
           field: use_name_for_messages
           type: bool
@@ -3635,6 +3701,30 @@ groups:
           field: esc_temperature_source
           table: djiOsdTempSource
           type: uint8_t
+        - name: dji_message_speed_source
+          description: "Sets the speed type displayed by the DJI OSD in craft name: GROUND, 3D, AIR"
+          default_value: "3D"
+          field: messageSpeedSource
+          table: osdSpeedSource
+          type: uint8_t
+        - name: dji_rssi_source
+          description: "Source of the DJI RSSI field: RSSI, CRSF_LQ"
+          default_value: "RSSI"
+          field: rssi_source
+          table: djiRssiSource
+          type: uint8_t
+        - name: dji_use_adjustments
+          description: "Show inflight adjustments in craft name field"
+          default_value: OFF
+          type: bool
+          field: useAdjustments
+        - name: dji_cn_alternating_duration
+          description: "Alternating duration of craft name elements, in tenths of a second"
+          default_value: 30
+          min: 1
+          max: 150
+          field: craftNameAlternatingDuration
+          type: uint8_t
 
   - name: PG_BEEPER_CONFIG
     type: beeperConfig_t
@@ -3658,50 +3748,6 @@ groups:
     headers: ["flight/power_limits.h"]
     condition: USE_POWER_LIMITS
     members:
-      - name: limit_cont_current
-        description: "Continous current limit (dA), set to 0 to disable"
-        default_value: 0
-        field: continuousCurrent
-        max: 4000
-      - name: limit_burst_current
-        description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
-        default_value: 0
-        field: burstCurrent
-        max: 4000
-      - name: limit_burst_current_time
-        description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
-        default_value: 0
-        field: burstCurrentTime
-        max: 3000
-      - name: limit_burst_current_falldown_time
-        description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
-        default_value: 0
-        field: burstCurrentFalldownTime
-        max: 3000
-      - name: limit_cont_power
-        condition: USE_ADC
-        description: "Continous power limit (dW), set to 0 to disable"
-        default_value: 0
-        field: continuousPower
-        max: 40000
-      - name: limit_burst_power
-        condition: USE_ADC
-        description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
-        default_value: 0
-        field: burstPower
-        max: 40000
-      - name: limit_burst_power_time
-        condition: USE_ADC
-        description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
-        default_value: 0
-        field: burstPowerTime
-        max: 3000
-      - name: limit_burst_power_falldown_time
-        condition: USE_ADC
-        description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
-        default_value: 0
-        field: burstPowerFalldownTime
-        max: 3000
       - name: limit_pi_p
         description: "Throttle attenuation PI control P term"
         default_value: 100

src/main/flight/failsafe.c

@@ -51,6 +51,7 @@
 
 #include "rx/rx.h"
 
+#include "sensors/battery.h"
 #include "sensors/sensors.h"
 
 /*
@@ -66,13 +67,12 @@
 
 static failsafeState_t failsafeState;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 2);
 
 PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT,                                   // 0.5 sec
     .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT,                 // 0.5 seconds (plus 200ms explicit delay)
     .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT,                           // 20sec
-    .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT,                             // default throttle off.
     .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT,                                   // default throttle low delay for "just disarm" on failsafe condition
     .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT,                           // default full failsafe procedure
     .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT,                   // 20 deg left
@@ -218,7 +218,7 @@ bool failsafeRequiresMotorStop(void)
 {
     return failsafeState.active &&
            failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
-           failsafeConfig()->failsafe_throttle < getThrottleIdleValue();
+           currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
 }
 
 void failsafeStartMonitoring(void)
@@ -264,13 +264,13 @@ void failsafeApplyControlInput(void)
         autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
         autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
         autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
-        autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+        autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
     }
     else {
         for (int i = 0; i < 3; i++) {
             autoRcCommand[i] = 0;
         }
-        autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+        autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
     }
 
     // Apply channel values

src/main/flight/failsafe.h

@@ -30,7 +30,6 @@
 
 
 typedef struct failsafeConfig_s {
-    uint16_t failsafe_throttle;                 // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
     uint16_t failsafe_throttle_low_delay;       // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND)
     uint8_t failsafe_delay;                     // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
     uint8_t failsafe_recovery_delay;            // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10)

src/main/flight/imu.c

@@ -241,7 +241,7 @@ static float imuGetPGainScaleFactor(void)
 
 static void imuResetOrientationQuaternion(const fpVector3_t * accBF)
 {
-    const float accNorm = sqrtf(vectorNormSquared(accBF));
+    const float accNorm = fast_fsqrtf(vectorNormSquared(accBF));
 
     orientation.q0 = accBF->z + accNorm;
     orientation.q1 = accBF->y;
@@ -436,12 +436,12 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
         // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision -
         // then we can safely use the "low angle" approximated version without loss of accuracy.
-        if (thetaMagnitudeSq < sqrtf(24.0f * 1e-6f)) {
+        if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) {
             quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f);
             deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f;
         }
         else {
-            const float thetaMagnitude = sqrtf(thetaMagnitudeSq);
+            const float thetaMagnitude = fast_fsqrtf(thetaMagnitudeSq);
             quaternionScale(&deltaQ, &deltaQ, sin_approx(thetaMagnitude) / thetaMagnitude);
             deltaQ.q0 = cos_approx(thetaMagnitude);
         }
@@ -483,7 +483,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
         accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis];
     }
 
-    const float accWeight_Nearness = bellCurve(sqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
+    const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
 
     // Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we 
     // should not use measured accel for AHRS comp
@@ -504,7 +504,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
     float accWeight_RateIgnore = 1.0f;
 
     if (ARMING_FLAG(ARMED) && STATE(FIXED_WING_LEGACY) && imuConfig()->acc_ignore_rate) {
-        const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
+        const float rotRateMagnitude = fast_fsqrtf(sq(imuMeasuredRotationBF.y) + sq(imuMeasuredRotationBF.z));
         const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT);
 
         if (imuConfig()->acc_ignore_slope) {

src/main/flight/kalman.c

@@ -30,45 +30,35 @@ FILE_COMPILE_FOR_SPEED
 
 kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
 
-static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q, uint16_t w, uint16_t sharpness)
+static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q)
 {
     memset(filter, 0, sizeof(kalman_t));
     filter->q = q * 0.03f; //add multiplier to make tuning easier
     filter->r = 88.0f;      //seeding R at 88.0f
     filter->p = 30.0f;      //seeding P at 30.0f
     filter->e = 1.0f;
-    filter->s = sharpness / 10.0f;
-    filter->w = w * 8;
+    filter->w = MAX_KALMAN_WINDOW_SIZE;         
     filter->inverseN = 1.0f / (float)(filter->w);
 }
 
-void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness)
+void gyroKalmanInitialize(uint16_t q)
 {
-    gyroKalmanInitAxis(&kalmanFilterStateRate[X], q, w, sharpness);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q, w, sharpness);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q, w, sharpness);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[X], q);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q);
 }
 
-float kalman_process(kalman_t *kalmanState, float input, float target)
+float kalman_process(kalman_t *kalmanState, float input)
 {
-    float targetAbs = fabsf(target);
     //project the state ahead using acceleration
     kalmanState->x += (kalmanState->x - kalmanState->lastX);
 
-    //figure out how much to boost or reduce our error in the estimate based on setpoint target.
-    //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint.
     //update last state
     kalmanState->lastX = kalmanState->x;
 
     if (kalmanState->lastX != 0.0f)
     {
-        // calculate the error and add multiply sharpness boost
-        float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
-
-        // give a boost to the setpoint, used to caluclate the kalman q, based on the error and setpoint/gyrodata
-        errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
-
-        kalmanState->e = fabsf(1.0f - (((targetAbs + 1.0f) * errorMultiplier) / fabsf(kalmanState->lastX)));
+        kalmanState->e = fabsf(1.0f - (kalmanState->setpoint / kalmanState->lastX));
     }
 
     //prediction update
@@ -92,7 +82,7 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate)
     kalmanState->varianceWindow[kalmanState->windex] = varianceElement;
 
     kalmanState->windex++;
-    if (kalmanState->windex >= kalmanState->w) {
+    if (kalmanState->windex > kalmanState->w) {
         kalmanState->windex = 0;
     }
 
@@ -108,13 +98,17 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate)
     kalmanState->r = squirt * VARIANCE_SCALE;
 }
 
-float gyroKalmanUpdate(uint8_t axis, float input, float setpoint)
+float NOINLINE gyroKalmanUpdate(uint8_t axis, float input)
 {
     updateAxisVariance(&kalmanFilterStateRate[axis], input);
 
     DEBUG_SET(DEBUG_KALMAN_GAIN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain
 
-    return kalman_process(&kalmanFilterStateRate[axis], input, setpoint);
+    return kalman_process(&kalmanFilterStateRate[axis], input);
+}
+
+void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint) {
+    kalmanFilterStateRate[axis].setpoint = setpoint;
 }
 
 #endif

src/main/flight/kalman.h

@@ -23,7 +23,7 @@
 #include "sensors/gyro.h"
 #include "common/filter.h"
 
-#define MAX_KALMAN_WINDOW_SIZE 512
+#define MAX_KALMAN_WINDOW_SIZE 64
 
 #define VARIANCE_SCALE 0.67f
 
@@ -36,12 +36,13 @@ typedef struct kalman
     float x;     //state
     float lastX; //previous state
     float e;
-    float s;
+
+    float setpoint;
     
     float axisVar;
     uint16_t windex;
-    float axisWindow[MAX_KALMAN_WINDOW_SIZE];
-    float varianceWindow[MAX_KALMAN_WINDOW_SIZE];
+    float axisWindow[MAX_KALMAN_WINDOW_SIZE + 1];
+    float varianceWindow[MAX_KALMAN_WINDOW_SIZE + 1];
     float axisSumMean;
     float axisMean;
     float axisSumVar;
@@ -49,5 +50,6 @@ typedef struct kalman
     uint16_t w;
 } kalman_t;
 
-void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness);
-float gyroKalmanUpdate(uint8_t axis, float input, float setpoint);
+void gyroKalmanInitialize(uint16_t q);
+float gyroKalmanUpdate(uint8_t axis, float input);
+void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint);

src/main/flight/mixer.c

@@ -83,14 +83,13 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
     .neutral = SETTING_3D_NEUTRAL_DEFAULT
 );
 
-PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 4);
 
 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
     .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
     .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
     .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
     .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
-    .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT
 );
 
 #ifdef BRUSHED_MOTORS
@@ -103,7 +102,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
 
 #define DEFAULT_MAX_THROTTLE    1850
 
-PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 8);
 
 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
@@ -112,12 +111,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .mincommand = SETTING_MIN_COMMAND_DEFAULT,
     .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT,
     .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT,
-    .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
-    .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
     .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT,            // Most brushless motors that we use are 14 poles
-#ifdef USE_DSHOT
-    .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
-#endif
 );
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
@@ -130,7 +124,7 @@ static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
 int getThrottleIdleValue(void)
 {
     if (!throttleIdleValue) {
-        throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle);
+        throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * currentBatteryProfile->motor.throttleIdle);
     }
 
     return throttleIdleValue;
@@ -330,7 +324,7 @@ static uint16_t handleOutputScaling(
 static void applyTurtleModeToMotors(void) {
 
     if (ARMING_FLAG(ARMED)) {
-        const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f;
+        const float flipPowerFactor = ((float)currentBatteryProfile->motor.turtleModePowerFactor)/100.0f;
         const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
         const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
         const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
@@ -347,8 +341,8 @@ static void applyTurtleModeToMotors(void) {
         float signRoll = rcCommand[ROLL] < 0 ? 1 : -1;
         float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1));
 
-        float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
-        float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
+        float stickDeflectionLength = fast_fsqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
+        float stickDeflectionExpoLength = fast_fsqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
 
         if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
             // If yaw is the dominant, disable pitch and roll
@@ -362,8 +356,8 @@ static void applyTurtleModeToMotors(void) {
         }
 
         const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) /
-                                                           (sqrtf(2.0f) * stickDeflectionLength) : 0;
-        const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
+                                                           (fast_fsqrtf(2.0f) * stickDeflectionLength) : 0;
+        const float cosThreshold = fast_fsqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
 
         if (cosPhi < cosThreshold) {
             // Enforce either roll or pitch exclusively, if not on diagonal
@@ -602,9 +596,9 @@ void FAST_CODE mixTable(const float dT)
 
         // Throttle scaling to limit max throttle when battery is full
     #ifdef USE_PROGRAMMING_FRAMEWORK
-        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin;
+        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
     #else
-        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin;
+        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
     #endif
         // Throttle compensation based on battery voltage
         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {

src/main/flight/mixer.h

@@ -62,7 +62,6 @@ typedef struct mixerConfig_s {
     uint8_t platformType;
     bool hasFlaps;
     int16_t appliedMixerPreset;
-    uint16_t fwMinThrottleDownPitchAngle;
 } mixerConfig_t;
 
 PG_DECLARE(mixerConfig_t, mixerConfig);
@@ -84,10 +83,7 @@ typedef struct motorConfig_s {
     uint16_t motorAccelTimeMs;              // Time limit for motor to accelerate from 0 to 100% throttle [ms]
     uint16_t motorDecelTimeMs;              // Time limit for motor to decelerate from 0 to 100% throttle [ms]
     uint16_t digitalIdleOffsetValue;
-    float throttleIdle;                     // Throttle IDLE value based on min_command, max_throttle, in percent
-    float throttleScale;                    // Scaling factor for throttle.
     uint8_t motorPoleCount;                 // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
-    uint8_t turtleModePowerFactor;          // Power factor from 0 to 100% of flip over after crash
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);

src/main/flight/pid.c

@@ -56,6 +56,7 @@ FILE_COMPILE_FOR_SPEED
 
 #include "rx/rx.h"
 
+#include "sensors/battery.h"
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"
@@ -169,7 +170,7 @@ static EXTENDED_FASTRAM bool levelingEnabled = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 4);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -299,16 +300,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .navFwPosHdgPidsumLimit = SETTING_NAV_FW_POS_HDG_PIDSUM_LIMIT_DEFAULT,
         .controlDerivativeLpfHz = SETTING_MC_CD_LPF_HZ_DEFAULT,
 
-#ifdef USE_GYRO_KALMAN
-        .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
-        .kalman_w = SETTING_SETPOINT_KALMAN_W_DEFAULT,
-        .kalman_sharpness = SETTING_SETPOINT_KALMAN_SHARPNESS_DEFAULT,
-        .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
-#endif
-
         .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
         .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
-        .fixedWingLevelTrimDeadband = SETTING_FW_LEVEL_PITCH_DEADBAND_DEFAULT,
 
 #ifdef USE_SMITH_PREDICTOR
         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
@@ -421,7 +414,7 @@ void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
 
 float getTotalRateTarget(void)
 {
-    return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
+    return fast_fsqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
 }
 
 float getAxisIterm(uint8_t axis)
@@ -607,7 +600,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
 
     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
-        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);        
+        angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, currentBatteryProfile->fwMinThrottleDownPitchAngle);
     }
 
     //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
@@ -616,21 +609,6 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
         DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
         DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
 
-        /* 
-         * fixedWingLevelTrim has opposite sign to rcCommand.
-         * Positive rcCommand means nose should point downwards
-         * Negative rcCommand mean nose should point upwards
-         * This is counter intuitive and a natural way suggests that + should mean UP
-         * This is why fixedWingLevelTrim has opposite sign to rcCommand
-         * Positive fixedWingLevelTrim means nose should point upwards
-         * Negative fixedWingLevelTrim means nose should point downwards
-         */
-        angleTarget -= fixedWingLevelTrim;   
-        DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
-    }
-
-        //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
-    if (axis == FD_PITCH && STATE(AIRPLANE)) {
         /*
          * fixedWingLevelTrim has opposite sign to rcCommand.
          * Positive rcCommand means nose should point downwards
@@ -641,6 +619,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
          * Negative fixedWingLevelTrim means nose should point downwards
          */
         angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
+        DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
     }
 
 #ifdef USE_SECONDARY_IMU
@@ -795,7 +774,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
     }
 
-    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
+    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
 
 #ifdef USE_AUTOTUNE_FIXED_WING
     if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
@@ -1126,9 +1105,7 @@ void FAST_CODE pidController(float dT)
         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
 
 #ifdef USE_GYRO_KALMAN
-        if (pidProfile()->kalmanEnabled) {
-            pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget);
-        }
+        gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget);
 #endif
 
 #ifdef USE_SMITH_PREDICTOR
@@ -1272,11 +1249,6 @@ void pidInit(void)
     }
 
     pidResetTPAFilter();
-#ifdef USE_GYRO_KALMAN
-    if (pidProfile()->kalmanEnabled) {
-        gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness);
-    }
-#endif
 
     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
 
@@ -1327,11 +1299,10 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
      */
     pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
 
-    //Iterm should freeze when pitch stick is deflected
+    //Iterm should freeze when sticks are deflected
     if (
         !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
-        rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) ||
-        rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband) ||
+        areSticksDeflected() ||
         (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) ||
         navigationIsControllingAltitude()
     ) {
@@ -1355,3 +1326,8 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
 
     previousArmingState = !!ARMING_FLAG(ARMED);
 }
+
+float getFixedWingLevelTrim(void)
+{
+    return STATE(AIRPLANE) ? fixedWingLevelTrim : 0;
+}

src/main/flight/pid.h

@@ -156,16 +156,8 @@ typedef struct pidProfile_s {
     uint16_t navFwPosHdgPidsumLimit;
     uint8_t controlDerivativeLpfHz;
 
-#ifdef USE_GYRO_KALMAN
-    uint16_t kalman_q;
-    uint16_t kalman_w;
-    uint16_t kalman_sharpness;
-    uint8_t kalmanEnabled;
-#endif
-
     float fixedWingLevelTrim;
     float fixedWingLevelTrimGain;
-    float fixedWingLevelTrimDeadband;
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;
@@ -233,3 +225,4 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
 
 void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
+float getFixedWingLevelTrim(void);

src/main/flight/pid_autotune.c

@@ -47,14 +47,14 @@
 #include "flight/pid.h"
 
 #define AUTOTUNE_FIXED_WING_MIN_FF              10
-#define AUTOTUNE_FIXED_WING_MAX_FF              200
+#define AUTOTUNE_FIXED_WING_MAX_FF              255
 #define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40
 #define AUTOTUNE_FIXED_WING_MIN_YAW_RATE        10
 #define AUTOTUNE_FIXED_WING_MAX_RATE            720
 #define AUTOTUNE_FIXED_WING_CONVERGENCE_RATE    10
 #define AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL     20      // ms
-#define AUTOTUNE_FIXED_WING_SAMPLES             1000    // Use averagea over the last 20 seconds
-#define AUTOTUNE_FIXED_WING_MIN_SAMPLES         250     // Start updating tune after 5 seconds
+#define AUTOTUNE_FIXED_WING_SAMPLES             1000    // Use average over the last 20 seconds of hard maneuvers
+#define AUTOTUNE_FIXED_WING_MIN_SAMPLES         250     // Start updating tune after 5 seconds of hard maneuvers
 
 PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2);
 

src/main/flight/power_limits.c

@@ -45,19 +45,9 @@ FILE_COMPILE_FOR_SPEED
 
 #define LIMITING_THR_FILTER_TCONST 50
 
-PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 1);
 
 PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig,
-    .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT,                            // dA
-    .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT,                                // dA
-    .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT,                       // dS
-    .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT,      // dS
-#ifdef USE_ADC
-    .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT,                                // dW
-    .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT,                                    // dW
-    .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT,                           // dS
-    .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT,          // dS
-#endif
     .piP = SETTING_LIMIT_PI_P_DEFAULT,
     .piI = SETTING_LIMIT_PI_I_DEFAULT,
     .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT,   // Hz
@@ -84,29 +74,29 @@ static bool wasLimitingPower = false;
 #endif
 
 void powerLimiterInit(void) {
-    if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) {
-        powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent;
+    if (currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) {
+        currentBatteryProfileMutable->powerLimits.burstCurrent = currentBatteryProfile->powerLimits.continuousCurrent;
     }
 
-    activeCurrentLimit = powerLimitsConfig()->burstCurrent;
+    activeCurrentLimit = currentBatteryProfile->powerLimits.burstCurrent;
 
-    uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
-    burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6;
-    burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6;
+    uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
+    burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6;
+    burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6;
 
     pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
     pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
 
 #ifdef USE_ADC
-    if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) {
-        powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower;
+    if (currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) {
+        currentBatteryProfileMutable->powerLimits.burstPower = currentBatteryProfile->powerLimits.continuousPower;
     }
 
-    activePowerLimit = powerLimitsConfig()->burstPower;
+    activePowerLimit = currentBatteryProfile->powerLimits.burstPower;
 
-    uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
-    burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6;
-    burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6;
+    uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
+    burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6;
+    burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6;
 
     pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
     pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
@@ -118,7 +108,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui
     float spentReserveChunk = continuousDiff * timeDelta;
     *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax);
 
-    if (powerLimitsConfig()->burstCurrentFalldownTime) {
+    if (currentBatteryProfile->powerLimits.burstCurrentFalldownTime) {
         return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10;
     }
 
@@ -127,7 +117,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui
 
 void currentLimiterUpdate(timeDelta_t timeDelta) {
     activeCurrentLimit = calculateActiveLimit(getAmperage(),
-                            powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent,
+                            currentBatteryProfile->powerLimits.continuousCurrent, currentBatteryProfile->powerLimits.burstCurrent,
                             &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax,
                             timeDelta);
 }
@@ -135,7 +125,7 @@ void currentLimiterUpdate(timeDelta_t timeDelta) {
 #ifdef USE_ADC
 void powerLimiterUpdate(timeDelta_t timeDelta) {
     activePowerLimit = calculateActiveLimit(getPower(),
-                            powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower,
+                            currentBatteryProfile->powerLimits.continuousPower, currentBatteryProfile->powerLimits.burstPower,
                             &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax,
                             timeDelta);
 }
@@ -192,7 +182,6 @@ void powerLimiterApply(int16_t *throttleCommand) {
         currentThrottleCommand = currentThrAttned;
     } else {
         wasLimitingCurrent = false;
-        currentThrAttnIntegrator = 0;
         pt1FilterReset(&currentThrAttnFilter, 0);
 
         currentThrottleCommand = *throttleCommand;
@@ -222,7 +211,6 @@ void powerLimiterApply(int16_t *throttleCommand) {
         powerThrottleCommand = powerThrAttned;
     } else {
         wasLimitingPower = false;
-        powerThrAttnIntegrator = 0;
         pt1FilterReset(&powerThrAttnFilter, 0);
 
         powerThrottleCommand = *throttleCommand;
@@ -256,18 +244,18 @@ bool powerLimiterIsLimitingPower(void) {
 
 // returns seconds
 float powerLimiterGetRemainingBurstTime(void) {
-    uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
+    uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
     float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
 
 #ifdef USE_ADC
-    uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
+    uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
     float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
 
-    if (!powerLimitsConfig()->continuousCurrent) {
+    if (!currentBatteryProfile->powerLimits.continuousCurrent) {
         return remainingPowerBurstTime;
     }
 
-    if (!powerLimitsConfig()->continuousPower) {
+    if (!currentBatteryProfile->powerLimits.continuousPower) {
         return remainingCurrentBurstTime;
     }
 

src/main/flight/power_limits.h

@@ -25,23 +25,13 @@
 
 #include <common/time.h>
 
+#include "platform.h"
+
 #include "config/parameter_group.h"
 
 #if defined(USE_POWER_LIMITS)
 
 typedef struct {
-    uint16_t continuousCurrent;         // dA
-    uint16_t burstCurrent;              // dA
-    uint16_t burstCurrentTime;          // ds
-    uint16_t burstCurrentFalldownTime;  // ds
-
-#ifdef USE_ADC
-    uint16_t continuousPower;           // dW
-    uint16_t burstPower;                // dW
-    uint16_t burstPowerTime;            // ds
-    uint16_t burstPowerFalldownTime;    // ds
-#endif
-
     uint16_t piP;
     uint16_t piI;
 

src/main/flight/rth_estimator.c

@@ -77,9 +77,9 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
 // pitch in degrees
 // output in Watt
 static float estimatePitchPower(float pitch) {
-    int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle;
-    altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
-    const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
+    int16_t altitudeChangeThrottle = (int16_t)pitch * currentBatteryProfile->nav.fw.pitch_to_throttle;
+    altitudeChangeThrottle = constrain(altitudeChangeThrottle, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
+    const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (currentBatteryProfile->nav.fw.cruise_throttle - getThrottleIdleValue());
     return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
 }
 

src/main/flight/servos.c

@@ -490,6 +490,7 @@ void processServoAutotrimMode(void)
 #define SERVO_AUTOTRIM_CENTER_MIN       1300
 #define SERVO_AUTOTRIM_CENTER_MAX       1700
 #define SERVO_AUTOTRIM_UPDATE_SIZE      5
+#define SERVO_AUTOTRIM_ATIITUDE_LIMIT   50       // 5 degrees
 
 void processContinuousServoAutotrim(const float dT)
 {
@@ -497,21 +498,22 @@ void processContinuousServoAutotrim(const float dT)
     static servoAutotrimState_e trimState = AUTOTRIM_IDLE;    
     static uint32_t servoMiddleUpdateCount;
 
-    const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
-    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
-    const float targetRateMagnitude = getTotalRateTarget();
-    const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+    const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
 
     if (ARMING_FLAG(ARMED)) {
         trimState = AUTOTRIM_COLLECTING;
         if ((millis() - lastUpdateTimeMs) > 500) {
             const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
             const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
-            const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f;
+            const bool sticksAreCentered = !areSticksDeflected();
+            const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT 
+                                            && ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT;
             if (
                 planeIsFlyingStraight && 
                 noRotationCommanded && 
                 planeIsFlyingLevel &&
+                sticksAreCentered &&
                 !FLIGHT_MODE(MANUAL_MODE) && 
                 isGPSHeadingValid() // TODO: proper flying detection
             ) { 

src/main/flight/wind_estimator.c

@@ -72,7 +72,7 @@ float getEstimatedHorizontalWindSpeed(uint16_t *angle)
         }
         *angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle);
     }
-    return sqrtf(sq(xWindSpeed) + sq(yWindSpeed));
+    return fast_fsqrtf(sq(xWindSpeed) + sq(yWindSpeed));
 }
 
 void updateWindEstimator(timeUs_t currentTimeUs)
@@ -133,7 +133,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z];
 
         // estimate airspeed it using equation 6
-        float V = (sqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / sqrtf(diffLengthSq);
+        float V = (fast_fsqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / fast_fsqrtf(diffLengthSq);
 
         fuselageDirectionSum[X] = fuselageDirection[X] + lastFuselageDirection[X];
         fuselageDirectionSum[Y] = fuselageDirection[Y] + lastFuselageDirection[Y];
@@ -155,8 +155,8 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;// equation 11
         wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;// equation 12
 
-        float prevWindLength = sqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z]));
-        float windLength = sqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z]));
+        float prevWindLength = fast_fsqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z]));
+        float windLength = fast_fsqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z]));
 
         if (windLength < prevWindLength + 2000) {
             // TODO: Better filtering

src/main/io/beeper.c

@@ -132,6 +132,10 @@ static const uint8_t beep_launchModeBeep[] = {
 static const uint8_t beep_launchModeLowThrottleBeep[] = {
     5, 5, 5, 5, 3, 100, BEEPER_COMMAND_STOP
 };
+// 4 short beeps and a pause. Warning motor about to start at idle throttle
+static const uint8_t beep_launchModeIdleStartBeep[] = {
+    5, 5, 5, 5, 5, 5, 5, 80, BEEPER_COMMAND_STOP
+};
 // short beeps
 static const uint8_t beep_hardwareFailure[] = {
     10, 10, BEEPER_COMMAND_STOP
@@ -196,11 +200,12 @@ typedef struct beeperTableEntry_s {
     { BEEPER_ENTRY(BEEPER_USB,                      18, NULL,                           "ON_USB") },
     { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED,      19, beep_launchModeBeep,            "LAUNCH_MODE") },
     { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_LOW_THROTTLE, 20, beep_launchModeLowThrottleBeep, "LAUNCH_MODE_LOW_THROTTLE") },
-    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN,      21, beep_camOpenBeep,               "CAM_CONNECTION_OPEN") },
-    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE,     22, beep_camCloseBeep,              "CAM_CONNECTION_CLOSED") },
+    { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_IDLE_START,   21, beep_launchModeIdleStartBeep,   "LAUNCH_MODE_IDLE_START") },
+    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN,      22, beep_camOpenBeep,               "CAM_CONNECTION_OPEN") },
+    { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE,     23, beep_camCloseBeep,              "CAM_CONNECTION_CLOSED") },
 
-    { BEEPER_ENTRY(BEEPER_ALL,                      23, NULL,                           "ALL") },
-    { BEEPER_ENTRY(BEEPER_PREFERENCE,               24, NULL,                           "PREFERED") },
+    { BEEPER_ENTRY(BEEPER_ALL,                      24, NULL,                           "ALL") },
+    { BEEPER_ENTRY(BEEPER_PREFERENCE,               25, NULL,                           "PREFERED") },
 };
 
 static const beeperTableEntry_t *currentBeeperEntry = NULL;
@@ -339,8 +344,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
 
     if (!beeperIsOn) {
 #ifdef USE_DSHOT
-        if (!areMotorsRunning()
-            && beeperConfig()->dshot_beeper_enabled
+        if (isMotorProtocolDshot() && !areMotorsRunning() && beeperConfig()->dshot_beeper_enabled
             && currentTimeUs - lastDshotBeeperCommandTimeUs > getDShotBeaconGuardDelayUs())
         {
             lastDshotBeeperCommandTimeUs = currentTimeUs;

src/main/io/beeper.h

@@ -44,6 +44,7 @@ typedef enum {
     BEEPER_USB,                         // Some boards have beeper powered USB connected
     BEEPER_LAUNCH_MODE_ENABLED,         // Fixed-wing launch mode enabled
     BEEPER_LAUNCH_MODE_LOW_THROTTLE,    // Fixed-wing launch mode enabled, but throttle is low
+    BEEPER_LAUNCH_MODE_IDLE_START,      // Fixed-wing launch mode enabled, motor about to start at idle after set delay
     BEEPER_CAM_CONNECTION_OPEN,         // When the 5 key simulation stated
     BEEPER_CAM_CONNECTION_CLOSE,        // When the 5 key simulation stop
 

src/main/io/gps_msp.c

@@ -89,7 +89,7 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr)
     gpsSol.velNED[X] = pkt->nedVelNorth;
     gpsSol.velNED[Y] = pkt->nedVelEast;
     gpsSol.velNED[Z] = pkt->nedVelDown;
-    gpsSol.groundSpeed = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
+    gpsSol.groundSpeed = fast_fsqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
     gpsSol.groundCourse = pkt->groundCourse / 10;   // in deg * 10
     gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
     gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);

src/main/io/gps_naza.c

@@ -199,7 +199,7 @@ static bool NAZA_parse_gps(void)
         gpsSol.eph = gpsConstrainEPE(h_acc / 10);   // hAcc in cm
         gpsSol.epv = gpsConstrainEPE(v_acc / 10);   // vAcc in cm
         gpsSol.numSat = _buffernaza.nav.satellites;
-        gpsSol.groundSpeed = sqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s
+        gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s
 
         // calculate gps heading from VELNE
         gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f));

src/main/io/osd.c

@@ -194,7 +194,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 4);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
 
 static int digitCount(int32_t value)
@@ -449,29 +449,36 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
 */
 void osdFormatAltitudeSymbol(char *buff, int32_t alt)
 {
+    int digits;
+    if (alt < 0) {
+        digits = 4;
+    } else {
+        digits = 3;
+        buff[0] = ' ';
+    }
     switch ((osd_unit_e)osdConfig()->units) {
         case OSD_UNIT_UK:
             FALLTHROUGH;
         case OSD_UNIT_IMPERIAL:
-            if (osdFormatCentiNumber(buff , CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) {
+            if (osdFormatCentiNumber(buff + 4 - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) {
                 // Scaled to kft
-                buff[3] = SYM_ALT_KFT;
+                buff[4] = SYM_ALT_KFT;
             } else {
                 // Formatted in feet
-                buff[3] = SYM_ALT_FT;
+                buff[4] = SYM_ALT_FT;
             }
-            buff[4] = '\0';
+            buff[5] = '\0';
             break;
         case OSD_UNIT_METRIC:
             // alt is alredy in cm
-            if (osdFormatCentiNumber(buff, alt, 1000, 0, 2, 3)) {
+            if (osdFormatCentiNumber(buff + 4 - digits, alt, 1000, 0, 2, digits)) {
                 // Scaled to km
-                buff[3] = SYM_ALT_KM;
+                buff[4] = SYM_ALT_KM;
             } else {
                 // Formatted in m
-                buff[3] = SYM_ALT_M;
+                buff[4] = SYM_ALT_M;
             }
-            buff[4] = '\0';
+            buff[5] = '\0';
             break;
     }
 }
@@ -1383,6 +1390,25 @@ static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY,
     displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr);
 }
 
+int8_t getGeoWaypointNumber(int8_t waypointIndex)
+{
+    static int8_t lastWaypointIndex = 1;
+    static int8_t geoWaypointIndex;
+
+    if (waypointIndex != lastWaypointIndex) {
+        lastWaypointIndex = geoWaypointIndex = waypointIndex;
+        for (uint8_t i = 0; i <= waypointIndex; i++) {
+            if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
+                posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
+                posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) {
+                    geoWaypointIndex -= 1;
+            }
+        }
+    }
+
+    return geoWaypointIndex + 1;
+}
+
 static bool osdDrawSingleElement(uint8_t item)
 {
     uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
@@ -1495,6 +1521,27 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
+    case OSD_GLIDESLOPE:
+        {
+            float horizontalSpeed = gpsSol.groundSpeed;
+            float sinkRate = -getEstimatedActualVelocity(Z);
+            static pt1Filter_t gsFilterState;
+            const timeMs_t currentTimeMs = millis();
+            static timeMs_t gsUpdatedTimeMs;
+            float glideSlope = horizontalSpeed / sinkRate;
+            glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs));
+            gsUpdatedTimeMs = currentTimeMs;
+
+            buff[0] = SYM_GLIDESLOPE;
+            if (glideSlope > 0.0f && glideSlope < 100.0f) {
+                osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3);
+            } else {
+                buff[1] = buff[2] = buff[3] = '-';
+            }
+            buff[4] = '\0';
+            break;
+        }
+
     case OSD_GPS_LAT:
         osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat);
         break;
@@ -1858,6 +1905,8 @@ static bool osdDrawSingleElement(uint8_t item)
             }
             if (!failsafeIsReceivingRxData()){
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
             break;
         }
@@ -1984,8 +2033,9 @@ static bool osdDrawSingleElement(uint8_t item)
                         fpVector3_t poi;
                         geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3));
                         int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude();
-                        while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
-                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i);
+                        j = getGeoWaypointNumber(j);
+                        while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more (48 = ascii 0)
+                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 48 + j, i);
                     }
                 }
             }
@@ -2028,7 +2078,8 @@ static bool osdDrawSingleElement(uint8_t item)
             rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
             pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
 #endif
-            pitchAngle -= DEGREES_TO_RADIANS(osdConfig()->camera_uptilt);
+            pitchAngle -= osdConfig()->ahi_camera_uptilt_comp ? DEGREES_TO_RADIANS(osdConfig()->camera_uptilt) : 0;
+            pitchAngle += DEGREES_TO_RADIANS(getFixedWingLevelTrim());
             if (osdConfig()->ahi_reverse_roll) {
                 rollAngle = -rollAngle;
             }
@@ -2193,15 +2244,15 @@ static bool osdDrawSingleElement(uint8_t item)
         return true;
 
     case OSD_NAV_FW_CRUISE_THR:
-        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
         return true;
 
     case OSD_NAV_FW_PITCH2THR:
-        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR);
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR);
         return true;
 
     case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
-        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)currentBatteryProfile->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
         return true;
 
     case OSD_FW_ALT_PID_OUTPUTS:
@@ -2283,7 +2334,7 @@ static bool osdDrawSingleElement(uint8_t item)
             dateTime_t dateTime;
             rtcGetDateTimeLocal(&dateTime);
             buff[0] = SYM_CLOCK;
-            tfp_sprintf(buff + 1, "%02u:%02u", dateTime.hours, dateTime.minutes);
+            tfp_sprintf(buff + 1, "%02u:%02u:%02u", dateTime.hours, dateTime.minutes, dateTime.seconds);
             break;
         }
 
@@ -2729,7 +2780,7 @@ static bool osdDrawSingleElement(uint8_t item)
         break;
 
     case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT:
-        if (powerLimitsConfig()->continuousCurrent) {
+        if (currentBatteryProfile->powerLimits.continuousCurrent) {
             osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3);
             buff[3] = SYM_AMP;
             buff[4] = '\0';
@@ -2743,7 +2794,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #ifdef USE_ADC
     case OSD_PLIMIT_ACTIVE_POWER_LIMIT:
         {
-            if (powerLimitsConfig()->continuousPower) {
+            if (currentBatteryProfile->powerLimits.continuousPower) {
                 bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3);
                 buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
                 buff[4] = '\0';
@@ -2881,6 +2932,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT,
     .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT,
     .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT,
+    .rssi_dbm_alarm = SETTING_OSD_RSSI_DBM_ALARM_DEFAULT,
 #endif
 #ifdef USE_TEMPERATURE_SENSOR
     .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT,
@@ -2894,6 +2946,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT,
     .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT,
     .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT,
+    .ahi_camera_uptilt_comp = SETTING_OSD_AHI_CAMERA_UPTILT_COMP_DEFAULT,
     .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT,
     .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT,
     .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT,
@@ -2956,6 +3009,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
     osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);
     osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1);
+    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);
@@ -2981,17 +3035,17 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     // OSD_VARIO_NUM at the right of OSD_VARIO
     osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7);
     osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11);
-    osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
-    osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6);
+    osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6);
 
     osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
     osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
 
 #ifdef USE_SERIALRX_CRSF
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(24, 12);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(24, 11);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(25, 9);
-    osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(25, 10);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9);
+    osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10);
 #endif
 
     osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
@@ -3544,7 +3598,7 @@ static void osdFilterData(timeUs_t currentTimeUs) {
     static timeUs_t lastRefresh = 0;
     float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
 
-    GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
+    GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
     for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
 
     if (lastRefresh) {
@@ -3828,7 +3882,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         // Countdown display for remaining Waypoints
                         char buf[6];
                         osdFormatDistanceSymbol(buf, posControl.wpDistance, 0);
-                        tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", posControl.activeWaypointIndex + 1, posControl.waypointCount, buf);
+                        tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf);
                         messages[messageCount++] = messageBuf;
                     } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
                         // WP hold time countdown in seconds
@@ -3864,7 +3918,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         // by OSD_FLYMODE.
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
                     }
-                    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
+                    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
                     }
                     if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {

src/main/io/osd.h

@@ -231,6 +231,7 @@ typedef enum {
     OSD_PLIMIT_REMAINING_BURST_TIME,
     OSD_PLIMIT_ACTIVE_CURRENT_LIMIT,
     OSD_PLIMIT_ACTIVE_POWER_LIMIT,
+    OSD_GLIDESLOPE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -312,6 +313,7 @@ typedef struct osdConfig_s {
 #ifdef USE_SERIALRX_CRSF
     int8_t snr_alarm; //CRSF SNR alarm in dB
     int8_t link_quality_alarm;
+    int16_t rssi_dbm_alarm; // in dBm
 #endif
 #ifdef USE_BARO
     int16_t baro_temp_alarm_min;
@@ -331,6 +333,7 @@ typedef struct osdConfig_s {
     uint8_t crosshairs_style; // from osd_crosshairs_style_e
     int8_t horizon_offset;
     int8_t camera_uptilt;
+    bool ahi_camera_uptilt_comp;
     uint8_t camera_fov_h;
     uint8_t camera_fov_v;
     uint8_t hud_margin_h;

src/main/io/osd_common.c

@@ -195,6 +195,6 @@ int16_t osdGet3DSpeed(void)
 {
     int16_t vert_speed = getEstimatedActualVelocity(Z);
     int16_t hor_speed = gpsSol.groundSpeed;
-    return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
+    return (int16_t)fast_fsqrtf(sq(hor_speed) + sq(vert_speed));
 }
 #endif

src/main/io/osd_dji_hd.c

@@ -49,6 +49,7 @@
 #include "fc/fc_msp_box.h"
 #include "fc/runtime_config.h"
 #include "fc/settings.h"
+#include "fc/rc_adjustments.h"
 
 #include "flight/imu.h"
 #include "flight/pid.h"
@@ -70,6 +71,7 @@
 #include "sensors/esc_sensor.h"
 #include "sensors/temperature.h"
 #include "sensors/pitotmeter.h"
+#include "sensors/boardalignment.h"
 
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
@@ -125,8 +127,25 @@ PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
     .use_name_for_messages  = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT,
     .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT,
     .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT,
+    .messageSpeedSource = SETTING_DJI_MESSAGE_SPEED_SOURCE_DEFAULT,
+    .rssi_source = SETTING_DJI_RSSI_SOURCE_DEFAULT,
+    .useAdjustments = SETTING_DJI_USE_ADJUSTMENTS_DEFAULT,
+    .craftNameAlternatingDuration = SETTING_DJI_CN_ALTERNATING_DURATION_DEFAULT
 );
 
+#define RSSI_BOUNDARY(PERCENT) (RSSI_MAX_VALUE / 100 * PERCENT)
+
+typedef enum {
+    DJI_OSD_CN_MESSAGES,
+    DJI_OSD_CN_THROTTLE,
+    DJI_OSD_CN_THROTTLE_AUTO_THR,
+    DJI_OSD_CN_AIR_SPEED,
+    DJI_OSD_CN_EFFICIENCY,
+    DJI_OSD_CN_DISTANCE,
+    DJI_OSD_CN_ADJUSTEMNTS,
+    DJI_OSD_CN_MAX_ELEMENTS
+} DjiCraftNameElements_t;
+
 // External dependency on looptime
 extern timeDelta_t cycleTime;
 
@@ -413,7 +432,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
     //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
 }
 
-static const char * osdArmingDisabledReasonMessage(void)
+static char * osdArmingDisabledReasonMessage(void)
 {
     switch (isArmingDisabledReason()) {
         case ARMING_DISABLED_FAILSAFE_SYSTEM:
@@ -522,7 +541,7 @@ static const char * osdArmingDisabledReasonMessage(void)
     return NULL;
 }
 
-static const char * osdFailsafePhaseMessage(void)
+static char * osdFailsafePhaseMessage(void)
 {
     // See failsafe.h for each phase explanation
     switch (failsafePhase()) {
@@ -563,7 +582,7 @@ static const char * osdFailsafePhaseMessage(void)
     return NULL;
 }
 
-static const char * osdFailsafeInfoMessage(void)
+static char * osdFailsafeInfoMessage(void)
 {
     if (failsafeIsReceivingRxData()) {
         // User must move sticks to exit FS mode
@@ -573,7 +592,7 @@ static const char * osdFailsafeInfoMessage(void)
     return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
 }
 
-static const char * navigationStateMessage(void)
+static char * navigationStateMessage(void)
 {
     switch (NAV_Status.state) {
         case MW_NAV_STATE_NONE:
@@ -649,16 +668,35 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
  * Converts velocity into a string based on the current unit system.
  * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
  */
-void osdDJIFormatVelocityStr(char* buff, int32_t vel )
+void osdDJIFormatVelocityStr(char* buff)
 {
+    char sourceBuf[4];
+    int vel = 0;
+    switch (djiOsdConfig()->messageSpeedSource) {
+        case OSD_SPEED_SOURCE_GROUND:
+            strcpy(sourceBuf, "GRD");
+            vel = gpsSol.groundSpeed;
+            break;
+        case OSD_SPEED_SOURCE_3D:
+            strcpy(sourceBuf, "3D");
+            vel = osdGet3DSpeed();
+            break;
+        case OSD_SPEED_SOURCE_AIR:
+            strcpy(sourceBuf, "AIR");
+#ifdef USE_PITOT
+            vel = pitot.airSpeed;
+#endif
+            break;
+    }
+
     switch (osdConfig()->units) {
         case OSD_UNIT_UK:
             FALLTHROUGH;
         case OSD_UNIT_IMPERIAL:
-            tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH");
+            tfp_sprintf(buff, "%s %3d MPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
             break;
         case OSD_UNIT_METRIC:
-            tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH");
+            tfp_sprintf(buff, "%s %3d KPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
             break;
     }
 }
@@ -726,79 +764,214 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
                 1, US2S(efficiencyTimeDelta));
 
             efficiencyUpdated = currentTimeUs;
-        }
-        else {
+        } else {
             value = eFilterState.state;
         }
     }
 
     if (value > 0 && value <= 999) {
         tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM");
-    }
-    else {
+    } else {
         tfp_sprintf(buff, "%s", "---mAhKM");
     }
 }
 
-static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
+static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
 {
-    // :W T S E D
-    //  | | | | Distance Trip
-    //  | | | Efficiency mA/KM
-    //  | | S 3dSpeed
-    //  | Throttle
-    //  Warnings
-    const char *message = " ";
-    const char *enabledElements = name + 1;
-    char djibuf[24];
-
-    // clear name from chars : and leading W
-    if (enabledElements[0] == 'W') {
-        enabledElements += 1;
-    }
-
-    int elemLen = strlen(enabledElements);
-
-    if (elemLen > 0) {
-        switch (enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen)]){
-            case 'T':
-                osdDJIFormatThrottlePosition(djibuf,true);
+    switch (adjustmentFunction) {
+        case ADJUSTMENT_RC_EXPO:
+            tfp_sprintf(buff, "RCE %d", currentControlRateProfile->stabilized.rcExpo8);
             break;
-            case 'S':
-                osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed());
+        case ADJUSTMENT_RC_YAW_EXPO:
+            tfp_sprintf(buff, "RCYE %3d", currentControlRateProfile->stabilized.rcYawExpo8);
             break;
-            case 'E':
-                osdDJIEfficiencyMahPerKM(djibuf);
+        case ADJUSTMENT_MANUAL_RC_EXPO:
+            tfp_sprintf(buff, "MRCE %3d", currentControlRateProfile->manual.rcExpo8);
             break;
-            case 'D':
-                osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
+        case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
+            tfp_sprintf(buff, "MRCYE %3d", currentControlRateProfile->manual.rcYawExpo8);
             break;
-            case 'W':
-                tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST");
+        case ADJUSTMENT_THROTTLE_EXPO:
+            tfp_sprintf(buff, "TE %3d", currentControlRateProfile->throttle.rcExpo8);
+            break;
+        case ADJUSTMENT_PITCH_ROLL_RATE:
+            tfp_sprintf(buff, "PRR %3d %3d", currentControlRateProfile->stabilized.rates[FD_PITCH], currentControlRateProfile->stabilized.rates[FD_ROLL]);
+            break;
+        case ADJUSTMENT_PITCH_RATE:
+            tfp_sprintf(buff, "PR %3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
+            break;
+        case ADJUSTMENT_ROLL_RATE:
+            tfp_sprintf(buff, "RR %3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
+            break;
+        case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
+            tfp_sprintf(buff, "MPRR %3d %3d", currentControlRateProfile->manual.rates[FD_PITCH], currentControlRateProfile->manual.rates[FD_ROLL]);
+            break;
+        case ADJUSTMENT_MANUAL_PITCH_RATE:
+            tfp_sprintf(buff, "MPR %3d", currentControlRateProfile->manual.rates[FD_PITCH]);
+            break;
+        case ADJUSTMENT_MANUAL_ROLL_RATE:
+            tfp_sprintf(buff, "MRR %3d", currentControlRateProfile->manual.rates[FD_ROLL]);
+            break;
+        case ADJUSTMENT_YAW_RATE:
+            tfp_sprintf(buff, "YR %3d", currentControlRateProfile->stabilized.rates[FD_YAW]);
+            break;
+        case ADJUSTMENT_MANUAL_YAW_RATE:
+            tfp_sprintf(buff, "MYR %3d", currentControlRateProfile->manual.rates[FD_YAW]);
+            break;
+        case ADJUSTMENT_PITCH_ROLL_P:
+            tfp_sprintf(buff, "PRP %3d %3d", pidBankMutable()->pid[PID_PITCH].P, pidBankMutable()->pid[PID_ROLL].P);
+            break;
+        case ADJUSTMENT_PITCH_P:
+            tfp_sprintf(buff, "PP %3d", pidBankMutable()->pid[PID_PITCH].P);
+            break;
+        case ADJUSTMENT_ROLL_P:
+            tfp_sprintf(buff, "RP %3d", pidBankMutable()->pid[PID_ROLL].P);
+            break;
+        case ADJUSTMENT_PITCH_ROLL_I:
+            tfp_sprintf(buff, "PRI %3d %3d", pidBankMutable()->pid[PID_PITCH].I, pidBankMutable()->pid[PID_ROLL].I);
+            break;
+        case ADJUSTMENT_PITCH_I:
+            tfp_sprintf(buff, "PI %3d", pidBankMutable()->pid[PID_PITCH].I);
+            break;
+        case ADJUSTMENT_ROLL_I:
+            tfp_sprintf(buff, "RI %3d", pidBankMutable()->pid[PID_ROLL].I);
+            break;
+        case ADJUSTMENT_PITCH_ROLL_D:
+            tfp_sprintf(buff, "PRD %3d %3d", pidBankMutable()->pid[PID_PITCH].D, pidBankMutable()->pid[PID_ROLL].D);
+            break;
+        case ADJUSTMENT_PITCH_ROLL_FF:
+            tfp_sprintf(buff, "PRFF %3d %3d", pidBankMutable()->pid[PID_PITCH].FF, pidBankMutable()->pid[PID_ROLL].FF);
+            break;
+        case ADJUSTMENT_PITCH_D:
+            tfp_sprintf(buff, "PD %3d", pidBankMutable()->pid[PID_PITCH].D);
+            break;
+        case ADJUSTMENT_PITCH_FF:
+            tfp_sprintf(buff, "PFF %3d", pidBankMutable()->pid[PID_PITCH].FF);
+            break;
+        case ADJUSTMENT_ROLL_D:
+            tfp_sprintf(buff, "RD %3d", pidBankMutable()->pid[PID_ROLL].D);
+            break;
+        case ADJUSTMENT_ROLL_FF:
+            tfp_sprintf(buff, "RFF %3d", pidBankMutable()->pid[PID_ROLL].FF);
+            break;
+        case ADJUSTMENT_YAW_P:
+            tfp_sprintf(buff, "YP %3d", pidBankMutable()->pid[PID_YAW].P);
+            break;
+        case ADJUSTMENT_YAW_I:
+            tfp_sprintf(buff, "YI  %3d", pidBankMutable()->pid[PID_YAW].I);
+            break;
+        case ADJUSTMENT_YAW_D:
+            tfp_sprintf(buff, "YD %3d", pidBankMutable()->pid[PID_YAW].D);
+            break;
+        case ADJUSTMENT_YAW_FF:
+            tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF);
+            break;
+        case ADJUSTMENT_NAV_FW_CRUISE_THR:
+            tfp_sprintf(buff, "CR %4d", currentBatteryProfileMutable->nav.fw.cruise_throttle);
+            break;
+        case ADJUSTMENT_NAV_FW_PITCH2THR:
+            tfp_sprintf(buff, "P2T %3d", currentBatteryProfileMutable->nav.fw.pitch_to_throttle);
+            break;
+        case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
+            tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees);
+            break;
+        case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
+            tfp_sprintf(buff, "PBA %3d", boardAlignment()->pitchDeciDegrees);
+            break;
+        case ADJUSTMENT_LEVEL_P:
+            tfp_sprintf(buff, "LP %3d", pidBankMutable()->pid[PID_LEVEL].P);
+            break;
+        case ADJUSTMENT_LEVEL_I:
+            tfp_sprintf(buff, "LI %3d", pidBankMutable()->pid[PID_LEVEL].I);
+            break;
+        case ADJUSTMENT_LEVEL_D:
+            tfp_sprintf(buff, "LD %3d", pidBankMutable()->pid[PID_LEVEL].D);
+            break;
+        case ADJUSTMENT_POS_XY_P:
+            tfp_sprintf(buff, "PXYP %3d", pidBankMutable()->pid[PID_POS_XY].P);
+            break;
+        case ADJUSTMENT_POS_XY_I:
+            tfp_sprintf(buff, "PXYI %3d", pidBankMutable()->pid[PID_POS_XY].I);
+            break;
+        case ADJUSTMENT_POS_XY_D:
+            tfp_sprintf(buff, "PXYD %3d", pidBankMutable()->pid[PID_POS_XY].D);
+            break;
+        case ADJUSTMENT_POS_Z_P:
+            tfp_sprintf(buff, "PZP %3d", pidBankMutable()->pid[PID_POS_Z].P);
+            break;
+        case ADJUSTMENT_POS_Z_I:
+            tfp_sprintf(buff, "PZI %3d", pidBankMutable()->pid[PID_POS_Z].I);
+            break;
+        case ADJUSTMENT_POS_Z_D:
+            tfp_sprintf(buff, "PZD %3d", pidBankMutable()->pid[PID_POS_Z].D);
+            break;
+        case ADJUSTMENT_HEADING_P:
+            tfp_sprintf(buff, "HP %3d", pidBankMutable()->pid[PID_HEADING].P);
+            break;
+        case ADJUSTMENT_VEL_XY_P:
+            tfp_sprintf(buff, "VXYP %3d", pidBankMutable()->pid[PID_VEL_XY].P);
+            break;
+        case ADJUSTMENT_VEL_XY_I:
+            tfp_sprintf(buff, "VXYI %3d", pidBankMutable()->pid[PID_VEL_XY].I);
+            break;
+        case ADJUSTMENT_VEL_XY_D:
+            tfp_sprintf(buff, "VXYD %3d", pidBankMutable()->pid[PID_VEL_XY].D);
+            break;
+        case ADJUSTMENT_VEL_Z_P:
+            tfp_sprintf(buff, "VZP %3d", pidBankMutable()->pid[PID_VEL_Z].P);
+            break;
+        case ADJUSTMENT_VEL_Z_I:
+            tfp_sprintf(buff, "VZI %3d", pidBankMutable()->pid[PID_VEL_Z].I);
+            break;
+        case ADJUSTMENT_VEL_Z_D:
+            tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
+            break;
+        case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
+            tfp_sprintf(buff, "MTDPA %4d", currentBatteryProfileMutable->fwMinThrottleDownPitchAngle);
+            break;
+        case ADJUSTMENT_TPA:
+            tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);
+            break;
+        case ADJUSTMENT_TPA_BREAKPOINT:
+            tfp_sprintf(buff, "TPABP %4d", currentControlRateProfile->throttle.pa_breakpoint);
+            break;
+        case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
+            tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness);
             break;
         default:
-                tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM");
+            tfp_sprintf(buff, "UNSUPPORTED");
             break;
     }
+}
 
-        if (djibuf[0] != '\0') {
-            message = djibuf;
-        }
+static bool osdDJIFormatAdjustments(char *buff)
+{
+    uint8_t adjustmentFunctions[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
+    uint8_t adjustmentCount = getActiveAdjustmentFunctions(adjustmentFunctions);
+
+    if (adjustmentCount > 0 && buff != NULL) {
+        osdDJIAdjustmentMessage(buff, adjustmentFunctions[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, adjustmentCount)]);
     }
 
-    if (name[1] == 'W') {
+    return adjustmentCount > 0;
+}
+
+
+static bool djiFormatMessages(char *buff)
+{
+    bool haveMessage = false;
     char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
     if (ARMING_FLAG(ARMED)) {
-            // Aircraft is armed. We might have up to 5
+        // Aircraft is armed. We might have up to 6
         // messages to show.
-            const char *messages[5];
+        char *messages[6];
         unsigned messageCount = 0;
 
         if (FLIGHT_MODE(FAILSAFE_MODE)) {
             // In FS mode while being armed too
-                const char *failsafePhaseMessage = osdFailsafePhaseMessage();
-                const char *failsafeInfoMessage = osdFailsafeInfoMessage();
-                const char *navStateFSMessage = navigationStateMessage();
+            char *failsafePhaseMessage = osdFailsafePhaseMessage();
+            char *failsafeInfoMessage = osdFailsafeInfoMessage();
+            char *navStateFSMessage = navigationStateMessage();
 
             if (failsafePhaseMessage) {
                 messages[messageCount++] = failsafePhaseMessage;
@@ -811,33 +984,20 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
             if (navStateFSMessage) {
                 messages[messageCount++] = navStateFSMessage;
             }
-
-                if (messageCount > 0) {
-                    message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
-                    if (message == failsafeInfoMessage) {
-                        // failsafeInfoMessage is not useful for recovering
-                        // a lost model, but might help avoiding a crash.
-                        // Blink to grab user attention.
-                    //doesnt work   TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        } else {
+#ifdef USE_SERIALRX_CRSF
+            if (rxLinkStatistics.rfMode == 0) {
+                messages[messageCount++] = "CRSF LOW RF";
             }
-                    // We're shoing either failsafePhaseMessage or
-                    // navStateFSMessage. Don't BLINK here since
-                    // having this text available might be crucial
-                    // during a lost aircraft recovery and blinking
-                    // will cause it to be missing from some frames.
-                }
-            }
-            else {
+#endif
             if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-                    const char *navStateMessage = navigationStateMessage();
+                char *navStateMessage = navigationStateMessage();
                 if (navStateMessage) {
                     messages[messageCount++] = navStateMessage;
                 }
-                }
-                else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
+            } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
                 messages[messageCount++] = "AUTOLAUNCH";
-                }
-                else {
+            } else {
                 if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
                     // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
                     // when it doesn't require ANGLE mode (required only in FW
@@ -845,56 +1005,121 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
                     // by OSD_FLYMODE.
                     messages[messageCount++] = "(ALT HOLD)";
                 }
-                    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
+
+                if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
                     messages[messageCount++] = "(AUTOTRIM)";
                 }
+
                 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
                     messages[messageCount++] = "(AUTOTUNE)";
                 }
+
+                if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) {
+                    messages[messageCount++] = "(AUTOLEVEL)";
+                }
+
                 if (FLIGHT_MODE(HEADFREE_MODE)) {
                     messages[messageCount++] = "(HEADFREE)";
                 }
+
+                if (FLIGHT_MODE(MANUAL_MODE)) {
+                    messages[messageCount++] = "(MANUAL)";
+                }
+            }
         }
         // Pick one of the available messages. Each message lasts
         // a second.
         if (messageCount > 0) {
-                    message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
+           strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);;
+           haveMessage = true;
         }
-            }
-        }
-        else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
+    } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
         unsigned invalidIndex;
         // Check if we're unable to arm for some reason
         if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
-                if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
+            if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
                 const setting_t *setting = settingGet(invalidIndex);
                 settingGetName(setting, messageBuf);
                 for (int ii = 0; messageBuf[ii]; ii++) {
                     messageBuf[ii] = sl_toupper(messageBuf[ii]);
                 }
-                    message = messageBuf;
-                }
-                else {
-                    message = "ERR SETTING";
+                strcpy(buff, messageBuf);
+            } else {
+                strcpy(buff, "ERR SETTING");
                 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
             }
-            }
-            else {
-                if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
-                    message = "CANT ARM";
+        } else {
+            if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
+                strcpy(buff, "CANT ARM");
                 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
             } else {
                 // Show the reason for not arming
-                    message = osdArmingDisabledReasonMessage();
+                strcpy(buff, osdArmingDisabledReasonMessage());
             }
         }
+        haveMessage = true;
+    }
+    return haveMessage;
+}
+
+static void djiSerializeCraftNameOverride(sbuf_t *dst)
+{
+    char djibuf[DJI_CRAFT_NAME_LENGTH] = "\0";
+    uint16_t *osdLayoutConfig = (uint16_t*)(osdLayoutsConfig()->item_pos[0]);
+
+    if (!(OSD_VISIBLE(osdLayoutConfig[OSD_MESSAGES]) && djiFormatMessages(djibuf))
+        && !(djiOsdConfig()->useAdjustments && osdDJIFormatAdjustments(djibuf))) {
+
+        DjiCraftNameElements_t activeElements[DJI_OSD_CN_MAX_ELEMENTS];
+        uint8_t activeElementsCount = 0;
+
+        if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS])) {
+            activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE;
+        }
+
+        if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS_AUTO_THR])) {
+            activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR;
+        }
+
+        if (OSD_VISIBLE(osdLayoutConfig[OSD_3D_SPEED])) {
+            activeElements[activeElementsCount++] = DJI_OSD_CN_AIR_SPEED;
+        }
+
+        if (OSD_VISIBLE(osdLayoutConfig[OSD_EFFICIENCY_MAH_PER_KM])) {
+            activeElements[activeElementsCount++] = DJI_OSD_CN_EFFICIENCY;
+        }
+
+        if (OSD_VISIBLE(osdLayoutConfig[OSD_TRIP_DIST])) {
+            activeElements[activeElementsCount++] = DJI_OSD_CN_DISTANCE;
+        }
+
+        switch (activeElements[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, activeElementsCount)])
+        {
+            case DJI_OSD_CN_THROTTLE:
+                osdDJIFormatThrottlePosition(djibuf, false);
+                break;
+            case DJI_OSD_CN_THROTTLE_AUTO_THR:
+                osdDJIFormatThrottlePosition(djibuf, true);
+                break;
+            case DJI_OSD_CN_AIR_SPEED:
+                osdDJIFormatVelocityStr(djibuf);
+                break;
+            case DJI_OSD_CN_EFFICIENCY:
+                osdDJIEfficiencyMahPerKM(djibuf);
+                break;
+            case DJI_OSD_CN_DISTANCE:
+                osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
+                break;
+            default:
+                break;
         }
     }
 
-    if (message[0] != '\0') {
-        sbufWriteData(dst, message, strlen(message));
+    if (djibuf[0] != '\0') {
+        sbufWriteData(dst, djibuf, strlen(djibuf));
     }
 }
+
 #endif
 
 
@@ -935,14 +1160,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
 
 #if defined(USE_OSD)
                 if (djiOsdConfig()->use_name_for_messages)  {
-                    if (name[0] == ':') {
-                        // If craft name starts with a semicolon - use it as a template for what we want to show
-                        djiSerializeCraftNameOverride(dst, name);
-                    }
-                    else {
-                        // Otherwise fall back to just warnings
-                        djiSerializeCraftNameOverride(dst, ":W");
-                    }
+                    djiSerializeCraftNameOverride(dst);
                 }
                 else
 #endif
@@ -1027,7 +1245,22 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
         case DJI_MSP_ANALOG:
             sbufWriteU8(dst,  constrain(getBatteryVoltage() / 10, 0, 255));
             sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
+#ifdef USE_SERIALRX_CRSF
+            // Range of RSSI field: 0-99: 99 = 150 hz , 0 - 98 50 hz / 4 hz
+            if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ) {
+                uint16_t scaledLq = 0;
+                if (rxLinkStatistics.rfMode >= 2) {
+                    scaledLq = RSSI_MAX_VALUE;
+                } else {
+                    scaledLq = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 0, RSSI_BOUNDARY(98));
+                }
+                sbufWriteU16(dst, scaledLq);
+            } else {
+#endif
                 sbufWriteU16(dst, getRSSI());
+#ifdef USE_SERIALRX_CRSF
+            }
+#endif
             sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
             sbufWriteU16(dst, getBatteryVoltage());
             break;

src/main/io/osd_dji_hd.h

@@ -62,12 +62,21 @@
 #define DJI_MSP_SET_PID                 202
 #define DJI_MSP_SET_RC_TUNING           204
 
+#define DJI_CRAFT_NAME_LENGTH           24
+#define DJI_ALTERNATING_DURATION_LONG   (djiOsdConfig()->craftNameAlternatingDuration * 100)
+#define DJI_ALTERNATING_DURATION_SHORT  1000
+
 enum djiOsdTempSource_e {
     DJI_OSD_TEMP_ESC    = 0,
     DJI_OSD_TEMP_CORE   = 1,
     DJI_OSD_TEMP_BARO   = 2
 };
 
+enum djiRssiSource_e {
+    DJI_RSSI = 0,
+    DJI_CRSF_LQ = 1
+};
+
 enum djiOsdProtoWorkarounds_e {
     DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA    = 1 << 0,
 };
@@ -76,6 +85,10 @@ typedef struct djiOsdConfig_s {
     uint8_t use_name_for_messages;
     uint8_t esc_temperature_source;
     uint8_t proto_workarounds;
+    uint8_t messageSpeedSource;
+    uint8_t rssi_source;
+    uint8_t useAdjustments;
+    uint8_t craftNameAlternatingDuration;
 } djiOsdConfig_t;
 
 PG_DECLARE(djiOsdConfig_t, djiOsdConfig);

src/main/navigation/navigation.c

@@ -96,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -143,7 +143,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     // MC-specific
     .mc = {
         .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT,                          // degrees
-        .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
         .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT,                // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed
 
 #ifdef USE_MR_BRAKING_MODE
@@ -167,12 +166,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT,                    // degrees
         .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT,                  // degrees
         .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT,                    // degrees
-        .cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT,
         .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT,                    // cm/s
         .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
-        .max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT,
-        .min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT,
-        .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT,                  // pwm units per degree of pitch (10pwm units ~ 1% throttle)
         .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
         .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
         .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT,                  // 75m
@@ -184,9 +179,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT,       // 3 m/s
         .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT,             // cm/s/s (1.9*G)
         .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT,        // 40ms
-        .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
-        .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT,         // Motor idle or MOTOR_STOP
         .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT,        // ms
+        .launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT,   // ms
         .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT,  // ms, time to gredually increase throttle from idle to launch
         .launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT,              // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
         .launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT,              // ms, min time in launch mode
@@ -1042,7 +1036,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
         timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
         if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
         float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
-        float centidegsPerIteration = rateTarget * timeDifference / 1000.0f;
+        float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
         posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
         DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
         posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
@@ -1740,7 +1734,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
     //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
     if (feature(FEATURE_FW_LAUNCH)) {
         throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
-        if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
+        if ((throttleStatus == THROTTLE_LOW) && (isRollPitchStickDeflected())) {
             abortFixedWingLaunch();
             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
         }
@@ -1946,7 +1940,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
     posControl.actualState.agl.vel.x = newVelX;
     posControl.actualState.agl.vel.y = newVelY;
 
-    posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
+    posControl.actualState.velXY = fast_fsqrtf(sq(newVelX) + sq(newVelY));
 
     // CASE 1: POS & VEL valid
     if (estPosValid && estVelValid) {
@@ -2080,7 +2074,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
  *-----------------------------------------------------------*/
 static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
 {
-    return sqrtf(sq(deltaX) + sq(deltaY));
+    return fast_fsqrtf(sq(deltaX) + sq(deltaY));
 }
 
 static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
@@ -2157,8 +2151,8 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
 static void updateHomePositionCompatibility(void)
 {
     geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
-    GPS_distanceToHome = posControl.homeDistance / 100;
-    GPS_directionToHome = posControl.homeDirection / 100;
+    GPS_distanceToHome = posControl.homeDistance * 0.01f;
+    GPS_directionToHome = posControl.homeDirection * 0.01f;
 }
 
 // Backdoor for RTH estimator
@@ -2845,13 +2839,23 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
         if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
+            static int8_t nonGeoWaypointCount = 0;
+
             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
                 posControl.waypointList[wpNumber - 1] = *wpData;
+                if(wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD || wpData->action == NAV_WP_ACTION_JUMP) {
+                    nonGeoWaypointCount += 1;
                     if(wpData->action == NAV_WP_ACTION_JUMP) {
                         posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
                     }
+                }
+
                 posControl.waypointCount = wpNumber;
                 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
+                posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
+                if (posControl.waypointListValid) {
+                    nonGeoWaypointCount = 0;
+                }
             }
         }
     }
@@ -2863,6 +2867,7 @@ void resetWaypointList(void)
     if (!ARMING_FLAG(ARMED)) {
         posControl.waypointCount = 0;
         posControl.waypointListValid = false;
+        posControl.geoWaypointCount = 0;
     }
 }
 
@@ -2934,8 +2939,15 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
 {
     gpsLocation_t wpLLH;
 
+    /* Default to home position if lat & lon = 0 or HOME flag set
+     * Applicable to WAYPOINT, HOLD_TIME & LANDING WP types */
+    if ((waypoint->lat == 0 && waypoint->lon == 0) || waypoint->flag == NAV_WP_FLAG_HOME) {
+        wpLLH.lat = GPS_home.lat;
+        wpLLH.lon = GPS_home.lon;
+    } else {
         wpLLH.lat = waypoint->lat;
         wpLLH.lon = waypoint->lon;
+    }
     wpLLH.alt = waypoint->alt;
 
     geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
@@ -3745,7 +3757,7 @@ static void GPS_distance_cm_bearing(int32_t currentLat1, int32_t currentLon1, in
     const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
     const float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown;
 
-    *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
+    *dist = fast_fsqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
 
     *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon));      // Convert the output radians to 100xdeg
 

src/main/navigation/navigation.h

@@ -232,7 +232,6 @@ typedef struct navConfig_s {
 
     struct {
         uint8_t  max_bank_angle;                // multicopter max banking angle (deg)
-        uint16_t hover_throttle;                // multicopter hover throttle
         uint16_t auto_disarm_delay;             // multicopter safety delay for landing detector
 
 #ifdef USE_MR_BRAKING_MODE
@@ -255,12 +254,8 @@ typedef struct navConfig_s {
         uint8_t  max_bank_angle;             // Fixed wing max banking angle (deg)
         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
-        uint16_t cruise_throttle;            // Cruise throttle
         uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
         uint8_t control_smoothness;          // The amount of smoothing to apply to controls for navigation
-        uint16_t min_throttle;               // Minimum allowed throttle in auto mode
-        uint16_t max_throttle;               // Maximum allowed throttle in auto mode
-        uint8_t  pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
         uint16_t pitch_to_throttle_smooth;    // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
         uint8_t  pitch_to_throttle_thresh;   // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
         uint16_t loiter_radius;              // Loiter radius when executing PH on a fixed wing
@@ -268,9 +263,8 @@ typedef struct navConfig_s {
         uint16_t launch_velocity_thresh;     // Velocity threshold for swing launch detection
         uint16_t launch_accel_thresh;        // Acceleration threshold for launch detection (cm/s/s)
         uint16_t launch_time_thresh;         // Time threshold for launch detection (ms)
-        uint16_t launch_idle_throttle;       // Throttle to keep at launch idle
-        uint16_t launch_throttle;            // Launch throttle
         uint16_t launch_motor_timer;         // Time to wait before setting launch_throttle (ms)
+        uint16_t launch_idle_motor_timer;    // Time to wait before motor starts at_idle throttle (ms)
         uint16_t launch_motor_spinup_time;   // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
         uint16_t launch_end_time;            // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
         uint16_t launch_min_time;	           // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
@@ -312,6 +306,7 @@ typedef enum {
 } navWaypointHeadings_e;
 
 typedef enum {
+    NAV_WP_FLAG_HOME = 0x48,
     NAV_WP_FLAG_LAST = 0xA5
 } navWaypointFlags_e;
 

src/main/navigation/navigation_fixedwing.c

@@ -51,6 +51,8 @@
 
 #include "rx/rx.h"
 
+#include "sensors/battery.h"
+
 // Base frequencies for smoothing pitch and roll
 #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f
 #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ     10.0f
@@ -132,10 +134,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     // velocity error. We use PID controller on altitude error and calculate desired pitch angle
 
     // Update energies
-    const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS;
+    const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS;
     const float demSKE = 0.0f;
 
-    const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS;
+    const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS;
     const float estSKE = 0.0f;
 
     // speedWeight controls balance between potential and kinetic energy used for pitch controller
@@ -249,7 +251,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
     float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
     float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
 
-    float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
+    float distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
 
     // Limit minimum forward velocity to 1 m/s
     float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
@@ -272,7 +274,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
         // We have temporary loiter target. Recalculate distance and position error
         posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
         posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
-        distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
+        distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
     }
 
     // Calculate virtual waypoint
@@ -316,7 +318,7 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr
         -limit,
         limit,
         yawPidFlags
-        ) / 100.0f;
+        ) * 0.01f;
 
     DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
     DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
@@ -483,18 +485,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
 
     if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
         // Unfiltered throttle correction outside of pitch deadband
-        return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
+        return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
     }
     else {
         // Filtered throttle correction inside of pitch deadband
-        return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle;
+        return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
     }
 }
 
 void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 {
-    int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
-    int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle;
+    int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
+    int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
 
     if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
         // ROLL >0 right, <0 left
@@ -529,15 +531,15 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
         }
 
-        uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
+        uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
 
         // Manual throttle increase
         if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
             if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95)
-                correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle);
+                correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
             else
                 correctedThrottleValue = motorConfig()->maxthrottle;
-            isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle);
+            isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
         } else {
             isAutoThrottleManuallyIncreased = false;
         }
@@ -600,7 +602,7 @@ void applyFixedWingEmergencyLandingController(void)
     rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
     rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
     rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
-    rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation_fw_launch.c

@@ -50,10 +50,13 @@
 
 #include "io/gps.h"
 
+#include "sensors/battery.h"
+
 #define SWING_LAUNCH_MIN_ROTATION_RATE      DEGREES_TO_RADIANS(100)     // expect minimum 100dps rotation rate
 #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500                              // ms
 #define UNUSED(x) ((void)(x))
 #define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE"
+#define FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE "WAITING FOR IDLE"
 #define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY"
 #define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT"
 #define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING"
@@ -61,6 +64,7 @@
 typedef enum {
     FW_LAUNCH_MESSAGE_TYPE_NONE = 0,
     FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE,
+    FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE,
     FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION,
     FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS,
     FW_LAUNCH_MESSAGE_TYPE_FINISHING
@@ -78,6 +82,7 @@ typedef enum {
 typedef enum {
     FW_LAUNCH_STATE_IDLE = 0,
     FW_LAUNCH_STATE_WAIT_THROTTLE,
+    FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
     FW_LAUNCH_STATE_MOTOR_IDLE,
     FW_LAUNCH_STATE_WAIT_DETECTION,
     FW_LAUNCH_STATE_DETECTED,
@@ -90,6 +95,7 @@ typedef enum {
 
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs);
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs);
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs);
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs);
@@ -111,6 +117,7 @@ typedef struct fixedWingLaunchData_s {
 } fixedWingLaunchData_t;
 
 static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch;
+static bool idleMotorAboutToStart;
 
 static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = {
 
@@ -125,19 +132,28 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
     [FW_LAUNCH_STATE_WAIT_THROTTLE] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
         .onEvent = {
-            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_MOTOR_IDLE,
+            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
             [FW_LAUNCH_EVENT_GOTO_DETECTION]        = FW_LAUNCH_STATE_WAIT_DETECTION
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
     },
 
+    [FW_LAUNCH_STATE_IDLE_MOTOR_DELAY] = {
+        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
+        .onEvent = {
+            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_MOTOR_IDLE,
+            [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE
+        },
+        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE
+    },
+
     [FW_LAUNCH_STATE_MOTOR_IDLE] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE,
         .onEvent = {
             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_WAIT_DETECTION,
             [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE
         },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
+        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE
     },
 
     [FW_LAUNCH_STATE_WAIT_DETECTION] = {
@@ -211,13 +227,13 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi
 
 static bool isThrottleIdleEnabled(void)
 {
-    return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
+    return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue();
 }
 
 static void applyThrottleIdleLogic(bool forceMixerIdle)
 {
     if (isThrottleIdleEnabled() && !forceMixerIdle) {
-        rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
+        rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle;
     }
     else {
         ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);           // If MOTOR_STOP is enabled mixer will keep motor stopped
@@ -237,7 +253,7 @@ static inline bool isLaunchMaxAltitudeReached(void)
 
 static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
 {
-    return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband();
+    return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && isRollPitchStickDeflected();
 }
 
 static void resetPidsIfNeeded(void) {
@@ -287,6 +303,24 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
     return FW_LAUNCH_EVENT_NONE;
 }
 
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
+{
+    if (isThrottleLow()) {
+        return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
+    }
+
+    applyThrottleIdleLogic(true);
+
+    if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_idle_motor_timer) {
+        idleMotorAboutToStart = false;
+        return FW_LAUNCH_EVENT_SUCCESS;
+    }
+    // 5 second warning motor about to start at idle, changes Beeper sound
+    idleMotorAboutToStart = navConfig()->fw.launch_idle_motor_timer - currentStateElapsedMs(currentTimeUs) < 5000;
+
+    return FW_LAUNCH_EVENT_NONE;
+}
+
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs)
 {
     if (isThrottleLow()) {
@@ -299,7 +333,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
         return FW_LAUNCH_EVENT_SUCCESS;
     }
     else {
-        rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
+        rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
     }
 
@@ -365,14 +399,14 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
 
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
-    const uint16_t launchThrottle = navConfig()->fw.launch_throttle;
+    const uint16_t launchThrottle = currentBatteryProfile->nav.fw.launch_throttle;
 
     if (elapsedTimeMs > motorSpinUpMs) {
         rcCommand[THROTTLE] = launchThrottle;
         return FW_LAUNCH_EVENT_SUCCESS;
     }
     else {
-        const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
+        const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
         rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs,  minIdleThrottle, launchThrottle);
     }
 
@@ -381,7 +415,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
 
 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs)
 {
-    rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
+    rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_throttle;
 
     if (isLaunchMaxAltitudeReached()) {
         return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
@@ -403,7 +437,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
 
-    if (areSticksDeflectedMoreThanPosHoldDeadband()) {
+    if (isRollPitchStickDeflected()) {
         return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state
     }
     if (elapsedTimeMs > endTimeMs) {
@@ -411,7 +445,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
     }
     else {
         // make a smooth transition from the launch state to the current state for throttle and the pitch angle
-        rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs,  navConfig()->fw.launch_throttle, rcCommand[THROTTLE]);
+        rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs,  currentBatteryProfile->nav.fw.launch_throttle, rcCommand[THROTTLE]);
         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
     }
 
@@ -441,8 +475,12 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
         beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
     }
     else {
+        if (idleMotorAboutToStart) {
+            beeper(BEEPER_LAUNCH_MODE_IDLE_START);
+        } else {
             beeper(BEEPER_LAUNCH_MODE_ENABLED);
         }
+    }
 }
 
 void resetFixedWingLaunchController(timeUs_t currentTimeUs)
@@ -476,6 +514,9 @@ const char * fixedWingLaunchStateMessage(void)
         case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE:
             return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE;
 
+        case FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE:
+            return FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE;
+
         case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION:
             return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;
 

src/main/navigation/navigation_multicopter.c

@@ -51,6 +51,8 @@
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
+#include "sensors/battery.h"
+
 /*-----------------------------------------------------------
  * Altitude controller for multicopter aircraft
  *-----------------------------------------------------------*/
@@ -104,8 +106,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
 {
     // Calculate min and max throttle boundaries (to compensate for integral windup)
-    const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)navConfig()->mc.hover_throttle;
-    const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle;
+    const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
+    const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
 
     posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
 
@@ -239,7 +241,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
     }
 
     // Update throttle controller
-    rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
 
     // Save processed throttle for future use
     rcCommandAdjustedThrottle = rcCommand[THROTTLE];
@@ -437,7 +439,7 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
     float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
 
     // Scale velocity to respect max_speed
-    float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
+    float newVelTotal = fast_fsqrtf(sq(newVelX) + sq(newVelY));
 
     /*
      * We override computed speed with max speed in following cases:
@@ -497,7 +499,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
     const float setpointX = posControl.desiredState.vel.x;
     const float setpointY = posControl.desiredState.vel.y;
-    const float setpointXY = sqrtf(powf(setpointX, 2)+powf(setpointY, 2));
+    const float setpointXY = fast_fsqrtf(powf(setpointX, 2)+powf(setpointY, 2));
 
     // Calculate velocity error
     const float velErrorX = setpointX - measurementX;
@@ -505,7 +507,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
     // Calculate XY-acceleration limit according to velocity error limit
     float accelLimitX, accelLimitY;
-    const float velErrorMagnitude = sqrtf(sq(velErrorX) + sq(velErrorY));
+    const float velErrorMagnitude = fast_fsqrtf(sq(velErrorX) + sq(velErrorY));
     if (velErrorMagnitude > 0.1f) {
         accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
         accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);
@@ -753,7 +755,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
             rcCommand[THROTTLE] = getThrottleIdleValue();
         }
         else {
-            rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+            rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
         }
 
         return;
@@ -780,7 +782,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     }
 
     // Update throttle controller
-    rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation_pos_estimator.c

@@ -164,7 +164,7 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
         predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
 
         /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
-        gpsDistance = sqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
+        gpsDistance = fast_fsqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
         if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
             isGlitching = false;
         }
@@ -640,7 +640,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
             const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y;
             const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x;
             const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y;
-            const float gpsPosResidualMag = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
+            const float gpsPosResidualMag = fast_fsqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
 
             //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
             const float gpsWeightScaler = 1.0f;

src/main/navigation/navigation_pos_estimator_flow.c

@@ -109,7 +109,7 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
         ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
         ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
 
-        ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
+        ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, fast_fsqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
     }
 
     DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));

src/main/navigation/navigation_private.h

@@ -356,6 +356,7 @@ typedef struct {
     navWaypoint_t               waypointList[NAV_MAX_WAYPOINTS];
     bool                        waypointListValid;
     int8_t                      waypointCount;
+    int8_t                      geoWaypointCount;   // total geospatial WPs in mission
     bool                        wpMissionRestart;   // mission restart from first waypoint
 
     navWaypointPosition_t       activeWaypoint;     // Local position and initial bearing, filled on waypoint activation

src/main/navigation/navigation_rover_boat.c

@@ -29,13 +29,19 @@ FILE_COMPILE_FOR_SIZE
 #ifdef USE_NAV
 
 #include "build/debug.h"
+
 #include "common/utils.h"
+
 #include "fc/rc_controls.h"
 #include "fc/config.h"
+
 #include "flight/mixer.h"
+
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
+#include "sensors/battery.h"
+
 static bool isYawAdjustmentValid = false;
 static int32_t navHeadingError;
 
@@ -125,7 +131,7 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
                 rcCommand[YAW] = posControl.rcAdjustment[YAW];
             }
 
-            rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
+            rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
         }
     }
 }
@@ -136,7 +142,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
         rcCommand[ROLL] = 0;
         rcCommand[PITCH] = 0;
         rcCommand[YAW] = 0;
-        rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+        rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
     } else if (navStateFlags & NAV_CTL_POS) {
         applyRoverBoatPositionController(currentTimeUs);
         applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);

src/main/programming/logic_condition.c

@@ -514,7 +514,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
-            return constrain(sqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX);
+            return constrain(fast_fsqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:

src/main/rx/mavlink.h

@@ -19,6 +19,7 @@
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-function"
+#define MAVLINK_COMM_NUM_BUFFERS 1
 #include "common/mavlink.h"
 #pragma GCC diagnostic pop
 

src/main/scheduler/scheduler.c

@@ -299,7 +299,9 @@ void FAST_CODE NOINLINE scheduler(void)
 #if defined(SCHEDULER_DEBUG)
         DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
 #endif
-    } else {
+    } 
+    
+    if (!selectedTask || forcedRealTimeTask) {
         // Execute system real-time callbacks and account for them to SYSTEM account
         const timeUs_t currentTimeBeforeTaskCall = micros();
         taskRunRealtimeCallbacks(currentTimeBeforeTaskCall);

src/main/sensors/acceleration.c

@@ -616,20 +616,20 @@ void updateAccExtremes(void)
         if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis];
     }
 
-    float gforce = sqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2]));
+    float gforce = fast_fsqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2]));
     if (gforce > acc.maxG) acc.maxG = gforce;
 }
 
 void accGetVibrationLevels(fpVector3_t *accVibeLevels)
 {
-    accVibeLevels->x = sqrtf(acc.accVibeSq[X]);
-    accVibeLevels->y = sqrtf(acc.accVibeSq[Y]);
-    accVibeLevels->z = sqrtf(acc.accVibeSq[Z]);
+    accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]);
+    accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]);
+    accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]);
 }
 
 float accGetVibrationLevel(void)
 {
-    return sqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
+    return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
 }
 
 uint32_t accGetClipCount(void)

src/main/sensors/battery.c

@@ -36,6 +36,7 @@
 #include "drivers/time.h"
 
 #include "fc/config.h"
+#include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
 #include "fc/runtime_config.h"
 #include "fc/stats.h"
@@ -93,7 +94,7 @@ static int32_t mWhDrawn = 0;                    // energy (milliWatt hours) draw
 batteryState_e batteryState;
 const batteryProfile_t *currentBatteryProfile;
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 0);
+PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 1);
 
 void pgResetFn_batteryProfiles(batteryProfile_t *instance)
 {
@@ -115,7 +116,54 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
                 .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT,
                 .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT,
                 .unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT,
+            },
+
+            .controlRateProfile = 0,
+
+            .motor = {
+                .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
+                .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
+#ifdef USE_DSHOT
+                .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
+#endif
+            },
+
+            .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT,                                 // default throttle off.
+
+            .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
+
+            .nav = {
+
+                .mc = {
+                    .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
+                },
+
+                .fw = {
+                    .cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT,
+                    .max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT,
+                    .min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT,
+                    .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT,                          // pwm units per degree of pitch (10pwm units ~ 1% throttle)
+                    .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
+                    .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT,                 // Motor idle or MOTOR_STOP
                 }
+
+            },
+
+#if defined(USE_POWER_LIMITS)
+            .powerLimits = {
+                .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT,                            // dA
+                .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT,                                // dA
+                .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT,                       // dS
+                .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT,      // dS
+#ifdef USE_ADC
+                .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT,                                // dW
+                .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT,                                    // dW
+                .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT,                           // dS
+                .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT,          // dS
+#endif // USE_ADC
+            }
+#endif // USE_POWER_LIMITS
+
         );
     }
 }
@@ -197,6 +245,9 @@ void setBatteryProfile(uint8_t profileIndex)
         profileIndex = 0;
     }
     currentBatteryProfile = batteryProfiles(profileIndex);
+    if ((currentBatteryProfile->controlRateProfile > 0) && (currentBatteryProfile->controlRateProfile < MAX_CONTROL_RATE_PROFILE_COUNT)) {
+        setConfigProfile(currentBatteryProfile->controlRateProfile - 1);
+    }
 }
 
 void activateBatteryProfile(void)

src/main/sensors/battery.h

@@ -18,13 +18,16 @@
 #pragma once
 
 #include "config/parameter_group.h"
+
 #include "drivers/time.h"
 
+#include "fc/settings.h"
+
+#include "sensors/battery_config_structs.h"
+
 #ifndef VBAT_SCALE_DEFAULT
 #define VBAT_SCALE_DEFAULT 1100
 #endif
-#define VBAT_SCALE_MIN 0
-#define VBAT_SCALE_MAX 65535
 
 #ifndef CURRENT_METER_SCALE
 #define CURRENT_METER_SCALE 400 // for Allegro ACS758LCB-100U (40mV/A)
@@ -35,90 +38,21 @@
 #endif
 
 #ifndef MAX_BATTERY_PROFILE_COUNT
-#define MAX_BATTERY_PROFILE_COUNT 3
+#define MAX_BATTERY_PROFILE_COUNT SETTING_CONSTANT_MAX_BATTERY_PROFILE_COUNT
 #endif
 
-typedef enum {
-    CURRENT_SENSOR_NONE = 0,
-    CURRENT_SENSOR_ADC,
-    CURRENT_SENSOR_VIRTUAL,
-    CURRENT_SENSOR_ESC,
-    CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
-} currentSensor_e;
-
-typedef enum {
-    VOLTAGE_SENSOR_NONE = 0,
-    VOLTAGE_SENSOR_ADC,
-    VOLTAGE_SENSOR_ESC
-} voltageSensor_e;
-
-typedef enum {
-    BAT_CAPACITY_UNIT_MAH,
-    BAT_CAPACITY_UNIT_MWH,
-} batCapacityUnit_e;
-
 typedef struct {
   uint8_t profile_index;
   uint16_t max_voltage;
 } profile_comp_t;
 
-typedef enum {
-    BAT_VOLTAGE_RAW,
-    BAT_VOLTAGE_SAG_COMP
-} batVoltageSource_e;
-
-typedef struct batteryMetersConfig_s {
-
-#ifdef USE_ADC
-    struct {
-        uint16_t scale;
-        voltageSensor_e type;
-    } voltage;
-#endif
-
-    struct {
-        int16_t scale;          // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
-        int16_t offset;         // offset of the current sensor in millivolt steps
-        currentSensor_e type;   // type of current meter used, either ADC or virtual
-    } current;
-
-    batVoltageSource_e voltageSource;
-
-    uint32_t cruise_power;      // power drawn by the motor(s) at cruise throttle/speed (cW)
-    uint16_t idle_power;        // power drawn by the system when the motor(s) are stopped (cW)
-    uint8_t rth_energy_margin;  // Energy that should be left after RTH (%), used for remaining time/distance before RTH
-
-    float throttle_compensation_weight;
-
-} batteryMetersConfig_t;
-
-typedef struct batteryProfile_s {
-
-#ifdef USE_ADC
-    uint8_t cells;
-
-    struct {
-        uint16_t cellDetect;    // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V)
-        uint16_t cellMax;       // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 421 (4.21V)
-        uint16_t cellMin;       // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V)
-        uint16_t cellWarning;   // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V)
-    } voltage;
-#endif
-
-    struct {
-        uint32_t value;         // mAh or mWh (see capacity.unit)
-        uint32_t warning;       // mAh or mWh (see capacity.unit)
-        uint32_t critical;      // mAh or mWh (see capacity.unit)
-        batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical
-    } capacity;
-
-} batteryProfile_t;
-
 PG_DECLARE(batteryMetersConfig_t, batteryMetersConfig);
 PG_DECLARE_ARRAY(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles);
 
 extern const batteryProfile_t *currentBatteryProfile;
 
+#define currentBatteryProfileMutable ((batteryProfile_t*)currentBatteryProfile)
+
 typedef enum {
     BATTERY_OK = 0,
     BATTERY_WARNING,

src/main/sensors/battery_config_structs.h

@@ -0,0 +1,144 @@
+/*
+ * This file is part of iNav
+ *
+ * iNav free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav distributed in the hope that it
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+typedef enum {
+    CURRENT_SENSOR_NONE = 0,
+    CURRENT_SENSOR_ADC,
+    CURRENT_SENSOR_VIRTUAL,
+    CURRENT_SENSOR_ESC,
+    CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
+} currentSensor_e;
+
+typedef enum {
+    VOLTAGE_SENSOR_NONE = 0,
+    VOLTAGE_SENSOR_ADC,
+    VOLTAGE_SENSOR_ESC
+} voltageSensor_e;
+
+typedef enum {
+    BAT_CAPACITY_UNIT_MAH,
+    BAT_CAPACITY_UNIT_MWH,
+} batCapacityUnit_e;
+
+typedef enum {
+    BAT_VOLTAGE_RAW,
+    BAT_VOLTAGE_SAG_COMP
+} batVoltageSource_e;
+
+typedef struct batteryMetersConfig_s {
+
+#ifdef USE_ADC
+    struct {
+        uint16_t scale;
+        voltageSensor_e type;
+    } voltage;
+#endif
+
+    struct {
+        int16_t scale;          // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
+        int16_t offset;         // offset of the current sensor in millivolt steps
+        currentSensor_e type;   // type of current meter used, either ADC or virtual
+    } current;
+
+    batVoltageSource_e voltageSource;
+
+    uint32_t cruise_power;      // power drawn by the motor(s) at cruise throttle/speed (cW)
+    uint16_t idle_power;        // power drawn by the system when the motor(s) are stopped (cW)
+    uint8_t rth_energy_margin;  // Energy that should be left after RTH (%), used for remaining time/distance before RTH
+
+    float throttle_compensation_weight;
+
+} batteryMetersConfig_t;
+
+typedef struct batteryProfile_s {
+
+#ifdef USE_ADC
+    uint8_t cells;
+
+    struct {
+        uint16_t cellDetect;    // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V)
+        uint16_t cellMax;       // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 421 (4.21V)
+        uint16_t cellMin;       // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V)
+        uint16_t cellWarning;   // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V)
+    } voltage;
+#endif
+
+    struct {
+        uint32_t value;                     // mAh or mWh (see capacity.unit)
+        uint32_t warning;                   // mAh or mWh (see capacity.unit)
+        uint32_t critical;                  // mAh or mWh (see capacity.unit)
+        batCapacityUnit_e unit;             // Describes unit of capacity.value, capacity.warning and capacity.critical
+    } capacity;
+
+    uint8_t controlRateProfile;
+
+    struct {
+        float throttleIdle;                 // Throttle IDLE value based on min_command, max_throttle, in percent
+        float throttleScale;                // Scaling factor for throttle.
+#ifdef USE_DSHOT
+        uint8_t turtleModePowerFactor;      // Power factor from 0 to 100% of flip over after crash
+#endif
+    } motor;
+
+    uint16_t failsafe_throttle;             // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
+
+    uint16_t fwMinThrottleDownPitchAngle;
+
+    struct {
+
+        struct {
+            uint16_t hover_throttle;        // multicopter hover throttle
+        } mc;
+
+        struct {
+            uint16_t cruise_throttle;       // Cruise throttle
+            uint16_t min_throttle;          // Minimum allowed throttle in auto mode
+            uint16_t max_throttle;          // Maximum allowed throttle in auto mode
+            uint8_t  pitch_to_throttle;     // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
+            uint16_t launch_idle_throttle;  // Throttle to keep at launch idle
+            uint16_t launch_throttle;       // Launch throttle
+        } fw;
+
+    } nav;
+
+#if defined(USE_POWER_LIMITS)
+    struct {
+        uint16_t continuousCurrent;         // dA
+        uint16_t burstCurrent;              // dA
+        uint16_t burstCurrentTime;          // ds
+        uint16_t burstCurrentFalldownTime;  // ds
+
+#ifdef USE_ADC
+        uint16_t continuousPower;           // dW
+        uint16_t burstPower;                // dW
+        uint16_t burstPowerTime;            // ds
+        uint16_t burstPowerFalldownTime;    // ds
+#endif // USE_ADC
+    } powerLimits;
+#endif // USE_POWER_LIMITS
+
+} batteryProfile_t;

src/main/sensors/compass.c

@@ -329,6 +329,16 @@ bool compassIsReady(void)
     return magUpdatedAtLeastOnce;
 }
 
+bool compassIsCalibrationComplete(void)
+{
+    if (STATE(COMPASS_CALIBRATED)) {
+        return true;
+    }
+    else {
+        return false;
+    }
+}
+
 void compassUpdate(timeUs_t currentTimeUs)
 {
     static sensorCalibrationState_t calState;

src/main/sensors/compass.h

@@ -78,5 +78,6 @@ bool compassInit(void);
 void compassUpdate(timeUs_t currentTimeUs);
 bool compassIsReady(void);
 bool compassIsHealthy(void);
+bool compassIsCalibrationComplete(void);
 
 #endif

src/main/sensors/gyro.c

@@ -75,6 +75,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/gyroanalyse.h"
 #include "flight/rpm_filter.h"
 #include "flight/dynamic_gyro_notch.h"
+#include "flight/kalman.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -111,7 +112,7 @@ STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 15);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@@ -142,6 +143,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
     .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
 #endif
+#ifdef USE_GYRO_KALMAN
+    .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
+    .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
+#endif
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -318,6 +323,11 @@ static void gyroInitFilters(void)
         }
     } 
 #endif
+#ifdef USE_GYRO_KALMAN
+    if (gyroConfig()->kalmanEnabled) {
+        gyroKalmanInitialize(gyroConfig()->kalman_q);
+    }
+#endif
 }
 
 bool gyroInit(void)
@@ -501,6 +511,12 @@ void FAST_CODE NOINLINE gyroFilter()
         }
 #endif
 
+#ifdef USE_GYRO_KALMAN
+        if (gyroConfig()->kalmanEnabled) {
+            gyroADCf = gyroKalmanUpdate(axis, gyroADCf);
+        }
+#endif
+
         gyro.gyroADCf[axis] = gyroADCf;
     }
 
@@ -517,6 +533,7 @@ void FAST_CODE NOINLINE gyroFilter()
         }
     }
 #endif
+
 }
 
 void FAST_CODE NOINLINE gyroUpdate()

src/main/sensors/gyro.h

@@ -63,7 +63,7 @@ typedef struct gyroConfig_s {
     uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
     uint16_t looptime;                      // imu loop time in us
     uint8_t  gyro_lpf;                      // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
-    uint8_t  gyro_anti_aliasing_lpf_hz;
+    uint16_t  gyro_anti_aliasing_lpf_hz;
     uint8_t  gyro_anti_aliasing_lpf_type;
 #ifdef USE_DUAL_GYRO
     uint8_t  gyro_to_use;
@@ -87,6 +87,10 @@ typedef struct gyroConfig_s {
     float alphaBetaGammaBoost;
     float alphaBetaGammaHalfLife;
 #endif
+#ifdef USE_GYRO_KALMAN
+    uint16_t kalman_q;
+    uint8_t kalmanEnabled;
+#endif
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/sensors/opflow.c

@@ -237,8 +237,8 @@ void opflowUpdate(timeUs_t currentTimeUs)
             else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) {
                 // Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough
                 const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime;
-                opflowCalibrationBodyAcc += sqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y]));
-                opflowCalibrationFlowAcc += sqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt;
+                opflowCalibrationBodyAcc += fast_fsqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y]));
+                opflowCalibrationFlowAcc += fast_fsqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt;
             }
         }
 

src/main/sensors/pitotmeter.c

@@ -223,7 +223,7 @@ STATIC_PROTOTHREAD(pitotThread)
             //
             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
-            pitot.airSpeed = pitotmeterConfig()->pitot_scale * sqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100;
+            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100;
         } else {
             performPitotCalibrationCycle();
             pitot.airSpeed = 0;

src/main/target/DALRCF405/target.h

@@ -151,3 +151,5 @@
 
 //TIMER
 #define MAX_PWM_OUTPUT_PORTS       10
+
+#define USE_DSHOT

src/main/target/FALCORE/config.c

@@ -89,7 +89,7 @@ void targetConfiguration(void)
     failsafeConfigMutable()->failsafe_delay = 5;
     failsafeConfigMutable()->failsafe_recovery_delay = 5;
     failsafeConfigMutable()->failsafe_off_delay = 200;
-    failsafeConfigMutable()->failsafe_throttle = 1200;
+    currentBatteryProfile->failsafe_throttle = 1200;
     failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH;
 
     boardAlignmentMutable()->rollDeciDegrees = 0;
@@ -120,7 +120,7 @@ void targetConfiguration(void)
     navConfigMutable()->general.rth_altitude = 1000;
 
     navConfigMutable()->mc.max_bank_angle = 30;
-    navConfigMutable()->mc.hover_throttle = 1500;
+    currentBatteryProfile->nav.mc.hover_throttle = 1500;
     navConfigMutable()->mc.auto_disarm_delay = 2000;
 
     /*

src/main/target/FLYWOOF745/CMakeLists.txt

@@ -0,0 +1,2 @@
+target_stm32f745xg(FLYWOOF745)
+target_stm32f745xg(FLYWOOF745NANO)

src/main/target/FLYWOOF745/target.c

@@ -0,0 +1,45 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <platform.h>
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/bus.h"
+
+const timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM,                                               0, 1), // PPM, DMA2_ST6
+
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,                       0, 0), // M1 , DMA1_ST7
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR,                       0, 0), // M2 , DMA1_ST2
+    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,                       0, 2), // M3 , DMA2_ST2
+    DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,                       0, 1), // M4 , DMA2_ST4
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO,    0, 0), // M5 , DMA2_ST7
+    DEF_TIM(TIM5, CH4, PA3,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO,    0, 0), // M6 , DMA1_ST1
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO,    0, 0), // M7 , DMA1_ST4
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO,    0, 0), // M8 , DMA1_ST5
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED,                                               0, 0), // LED_STRIP, DMA1_ST0
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/FLYWOOF745/target.h

@@ -0,0 +1,187 @@
+/*
+ * This file is part of INAV.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef FLYWOOF745
+#define TARGET_BOARD_IDENTIFIER "FWF7"
+#define USBD_PRODUCT_STRING "FLYWOOF745"
+#else
+#define TARGET_BOARD_IDENTIFIER "FW7N"
+#define USBD_PRODUCT_STRING "FLYWOOF745NANO"
+#endif
+
+#define LED0                PA2//
+
+#define BEEPER              PD15//
+#define BEEPER_INVERTED
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
+#define USE_MPU_DATA_READY_SIGNAL
+#define USE_EXTI
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG//
+#define GYRO_INT_EXTI           PE1//
+#define MPU6000_CS_PIN          SPI4_NSS_PIN//
+#define MPU6000_SPI_BUS         BUS_SPI4//
+
+
+#define USB_IO
+#define USE_VCP
+#define VBUS_SENSING_ENABLED
+#define VBUS_SENSING_PIN          PA8//
+
+#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
+
+#ifdef FLYWOOF745
+#define UART3_TX_PIN            PB10//
+#define UART3_RX_PIN            PB11//
+#else
+#define UART3_TX_PIN            PD8//
+#define UART3_RX_PIN            PD9//
+#endif
+
+#define USE_UART4
+#define UART4_TX_PIN            PA0//
+#define UART4_RX_PIN            PA1//
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12//
+#define UART5_RX_PIN            PD2//
+
+#define USE_UART6
+#define UART6_TX_PIN            PC6//
+#define UART6_RX_PIN            PC7//
+
+#define USE_UART7
+#define UART7_TX_PIN            PE8//
+#define UART7_RX_PIN            PE7//
+
+#define SERIAL_PORT_COUNT 8 //VCP,UART1,UART2,UART3,UART4,UART5,UART6,UART7
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1   //FLASH
+#define USE_SPI_DEVICE_2   //OSD
+#define USE_SPI_DEVICE_4   //ICM20689
+
+#define SPI1_NSS_PIN            PA4
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define SPI2_NSS_PIN            PB12
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define SPI4_NSS_PIN            PE4
+#define SPI4_SCK_PIN            PE2
+#define SPI4_MISO_PIN           PE5
+#define SPI4_MOSI_PIN           PE6
+
+#define USE_OSD
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          SPI2_NSS_PIN
+
+#define M25P16_CS_PIN           SPI1_NSS_PIN
+#define M25P16_SPI_BUS          BUS_SPI1
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB6
+#define I2C1_SDA                PB7
+
+#ifdef FLYWOOF745NANO										
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL                PB10
+#define I2C2_SDA                PB11
+
+//External I2C bus is different than internal one
+#define MAG_I2C_BUS             BUS_I2C2
+#define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define RANGEFINDER_I2C_BUS     BUS_I2C2
+#define DEFAULT_I2C_BUS         BUS_I2C2
+
+#else
+
+//External I2C bus is the same as interal one
+#define MAG_I2C_BUS             BUS_I2C1
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define DEFAULT_I2C_BUS         BUS_I2C1
+
+#endif
+
+#define USE_BARO
+#define USE_BARO_BMP280
+#define BARO_I2C_BUS            BUS_I2C1
+
+#define USE_MAG
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_MAG3110
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_LIS3MDL
+
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN           PC2
+#define ADC_CHANNEL_2_PIN           PC3
+#define ADC_CHANNEL_3_PIN           PC5
+
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_1
+#define VBAT_ADC_CHANNEL            ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+#define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_VBAT | FEATURE_BLACKBOX)
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_UART           SERIAL_PORT_USART1
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+
+#define USE_LED_STRIP
+#define WS2811_PIN                      PD12 //  //TIM4_CH1
+
+#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       8

src/main/target/MATEKF722SE/CMakeLists.txt

@@ -1,2 +1,3 @@
 target_stm32f722xe(MATEKF722SE)
 target_stm32f722xe(MATEKF722MINI)
+target_stm32f722xe(MATEKF722SE_8MOTOR)

src/main/target/MATEKF722SE/target.c

@@ -38,8 +38,13 @@ const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM2, CH1, PA15,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5   UP1-7   D(1, 5, 3) - clash with S2
     DEF_TIM(TIM2, CH2, PB3,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6   UP1-7   D(1, 6, 3)
 
+#if (defined(MATEKF722SE_8MOTOR))
+    DEF_TIM(TIM4, CH1, PB6,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S7   UP1-6   D(1, 0, 2)
+    DEF_TIM(TIM4, CH2, PB7,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S8   UP1-6   D(1, 3, 2)
+#else
     DEF_TIM(TIM4, CH1, PB6,   TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0),   // S7   UP1-6   D(1, 0, 2)
     DEF_TIM(TIM4, CH2, PB7,   TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0),   // S8   UP1-6   D(1, 3, 2)
+#endif
 
     DEF_TIM(TIM1, CH1, PA8,  TIM_USE_LED, 0, 2),                            // LED          D(2, 6, 0)
 

src/main/target/MATEKH743/target.c

@@ -29,25 +29,22 @@
 BUSDEV_REGISTER_SPI_TAG(busdev_imu1_6000,   DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,    IMU1_EXTI_PIN,  0,  DEVFLAGS_NONE,  IMU1_ALIGN);
 BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500,   DEVHW_MPU6500,  IMU2_SPI_BUS,   IMU2_CS_PIN,    IMU2_EXTI_PIN,  1,  DEVFLAGS_NONE,  IMU2_ALIGN);
 
-//BUSDEV_REGISTER_SPI_TAG(busdev_imu1_6500,   DEVHW_MPU6500,  IMU1_SPI_BUS,   IMU1_CS_PIN,    IMU1_EXTI_PIN,  0,  DEVFLAGS_NONE,  IMU1_ALIGN);
-//BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6000,   DEVHW_MPU6000,  IMU2_SPI_BUS,   IMU2_CS_PIN,    IMU2_EXTI_PIN,  1,  DEVFLAGS_NONE,  IMU2_ALIGN);
-
 const timerHardware_t timerHardware[] = {
-    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2),   // S1
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),   // S1
     DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2),   // S2
 
-    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S3  
-    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S4
-    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5
-    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3),   // S3  
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4),   // S4
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5),   // S5
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6),   // S6
 
-    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S7
-    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S8
-    DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S9
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7),   // S7
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 8),   // S8
+    DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S9
     DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
 
-    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S11
-    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S12
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S11
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S12 DMA_NONE
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 0),    // LED_2812
     DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM

src/main/target/MATEKH743/target.h

@@ -18,8 +18,8 @@
 
 #pragma once
 
-#define TARGET_BOARD_IDENTIFIER "H765"
-#define USBD_PRODUCT_STRING     "MATEKH765"
+#define TARGET_BOARD_IDENTIFIER "H743"
+#define USBD_PRODUCT_STRING     "MATEKH743"
 
 #define LED0                    PE3
 #define LED1                    PE4
@@ -27,9 +27,6 @@
 #define BEEPER                  PA15
 #define BEEPER_INVERTED
 
-
-
-
 // *************** UART *****************************
 #define USE_VCP
 
@@ -67,23 +64,13 @@
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
 #define SERIALRX_UART           SERIAL_PORT_USART6
 
-
-
-#define MAX_PWM_OUTPUT_PORTS        15
-
-
-
-
-#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
-
 // *************** IMU generic ***********************
 #define USE_DUAL_GYRO
 #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
 
+#define USE_EXTI
+#define USE_MPU_DATA_READY_SIGNAL
+
 // *************** SPI1 IMU1 *************************
 #define USE_SPI
 #define USE_SPI_DEVICE_1
@@ -105,12 +92,10 @@
 #define SPI2_MISO_PIN           PB14
 #define SPI2_MOSI_PIN           PB15
 
-/*
 #define USE_OSD
 #define USE_MAX7456
 #define MAX7456_SPI_BUS         BUS_SPI2
 #define MAX7456_CS_PIN          PB12
-*/
 
 // *************** SPI3 SD BLACKBOX*******************
 /*
@@ -146,6 +131,7 @@
 #define BARO_I2C_BUS            BUS_I2C2
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
+#define USE_BARO_DPS310
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
@@ -163,20 +149,16 @@
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
 
-
-
-#if 0
-#define USE_EXTI
-#define USE_MPU_DATA_READY_SIGNAL
-
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                ADC1
 
-#define ADC_CHANNEL_1_PIN           PC2
-#define ADC_CHANNEL_2_PIN           PC3
-#define ADC_CHANNEL_3_PIN           PC1
-#define ADC_CHANNEL_4_PIN           PC0
+#define ADC_CHANNEL_1_PIN           PC0  //ADC123 VBAT1
+#define ADC_CHANNEL_2_PIN           PC1  //ADC123 CURR1
+#define ADC_CHANNEL_3_PIN           PC5  //ADC12  RSSI
+#define ADC_CHANNEL_4_PIN           PC4  //ADC12  AirS
+#define ADC_CHANNEL_5_PIN           PA4  //ADC12  VB2
+#define ADC_CHANNEL_6_PIN           PA7  //ADC12  CU2
 
 #define VBAT_ADC_CHANNEL            ADC_CHN_1
 #define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
@@ -198,7 +180,13 @@
 
 #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        15
 #define USE_DSHOT
 #define USE_ESC_SENSOR
 #define USE_SERIALSHOT
-#endif

src/main/target/common_post.h

@@ -61,9 +61,12 @@ extern uint8_t __config_end;
 #undef USE_PWM_SERVO_DRIVER
 #endif
 
+#define USE_ARM_MATH // try to use FPU functions
+
 #if defined(SIMULATOR_BUILD) || defined(UNIT_TEST)
 // This feature uses 'arm_math.h', which does not exist for x86.
 #undef USE_DYNAMIC_FILTERS
+#undef USE_ARM_MATH
 #endif
 
 //Defines for compiler optimizations

src/main/target/link/stm32_flash_F765xI.ld

@@ -1,55 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f765.ld
-**
-**  Abstract    : Linker script for STM32F765xITx Device with
-**                2048KByte FLASH, 512KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x00000000 to 0x00003FFF   16K TCM RAM,
-
-0x08000000 to 0x081FFFFF 2048K full flash,
-0x08000000 to 0x08007FFF   32K isr vector, startup code,
-0x08008000 to 0x0800FFFF   32K config,
-0x08010000 to 0x081FFFFF  1984K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rx)           : ORIGIN = 0x00000000, LENGTH = 16K
-
-    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 32K
-    ITCM_FLASH_CFG (r)      : ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)        : ORIGIN = 0x00210000, LENGTH = 1984K
-
-    AXIM_FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 32K
-    AXIM_FLASH_CFG (r)      : ORIGIN = 0x08008000, LENGTH = 32K
-    AXIM_FLASH1 (rx)        : ORIGIN = 0x08010000, LENGTH = 1984K
-
-    DTCM_RAM (rwx)          : ORIGIN = 0x20000000, LENGTH = 128K
-    SRAM1 (rwx)             : ORIGIN = 0x20020000, LENGTH = 368K
-    SRAM2 (rwx)             : ORIGIN = 0x2007C000, LENGTH = 16K
-    MEMORY_B1 (rx)          : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("FLASH",        AXIM_FLASH)
-REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG)
-REGION_ALIAS("FLASH1",       AXIM_FLASH1)
-
-REGION_ALIAS("STACKRAM", DTCM_RAM)
-REGION_ALIAS("FASTRAM",  DTCM_RAM)
-REGION_ALIAS("RAM",      SRAM1)
-
-INCLUDE "stm32_flash_f7_split.ld"

src/main/target/link/stm32_flash_F765xI_bl.ld

@@ -1,56 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f765.ld
-**
-**  Abstract    : Linker script for STM32F765xITx Device with
-**                2048KByte FLASH, 512KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x00000000 to 0x00003FFF   16K TCM RAM,
-
-0x08000000 to 0x081FFFFF 2048K full flash,
-0x08000000 to 0x08007FFF   32K isr vector, startup code,
-0x08008000 to 0x0800FFFF   32K config,
-0x08010000 to 0x081FFFFF  1984K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rx)           : ORIGIN = 0x00000000, LENGTH = 16K
-
-    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 32K
-    ITCM_FLASH_CFG (r)      : ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)        : ORIGIN = 0x00210000, LENGTH = 1984K
-
-    AXIM_FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 32K
-    AXIM_FIRMWARE (rx)      : ORIGIN = 0x08008000, LENGTH = 32K
-    AXIM_FLASH_CFG (r)      : ORIGIN = 0x08010000, LENGTH = 32K
-
-    DTCM_RAM (rwx)          : ORIGIN = 0x20000000, LENGTH = 128K
-    SRAM1 (rwx)             : ORIGIN = 0x20020000, LENGTH = 368K
-    SRAM2 (rwx)             : ORIGIN = 0x2007C000, LENGTH = 16K
-    MEMORY_B1 (rx)          : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("FLASH",        AXIM_FLASH)
-REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG)
-
-REGION_ALIAS("STACKRAM", DTCM_RAM)
-REGION_ALIAS("FASTRAM",  DTCM_RAM)
-REGION_ALIAS("RAM",      SRAM1)
-
-__firmware_start = ORIGIN(AXIM_FIRMWARE);
-
-INCLUDE "stm32_flash_f7.ld"

src/main/target/link/stm32_flash_F765xI_for_bl.ld

@@ -1,57 +0,0 @@
-/*
-*****************************************************************************
-**
-**  File        : stm32_flash_f765.ld
-**
-**  Abstract    : Linker script for STM32F765xITx Device with
-**                2048KByte FLASH, 512KByte RAM
-**
-*****************************************************************************
-*/
-
-/* Stack & Heap sizes */
-_Min_Heap_Size = 0;
-_Min_Stack_Size = 0x1800;
-
-/* Entry Point */
-ENTRY(Reset_Handler)
-
-/*
-0x00000000 to 0x00003FFF   16K TCM RAM,
-
-0x08000000 to 0x081FFFFF 2048K full flash,
-0x08000000 to 0x08007FFF   32K isr vector, startup code,
-0x08008000 to 0x0800FFFF   32K config,
-0x08010000 to 0x081FFFFF  1984K firmware,
-*/
-
-/* Specify the memory areas */
-MEMORY
-{
-    ITCM_RAM (rx)           : ORIGIN = 0x00000000, LENGTH = 16K
-
-    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 32K
-    ITCM_FLASH_CFG (r)      : ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)        : ORIGIN = 0x00210000, LENGTH = 1984K
-
-    AXIM_FLASH (rx)         : ORIGIN = 0x08008000, LENGTH = 32K
-    AXIM_FLASH_CFG (r)      : ORIGIN = 0x08010000, LENGTH = 32K
-    AXIM_FLASH1 (rx)        : ORIGIN = 0x08018000, LENGTH = 1984K
-
-    DTCM_RAM (rwx)          : ORIGIN = 0x20000000, LENGTH = 128K
-    SRAM1 (rwx)             : ORIGIN = 0x20020000, LENGTH = 368K
-    SRAM2 (rwx)             : ORIGIN = 0x2007C000, LENGTH = 16K
-    MEMORY_B1 (rx)          : ORIGIN = 0x60000000, LENGTH = 0K
-}
-
-REGION_ALIAS("FLASH",        AXIM_FLASH)
-REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG)
-REGION_ALIAS("FLASH1",       AXIM_FLASH1)
-
-REGION_ALIAS("STACKRAM", DTCM_RAM)
-REGION_ALIAS("FASTRAM",  DTCM_RAM)
-REGION_ALIAS("RAM",      SRAM1)
-
-__firmware_start = ORIGIN(AXIM_FLASH);
-
-INCLUDE "stm32_flash_f7_split.ld"

src/main/telemetry/mavlink.c

@@ -89,6 +89,7 @@
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-function"
+#define MAVLINK_COMM_NUM_BUFFERS 1
 #include "common/mavlink.h"
 #pragma GCC diagnostic pop
 
@@ -326,7 +327,14 @@ void checkMAVLinkTelemetryState(void)
 static void mavlinkSendMessage(void)
 {
     uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN];
-    if (telemetryConfig()->mavlink.version == 1) mavSendMsg.magic = MAVLINK_STX_MAVLINK1;
+
+    mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0);
+    if (telemetryConfig()->mavlink.version == 1) {
+        chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+    } else {
+        chan_state->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+    }
+
     int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg);
 
     for (int i = 0; i < msgLength; i++) {
@@ -502,8 +510,9 @@ void mavlinkSendRCChannelsAndRSSI(void)
         GET_CHANNEL_VALUE(6),
         // chan8_raw RC channel 8 value, in microseconds
         GET_CHANNEL_VALUE(7),
-        // rssi Receive signal strength indicator, 0: 0%, 255: 100%
-        scaleRange(getRSSI(), 0, 1023, 0, 255));
+        // rssi Receive signal strength indicator, 0: 0%, 254: 100%
+		//https://github.com/mavlink/mavlink/issues/1027
+        scaleRange(getRSSI(), 0, 1023, 0, 254));
 #undef GET_CHANNEL_VALUE
 
     mavlinkSendMessage();
@@ -733,7 +742,7 @@ void mavlinkSendHUDAndHeartbeat(void)
             mavSystemState = MAV_STATE_ACTIVE;
         }
     }
-    else if (isCalibrating()) {
+    else if (areSensorsCalibrating()) {
         mavSystemState = MAV_STATE_CALIBRATING;
     }
     else {

src/utils/settings.rb

@@ -907,8 +907,8 @@ class Generator
                 unsigned = !$~[:unsigned].empty?
                 bitsize = $~[:bitsize].to_i
                 type_range = unsigned ? 0..(2**bitsize-1) : (-2**(bitsize-1)+1)..(2**(bitsize-1)-1)
-                min = type_range.min if min =~ /\AU?INT\d+_MIN\Z/
-                max = type_range.max if max =~ /\AU?INT\d+_MAX\Z/
+                min = type_range.min if min.to_s =~ /\AU?INT\d+_MIN\Z/
+                max = type_range.max if max.to_s =~ /\AU?INT\d+_MAX\Z/
                 raise "Member #{name} default value has an invalid type, integer or symbol expected" unless default_value.is_a? Integer or default_value.is_a? Symbol
                 raise "Member #{name} default value is outside type's storage range, min #{type_range.min}, max #{type_range.max}" unless default_value.is_a? Symbol or type_range === default_value
                 raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max'

src/utils/update_cli_docs.py

@@ -38,7 +38,7 @@ def parse_settings_yaml():
     with open(SETTINGS_YAML_PATH, "r") as settings_yaml:
         return yaml.load(settings_yaml, Loader=yaml.Loader)
 
-def generate_md_table_from_yaml(settings_yaml):
+def generate_md_from_yaml(settings_yaml):
     """Generate a sorted markdown table with description & default value for each setting"""
     params = {}
     
@@ -84,20 +84,19 @@ def generate_md_table_from_yaml(settings_yaml):
                     "max": member["max"] if "max" in member else ""
                 }
     
-    # MD table header
-    md_table_lines = [
-        "| Variable Name | Default Value | Min | Max | Description |\n",
-        "| ------------- | ------------- | --- | --- | ----------- |\n",
-        ]
-    
-    # Sort the settings by name and build the rows of the table
+    # Sort the settings by name and build the doc
+    output_lines = []
     for param in sorted(params.items()):
-        md_table_lines.append("| {} | {} | {} | {} | {} |\n".format(
-            param[0], param[1]['default'], param[1]['min'], param[1]['max'], param[1]['description']
-        ))
+        output_lines.extend([
+            f"### {param[0]}\n\n",
+            f"{param[1]['description'] if param[1]['description'] else '_// TODO_'}\n\n",
+            "| Default | Min | Max |\n| --- | --- | --- |\n",
+            f"| {param[1]['default']} | {param[1]['min']} | {param[1]['max']} |\n\n",
+            "---\n\n"
+        ])
     
-    # Return the assembled table
-    return md_table_lines
+    # Return the assembled doc body
+    return output_lines
 
 def write_settings_md(lines):
     """Write the contents of the CLI settings docs"""
@@ -185,9 +184,9 @@ if __name__ == "__main__":
         defaults_match = check_defaults(settings_yaml)
         quit(0 if defaults_match else 1)
     
-    md_table_lines = generate_md_table_from_yaml(settings_yaml)
-    settings_md_lines = \
-        ["# CLI Variable Reference\n", "\n" ] + \
-        md_table_lines + \
-        ["\n", "> Note: this table is autogenerated. Do not edit it manually."]
-    write_settings_md(settings_md_lines)
+    output_lines = generate_md_from_yaml(settings_yaml)
+    output_lines = [
+        "# CLI Variable Reference\n\n",
+        "> Note: this document is autogenerated. Do not edit it manually.\n\n"
+    ] + output_lines
+    write_settings_md(output_lines)