Merge branch 'master' into v3.2-eachi

.gitignore

@@ -37,3 +37,6 @@ stm32.mak
 # artefacts for CLion
 /cmake-build-debug/
 /CMakeLists.txt
+
+.vagrant
+ubuntu*.log

.travis.yml

@@ -32,6 +32,7 @@ addons:
       - git
       - libc6-i386
       - time
+      - libblocksruntime-dev
 
 # We use cpp for unit tests, and c for the main project.
 language: cpp

README.md

@@ -101,7 +101,7 @@ Big thanks to current and past contributors:
 * Blackman, Jason (blckmn)
 * ctzsnooze
 * Höglund, Anders (andershoglund)
-* Ledvin, Peter (ledvinap) - **IO code awesomeness!**
+* Ledvina, Petr (ledvinap) - **IO code awesomeness!**
 * kc10kevin
 * Keeble, Gary (MadmanK)
 * Keller, Michael (mikeller) - **Configurator brilliance**

Vagrantfile

@@ -16,6 +16,10 @@ Vagrant.configure(2) do |config|
 
   config.vm.provider "virtualbox" do |v|
       v.memory = 4096
+      v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-interval", 10000]
+      v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-min-adjust", 100]
+      v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", 1]
+      v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-threshold", 10000]
   end
 
   # Enable provisioning with a shell script. Additional provisioners such as
@@ -27,6 +31,7 @@ Vagrant.configure(2) do |config|
     apt-get update
     apt-get install -y git gcc-arm-embedded=6-2017q2-1~xenial1
     apt-get install -y make python gcc clang
+    apt-get install -y libblocksruntime-dev
   SHELL
 end
 

make/mcu/SITL.mk

@@ -25,7 +25,6 @@ MCU_EXCLUDES = \
             drivers/dma.c \
             drivers/pwm_output.c \
             drivers/timer.c \
-            drivers/light_led.c \
             drivers/system.c \
             drivers/rcc.c \
             drivers/serial_escserial.c \

make/source.mk

@@ -159,6 +159,7 @@ FC_SRC = \
             telemetry/smartport.c \
             telemetry/ltm.c \
             telemetry/mavlink.c \
+            telemetry/msp_shared.c \
             telemetry/ibus.c \
             telemetry/ibus_shared.c \
             sensors/esc_sensor.c \

src/main/blackbox/blackbox.c

@@ -318,9 +318,6 @@ typedef struct blackboxSlowState_s {
     bool rxFlightChannelsValid;
 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 
-//From mixer.c:
-extern float motorOutputHigh, motorOutputLow;
-
 //From rc_controls.c
 extern boxBitmask_t rcModeActivationMask;
 
@@ -1701,7 +1698,7 @@ void blackboxInit(void)
     // by default p_denom is 32 and a P-frame is written every 1ms
     // if p_denom is zero then no P-frames are logged
     if (blackboxConfig()->p_denom == 0) {
-        blackboxPInterval = 0;
+        blackboxPInterval = 0; // blackboxPInterval not used when p_denom is zero, so just set it to zero
     } else if (blackboxConfig()->p_denom > blackboxIInterval && blackboxIInterval >= 32) {
         blackboxPInterval = 1;
     } else {

src/main/blackbox/blackbox_io.c

@@ -55,7 +55,7 @@ static struct {
 
 #endif // USE_SDCARD
 
-void blackboxOpen()
+void blackboxOpen(void)
 {
     serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
     if (sharedBlackboxAndMspPort) {
@@ -351,7 +351,7 @@ static void blackboxLogFileCreated(afatfsFilePtr_t file)
     }
 }
 
-static void blackboxCreateLogFile()
+static void blackboxCreateLogFile(void)
 {
     uint32_t remainder = blackboxSDCard.largestLogFileNumber + 1;
 
@@ -372,7 +372,7 @@ static void blackboxCreateLogFile()
  *
  * Keep calling until the function returns true (open is complete).
  */
-static bool blackboxSDCardBeginLog()
+static bool blackboxSDCardBeginLog(void)
 {
     fatDirectoryEntry_t *directoryEntry;
 
@@ -511,7 +511,7 @@ bool isBlackboxDeviceFull(void)
     }
 }
 
-unsigned int blackboxGetLogNumber()
+unsigned int blackboxGetLogNumber(void)
 {
 #ifdef USE_SDCARD
     return blackboxSDCard.largestLogFileNumber;
@@ -523,7 +523,7 @@ unsigned int blackboxGetLogNumber()
  * Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can
  * transmit this iteration.
  */
-void blackboxReplenishHeaderBudget()
+void blackboxReplenishHeaderBudget(void)
 {
     int32_t freeSpace;
 

src/main/blackbox/blackbox_io.h

@@ -53,7 +53,7 @@ bool blackboxDeviceBeginLog(void);
 bool blackboxDeviceEndLog(bool retainLog);
 
 bool isBlackboxDeviceFull(void);
-unsigned int blackboxGetLogNumber();
+unsigned int blackboxGetLogNumber(void);
 
-void blackboxReplenishHeaderBudget();
+void blackboxReplenishHeaderBudget(void);
 blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes);

src/main/build/atomic.c

@@ -0,0 +1,7 @@
+#include <stdint.h>
+
+#include "atomic.h"
+
+#if defined(UNIT_TEST)
+uint8_t atomic_BASEPRI;
+#endif

src/main/build/atomic.h

@@ -17,76 +17,133 @@
 
 #pragma once
 
-// only set_BASEPRI is implemented in device library. It does always create memory barrirer
+#include <stdint.h>
+
+#if !defined(UNIT_TEST)
+// BASEPRI manipulation functions
+// only set_BASEPRI is implemented in device library. It does always create memory barrier
 // missing versions are implemented here
 
-// set BASEPRI and BASEPRI_MAX register, but do not create memory barrier
+// set BASEPRI register, do not create memory barrier
 __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri)
 {
    __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) );
 }
 
+// set BASEPRI_MAX register, do not create memory barrier
 __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri)
 {
    __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
 }
 
-#if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
+// set BASEPRI_MAX register, with memory barrier
+# if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
 __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri)
 {
     __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" );
 }
+# endif
+
 #endif
 
-// cleanup BASEPRI restore function, with global memory barrier
+#if defined(UNIT_TEST)
+// atomic related functions for unittest.
+
+extern uint8_t atomic_BASEPRI;
+
+static inline uint8_t __get_BASEPRI(void)
+{
+    return atomic_BASEPRI;
+}
+
+// restore BASEPRI (called as cleanup function), with global memory barrier
+static inline void __basepriRestoreMem(uint8_t *val)
+{
+    atomic_BASEPRI = *val;
+    asm volatile ("": : :"memory"); // compiler memory barrier
+}
+
+// increase BASEPRI, with global memory barrier, returns true
+static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
+{
+    if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) {
+        atomic_BASEPRI = prio;
+    }
+    asm volatile ("": : :"memory"); // compiler memory barrier
+    return 1;
+}
+
+// restore BASEPRI (called as cleanup function), no memory barrier
+static inline void __basepriRestore(uint8_t *val)
+{
+    atomic_BASEPRI = *val;
+}
+
+// increase BASEPRI, no memory barrier, returns true
+static inline uint8_t __basepriSetRetVal(uint8_t prio)
+{
+    if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) {
+        atomic_BASEPRI = prio;
+    }
+    return 1;
+}
+
+#else
+// ARM BASEPRI manipulation
+
+// restore BASEPRI (called as cleanup function), with global memory barrier
 static inline void __basepriRestoreMem(uint8_t *val)
 {
     __set_BASEPRI(*val);
 }
 
-// set BASEPRI_MAX function, with global memory barrier, returns true
+// set BASEPRI_MAX, with global memory barrier, returns true
 static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
 {
     __set_BASEPRI_MAX(prio);
     return 1;
 }
 
-// cleanup BASEPRI restore function, no memory barrier
+// restore BASEPRI (called as cleanup function), no memory barrier
 static inline void __basepriRestore(uint8_t *val)
 {
     __set_BASEPRI_nb(*val);
 }
 
-// set BASEPRI_MAX function, no memory barrier, returns true
+// set BASEPRI_MAX, no memory barrier, returns true
 static inline uint8_t __basepriSetRetVal(uint8_t prio)
 {
     __set_BASEPRI_MAX_nb(prio);
     return 1;
 }
 
-// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
-// Full memory barrier is placed at start and exit of block
-#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
+#endif
+
+// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit.
+// All exit paths are handled. Implemented as for loop, does intercept break and continue
+// Full memory barrier is placed at start and at exit of block
+// __unused__ attribute is used to supress CLang warning
+#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__ ((__cleanup__ (__basepriRestoreMem), __unused__)) = __get_BASEPRI(), \
                                      __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
 
 // Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier.
-// Be careful when using this, you must use some method to prevent optimizer form breaking things
+// Be careful when using this, you must use some method to prevent optimizer from breaking things
 // - lto is used for Cleanflight compilation, so function call is not memory barrier
 // - use ATOMIC_BARRIER or volatile to protect used variables
 // - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
 // - gcc 5 and later works as intended, generating quite optimal code
-#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
+#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__ ((__cleanup__ (__basepriRestore), __unused__)) = __get_BASEPRI(), \
                                     __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
 
 // ATOMIC_BARRIER
 // Create memory barrier
 // - at the beginning of containing block (value of parameter must be reread from memory)
 // - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use)
-// On gcc 5 and higher, this protects only memory passed as parameter (any type should work)
+// On gcc 5 and higher, this protects only memory passed as parameter (any type can be used)
 // this macro can be used only ONCE PER LINE, but multiple uses per block are fine
 
 #if (__GNUC__ > 6)
-#warning "Please verify that ATOMIC_BARRIER works as intended"
+# warning "Please verify that ATOMIC_BARRIER works as intended"
 // increment version number if BARRIER works
 // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
 // you should check that local variable scope with cleanup spans entire block
@@ -98,15 +155,37 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 # define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
 #endif
 
-// this macro uses local function for cleanup. CLang block can be substituded
+#define ATOMIC_BARRIER_ENTER(dataPtr, refStr)                              \
+    __asm__ volatile ("\t# barrier (" refStr ") enter\n" : "+m" (*(dataPtr)))
+
+#define ATOMIC_BARRIER_LEAVE(dataPtr, refStr)                              \
+    __asm__ volatile ("\t# barrier (" refStr ") leave\n" : "m" (*(dataPtr)))
+
+#if defined(__clang__)
+// CLang version, using Objective C-style block
+// based on https://stackoverflow.com/questions/24959440/rewrite-gcc-cleanup-macro-with-nested-function-for-clang
+typedef void (^__cleanup_block)(void);
+static inline void __do_cleanup(__cleanup_block * b) { (*b)(); }
+
+#define ATOMIC_BARRIER(data)                                            \
+    typeof(data) *__UNIQL(__barrier) = &data;                           \
+    ATOMIC_BARRIER_ENTER(__UNIQL(__barrier), #data);                    \
+    __cleanup_block __attribute__((cleanup(__do_cleanup) __unused__)) __UNIQL(__cleanup) = \
+        ^{  ATOMIC_BARRIER_LEAVE(__UNIQL(__barrier), #data); };         \
+    do {} while(0)                                                      \
+/**/
+#else
+// gcc version, uses local function for cleanup.
 #define ATOMIC_BARRIER(data)                                            \
     __extension__ void  __UNIQL(__barrierEnd)(typeof(data) **__d) {     \
-        __asm__ volatile ("\t# barrier(" #data ")  end\n" : : "m" (**__d));                          \
+         ATOMIC_BARRIER_LEAVE(*__d, #data);                             \
     }                                                                   \
-    typeof(data)  __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
-    __asm__ volatile ("\t# barrier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
+    typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
+    ATOMIC_BARRIER_ENTER(__UNIQL(__barrier), #data);                    \
+    do {} while(0)                                                      \
+/**/
+#endif
 
-
-// define these wrappers for atomic operations, use gcc buildins
+// define these wrappers for atomic operations, using gcc builtins
 #define ATOMIC_OR(ptr, val) __sync_fetch_and_or(ptr, val)
 #define ATOMIC_AND(ptr, val) __sync_fetch_and_and(ptr, val)

src/main/build/version.h

@@ -36,3 +36,5 @@ extern const char* const buildDate;  // "MMM DD YYYY" MMM = Jan/Feb/...
 
 #define BUILD_TIME_LENGTH 8
 extern const char* const buildTime;  // "HH:MM:SS"
+
+#define MSP_API_VERSION_STRING STR(API_VERSION_MAJOR) "." STR(API_VERSION_MINOR)

src/main/cms/cms_menu_blackbox.c

@@ -69,7 +69,7 @@ static char cmsx_BlackboxStatus[CMS_BLACKBOX_STRING_LENGTH];
 static char cmsx_BlackboxDeviceStorageUsed[CMS_BLACKBOX_STRING_LENGTH];
 static char cmsx_BlackboxDeviceStorageFree[CMS_BLACKBOX_STRING_LENGTH];
 
-static void cmsx_Blackbox_GetDeviceStatus()
+static void cmsx_Blackbox_GetDeviceStatus(void)
 {
     char * unit = "B";
 #if defined(USE_SDCARD) || defined(USE_FLASHFS)

src/main/cms/cms_menu_imu.c

@@ -391,6 +391,77 @@ static CMS_Menu cmsx_menuFilterPerProfile = {
     .entries = cmsx_menuFilterPerProfileEntries,
 };
 
+#ifdef USE_COPY_PROFILE_CMS_MENU
+
+static uint8_t cmsx_dstPidProfile;
+static uint8_t cmsx_dstControlRateProfile;
+
+static const char * const cmsx_ProfileNames[] = {
+    "-",
+    "1",
+    "2",
+    "3"
+};
+
+static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
+static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
+
+static long cmsx_menuCopyProfile_onEnter(void)
+{
+    cmsx_dstPidProfile = 0;
+    cmsx_dstControlRateProfile = 0;
+
+    return 0;
+}
+
+static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
+{
+    UNUSED(pDisplay);
+    UNUSED(ptr);
+
+    if (cmsx_dstPidProfile > 0) {
+        pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
+    }
+
+    return 0;
+}
+
+static long cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr)
+{
+    UNUSED(pDisplay);
+    UNUSED(ptr);
+
+    if (cmsx_dstControlRateProfile > 0) {
+        copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex());
+    }
+
+    return 0;
+}
+
+static OSD_Entry cmsx_menuCopyProfileEntries[] = 
+{
+    { "-- COPY PROFILE --", OME_Label, NULL, NULL, 0},
+    
+    { "CPY PID PROF TO",   OME_TAB,      NULL,                        &cmsx_PidProfileTable, 0 },
+    { "COPY PP",           OME_Funcall,  cmsx_CopyPidProfile,         NULL, 0 },
+    { "CPY RATE PROF TO",  OME_TAB,      NULL,                        &cmsx_ControlRateProfileTable, 0 },
+    { "COPY RP",           OME_Funcall,  cmsx_CopyControlRateProfile, NULL, 0 },
+
+    { "BACK", OME_Back, NULL, NULL, 0 },
+    { NULL, OME_END, NULL, NULL, 0 }
+};
+
+CMS_Menu cmsx_menuCopyProfile = {
+    .GUARD_text = "XCPY",
+    .GUARD_type = OME_MENU,
+    .onEnter = cmsx_menuCopyProfile_onEnter,
+    .onExit = NULL,
+    .onGlobalExit = NULL,
+    .entries = cmsx_menuCopyProfileEntries,
+};
+
+#endif
+
 static OSD_Entry cmsx_menuImuEntries[] =
 {
     { "-- IMU --", OME_Label, NULL, NULL, 0},
@@ -404,6 +475,9 @@ static OSD_Entry cmsx_menuImuEntries[] =
     {"RATE",      OME_Submenu, cmsMenuChange,                 &cmsx_menuRateProfile,                                         0},
 
     {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal,                                        0},
+#ifdef USE_COPY_PROFILE_CMS_MENU
+    {"COPY PROF", OME_Submenu, cmsMenuChange,                 &cmsx_menuCopyProfile,                                         0},
+#endif
 
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
@@ -417,4 +491,5 @@ CMS_Menu cmsx_menuImu = {
     .onGlobalExit = NULL,
     .entries = cmsx_menuImuEntries,
 };
+
 #endif // CMS

src/main/cms/cms_menu_vtx_smartaudio.c

@@ -21,7 +21,7 @@
 
 #include "platform.h"
 
-#ifdef CMS
+#if defined(CMS) && defined(VTX_SMARTAUDIO)
 
 #include "common/printf.h"
 #include "common/utils.h"

src/main/cms/cms_menu_vtx_tramp.c

@@ -21,7 +21,8 @@
 
 #include "platform.h"
 
-#ifdef CMS
+#if defined(CMS) && defined(VTX_TRAMP)
+
 #include "common/printf.h"
 #include "common/utils.h"
 
@@ -155,7 +156,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self)
     return MENU_CHAIN_BACK;
 }
 
-static void trampCmsInitSettings()
+static void trampCmsInitSettings(void)
 {
     if (trampBand > 0) trampCmsBand = trampBand;
     if (trampChannel > 0) trampCmsChan = trampChannel;
@@ -173,7 +174,7 @@ static void trampCmsInitSettings()
     }
 }
 
-static long trampCmsOnEnter()
+static long trampCmsOnEnter(void)
 {
     trampCmsInitSettings();
     return 0;

src/main/common/crc.c

@@ -77,3 +77,25 @@ void crc8_dvb_s2_sbuf_append(sbuf_t *dst, uint8_t *start)
     }
     sbufWriteU8(dst, crc);
 }
+
+uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length)
+{
+    const uint8_t *p = (const uint8_t *)data;
+    const uint8_t *pend = p + length;
+
+    for (; p != pend; p++) {
+        crc ^= *p;
+    }
+    return crc;
+}
+
+void crc8_xor_sbuf_append(sbuf_t *dst, uint8_t *start)
+{
+    uint8_t crc = 0;
+    const uint8_t *end = dst->ptr;
+    for (uint8_t *ptr = start; ptr < end; ++ptr) {
+        crc ^= *ptr;
+    }
+    sbufWriteU8(dst, crc);
+}
+

src/main/common/crc.h

@@ -17,9 +17,12 @@
 
 #pragma once
 
+struct sbuf_s;
+
 uint16_t crc16_ccitt(uint16_t crc, unsigned char a);
 uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length);
 uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a);
 uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length);
-struct sbuf_s;
 void crc8_dvb_s2_sbuf_append(struct sbuf_s *dst, uint8_t *start);
+uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length);
+void crc8_xor_sbuf_append(struct sbuf_s *dst, uint8_t *start);

src/main/common/filter.c

@@ -295,6 +295,7 @@ float firFilterLastInput(const firFilter_t *filter)
 
 void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
 {
+    memset(filter, 0, sizeof(firFilterDenoise_t));
     filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE);
 }
 
@@ -303,12 +304,14 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
 {
     filter->state[filter->index] = input;
     filter->movingSum += filter->state[filter->index++];
-    if (filter->index == filter->targetCount)
+    if (filter->index == filter->targetCount) {
         filter->index = 0;
+    }
     filter->movingSum -= filter->state[filter->index];
 
-    if (filter->targetCount >= filter->filledCount)
+    if (filter->targetCount >= filter->filledCount) {
         return filter->movingSum / filter->targetCount;
-    else
+    } else {
         return filter->movingSum / ++filter->filledCount + 1;
+    }
 }

src/main/common/filter.h

@@ -44,7 +44,7 @@ typedef struct biquadFilter_s {
     float x1, x2, y1, y2;
 } biquadFilter_t;
 
-typedef struct firFilterDenoise_s{
+typedef struct firFilterDenoise_s {
     int filledCount;
     int targetCount;
     int index;

src/main/common/streambuf.c

@@ -20,6 +20,13 @@
 
 #include "streambuf.h"
 
+sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
+{
+    sbuf->ptr = ptr;
+    sbuf->end = end;
+    return sbuf;
+}
+
 void sbufWriteU8(sbuf_t *dst, uint8_t val)
 {
     *dst->ptr++ = val;
@@ -54,6 +61,12 @@ void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val)
 }
 
 
+void sbufFill(sbuf_t *dst, uint8_t data, int len)
+{
+    memset(dst->ptr, data, len);
+    dst->ptr += len;
+}
+
 void sbufWriteData(sbuf_t *dst, const void *data, int len)
 {
     memcpy(dst->ptr, data, len);
@@ -65,6 +78,11 @@ void sbufWriteString(sbuf_t *dst, const char *string)
     sbufWriteData(dst, string, strlen(string));
 }
 
+void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string)
+{
+    sbufWriteData(dst, string, strlen(string) + 1);
+}
+
 uint8_t sbufReadU8(sbuf_t *src)
 {
     return *src->ptr++;

src/main/common/streambuf.h

@@ -20,20 +20,23 @@
 #include <stdint.h>
 
 // simple buffer-based serializer/deserializer without implicit size check
-// little-endian encoding implemneted now
 
 typedef struct sbuf_s {
-    uint8_t *ptr;          // data pointer must be first (sbuff_t* is equivalent to uint8_t **)
+    uint8_t *ptr;          // data pointer must be first (sbuf_t* is equivalent to uint8_t **)
     uint8_t *end;
 } sbuf_t;
 
+sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end);
+
 void sbufWriteU8(sbuf_t *dst, uint8_t val);
 void sbufWriteU16(sbuf_t *dst, uint16_t val);
 void sbufWriteU32(sbuf_t *dst, uint32_t val);
 void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val);
 void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val);
+void sbufFill(sbuf_t *dst, uint8_t data, int len);
 void sbufWriteData(sbuf_t *dst, const void *data, int len);
 void sbufWriteString(sbuf_t *dst, const char *string);
+void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string);
 
 uint8_t sbufReadU8(sbuf_t *src);
 uint16_t sbufReadU16(sbuf_t *src);

src/main/common/string_light.c

@@ -0,0 +1,74 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <limits.h>
+
+#include "string_light.h"
+#include "typeconversion.h"
+
+int sl_isalnum(int c)
+{
+    return sl_isdigit(c) || sl_isupper(c) || sl_islower(c);
+}
+
+int sl_isdigit(int c)
+{
+    return (c >= '0' && c <= '9');
+}
+
+int sl_isupper(int c)
+{
+    return (c >= 'A' && c <= 'Z');
+}
+
+int sl_islower(int c)
+{
+    return (c >= 'a' && c <= 'z');
+}
+
+int sl_tolower(int c)
+{
+    return sl_isupper(c) ? (c) - 'A' + 'a' : c;
+}
+
+int sl_toupper(int c)
+{
+    return sl_islower(c) ? (c) - 'a' + 'A' : c;
+}
+
+int sl_strcasecmp(const char * s1, const char * s2)
+{
+    return sl_strncasecmp(s1, s2, INT_MAX);
+}
+
+int sl_strncasecmp(const char * s1, const char * s2, int n)
+{
+    const unsigned char * ucs1 = (const unsigned char *) s1;
+    const unsigned char * ucs2 = (const unsigned char *) s2;
+
+    int d = 0;
+
+    for ( ; n != 0; n--) {
+        const int c1 = sl_tolower(*ucs1++);
+        const int c2 = sl_tolower(*ucs2++);
+        if (((d = c1 - c2) != 0) || (c2 == '\0')) {
+            break;
+        }
+    }
+
+    return d;
+}

src/main/common/string_light.h

@@ -0,0 +1,28 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+int sl_isalnum(int c);
+int sl_isdigit(int c);
+int sl_isupper(int c);
+int sl_islower(int c);
+int sl_tolower(int c);
+int sl_toupper(int c);
+
+int sl_strcasecmp(const char * s1, const char * s2);
+int sl_strncasecmp(const char * s1, const char * s2, int n);

src/main/config/config_eeprom.c

@@ -33,11 +33,7 @@
 
 #include "drivers/system.h"
 
-#ifdef EEPROM_IN_RAM
-extern uint8_t eepromData[EEPROM_SIZE];
-# define __config_start (*eepromData)
-# define __config_end (*ARRAYEND(eepromData))
-#else
+#ifndef EEPROM_IN_RAM
 extern uint8_t __config_start;   // configured via linker script when building binaries.
 extern uint8_t __config_end;
 #endif

src/main/config/config_eeprom.h

@@ -20,7 +20,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-#define EEPROM_CONF_VERSION 161
+#define EEPROM_CONF_VERSION 163
 
 bool isEEPROMContentValid(void);
 bool loadEEPROM(void);

src/main/config/config_streamer.c

@@ -23,8 +23,10 @@
 
 #include "config/config_streamer.h"
 
+#ifndef EEPROM_IN_RAM
 extern uint8_t __config_start;   // configured via linker script when building binaries.
 extern uint8_t __config_end;
+#endif
 
 #if !defined(FLASH_PAGE_SIZE)
 // F1

src/main/config/feature.c

@@ -43,7 +43,7 @@ void intFeatureClearAll(uint32_t *features)
     *features = 0;
 }
 
-void latchActiveFeatures()
+void latchActiveFeatures(void)
 {
     activeFeaturesLatch = featureConfig()->enabledFeatures;
 }
@@ -68,7 +68,7 @@ void featureClear(uint32_t mask)
     intFeatureClear(mask, &featureConfigMutable()->enabledFeatures);
 }
 
-void featureClearAll()
+void featureClearAll(void)
 {
     intFeatureClearAll(&featureConfigMutable()->enabledFeatures);
 }

src/main/config/parameter_group.c

@@ -39,7 +39,7 @@ static uint8_t *pgOffset(const pgRegistry_t* reg)
     return reg->address;
 }
 
-static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base)
+void pgResetInstance(const pgRegistry_t *reg, uint8_t *base)
 {
     const uint16_t regSize = pgSize(reg);
 
@@ -85,7 +85,7 @@ int pgStore(const pgRegistry_t* reg, void *to, int size)
     return take;
 }
 
-void pgResetAll()
+void pgResetAll(void)
 {
     PG_FOREACH(reg) {
         pgReset(reg);

src/main/config/parameter_group.h

@@ -186,6 +186,7 @@ const pgRegistry_t* pgFind(pgn_t pgn);
 
 void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version);
 int pgStore(const pgRegistry_t* reg, void *to, int size);
-void pgResetAll();
+void pgResetAll(void);
+void pgResetInstance(const pgRegistry_t *reg, uint8_t *base);
 bool pgResetCopy(void *copy, pgn_t pgn);
 void pgReset(const pgRegistry_t* reg);

src/main/config/parameter_group_ids.h

@@ -114,8 +114,9 @@
 #define PG_ESCSERIAL_CONFIG 521
 #define PG_CAMERA_CONTROL_CONFIG 522
 #define PG_FRSKY_D_CONFIG 523
-#define PG_FLYSKY_CONFIG 524
-#define PG_BETAFLIGHT_END 524
+#define PG_MAX7456_CONFIG 524
+#define PG_FLYSKY_CONFIG 525
+#define PG_BETAFLIGHT_END 525
 
 
 // OSD configuration (subject to change)

src/main/drivers/accgyro/accgyro.h

@@ -42,8 +42,10 @@
 
 typedef enum {
     GYRO_RATE_1_kHz,
+    GYRO_RATE_1100_Hz,
     GYRO_RATE_3200_Hz,
     GYRO_RATE_8_kHz,
+    GYRO_RATE_9_kHz,
     GYRO_RATE_32_kHz,
 } gyroRateKHz_e;
 
@@ -58,6 +60,7 @@ typedef struct gyroDev_s {
     int32_t gyroZero[XYZ_AXIS_COUNT];
     int32_t gyroADC[XYZ_AXIS_COUNT];                        // gyro data after calibration and alignment
     int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+    int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT];
     int16_t temperature;
     uint8_t lpf;
     gyroRateKHz_e gyroRateKHz;
@@ -70,6 +73,7 @@ typedef struct gyroDev_s {
     mpuDetectionResult_t mpuDetectionResult;
     ioTag_t mpuIntExtiTag;
     mpuConfiguration_t mpuConfiguration;
+    bool gyro_high_fsr;
 } gyroDev_t;
 
 typedef struct accDev_s {
@@ -86,6 +90,7 @@ typedef struct accDev_s {
     sensor_align_e accAlign;
     mpuDetectionResult_t mpuDetectionResult;
     mpuConfiguration_t mpuConfiguration;
+    bool acc_high_fsr;
 } accDev_t;
 
 static inline void accDevLock(accDev_t *acc)

src/main/drivers/accgyro/accgyro_mpu.c

@@ -39,16 +39,17 @@
 #include "drivers/system.h"
 #include "drivers/time.h"
 
-#include "accgyro.h"
-#include "accgyro_mpu3050.h"
-#include "accgyro_mpu6050.h"
-#include "accgyro_mpu6500.h"
-#include "accgyro_spi_bmi160.h"
-#include "accgyro_spi_icm20689.h"
-#include "accgyro_spi_mpu6000.h"
-#include "accgyro_spi_mpu6500.h"
-#include "accgyro_spi_mpu9250.h"
-#include "accgyro_mpu.h"
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_mpu3050.h"
+#include "drivers/accgyro/accgyro_mpu6050.h"
+#include "drivers/accgyro/accgyro_mpu6500.h"
+#include "drivers/accgyro/accgyro_spi_bmi160.h"
+#include "drivers/accgyro/accgyro_spi_icm20649.h"
+#include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_mpu6000.h"
+#include "drivers/accgyro/accgyro_spi_mpu6500.h"
+#include "drivers/accgyro/accgyro_spi_mpu9250.h"
+#include "drivers/accgyro/accgyro_mpu.h"
 
 
 mpuResetFnPtr mpuResetFn;
@@ -289,6 +290,22 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
     }
 #endif
 
+#ifdef USE_GYRO_SPI_ICM20649
+#ifdef ICM20649_SPI_INSTANCE
+    spiBusSetInstance(&gyro->bus, ICM20649_SPI_INSTANCE);
+#endif
+#ifdef ICM20649_CS_PIN
+    gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20649_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
+#endif
+    sensor = icm20649SpiDetect(&gyro->bus);
+    if (sensor != MPU_NONE) {
+        gyro->mpuDetectionResult.sensor = sensor;
+        gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer;
+        gyro->mpuConfiguration.writeFn = spiBusWriteRegister;
+        return true;
+    }
+#endif
+
 #ifdef USE_GYRO_SPI_ICM20689
 #ifdef ICM20689_SPI_INSTANCE
     spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE);

src/main/drivers/accgyro/accgyro_mpu.h

@@ -23,7 +23,8 @@
 
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
-#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) ||  defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) ||  defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \
+ || defined(USE_GYRO_SPI_ICM20689)
 #define GYRO_USES_SPI
 #endif
 
@@ -40,6 +41,7 @@
 #define ICM20601_WHO_AM_I_CONST             (0xAC)
 #define ICM20602_WHO_AM_I_CONST             (0x12)
 #define ICM20608G_WHO_AM_I_CONST            (0xAF)
+#define ICM20649_WHO_AM_I_CONST             (0xE1)
 #define ICM20689_WHO_AM_I_CONST             (0x98)
 
 
@@ -189,6 +191,7 @@ typedef enum {
     ICM_20601_SPI,
     ICM_20602_SPI,
     ICM_20608_SPI,
+    ICM_20649_SPI,
     ICM_20689_SPI,
     BMI_160_SPI,
 } mpuSensor_e;

src/main/drivers/accgyro/accgyro_spi_icm20649.c

@@ -0,0 +1,200 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "drivers/bus_spi.h"
+#include "drivers/exti.h"
+#include "drivers/gyro_sync.h"
+#include "drivers/io.h"
+#include "drivers/sensor.h"
+#include "drivers/time.h"
+
+#include "accgyro.h"
+#include "accgyro_mpu.h"
+#include "accgyro_spi_icm20649.h"
+
+static void icm20649SpiInit(const busDevice_t *bus)
+{
+    static bool hardwareInitialised = false;
+
+    if (hardwareInitialised) {
+        return;
+    }
+
+    IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0);
+    IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
+    IOHi(bus->busdev_u.spi.csnPin);
+
+    // all registers can be read/written at full speed (7MHz +-10%)
+    // TODO verify that this works at 9MHz and 10MHz on non F7
+    spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
+
+    hardwareInitialised = true;
+}
+
+uint8_t icm20649SpiDetect(const busDevice_t *bus)
+{
+    icm20649SpiInit(bus);
+
+    spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
+
+    spiBusWriteRegister(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
+    delay(15);
+
+    spiBusWriteRegister(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
+
+    uint8_t icmDetected = MPU_NONE;
+    uint8_t attemptsRemaining = 20;
+    do {
+        delay(150);
+        const uint8_t whoAmI = spiBusReadRegister(bus, ICM20649_RA_WHO_AM_I);
+        if (whoAmI == ICM20649_WHO_AM_I_CONST) {
+            icmDetected = ICM_20649_SPI;
+        } else {
+            icmDetected = MPU_NONE;
+        }
+        if (icmDetected != MPU_NONE) {
+            break;
+        }
+        if (!attemptsRemaining) {
+            return MPU_NONE;
+        }
+    } while (attemptsRemaining--);
+
+    return icmDetected;
+
+}
+
+void icm20649AccInit(accDev_t *acc)
+{
+    // 2,048 LSB/g 16g
+    // 1,024 LSB/g 30g
+    acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048;
+
+    spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD);
+
+    acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
+    delay(15);
+    const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
+    acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
+    delay(15);
+    acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
+    delay(15);
+}
+
+bool icm20649SpiAccDetect(accDev_t *acc)
+{
+    if (acc->mpuDetectionResult.sensor != ICM_20649_SPI) {
+        return false;
+    }
+
+    acc->initFn = icm20649AccInit;
+    acc->readFn = icm20649AccRead;
+
+    return true;
+}
+
+
+void icm20649GyroInit(gyroDev_t *gyro)
+{
+    mpuGyroInit(gyro);
+
+    spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); // ensure proper speed
+
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
+    delay(15);
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
+    delay(100);
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
+    delay(15);
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
+    delay(15);
+    const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
+    uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
+    raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3;
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
+    delay(15);
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
+    delay(100);
+
+    // Data ready interrupt configuration
+    // back to bank 0
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
+    delay(15);
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11);  // INT_ANYRD_2CLEAR, BYPASS_EN
+    delay(15);
+
+#ifdef USE_MPU_DATA_READY_SIGNAL
+    gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
+#endif
+}
+
+bool icm20649SpiGyroDetect(gyroDev_t *gyro)
+{
+    if (gyro->mpuDetectionResult.sensor != ICM_20649_SPI)
+        return false;
+
+    gyro->initFn = icm20649GyroInit;
+    gyro->readFn = icm20649GyroReadSPI;
+
+    // 16.4 dps/lsb 2kDps
+    //  8.2 dps/lsb 4kDps
+    gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f);
+
+    return true;
+}
+
+bool icm20649GyroReadSPI(gyroDev_t *gyro)
+{
+    static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+    uint8_t data[7];
+
+    const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7);
+    if (!ack) {
+        return false;
+    }
+
+    gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
+    gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
+    gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
+
+    return true;
+}
+
+bool icm20649AccRead(accDev_t *acc)
+{
+    uint8_t data[6];
+
+    const bool ack = acc->mpuConfiguration.readFn(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
+    if (!ack) {
+        return false;
+    }
+
+    acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
+    acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
+    acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
+
+    return true;
+}

src/main/drivers/accgyro/accgyro_spi_icm20649.h

@@ -0,0 +1,64 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "drivers/bus.h"
+
+#define ICM20649_BIT_RESET                  (0x80)
+
+#define ICM20649_RA_REG_BANK_SEL       0x7F
+
+// BANK 0
+#define ICM20649_RA_WHO_AM_I           0x00
+#define ICM20649_RA_PWR_MGMT_1         0x06
+#define ICM20649_RA_PWR_MGMT_2         0x07
+#define ICM20649_RA_INT_PIN_CFG        0x0F
+#define ICM20649_RA_INT_ENABLE_1       0x11
+#define ICM20649_RA_GYRO_XOUT_H        0x33
+#define ICM20649_RA_ACCEL_XOUT_H       0x2D
+
+// BANK 2
+#define ICM20649_RA_GYRO_SMPLRT_DIV    0x00
+#define ICM20649_RA_GYRO_CONFIG_1      0x01
+#define ICM20649_RA_ACCEL_CONFIG       0x14
+
+enum icm20649_gyro_fsr_e {
+    ICM20649_FSR_500DPS = 0,
+    ICM20649_FSR_1000DPS,
+    ICM20649_FSR_2000DPS,
+    ICM20649_FSR_4000DPS,
+    NUM_ICM20649_GYRO_FSR
+};
+
+enum icm20649_accel_fsr_e {
+    ICM20649_FSR_4G = 0,
+    ICM20649_FSR_8G,
+    ICM20649_FSR_16G,
+    ICM20649_FSR_30G,
+    NUM_ICM20649_ACCEL_FSR
+};
+
+void icm20649AccInit(accDev_t *acc);
+void icm20649GyroInit(gyroDev_t *gyro);
+
+uint8_t icm20649SpiDetect(const busDevice_t *bus);
+
+bool icm20649SpiAccDetect(accDev_t *acc);
+bool icm20649SpiGyroDetect(gyroDev_t *gyro);
+
+bool icm20649GyroReadSPI(gyroDev_t *gyro);
+bool icm20649AccRead(accDev_t *acc);

src/main/drivers/bus_spi.h

@@ -87,6 +87,8 @@ typedef enum SPIDevice {
 #define SPI_DEV_TO_CFG(x)   ((x) + 1)
 
 void spiPreInitCs(ioTag_t iotag);
+void spiPreInitCsOutPU(ioTag_t iotag);
+
 bool spiInit(SPIDevice device);
 void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor);
 uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data);

src/main/drivers/bus_spi_config.c

@@ -25,7 +25,14 @@
 
 // Bring a pin for possible CS line to pull-up state in preparation for
 // sequential initialization by relevant drivers.
-// Note that the pin is set to input for safety at this point.
+
+// There are two versions:
+// spiPreInitCs set the pin to input with pullup (IOCFG_IPU) for safety at this point.
+// spiPreInitCsOutPU which actually drive the pin for digital hi.
+//
+// The later is required for SPI slave devices on some targets, interfaced through level shifters, such as Kakute F4.
+// Note that with this handling, a pin declared as CS pin for MAX7456 needs special care when re-purposing the pin for other, especially, input uses.
+// This will/should be fixed when we go fully reconfigurable.
 
 void spiPreInitCs(ioTag_t iotag)
 {
@@ -35,3 +42,13 @@ void spiPreInitCs(ioTag_t iotag)
         IOConfigGPIO(io, IOCFG_IPU);
     }
 }
+
+void spiPreInitCsOutPU(ioTag_t iotag)
+{
+    IO_t io = IOGetByTag(iotag);
+    if (io) {
+        IOInit(io, OWNER_SPI_PREINIT, 0);
+        IOConfigGPIO(io, IOCFG_OUT_PP);
+        IOHi(io);
+    }
+}

src/main/drivers/camera_control.c

@@ -63,6 +63,7 @@ PG_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig,
     .mode = CAMERA_CONTROL_MODE_HARDWARE_PWM,
     .refVoltage = 330,
     .keyDelayMs = 180,
+    .internalResistance = 470,
     .ioTag = IO_TAG(CAMERA_CONTROL_PIN)
 );
 
@@ -76,14 +77,14 @@ static struct {
 static uint32_t endTimeMillis;
 
 #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
-void TIM6_DAC_IRQHandler()
+void TIM6_DAC_IRQHandler(void)
 {
     IOHi(cameraControlRuntime.io);
 
     TIM6->SR = 0;
 }
 
-void TIM7_IRQHandler()
+void TIM7_IRQHandler(void)
 {
     IOLo(cameraControlRuntime.io);
 
@@ -91,7 +92,7 @@ void TIM7_IRQHandler()
 }
 #endif
 
-void cameraControlInit()
+void cameraControlInit(void)
 {
     if (cameraControlConfig()->ioTag == IO_TAG_NONE)
         return;
@@ -107,10 +108,10 @@ void cameraControlInit()
             return;
         }
 
-        #ifdef USE_HAL_DRIVER
-        IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
+        #ifdef STM32F1
+            IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP);
         #else
-        IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP);
+            IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
         #endif
 
         pwmOutConfig(&cameraControlRuntime.channel, timerHardware, CAMERA_CONTROL_TIMER_HZ, CAMERA_CONTROL_PWM_RESOLUTION, 0, 0);
@@ -158,13 +159,12 @@ void cameraControlProcess(uint32_t currentTimeUs)
     }
 }
 
-static const int cameraPullUpResistance = 47000;
 static const int buttonResistanceValues[] = { 45000, 27000, 15000, 6810, 0 };
 
 static float calculateKeyPressVoltage(const cameraControlKey_e key)
 {
     const int buttonResistance = buttonResistanceValues[key];
-    return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (cameraPullUpResistance + buttonResistance);
+    return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance);
 }
 
 #if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)

src/main/drivers/camera_control.h

@@ -38,15 +38,18 @@ typedef enum {
 
 typedef struct cameraControlConfig_s {
     cameraControlMode_e mode;
+    // measured in 10 mV steps
     uint16_t refVoltage;
     uint16_t keyDelayMs;
+    // measured 100 Ohm steps
+    uint16_t internalResistance;
 
     ioTag_t ioTag;
 } cameraControlConfig_t;
 
 PG_DECLARE(cameraControlConfig_t, cameraControlConfig);
 
-void cameraControlInit();
+void cameraControlInit(void);
 
 void cameraControlProcess(uint32_t currentTimeUs);
 void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs);

src/main/drivers/cc2500.c

@@ -73,7 +73,7 @@ void cc2500SetPower(uint8_t power)
     cc2500WriteReg(CC2500_3E_PATABLE, patable[power]);
 }
 
-uint8_t cc2500Reset()
+uint8_t cc2500Reset(void)
 {
     cc2500Strobe(CC2500_SRES);
     delayMicroseconds(1000); // 1000us

src/main/drivers/cc2500.h

@@ -150,4 +150,4 @@ uint8_t cc2500ReadReg(uint8_t reg);
 void cc2500Strobe(uint8_t address);
 uint8_t cc2500WriteReg(uint8_t address, uint8_t data);
 void cc2500SetPower(uint8_t power);
-uint8_t cc2500Reset();
+uint8_t cc2500Reset(void);

src/main/drivers/compass/compass_ak8963.c

@@ -152,7 +152,7 @@ static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
 }
 #endif
 
-static bool ak8963Init()
+static bool ak8963Init(void)
 {
     uint8_t calibration[3];
     uint8_t status;

src/main/drivers/compass/compass_ak8975.c

@@ -59,7 +59,7 @@
 #define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
 #define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
 
-static bool ak8975Init()
+static bool ak8975Init(void)
 {
     uint8_t buffer[3];
     uint8_t status;

src/main/drivers/exti.c

@@ -207,9 +207,9 @@ void EXTI_IRQHandler(void)
 
 _EXTI_IRQ_HANDLER(EXTI0_IRQHandler);
 _EXTI_IRQ_HANDLER(EXTI1_IRQHandler);
-#if defined(STM32F1) || defined(STM32F7)
+#if defined(STM32F1) || defined(STM32F4) || defined(STM32F7)
 _EXTI_IRQ_HANDLER(EXTI2_IRQHandler);
-#elif defined(STM32F3) || defined(STM32F4)
+#elif defined(STM32F3)
 _EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler);
 #else
 # warning "Unknown CPU"

src/main/drivers/gyro_sync.c

@@ -36,17 +36,32 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin
             gyro->gyroRateKHz = GYRO_RATE_32_kHz;
             gyroSamplePeriod = 31.5f;
         } else {
-#ifdef USE_ACCGYRO_BMI160
-            gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
-            gyroSamplePeriod = 312.0f;
-#else
-            gyro->gyroRateKHz = GYRO_RATE_8_kHz;
-            gyroSamplePeriod = 125.0f;
-#endif
+            switch (gyro->mpuDetectionResult.sensor) {
+            case BMI_160_SPI:
+                gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
+                gyroSamplePeriod = 312.0f;
+                break;
+            case ICM_20649_SPI:
+                gyro->gyroRateKHz = GYRO_RATE_9_kHz;
+                gyroSamplePeriod = 1000000.0f / 9000.0f;
+                break;
+            default:
+                gyro->gyroRateKHz = GYRO_RATE_8_kHz;
+                gyroSamplePeriod = 125.0f;
+                break;
+            }
         }
     } else {
-        gyro->gyroRateKHz = GYRO_RATE_1_kHz;
-        gyroSamplePeriod = 1000.0f;
+        switch (gyro->mpuDetectionResult.sensor) {
+        case ICM_20649_SPI:
+            gyro->gyroRateKHz = GYRO_RATE_1100_Hz;
+            gyroSamplePeriod = 1000000.0f / 1100.0f;
+            break;
+        default:
+            gyro->gyroRateKHz = GYRO_RATE_1_kHz;
+            gyroSamplePeriod = 1000.0f;
+            break;
+        }
         gyroSyncDenominator = 1; // Always full Sampling 1khz
     }
 

src/main/drivers/io.c

@@ -86,21 +86,22 @@ ioRec_t* IO_Rec(IO_t io)
 
 GPIO_TypeDef* IO_GPIO(IO_t io)
 {
-    ioRec_t *ioRec = IO_Rec(io);
+    const ioRec_t *ioRec = IO_Rec(io);
     return ioRec->gpio;
 }
 
 uint16_t IO_Pin(IO_t io)
 {
-    ioRec_t *ioRec = IO_Rec(io);
+    const ioRec_t *ioRec = IO_Rec(io);
     return ioRec->pin;
 }
 
 // port index, GPIOA == 0
 int IO_GPIOPortIdx(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return -1;
+    }
     return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10);     // ports are 0x400 apart
 }
 
@@ -117,8 +118,9 @@ int IO_GPIO_PortSource(IO_t io)
 // zero based pin index
 int IO_GPIOPinIdx(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return -1;
+    }
     return 31 - __builtin_clz(IO_Pin(io));  // CLZ is a bit faster than FFS
 }
 
@@ -135,8 +137,9 @@ int IO_GPIO_PinSource(IO_t io)
 // mask on stm32f103, bit index on stm32f303
 uint32_t IO_EXTI_Line(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return 0;
+    }
 #if defined(STM32F1)
     return 1 << IO_GPIOPinIdx(io);
 #elif defined(STM32F3)
@@ -154,8 +157,9 @@ uint32_t IO_EXTI_Line(IO_t io)
 
 bool IORead(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return false;
+    }
 #if defined(USE_HAL_DRIVER)
     return (LL_GPIO_ReadInputPort(IO_GPIO(io)) & IO_Pin(io));
 #else
@@ -165,8 +169,9 @@ bool IORead(IO_t io)
 
 void IOWrite(IO_t io, bool hi)
 {
-    if (!io)
+    if (!io) {
         return;
+    }
 #if defined(USE_HAL_DRIVER)
     LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io) << (hi ? 0 : 16));
 #elif defined(STM32F4)
@@ -183,8 +188,9 @@ void IOWrite(IO_t io, bool hi)
 
 void IOHi(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return;
+    }
 #if defined(USE_HAL_DRIVER)
     LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io));
 #elif defined(STM32F4)
@@ -196,8 +202,9 @@ void IOHi(IO_t io)
 
 void IOLo(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return;
+    }
 #if defined(USE_HAL_DRIVER)
     LL_GPIO_ResetOutputPin(IO_GPIO(io), IO_Pin(io));
 #elif defined(STM32F4)
@@ -209,8 +216,9 @@ void IOLo(IO_t io)
 
 void IOToggle(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return;
+    }
 
     uint32_t mask = IO_Pin(io);
     // Read pin state from ODR but write to BSRR because it only changes the pins
@@ -238,8 +246,9 @@ void IOToggle(IO_t io)
 // claim IO pin, set owner and resources
 void IOInit(IO_t io, resourceOwner_e owner, uint8_t index)
 {
-    if (!io)
+    if (!io) {
         return;
+    }
     ioRec_t *ioRec = IO_Rec(io);
     ioRec->owner = owner;
     ioRec->index = index;
@@ -247,17 +256,19 @@ void IOInit(IO_t io, resourceOwner_e owner, uint8_t index)
 
 void IORelease(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return;
+    }
     ioRec_t *ioRec = IO_Rec(io);
     ioRec->owner = OWNER_FREE;
 }
 
 resourceOwner_e IOGetOwner(IO_t io)
 {
-    if (!io)
+    if (!io) {
         return OWNER_FREE;
-    ioRec_t *ioRec = IO_Rec(io);
+    }
+    const ioRec_t *ioRec = IO_Rec(io);
     return ioRec->owner;
 }
 
@@ -265,9 +276,11 @@ resourceOwner_e IOGetOwner(IO_t io)
 
 void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 {
-    if (!io)
+    if (!io) {
         return;
-    rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+    }
+
+    const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
     RCC_ClockCmd(rcc, ENABLE);
 
     GPIO_InitTypeDef init = {
@@ -287,9 +300,11 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 
 void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
 {
-    if (!io)
+    if (!io) {
         return;
-    rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+    }
+
+    const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
     RCC_ClockCmd(rcc, ENABLE);
 
     LL_GPIO_InitTypeDef init = {
@@ -308,9 +323,11 @@ void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
 
 void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 {
-    if (!io)
+    if (!io) {
         return;
-    rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+    }
+
+    const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
     RCC_ClockCmd(rcc, ENABLE);
 
     GPIO_InitTypeDef init = {
@@ -325,10 +342,11 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 
 void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
 {
-    if (!io)
+    if (!io) {
         return;
+    }
 
-    rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+    const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
     RCC_ClockCmd(rcc, ENABLE);
     GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af);
 
@@ -360,7 +378,8 @@ ioRec_t ioRecs[1];
 
 // initialize all ioRec_t structures from ROM
 // currently only bitmask is used, this may change in future
-void IOInitGlobal(void) {
+void IOInitGlobal(void)
+{
     ioRec_t *ioRec = ioRecs;
 
     for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) {
@@ -376,14 +395,16 @@ void IOInitGlobal(void) {
 
 IO_t IOGetByTag(ioTag_t tag)
 {
-    int portIdx = DEFIO_TAG_GPIOID(tag);
-    int pinIdx = DEFIO_TAG_PIN(tag);
+    const int portIdx = DEFIO_TAG_GPIOID(tag);
+    const int pinIdx = DEFIO_TAG_PIN(tag);
 
-    if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT)
+    if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT) {
         return NULL;
+    }
     // check if pin exists
-    if (!(ioDefUsedMask[portIdx] & (1 << pinIdx)))
+    if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) {
         return NULL;
+    }
     // count bits before this pin on single port
     int offset = __builtin_popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]);
     // and add port offset

src/main/drivers/light_ws2811strip_hal.c

@@ -38,17 +38,15 @@ static TIM_HandleTypeDef TimHandle;
 static uint16_t timerChannel = 0;
 static bool timerNChannel = false;
 
-void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
-{
-    if (htim->Instance == TimHandle.Instance) {
-        //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL);
-        ws2811LedDataTransferInProgress = 0;
-    }
-}
-
 void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
 {
     HAL_DMA_IRQHandler(TimHandle.hdma[descriptor->userParam]);
+    if(timerNChannel) {
+        HAL_TIMEx_PWMN_Stop_DMA(&TimHandle,timerChannel);
+    } else {
+        HAL_TIM_PWM_Stop_DMA(&TimHandle,timerChannel);
+    }
+    ws2811LedDataTransferInProgress = 0;
 }
 
 void ws2811LedStripHardwareInit(ioTag_t ioTag)
@@ -86,7 +84,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
 
     ws2811IO = IOGetByTag(ioTag);
     IOInit(ws2811IO, OWNER_LED_STRIP, 0);
-    IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction);
+    IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction);
 
     __DMA1_CLK_ENABLE();
 

src/main/drivers/light_ws2811strip_stdperiph.c

@@ -22,6 +22,8 @@
 
 #ifdef LED_STRIP
 
+#include "build/debug.h"
+
 #include "drivers/io.h"
 #include "drivers/nvic.h"
 
@@ -99,14 +101,21 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
     /* PWM1 Mode configuration */
     TIM_OCStructInit(&TIM_OCInitStructure);
     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+
     if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
+#ifndef TEMPORARY_FIX_FOR_LED_ON_NCHAN_AND_HAVE_OUTPUT_INVERTED_FIX_ME_FOR_3_3
+        TIM_OCInitStructure.TIM_OCNPolarity =  TIM_OCNPolarity_High;
+#else
+        TIM_OCInitStructure.TIM_OCNPolarity =  (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
+#endif
     } else {
         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+        TIM_OCInitStructure.TIM_OCPolarity =  (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
     }
-    TIM_OCInitStructure.TIM_OCPolarity =  (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
+
     TIM_OCInitStructure.TIM_Pulse = 0;
 
     timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure);
@@ -115,7 +124,12 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
     TIM_CtrlPWMOutputs(timer, ENABLE);
     TIM_ARRPreloadConfig(timer, ENABLE);
 
-    TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
+    if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+        TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable);
+    } else {
+        TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
+    }
+
     TIM_Cmd(timer, ENABLE);
 
     dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0);

src/main/drivers/max7456.c

@@ -24,8 +24,13 @@
 
 #ifdef USE_MAX7456
 
+#include "build/debug.h"
+
 #include "common/printf.h"
 
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "drivers/bus_spi.h"
 #include "drivers/dma.h"
 #include "drivers/io.h"
@@ -36,6 +41,8 @@
 #include "drivers/time.h"
 #include "drivers/vcd.h"
 
+#include "fc/config.h" // For systemConfig()
+
 // VM0 bits
 #define VIDEO_BUFFER_DISABLE        0x01
 #define MAX7456_RESET               0x02
@@ -148,6 +155,10 @@
 #define NVM_RAM_SIZE            54
 #define WRITE_NVR               0xA0
 
+// Device type
+#define MAX7456_DEVICE_TYPE_MAX 0
+#define MAX7456_DEVICE_TYPE_AT  1
+
 #define CHARS_PER_LINE      30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
 
 // On shared SPI buss we want to change clock for OSD chip and restore for other devices.
@@ -164,6 +175,12 @@
     #define DISABLE_MAX7456       IOHi(max7456CsPin)
 #endif
 
+#ifndef MAX7456_SPI_CLK
+#define MAX7456_SPI_CLK           (SPI_CLOCK_STANDARD)
+#endif
+
+static uint16_t max7456SpiClock = MAX7456_SPI_CLK;
+
 uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
 
 // We write everything in screenBuffer and then compare
@@ -193,6 +210,15 @@ static bool  max7456Lock        = false;
 static bool fontIsLoading       = false;
 static IO_t max7456CsPin        = IO_NONE;
 
+static uint8_t max7456DeviceType;
+
+
+PG_REGISTER_WITH_RESET_TEMPLATE(max7456Config_t, max7456Config, PG_MAX7456_CONFIG, 0);
+
+PG_RESET_TEMPLATE(max7456Config_t, max7456Config,
+    .clockConfig = MAX7456_CLOCK_CONFIG_OC, // SPI clock based on device type and overclock state
+);
+
 
 static uint8_t max7456Send(uint8_t add, uint8_t data)
 {
@@ -387,6 +413,7 @@ void max7456ReInit(void)
 
 
 // Here we init only CS and try to init MAX for first time.
+// Also detect device type (MAX v.s. AT)
 
 void max7456Init(const vcdProfile_t *pVcdProfile)
 {
@@ -399,7 +426,44 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
     IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);
     IOHi(max7456CsPin);
 
-    spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    // Detect device type by writing and reading CA[8] bit at CMAL[6].
+    // Do this at half the speed for safety.
+    spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK * 2);
+
+    max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit
+
+    if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) {
+        max7456DeviceType = MAX7456_DEVICE_TYPE_AT;
+    } else {
+        max7456DeviceType = MAX7456_DEVICE_TYPE_MAX;
+    }
+
+#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
+    // Determine SPI clock divisor based on config and the device type.
+
+    switch (max7456Config()->clockConfig) {
+    case MAX7456_CLOCK_CONFIG_HALF:
+        max7456SpiClock = MAX7456_SPI_CLK * 2;
+        break;
+
+    case MAX7456_CLOCK_CONFIG_OC:
+        max7456SpiClock = (systemConfig()->cpu_overclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? MAX7456_SPI_CLK * 2 : MAX7456_SPI_CLK;
+        break;
+
+    case MAX7456_CLOCK_CONFIG_FULL:
+        max7456SpiClock = MAX7456_SPI_CLK;
+        break;
+    }
+
+#ifdef DEBUG_MAX7456_SPI_CLOCK
+    debug[0] = systemConfig()->cpu_overclock;
+    debug[1] = max7456DeviceType;
+    debug[2] = max7456SpiClock;
+#endif
+#endif
+
+    spiSetDivisor(MAX7456_SPI_INSTANCE, max7456SpiClock);
+
     // force soft reset on Max7456
     ENABLE_MAX7456;
     max7456Send(MAX7456ADD_VM0, MAX7456_RESET);
@@ -484,8 +548,6 @@ bool max7456DmaInProgress(void)
 #endif
 }
 
-#include "build/debug.h"
-
 void max7456DrawScreen(void)
 {
     uint8_t stallCheck;

src/main/drivers/max7456.h

@@ -48,3 +48,13 @@ void    max7456ClearScreen(void);
 void    max7456RefreshAll(void);
 uint8_t* max7456GetScreenBuffer(void);
 bool    max7456DmaInProgress(void);
+
+typedef struct max7456Config_s {
+    uint8_t clockConfig; // 0 = force half clock, 1 = half if OC, 2 = force full
+} max7456Config_t;
+
+#define MAX7456_CLOCK_CONFIG_HALF 0
+#define MAX7456_CLOCK_CONFIG_OC   1
+#define MAX7456_CLOCK_CONFIG_FULL 2
+
+PG_DECLARE(max7456Config_t, max7456Config);

src/main/drivers/pwm_output.h

@@ -39,11 +39,11 @@
 
 typedef enum {
     DSHOT_CMD_MOTOR_STOP = 0,
-    DSHOT_CMD_BEEP1,
-    DSHOT_CMD_BEEP2,
-    DSHOT_CMD_BEEP3,
-    DSHOT_CMD_BEEP4,
-    DSHOT_CMD_BEEP5,
+    DSHOT_CMD_BEACON1,
+    DSHOT_CMD_BEACON2,
+    DSHOT_CMD_BEACON3,
+    DSHOT_CMD_BEACON4,
+    DSHOT_CMD_BEACON5,
     DSHOT_CMD_ESC_INFO, // V2 includes settings
     DSHOT_CMD_SPIN_DIRECTION_1,
     DSHOT_CMD_SPIN_DIRECTION_2,
@@ -53,6 +53,14 @@ typedef enum {
     DSHOT_CMD_SAVE_SETTINGS,
     DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
     DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
+    DSHOT_CMD_LED0_ON, // BLHeli32 only
+    DSHOT_CMD_LED1_ON, // BLHeli32 only
+    DSHOT_CMD_LED2_ON, // BLHeli32 only
+    DSHOT_CMD_LED3_ON, // BLHeli32 only
+    DSHOT_CMD_LED0_OFF, // BLHeli32 only
+    DSHOT_CMD_LED1_OFF, // BLHeli32 only
+    DSHOT_CMD_LED2_OFF, // BLHeli32 only
+    DSHOT_CMD_LED3_OFF, // BLHeli32 only
     DSHOT_CMD_MAX = 47
 } dshotCommands_e;
 

src/main/drivers/pwm_output_dshot_hal.c

@@ -83,6 +83,11 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
 {
     motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
     HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]);
+    if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+        HAL_TIMEx_PWMN_Stop_DMA(&motor->TimHandle,motor->timerHardware->channel);
+    } else {
+        HAL_TIM_PWM_Stop_DMA(&motor->TimHandle,motor->timerHardware->channel);
+    }
 }
 
 void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
@@ -96,7 +101,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     const uint8_t timerIndex = getTimerIndex(timer);
 
     IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
-    IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction);
+    IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction);
 
     __DMA1_CLK_ENABLE();
 

src/main/drivers/rx_pwm.c

@@ -385,12 +385,9 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
         IOInit(io, OWNER_PWMINPUT, RESOURCE_INDEX(channel));
 #ifdef STM32F1
         IOConfigGPIO(io, IOCFG_IPD);
-#elif defined(STM32F7)
-        IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
 #else
-        IOConfigGPIO(io, IOCFG_AF_PP);
+        IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
 #endif
-
         timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ);
         timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
         timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
@@ -442,10 +439,8 @@ void ppmRxInit(const ppmConfig_t *ppmConfig)
     IOInit(io, OWNER_PPMINPUT, 0);
 #ifdef STM32F1
     IOConfigGPIO(io, IOCFG_IPD);
-#elif defined(STM32F7)
-    IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
 #else
-    IOConfigGPIO(io, IOCFG_AF_PP);
+    IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
 #endif
 
     timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ);

src/main/drivers/sdcard.c

@@ -632,7 +632,7 @@ static bool sdcard_setBlockLength(uint32_t blockLen)
 /*
  * Returns true if the card is ready to accept read/write commands.
  */
-static bool sdcard_isReady()
+static bool sdcard_isReady(void)
 {
     return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS;
 }
@@ -647,7 +647,7 @@ static bool sdcard_isReady()
  *                                    the SDCARD_READY state.
  *
  */
-static sdcardOperationStatus_e sdcard_endWriteBlocks()
+static sdcardOperationStatus_e sdcard_endWriteBlocks(void)
 {
     sdcard.multiWriteBlocksRemain = 0;
 

src/main/drivers/sdcard.h

@@ -66,11 +66,11 @@ sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer,
 void sdcardInsertionDetectDeinit(void);
 void sdcardInsertionDetectInit(void);
 
-bool sdcard_isInserted();
-bool sdcard_isInitialized();
-bool sdcard_isFunctional();
+bool sdcard_isInserted(void);
+bool sdcard_isInitialized(void);
+bool sdcard_isFunctional(void);
 
-bool sdcard_poll();
-const sdcardMetadata_t* sdcard_getMetadata();
+bool sdcard_poll(void);
+const sdcardMetadata_t* sdcard_getMetadata(void);
 
 void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback);

src/main/drivers/serial_escserial.c

@@ -660,6 +660,10 @@ static serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceive
 
     if (mode != PROTOCOL_KISSALL) {
         escSerial->rxTimerHardware = &(timerHardware[output]);
+        // N-Channels can't be used as RX.
+        if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+            return NULL;
+        }
 #ifdef USE_HAL_DRIVER
         escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
 #endif
@@ -944,7 +948,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
     else {
         uint8_t first_output = 0;
         for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
-            if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) {
+            if (timerHardware[i].usageFlags & TIM_USE_MOTOR) {
                 first_output = i;
                 break;
             }

src/main/drivers/serial_softserial.c

@@ -131,18 +131,14 @@ static void serialInputPortActivate(softSerial_t *softSerial)
     if (softSerial->port.options & SERIAL_INVERTED) {
 #ifdef STM32F1
         IOConfigGPIO(softSerial->rxIO, IOCFG_IPD);
-#elif defined(STM32F7)
-        IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction);
 #else
-        IOConfigGPIO(softSerial->rxIO, IOCFG_AF_PP_PD);
+        IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction);
 #endif
     } else {
 #ifdef STM32F1
         IOConfigGPIO(softSerial->rxIO, IOCFG_IPU);
-#elif defined(STM32F7)
-        IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction);
 #else
-        IOConfigGPIO(softSerial->rxIO, IOCFG_AF_PP_UP);
+        IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction);
 #endif
     }
 
@@ -165,35 +161,35 @@ static void serialInputPortDeActivate(softSerial_t *softSerial)
     TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Disable);
 #endif
 
-#ifdef STM32F7
-    IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction);
-#else
+#ifdef STM32F1
     IOConfigGPIO(softSerial->rxIO, IOCFG_IN_FLOATING);
+#else
+    IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction);
 #endif
     softSerial->rxActive = false;
 }
 
 static void serialOutputPortActivate(softSerial_t *softSerial)
 {
-#ifdef STM32F7
+#ifdef STM32F1
+    IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP);
+#else
     if (softSerial->exTimerHardware)
         IOConfigGPIOAF(softSerial->txIO, IOCFG_OUT_PP, softSerial->exTimerHardware->alternateFunction);
     else
         IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP);
-#else
-    IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP);
 #endif
 }
 
 static void serialOutputPortDeActivate(softSerial_t *softSerial)
 {
-#ifdef STM32F7
+#ifdef STM32F1
+    IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING);
+#else
     if (softSerial->exTimerHardware)
         IOConfigGPIOAF(softSerial->txIO, IOCFG_IN_FLOATING, softSerial->exTimerHardware->alternateFunction);
     else
         IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING);
-#else
-    IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING);
 #endif
 }
 
@@ -254,9 +250,10 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb
     if (options & SERIAL_BIDIR) {
         // If RX and TX pins are both assigned, we CAN use either with a timer.
         // However, for consistency with hardware UARTs, we only use TX pin,
-        // and this pin must have a timer.
-        if (!timerTx)
+        // and this pin must have a timer, and it should not be N-Channel.
+        if (!timerTx || (timerTx->output & TIMER_OUTPUT_N_CHANNEL)) {
             return NULL;
+        }
 
         softSerial->timerHardware = timerTx;
         softSerial->txIO = txIO;
@@ -264,9 +261,10 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb
         IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(portIndex + RESOURCE_SOFT_OFFSET));
     } else {
         if (mode & MODE_RX) {
-            // Need a pin & a timer on RX
-            if (!(tagRx && timerRx))
+            // Need a pin & a timer on RX. Channel should not be N-Channel.
+            if (!timerRx || (timerRx->output & TIMER_OUTPUT_N_CHANNEL)) {
                 return NULL;
+            }
 
             softSerial->rxIO = rxIO;
             softSerial->timerHardware = timerRx;

src/main/drivers/serial_uart_hal.c

@@ -176,7 +176,8 @@ void uartReconfigure(uartPort_t *uartPort)
 
             __HAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0);
         } else {
-            __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE);
+            /* Enable the UART Transmit Data Register Empty Interrupt */
+            SET_BIT(uartPort->USARTx->CR1, USART_CR1_TXEIE);
         }
     }
     return;

src/main/drivers/serial_uart_stm32f7xx.c

@@ -47,7 +47,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART1_RX_DMA
         .rxDMAStream = DMA2_Stream5,
 #endif
+#ifdef USE_UART1_TX_DMA
         .txDMAStream = DMA2_Stream7,
+#endif
         .rxPins = { DEFIO_TAG_E(PA10), DEFIO_TAG_E(PB7), IO_TAG_NONE },
         .txPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PB6), IO_TAG_NONE },
         .af = GPIO_AF7_USART1,
@@ -70,7 +72,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART2_RX_DMA
         .rxDMAStream = DMA1_Stream5,
 #endif
+#ifdef USE_UART2_TX_DMA
         .txDMAStream = DMA1_Stream6,
+#endif
         .rxPins = { DEFIO_TAG_E(PA3), DEFIO_TAG_E(PD6), IO_TAG_NONE },
         .txPins = { DEFIO_TAG_E(PA2), DEFIO_TAG_E(PD5), IO_TAG_NONE },
         .af = GPIO_AF7_USART2,
@@ -93,7 +97,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART3_RX_DMA
         .rxDMAStream = DMA1_Stream1,
 #endif
+#ifdef USE_UART3_TX_DMA
         .txDMAStream = DMA1_Stream3,
+#endif
         .rxPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PC11), DEFIO_TAG_E(PD9) },
         .txPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PC10), DEFIO_TAG_E(PD8) },
         .af = GPIO_AF7_USART3,
@@ -116,7 +122,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART4_RX_DMA
         .rxDMAStream = DMA1_Stream2,
 #endif
+#ifdef USE_UART4_TX_DMA
         .txDMAStream = DMA1_Stream4,
+#endif
         .rxPins = { DEFIO_TAG_E(PA1), DEFIO_TAG_E(PC11), IO_TAG_NONE },
         .txPins = { DEFIO_TAG_E(PA0), DEFIO_TAG_E(PC10), IO_TAG_NONE },
         .af = GPIO_AF8_UART4,
@@ -139,7 +147,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART5_RX_DMA
         .rxDMAStream = DMA1_Stream0,
 #endif
+#ifdef USE_UART5_TX_DMA
         .txDMAStream = DMA1_Stream7,
+#endif
         .rxPins = { DEFIO_TAG_E(PD2), IO_TAG_NONE, IO_TAG_NONE },
         .txPins = { DEFIO_TAG_E(PC12), IO_TAG_NONE, IO_TAG_NONE },
         .af = GPIO_AF8_UART5,
@@ -162,7 +172,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART6_RX_DMA
         .rxDMAStream = DMA2_Stream1,
 #endif
+#ifdef USE_UART6_TX_DMA
         .txDMAStream = DMA2_Stream6,
+#endif
         .rxPins = { DEFIO_TAG_E(PC7), DEFIO_TAG_E(PG9), IO_TAG_NONE },
         .txPins = { DEFIO_TAG_E(PC6), DEFIO_TAG_E(PG14), IO_TAG_NONE },
         .af = GPIO_AF8_USART6,
@@ -185,7 +197,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART7_RX_DMA
         .rxDMAStream = DMA1_Stream3,
 #endif
+#ifdef USE_UART7_TX_DMA
         .txDMAStream = DMA1_Stream1,
+#endif
         .rxPins = { DEFIO_TAG_E(PE7), DEFIO_TAG_E(PF6), IO_TAG_NONE },
         .txPins = { DEFIO_TAG_E(PE8), DEFIO_TAG_E(PF7), IO_TAG_NONE },
         .af = GPIO_AF8_UART7,
@@ -208,7 +222,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART8_RX_DMA
         .rxDMAStream = DMA1_Stream6,
 #endif
+#ifdef USE_UART8_TX_DMA
         .txDMAStream = DMA1_Stream0,
+#endif
         .rxPins = { DEFIO_TAG_E(PE0), IO_TAG_NONE, IO_TAG_NONE },
         .txPins = { DEFIO_TAG_E(PE1), IO_TAG_NONE, IO_TAG_NONE },
         .af = GPIO_AF8_UART8,
@@ -228,9 +244,8 @@ void uartIrqHandler(uartPort_t *s)
 {
     UART_HandleTypeDef *huart = &s->Handle;
     /* UART in mode Receiver ---------------------------------------------------*/
-    if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET))
-    {
-        uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff);
+    if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) {
+        uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t) 0xff);
 
         if (s->port.rxCallback) {
             s->port.rxCallback(rbyte);
@@ -247,42 +262,51 @@ void uartIrqHandler(uartPort_t *s)
     }
 
     /* UART parity error interrupt occurred -------------------------------------*/
-    if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET))
-    {
-      __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF);
+    if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) {
+        __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF);
     }
 
     /* UART frame error interrupt occurred --------------------------------------*/
-    if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET))
-    {
-      __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF);
+    if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) {
+        __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF);
     }
 
     /* UART noise error interrupt occurred --------------------------------------*/
-    if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET))
-    {
-      __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF);
+    if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) {
+        __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF);
     }
 
     /* UART Over-Run interrupt occurred -----------------------------------------*/
-    if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET))
-    {
-      __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF);
+    if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) {
+        __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF);
     }
 
     /* UART in mode Transmitter ------------------------------------------------*/
-    if ((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET))
-    {
-        HAL_UART_IRQHandler(huart);
+    if (!s->txDMAStream && (__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) {
+        /* Check that a Tx process is ongoing */
+        if (huart->gState != HAL_UART_STATE_BUSY_TX) {
+            if (s->port.txBufferTail == s->port.txBufferHead) {
+                huart->TxXferCount = 0;
+                /* Disable the UART Transmit Data Register Empty Interrupt */
+                CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE);
+            } else {
+                if ((huart->Init.WordLength == UART_WORDLENGTH_9B) && (huart->Init.Parity == UART_PARITY_NONE)) {
+                    huart->Instance->TDR = (((uint16_t) s->port.txBuffer[s->port.txBufferTail]) & (uint16_t) 0x01FFU);
+                } else {
+                    huart->Instance->TDR = (uint8_t)(s->port.txBuffer[s->port.txBufferTail]);
+                }
+                s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
+            }
+        }
     }
 
     /* UART in mode Transmitter (transmission end) -----------------------------*/
-    if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET))
-    {
+    if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) {
         HAL_UART_IRQHandler(huart);
-        handleUsartTxDma(s);
+        if (s->txDMAStream) {
+            handleUsartTxDma(s);
+        }
     }
-
 }
 
 static void handleUsartTxDma(uartPort_t *s)
@@ -329,8 +353,15 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
         s->rxDMAChannel = hardware->DMAChannel;
         s->rxDMAStream = hardware->rxDMAStream;
     }
-    s->txDMAChannel = hardware->DMAChannel;
-    s->txDMAStream = hardware->txDMAStream;
+
+    if (hardware->txDMAStream) {
+        s->txDMAChannel = hardware->DMAChannel;
+        s->txDMAStream = hardware->txDMAStream;
+
+        // DMA TX Interrupt
+        dmaInit(hardware->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
+        dmaSetHandler(hardware->txIrq, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev);
+    }
 
     s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR;
     s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR;
@@ -362,16 +393,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
         }
     }
 
-    // DMA TX Interrupt
-    dmaInit(hardware->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-    dmaSetHandler(hardware->txIrq, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev);
-
-
-    //HAL_NVIC_SetPriority(hardware->txIrq, NVIC_PRIORITY_BASE(hardware->txPriority), NVIC_PRIORITY_SUB(hardware->txPriority));
-    //HAL_NVIC_EnableIRQ(hardware->txIrq);
-
-    if (!s->rxDMAChannel)
-    {
+    if (!s->rxDMAChannel) {
         HAL_NVIC_SetPriority(hardware->rxIrq, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority));
         HAL_NVIC_EnableIRQ(hardware->rxIrq);
     }

src/main/drivers/serial_usb_vcp.c

@@ -154,8 +154,9 @@ static void usbVcpBeginWrite(serialPort_t *instance)
     port->buffering = true;
 }
 
-uint32_t usbTxBytesFree()
+static uint32_t usbTxBytesFree(const serialPort_t *instance)
 {
+    UNUSED(instance);
     return CDC_Send_FreeBytes();
 }
 

src/main/drivers/timer.c

@@ -709,6 +709,10 @@ void timerInit(void)
 #if defined(STM32F3) || defined(STM32F4)
     for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) {
         const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex];
+        if (timerHardwarePtr->usageFlags == TIM_USE_NONE) {
+            continue;
+        }
+        // XXX IOConfigGPIOAF in timerInit should eventually go away.
         IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction);
     }
 #endif

src/main/drivers/timer_hal.c

@@ -807,6 +807,10 @@ void timerInit(void)
 #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
     for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) {
         const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex];
+        if (timerHardwarePtr->usageFlags == TIM_USE_NONE) {
+            continue;
+        }
+        // XXX IOConfigGPIOAF in timerInit should eventually go away.
         IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction);
     }
 #endif

src/main/drivers/vtx_common.c

@@ -42,6 +42,11 @@ void vtxCommonRegisterDevice(vtxDevice_t *pDevice)
     vtxDevice = pDevice;
 }
 
+bool vtxCommonDeviceRegistered(void)
+{
+    return vtxDevice;
+}
+
 void vtxCommonProcess(uint32_t currentTimeUs)
 {
     if (!vtxDevice)

src/main/drivers/vtx_common.h

@@ -78,6 +78,7 @@ typedef struct vtxVTable_s {
 
 void vtxCommonInit(void);
 void vtxCommonRegisterDevice(vtxDevice_t *pDevice);
+bool vtxCommonDeviceRegistered(void);
 
 // VTable functions
 void vtxCommonProcess(uint32_t currentTimeUs);

src/main/drivers/vtx_rtc6705_soft_spi.c

@@ -65,11 +65,12 @@ void rtc6705IOInit(void)
     rtc6705LePin   = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN));
     rtc6705ClkPin  = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN));
 
-    IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET);
-    IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP);
-
     IOInit(rtc6705LePin, OWNER_SPI_CS, RESOURCE_SOFT_OFFSET);
     IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP);
+    RTC6705_SPILE_ON;
+
+    IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET);
+    IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP);
 
     IOInit(rtc6705ClkPin, OWNER_SPI_SCK, RESOURCE_SOFT_OFFSET);
     IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP);

src/main/fc/cli.c

@@ -114,6 +114,8 @@ extern uint8_t __config_end;
 #include "io/vtx_rtc6705.h"
 #include "io/vtx_control.h"
 
+#include "msp/msp_protocol.h"
+
 #include "rx/rx.h"
 #include "rx/spektrum.h"
 #include "rx/frsky_d.h"
@@ -205,7 +207,7 @@ static void cliPrint(const char *str)
     bufWriterFlush(cliWriter);
 }
 
-static void cliPrintLinefeed()
+static void cliPrintLinefeed(void)
 {
     cliPrint("\r\n");
 }
@@ -402,12 +404,18 @@ static uint16_t getValueOffset(const clivalue_t *value)
     return 0;
 }
 
-STATIC_UNIT_TESTED void *getValuePointer(const clivalue_t *value)
+void *cliGetValuePointer(const clivalue_t *value)
 {
     const pgRegistry_t* rec = pgFind(value->pgn);
     return CONST_CAST(void *, rec->address + getValueOffset(value));
 }
 
+const void *cliGetDefaultPointer(const clivalue_t *value)
+{
+    const pgRegistry_t* rec = pgFind(value->pgn);
+    return rec->address + getValueOffset(value);
+}
+
 static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask)
 {
     const pgRegistry_t *pg = pgFind(value->pgn);
@@ -448,7 +456,7 @@ static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask)
 
 static void cliPrintVar(const clivalue_t *var, bool full)
 {
-    const void *ptr = getValuePointer(var);
+    const void *ptr = cliGetValuePointer(var);
 
     printValuePointer(var, ptr, full);
 }
@@ -481,7 +489,7 @@ static void cliPrintVarRange(const clivalue_t *var)
 
 static void cliSetVar(const clivalue_t *var, const int16_t value)
 {
-    void *ptr = getValuePointer(var);
+    void *ptr = cliGetValuePointer(var);
 
     switch (var->type & VALUE_TYPE_MASK) {
     case VAR_UINT8:
@@ -888,18 +896,18 @@ static void cliSerialPassthrough(char *cmdline)
 
     while (tok != NULL) {
         switch (index) {
-            case 0:
-                id = atoi(tok);
-                break;
-            case 1:
-                baud = atoi(tok);
-                break;
-            case 2:
-                if (strstr(tok, "rx") || strstr(tok, "RX"))
-                    mode |= MODE_RX;
-                if (strstr(tok, "tx") || strstr(tok, "TX"))
-                    mode |= MODE_TX;
-                break;
+        case 0:
+            id = atoi(tok);
+            break;
+        case 1:
+            baud = atoi(tok);
+            break;
+        case 2:
+            if (strstr(tok, "rx") || strstr(tok, "RX"))
+                mode |= MODE_RX;
+            if (strstr(tok, "tx") || strstr(tok, "TX"))
+                mode |= MODE_TX;
+            break;
         }
         index++;
         tok = strtok_r(NULL, " ", &saveptr);
@@ -1696,28 +1704,28 @@ static void cliSdInfo(char *cmdline)
     cliPrint("'\r\n" "Filesystem: ");
 
     switch (afatfs_getFilesystemState()) {
-        case AFATFS_FILESYSTEM_STATE_READY:
-            cliPrint("Ready");
+    case AFATFS_FILESYSTEM_STATE_READY:
+        cliPrint("Ready");
         break;
-        case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
-            cliPrint("Initializing");
+    case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
+        cliPrint("Initializing");
         break;
-        case AFATFS_FILESYSTEM_STATE_UNKNOWN:
-        case AFATFS_FILESYSTEM_STATE_FATAL:
-            cliPrint("Fatal");
+    case AFATFS_FILESYSTEM_STATE_UNKNOWN:
+    case AFATFS_FILESYSTEM_STATE_FATAL:
+        cliPrint("Fatal");
 
-            switch (afatfs_getLastError()) {
-                case AFATFS_ERROR_BAD_MBR:
-                    cliPrint(" - no FAT MBR partitions");
-                break;
-                case AFATFS_ERROR_BAD_FILESYSTEM_HEADER:
-                    cliPrint(" - bad FAT header");
-                break;
-                case AFATFS_ERROR_GENERIC:
-                case AFATFS_ERROR_NONE:
-                    ; // Nothing more detailed to print
-                break;
-            }
+        switch (afatfs_getLastError()) {
+        case AFATFS_ERROR_BAD_MBR:
+            cliPrint(" - no FAT MBR partitions");
+            break;
+        case AFATFS_ERROR_BAD_FILESYSTEM_HEADER:
+            cliPrint(" - bad FAT header");
+            break;
+        case AFATFS_ERROR_GENERIC:
+        case AFATFS_ERROR_NONE:
+            ; // Nothing more detailed to print
+            break;
+        }
         break;
     }
     cliPrintLinefeed();
@@ -2253,64 +2261,98 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) {
 
 #ifdef USE_DSHOT
 
-#define ESC_INFO_V1_EXPECTED_FRAME_SIZE 15
-#define ESC_INFO_V2_EXPECTED_FRAME_SIZE 21
+#define ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE 15
+#define ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE 21
+#define ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE 64
+
+enum {
+    ESC_INFO_KISS_V1,
+    ESC_INFO_KISS_V2,
+    ESC_INFO_BLHELI32
+};
 
 #define ESC_INFO_VERSION_POSITION 12
 
-void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
+void printEscInfo(const uint8_t *escInfoBuffer, uint8_t bytesRead)
 {
     bool escInfoReceived = false;
     if (bytesRead > ESC_INFO_VERSION_POSITION) {
-        uint8_t escInfoVersion = 0;
-        uint8_t frameLength = 0;
-        if (escInfoBytes[ESC_INFO_VERSION_POSITION] == 255) {
-            escInfoVersion = 2;
-            frameLength = ESC_INFO_V2_EXPECTED_FRAME_SIZE;
+        uint8_t escInfoVersion;
+        uint8_t frameLength;
+        if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 254) {
+            escInfoVersion = ESC_INFO_BLHELI32;
+            frameLength = ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE;
+        } else if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 255) {
+            escInfoVersion = ESC_INFO_KISS_V2;
+            frameLength = ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE;
         } else {
-            escInfoVersion = 1;
-            frameLength = ESC_INFO_V1_EXPECTED_FRAME_SIZE;
+            escInfoVersion = ESC_INFO_KISS_V1;
+            frameLength = ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE;
         }
 
-        if (((escInfoVersion == 1) && (bytesRead == ESC_INFO_V1_EXPECTED_FRAME_SIZE))
-            || ((escInfoVersion == 2) && (bytesRead == ESC_INFO_V2_EXPECTED_FRAME_SIZE))) {
+        if (bytesRead == frameLength) {
             escInfoReceived = true;
 
-            if (calculateCrc8(escInfoBytes, frameLength - 1) == escInfoBytes[frameLength - 1]) {
+            if (calculateCrc8(escInfoBuffer, frameLength - 1) == escInfoBuffer[frameLength - 1]) {
                 uint8_t firmwareVersion = 0;
-                char firmwareSubVersion = 0;
+                uint8_t firmwareSubVersion = 0;
                 uint8_t escType = 0;
                 switch (escInfoVersion) {
-                case 1:
-                    firmwareVersion = escInfoBytes[12];
-                    firmwareSubVersion = (char)((escInfoBytes[13] & 0x1f) + 97);
-                    escType = (escInfoBytes[13] & 0xe0) >> 5;
+                case ESC_INFO_KISS_V1:
+                    firmwareVersion = escInfoBuffer[12];
+                    firmwareSubVersion = (escInfoBuffer[13] & 0x1f) + 97;
+                    escType = (escInfoBuffer[13] & 0xe0) >> 5;
 
                     break;
-                case 2:
-                    firmwareVersion = escInfoBytes[13];
-                    firmwareSubVersion = (char)escInfoBytes[14];
-                    escType = escInfoBytes[15];
+                case ESC_INFO_KISS_V2:
+                    firmwareVersion = escInfoBuffer[13];
+                    firmwareSubVersion = escInfoBuffer[14];
+                    escType = escInfoBuffer[15];
+
+                    break;
+                case ESC_INFO_BLHELI32:
+                    firmwareVersion = escInfoBuffer[13];
+                    firmwareSubVersion = escInfoBuffer[14];
+                    escType = escInfoBuffer[15];
 
                     break;
                 }
 
                 cliPrint("ESC: ");
-                switch (escType) {
-                case 1:
-                    cliPrintLine("KISS8A");
+                switch (escInfoVersion) {
+                case ESC_INFO_KISS_V1:
+                case ESC_INFO_KISS_V2:
+                    switch (escType) {
+                    case 1:
+                        cliPrintLine("KISS8A");
+
+                        break;
+                    case 2:
+                        cliPrintLine("KISS16A");
+
+                        break;
+                    case 3:
+                        cliPrintLine("KISS24A");
+
+                        break;
+                    case 5:
+                        cliPrintLine("KISS Ultralite");
+
+                        break;
+                    default:
+                        cliPrintLine("unknown");
+
+                        break;
+                    }
+
                     break;
-                case 2:
-                    cliPrintLine("KISS16A");
-                    break;
-                case 3:
-                    cliPrintLine("KISS24A");
-                    break;
-                case 5:
-                    cliPrintLine("KISS Ultralite");
-                    break;
-                default:
-                    cliPrintLine("unknown");
+                case ESC_INFO_BLHELI32:
+                    {
+                        char *escType = (char *)(escInfoBuffer + 31);
+                        escType[32] = 0;
+                        cliPrintLine(escType);
+                    }
+
                     break;
                 }
 
@@ -2319,14 +2361,64 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
                     if (i && (i % 3 == 0)) {
                         cliPrint("-");
                     }
-                    cliPrintf("%02x", escInfoBytes[i]);
+                    cliPrintf("%02x", escInfoBuffer[i]);
                 }
                 cliPrintLinefeed();
 
-                cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, firmwareSubVersion);
-                if (escInfoVersion == 2) {
-                    cliPrintLinef("Rotation: %s", escInfoBytes[16] ? "reversed" : "normal");
-                    cliPrintLinef("3D: %s", escInfoBytes[17] ? "on" : "off");
+                switch (escInfoVersion) {
+                case ESC_INFO_KISS_V1:
+                case ESC_INFO_KISS_V2:
+                    cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, (char)firmwareSubVersion);
+
+                    break;
+                case ESC_INFO_BLHELI32:
+                    cliPrintLinef("Firmware: %d.%02d%", firmwareVersion, firmwareSubVersion);
+
+                    break;
+                }
+                if (escInfoVersion == ESC_INFO_KISS_V2 || escInfoVersion == ESC_INFO_BLHELI32) {
+                    cliPrintLinef("Rotation: %s", escInfoBuffer[16] ? "reversed" : "normal");
+                    cliPrintLinef("3D: %s", escInfoBuffer[17] ? "on" : "off");
+                    if (escInfoVersion == ESC_INFO_BLHELI32) {
+                        uint8_t setting = escInfoBuffer[18];
+                        cliPrint("Low voltage limit: ");
+                        switch (setting) {
+                        case 0:
+                            cliPrintLine("off");
+
+                            break;
+                        case 255:
+                            cliPrintLine("unsupported");
+
+                            break;
+                        default:
+                            cliPrintLinef("%d.%01d", setting / 10, setting % 10);
+
+                            break;
+                        }
+
+                        setting = escInfoBuffer[19];
+                        cliPrint("Current limit: ");
+                        switch (setting) {
+                        case 0:
+                            cliPrintLine("off");
+
+                            break;
+                        case 255:
+                            cliPrintLine("unsupported");
+
+                            break;
+                        default:
+                            cliPrintLinef("%d", setting);
+
+                            break;
+                        }
+
+                        for (int i = 0; i < 4; i++) {
+                            setting = escInfoBuffer[i + 20];
+                            cliPrintLinef("LED %d: %s", i, setting ? (setting == 255) ? "unsupported" : "on" : "off");
+                        }
+                    }
                 }
             } else {
                 cliPrintLine("Checksum error.");
@@ -2341,14 +2433,15 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
 
 static void executeEscInfoCommand(uint8_t escIndex)
 {
-    uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE];
     cliPrintLinef("Info for ESC %d:", escIndex);
 
-    startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE);
+    uint8_t escInfoBuffer[ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE];
+
+    startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE);
 
     pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO);
 
-    delay(5);
+    delay(10);
 
     printEscInfo(escInfoBuffer, getNumberEscBytesRead());
 }
@@ -2365,22 +2458,30 @@ static void cliDshotProg(char *cmdline)
     char *pch = strtok_r(cmdline, " ", &saveptr);
     int pos = 0;
     int escIndex = 0;
+    bool firstCommand = true;
     while (pch != NULL) {
         switch (pos) {
-            case 0:
-                escIndex = parseOutputIndex(pch, true);
-                if (escIndex == -1) {
-                    return;
-                }
-
-                break;
-            default:
-                pwmDisableMotors();
+        case 0:
+            escIndex = parseOutputIndex(pch, true);
+            if (escIndex == -1) {
+                return;
+            }
 
+            break;
+        default:
+            {
                 int command = atoi(pch);
                 if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
-                    if (command == DSHOT_CMD_ESC_INFO) {
-                        delay(5); // Wait for potential ESC telemetry transmission to finish
+                    if (firstCommand) {
+                        pwmDisableMotors();
+
+                        if (command == DSHOT_CMD_ESC_INFO) {
+                            delay(5); // Wait for potential ESC telemetry transmission to finish
+                        } else {
+                            delay(1);
+                        }
+
+                        firstCommand = false;
                     }
 
                     if (command != DSHOT_CMD_ESC_INFO) {
@@ -2403,8 +2504,9 @@ static void cliDshotProg(char *cmdline)
                 } else {
                     cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1);
                 }
+            }
 
-                break;
+            break;
         }
 
         pos++;
@@ -2431,34 +2533,34 @@ static void cliEscPassthrough(char *cmdline)
     int escIndex = 0;
     while (pch != NULL) {
         switch (pos) {
-            case 0:
-                if (strncasecmp(pch, "sk", strlen(pch)) == 0) {
-                    mode = PROTOCOL_SIMONK;
-                } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) {
-                    mode = PROTOCOL_BLHELI;
-                } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) {
-                    mode = PROTOCOL_KISS;
-                } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) {
-                    mode = PROTOCOL_KISSALL;
-                } else {
-                    cliShowParseError();
-
-                    return;
-                }
-                break;
-            case 1:
-                escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS);
-                if (escIndex == -1) {
-                    return;
-                }
-
-                break;
-            default:
+        case 0:
+            if (strncasecmp(pch, "sk", strlen(pch)) == 0) {
+                mode = PROTOCOL_SIMONK;
+            } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) {
+                mode = PROTOCOL_BLHELI;
+            } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) {
+                mode = PROTOCOL_KISS;
+            } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) {
+                mode = PROTOCOL_KISSALL;
+            } else {
                 cliShowParseError();
 
                 return;
+            }
+            break;
+        case 1:
+            escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS);
+            if (escIndex == -1) {
+                return;
+            }
 
-                break;
+            break;
+        default:
+            cliShowParseError();
+
+            return;
+
+            break;
 
         }
         pos++;
@@ -2660,11 +2762,21 @@ static void cliSave(char *cmdline)
 
 static void cliDefaults(char *cmdline)
 {
-    UNUSED(cmdline);
+    bool saveConfigs;
+
+    if (isEmpty(cmdline)) {
+        saveConfigs = true;
+    } else if (strncasecmp(cmdline, "nosave", 6) == 0) {
+        saveConfigs = false;
+    } else {
+        return;
+    }
 
     cliPrintHashLine("resetting to defaults");
-    resetEEPROM();
-    cliReboot();
+    resetConfigs();
+    if (saveConfigs) {
+        cliSave(NULL);
+    }
 }
 
 STATIC_UNIT_TESTED void cliGet(char *cmdline)
@@ -2743,7 +2855,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline)
                 bool valueChanged = false;
                 int16_t value  = 0;
                 switch (val->type & VALUE_MODE_MASK) {
-                    case MODE_DIRECT: {
+                case MODE_DIRECT: {
                         int16_t value = atoi(eqptr);
 
                         if (value >= val->config.minmax.min && value <= val->config.minmax.max) {
@@ -2753,7 +2865,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline)
                     }
 
                     break;
-                    case MODE_LOOKUP: {
+                case MODE_LOOKUP: {
                         const lookupTableEntry_t *tableEntry = &lookupTables[val->config.lookup.tableIndex];
                         bool matched = false;
                         for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) {
@@ -2769,7 +2881,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline)
                     }
 
                     break;
-                    case MODE_ARRAY: {
+                case MODE_ARRAY: {
                         const uint8_t arrayLength = val->config.array.length;
                         char *valPtr = eqptr;
 
@@ -2788,7 +2900,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline)
                                 default:
                                 case VAR_UINT8: {
                                     // fetch data pointer
-                                    uint8_t *data = (uint8_t *)getValuePointer(val) + i;
+                                    uint8_t *data = (uint8_t *)cliGetValuePointer(val) + i;
                                     // store value
                                     *data = (uint8_t)atoi((const char*) valPtr);
                                     }
@@ -2796,7 +2908,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline)
 
                                 case VAR_INT8: {
                                     // fetch data pointer
-                                    int8_t *data = (int8_t *)getValuePointer(val) + i;
+                                    int8_t *data = (int8_t *)cliGetValuePointer(val) + i;
                                     // store value
                                     *data = (int8_t)atoi((const char*) valPtr);
                                     }
@@ -2804,7 +2916,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline)
 
                                 case VAR_UINT16: {
                                     // fetch data pointer
-                                    uint16_t *data = (uint16_t *)getValuePointer(val) + i;
+                                    uint16_t *data = (uint16_t *)cliGetValuePointer(val) + i;
                                     // store value
                                     *data = (uint16_t)atoi((const char*) valPtr);
                                     }
@@ -2812,7 +2924,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline)
 
                                 case VAR_INT16: {
                                     // fetch data pointer
-                                    int16_t *data = (int16_t *)getValuePointer(val) + i;
+                                    int16_t *data = (int16_t *)cliGetValuePointer(val) + i;
                                     // store value
                                     *data = (int16_t)atoi((const char*) valPtr);
                                     }
@@ -2979,14 +3091,15 @@ static void cliVersion(char *cmdline)
 {
     UNUSED(cmdline);
 
-    cliPrintLinef("# %s / %s (%s) %s %s / %s (%s)",
+    cliPrintLinef("# %s / %s (%s) %s %s / %s (%s) MSP API: %s",
         FC_FIRMWARE_NAME,
         targetName,
         systemConfig()->boardIdentifier,
         FC_VERSION_STRING,
         buildDate,
         buildTime,
-        shortGitRevision
+        shortGitRevision,
+        MSP_API_VERSION_STRING
     );
 }
 
@@ -3332,7 +3445,7 @@ static void printConfig(char *cmdline, bool doDiff)
 
         if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) {
             cliPrintHashLine("reset configuration to default settings");
-            cliPrint("defaults");
+            cliPrint("defaults nosave");
             cliPrintLinefeed();
         }
 
@@ -3493,14 +3606,11 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
         "\t<+|->[name]", cliBeeper),
 #endif
-#ifdef USE_RX_FRSKY_D
-    CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind),
-#endif
+    CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader),
 #ifdef LED_STRIP
     CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
 #endif
-    CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
-    CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader),
+    CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", "[nosave]", cliDefaults),
     CLI_COMMAND_DEF("diff", "list configuration changes from default",
         "[master|profile|rates|all] {defaults}", cliDiff),
 #ifdef USE_DSHOT
@@ -3522,6 +3632,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
     CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
 #endif
+#ifdef USE_RX_FRSKY_D
+    CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind),
+#endif
 #endif
     CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
 #ifdef GPS

src/main/fc/cli.h

@@ -19,6 +19,10 @@
 
 extern uint8_t cliMode;
 
+struct clivalue_s;
+void *cliGetValuePointer(const struct clivalue_s *value);
+const void *cliGetDefaultPointer(const struct clivalue_s *value);
+
 struct serialConfig_s;
 void cliInit(const struct serialConfig_s *serialConfig);
 void cliProcess(void);

src/main/fc/config.c

@@ -124,7 +124,7 @@ PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig,
     .name = { 0 }
 );
 
-PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 1);
 
 #ifndef USE_OSD_SLAVE
 #if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
@@ -156,12 +156,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
 );
 #endif
 
-#ifdef BEEPER
-PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0);
-PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig,
-    .dshotForward = true
-);
-#endif
 #ifdef USE_ADC
 PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0);
 #endif
@@ -273,7 +267,6 @@ static void setPidProfile(uint8_t pidProfileIndex)
     if (pidProfileIndex < MAX_PROFILE_COUNT) {
         systemConfigMutable()->pidProfileIndex = pidProfileIndex;
         currentPidProfile = pidProfilesMutable(pidProfileIndex);
-        pidInit(currentPidProfile); // re-initialise pid controller to re-initialise filters and config
     }
 }
 
@@ -313,6 +306,7 @@ void activateConfig(void)
 
     resetAdjustmentStates();
 
+    pidInit(currentPidProfile);
     useRcControlsConfig(currentPidProfile);
     useAdjustmentConfig(currentPidProfile);
 
@@ -348,6 +342,13 @@ void validateAndFixConfig(void)
 #endif
 
 #ifndef USE_OSD_SLAVE
+    if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {
+        systemConfigMutable()->activeRateProfile = 0;
+    }
+    if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) {
+        systemConfigMutable()->pidProfileIndex = 0;
+    }
+
     if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
         motorConfigMutable()->mincommand = 1000;
     }
@@ -376,7 +377,7 @@ void validateAndFixConfig(void)
     if (featureConfigured(FEATURE_RX_SPI)) {
         featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP);
     }
-#endif
+#endif // USE_RX_SPI
 
     if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
         featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI);
@@ -387,7 +388,7 @@ void validateAndFixConfig(void)
         if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
             batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
         }
-#endif
+#endif // STM32F10X
         // software serial needs free PWM ports
         featureClear(FEATURE_SOFTSERIAL);
     }
@@ -404,9 +405,9 @@ void validateAndFixConfig(void)
         if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
             batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
         }
-#endif
+#endif // STM32F10X
     }
-#endif
+#endif // USE_SOFTSPI
 
     // Prevent invalid notch cutoff
     if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) {
@@ -414,7 +415,7 @@ void validateAndFixConfig(void)
     }
 
     validateAndFixGyroConfig();
-#endif
+#endif // USE_OSD_SLAVE
 
     if (!isSerialConfigValid(serialConfig())) {
         pgResetFn_serialConfig(serialConfigMutable());
@@ -507,17 +508,13 @@ void validateAndFixGyroConfig(void)
         gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
     }
 
-    float samplingTime = 0.000125f;
-
     if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
         pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
         gyroConfigMutable()->gyro_sync_denom = 1;
         gyroConfigMutable()->gyro_use_32khz = false;
-        samplingTime = 0.001f;
     }
 
     if (gyroConfig()->gyro_use_32khz) {
-        samplingTime = 0.00003125;
         // F1 and F3 can't handle high sample speed.
 #if defined(STM32F1)
         gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
@@ -530,44 +527,69 @@ void validateAndFixGyroConfig(void)
 #endif
     }
 
+    float samplingTime;
+    switch (gyroMpuDetectionResult()->sensor) {
+    case ICM_20649_SPI:
+        samplingTime = 1.0f / 9000.0f;
+        break;
+    case BMI_160_SPI:
+        samplingTime = 0.0003125f;
+        break;
+    default:
+        samplingTime = 0.000125f;
+        break;
+    }
+    if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
+        switch (gyroMpuDetectionResult()->sensor) {
+        case ICM_20649_SPI:
+            samplingTime = 1.0f / 1100.0f;
+            break;
+        default:
+            samplingTime = 0.001f;
+            break;
+        }
+    }
+    if (gyroConfig()->gyro_use_32khz) {
+        samplingTime = 0.00003125;
+    }
+
     // check for looptime restrictions based on motor protocol. Motor times have safety margin
-    const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
     float motorUpdateRestriction;
     switch (motorConfig()->dev.motorPwmProtocol) {
-        case (PWM_TYPE_STANDARD):
+    case PWM_TYPE_STANDARD:
             motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE;
             break;
-        case (PWM_TYPE_ONESHOT125):
+    case PWM_TYPE_ONESHOT125:
             motorUpdateRestriction = 0.0005f;
             break;
-        case (PWM_TYPE_ONESHOT42):
+    case PWM_TYPE_ONESHOT42:
             motorUpdateRestriction = 0.0001f;
             break;
 #ifdef USE_DSHOT
-        case (PWM_TYPE_DSHOT150):
+    case PWM_TYPE_DSHOT150:
             motorUpdateRestriction = 0.000250f;
             break;
-        case (PWM_TYPE_DSHOT300):
+    case PWM_TYPE_DSHOT300:
             motorUpdateRestriction = 0.0001f;
             break;
 #endif
-        default:
-            motorUpdateRestriction = 0.00003125f;
+    default:
+        motorUpdateRestriction = 0.00003125f;
+        break;
     }
 
-    if (!motorConfig()->dev.useUnsyncedPwm) {
-        if (pidLooptime < motorUpdateRestriction) {
-            const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
-
-            pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
-        }
-    } else {
+    if (motorConfig()->dev.useUnsyncedPwm) {
         // Prevent overriding the max rate of motors
         if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) {
             const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
-
             motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
         }
+    } else {
+        const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
+        if (pidLooptime < motorUpdateRestriction) {
+            const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
+            pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
+        }
     }
 }
 #endif
@@ -582,19 +604,12 @@ void readEEPROM(void)
     if (!loadEEPROM()) {
         failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
     }
-#ifndef USE_OSD_SLAVE
-    if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {// sanity check
-        systemConfigMutable()->activeRateProfile = 0;
-    }
-    setControlRateProfile(systemConfig()->activeRateProfile);
-
-    if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) {// sanity check
-        systemConfigMutable()->pidProfileIndex = 0;
-    }
-    setPidProfile(systemConfig()->pidProfileIndex);
-#endif
 
     validateAndFixConfig();
+#ifndef USE_OSD_SLAVE
+    setControlRateProfile(systemConfig()->activeRateProfile);
+    setPidProfile(systemConfig()->pidProfileIndex);
+#endif
     activateConfig();
 
 #ifndef USE_OSD_SLAVE

src/main/fc/config.h

@@ -110,7 +110,7 @@ void setPreferredBeeperOffMask(uint32_t mask);
 void initEEPROM(void);
 void resetEEPROM(void);
 void readEEPROM(void);
-void writeEEPROM();
+void writeEEPROM(void);
 void ensureEEPROMContainsValidData(void);
 
 void saveConfigAndNotify(void);

src/main/fc/controlrate_profile.c

@@ -71,3 +71,11 @@ void changeControlRateProfile(uint8_t controlRateProfileIndex)
     setControlRateProfile(controlRateProfileIndex);
     generateThrottleCurve();
 }
+
+void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex) {
+    if ((dstControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1 && srcControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1)
+        && dstControlRateProfileIndex != srcControlRateProfileIndex
+    ) {
+        memcpy(controlRateProfilesMutable(dstControlRateProfileIndex), controlRateProfilesMutable(srcControlRateProfileIndex), sizeof(controlRateConfig_t));
+    }
+}

src/main/fc/controlrate_profile.h

@@ -41,3 +41,5 @@ extern controlRateConfig_t *currentControlRateProfile;
 
 void setControlRateProfile(uint8_t controlRateProfileIndex);
 void changeControlRateProfile(uint8_t controlRateProfileIndex);
+
+void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex);

src/main/fc/fc_core.c

@@ -109,6 +109,8 @@ int16_t magHold;
 int16_t headFreeModeHold;
 
 static bool reverseMotors = false;
+static bool flipOverAfterCrashMode = false;
+
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
 bool isRXDataNew;
@@ -129,7 +131,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
     saveConfigAndNotify();
 }
 
-static bool isCalibrating()
+static bool isCalibrating(void)
 {
 #ifdef BARO
     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
@@ -188,7 +190,7 @@ void updateArmingStatus(void)
             unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
         }
 
-        if (!STATE(SMALL_ANGLE)) {
+        if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
             setArmingDisabled(ARMING_DISABLED_ANGLE);
         } else {
             unsetArmingDisabled(ARMING_DISABLED_ANGLE);
@@ -261,14 +263,19 @@ void tryArm(void)
             return;
         }
 #ifdef USE_DSHOT
-        if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXDSHOTREVERSE)) {
-            if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) {
-                reverseMotors = false;
+        if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
+            pwmDisableMotors();
+            delay(1);
+
+            if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
+                flipOverAfterCrashMode = false;
                 pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL);
             } else {
-                reverseMotors = true;
+                flipOverAfterCrashMode = true;
                 pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED);
             }
+
+            pwmEnableMotors();
         }
 #endif
 
@@ -361,7 +368,9 @@ void updateMagHold(void)
 }
 #endif
 
-
+/*
+ * processRx called from taskUpdateRxMain
+ */
 void processRx(timeUs_t currentTimeUs)
 {
     static bool armedBeeperOn = false;
@@ -719,7 +728,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     }
 }
 
-bool isMotorsReversed()
+bool isMotorsReversed(void)
 {
     return reverseMotors;
 }
+bool isFlipOverAfterCrashMode(void)
+{
+    return flipOverAfterCrashMode;
+}

src/main/fc/fc_core.h

@@ -36,7 +36,7 @@ PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
 
 union rollAndPitchTrims_u;
 void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
-void handleInflightCalibrationStickPosition();
+void handleInflightCalibrationStickPosition(void);
 
 void resetArmingDisabled(void);
 
@@ -49,3 +49,4 @@ void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);
 bool isMotorsReversed(void);
+bool isFlipOverAfterCrashMode(void);

src/main/fc/fc_init.c

@@ -200,6 +200,9 @@ void spiPreInit(void)
 #ifdef USE_GYRO_SPI_MPU9250
     spiPreInitCs(IO_TAG(MPU9250_CS_PIN));
 #endif
+#ifdef USE_GYRO_SPI_ICM20649
+    spiPreInitCs(IO_TAG(ICM20649_CS_PIN));
+#endif
 #ifdef USE_GYRO_SPI_ICM20689
     spiPreInitCs(IO_TAG(ICM20689_CS_PIN));
 #endif
@@ -210,7 +213,7 @@ void spiPreInit(void)
     spiPreInitCs(IO_TAG(L3GD20_CS_PIN));
 #endif
 #ifdef USE_MAX7456
-    spiPreInitCs(IO_TAG(MAX7456_SPI_CS_PIN));
+    spiPreInitCsOutPU(IO_TAG(MAX7456_SPI_CS_PIN)); // XXX 3.2 workaround for Kakute F4. See comment for spiPreInitCSOutPU.
 #endif
 #ifdef USE_SDCARD
     spiPreInitCs(IO_TAG(SDCARD_SPI_CS_PIN));
@@ -270,20 +273,6 @@ void init(void)
        resetEEPROM();
     }
 
-#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
-    // If F4 Overclocking is set and System core clock is not correct a reset is forced
-    if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) {
-        *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX
-        __disable_irq();
-        NVIC_SystemReset();
-    } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) {
-        *((uint32_t *)0x2001FFF8) = 0x0;        // 128KB SRAM STM32F4XX
-        __disable_irq();
-        NVIC_SystemReset();
-    }
-
-#endif
-
     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 
     //i2cSetOverclock(masterConfig.i2c_overclock);
@@ -348,6 +337,20 @@ void init(void)
     }
 #endif
 
+#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
+    // If F4 Overclocking is set and System core clock is not correct a reset is forced
+    if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) {
+        *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX
+        __disable_irq();
+        NVIC_SystemReset();
+    } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) {
+        *((uint32_t *)0x2001FFF8) = 0x0;        // 128KB SRAM STM32F4XX
+        __disable_irq();
+        NVIC_SystemReset();
+    }
+
+#endif
+
     delay(100);
 
     timerInit();  // timer must be initialized before any channel is allocated
@@ -373,32 +376,6 @@ void init(void)
     serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 #endif
 
-    mixerInit(mixerConfig()->mixerMode);
-#ifdef USE_SERVOS
-    servosInit();
-#endif
-
-    uint16_t idlePulse = motorConfig()->mincommand;
-    if (feature(FEATURE_3D)) {
-        idlePulse = flight3DConfig()->neutral3d;
-    }
-
-    if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
-        featureClear(FEATURE_3D);
-        idlePulse = 0; // brushed motors
-    }
-
-    mixerConfigureOutput();
-    motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
-
-#ifdef USE_SERVOS
-    servoConfigureOutput();
-    if (isMixerUsingServos()) {
-        //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
-        servoDevInit(&servoConfig()->dev);
-    }
-#endif
-
     if (0) {}
 #if defined(USE_PPM)
     else if (feature(FEATURE_RX_PPM)) {
@@ -411,8 +388,6 @@ void init(void)
     }
 #endif
 
-    systemState |= SYSTEM_STATE_MOTORS_READY;
-
 #ifdef BEEPER
     beeperInit(beeperDevConfig());
 #endif
@@ -525,10 +500,33 @@ void init(void)
     LED0_OFF;
     LED1_OFF;
 
-    // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
+    // gyro.targetLooptime set in sensorsAutodetect(),
+    // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
+    validateAndFixGyroConfig();
     pidInit(currentPidProfile);
+    setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
+
+    mixerInit(mixerConfig()->mixerMode);
+
+    uint16_t idlePulse = motorConfig()->mincommand;
+    if (feature(FEATURE_3D)) {
+        idlePulse = flight3DConfig()->neutral3d;
+    }
+    if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
+        featureClear(FEATURE_3D);
+        idlePulse = 0; // brushed motors
+    }
+    mixerConfigureOutput();
+    motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
+    systemState |= SYSTEM_STATE_MOTORS_READY;
 
 #ifdef USE_SERVOS
+    servosInit();
+    servoConfigureOutput();
+    if (isMixerUsingServos()) {
+        //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
+        servoDevInit(&servoConfig()->dev);
+    }
     servosFilterInit();
 #endif
 
@@ -674,7 +672,7 @@ void init(void)
 #endif
 
 #ifdef VTX_RTC6705
-#ifdef VTX_RTC6705OPTIONAL
+#ifdef VTX_RTC6705_OPTIONAL
     if (!vtxCommonDeviceRegistered()) // external VTX takes precedence when configured.
 #endif
     {

src/main/fc/fc_msp.c

@@ -108,6 +108,7 @@
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
+#include "sensors/esc_sensor.h"
 
 #include "telemetry/telemetry.h"
 
@@ -310,7 +311,12 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
     sbufWriteU32(dst, address);
 
     // legacy format does not support compression
+#ifdef USE_HUFFMAN
     const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN;
+#else
+    const uint8_t compressionMethod = NO_COMPRESSION;
+    UNUSED(allowCompression);
+#endif
 
     if (compressionMethod == NO_COMPRESSION) {
         if (!useLegacyFormat) {
@@ -750,8 +756,10 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
 
             if (acc.dev.acc_1G > 512*4) {
                 scale = 8;
-            } else if (acc.dev.acc_1G >= 512) {
+            } else if (acc.dev.acc_1G > 512*2) {
                 scale = 4;
+            } else if (acc.dev.acc_1G >= 512) {
+                scale = 2;
             } else {
                 scale = 1;
             }
@@ -923,6 +931,16 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         break;
 #endif
 
+#ifdef USE_DSHOT
+    case MSP_ESC_SENSOR_DATA:
+        sbufWriteU8(dst, getMotorCount());
+        for (int i = 0; i < getMotorCount(); i++) {         
+            sbufWriteU8(dst, getEscSensorData(i)->temperature);
+            sbufWriteU16(dst, getEscSensorData(i)->rpm);
+        }
+        break;
+#endif
+
 #ifdef GPS
     case MSP_GPS_CONFIG:
         sbufWriteU8(dst, gpsConfig()->provider);
@@ -1162,7 +1180,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
         sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
         sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
-        sbufWriteU8(dst, currentPidProfile->levelSensitivity);
+        sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
         sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
         sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
         break;
@@ -1314,6 +1332,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         }
         break;
 
+    case MSP_COPY_PROFILE:
+        value = sbufReadU8(src);        // 0 = pid profile, 1 = control rate profile
+        uint8_t dstProfileIndex = sbufReadU8(src);
+        uint8_t srcProfileIndex = sbufReadU8(src);
+        if (value == 0) {
+            pidCopyProfile(dstProfileIndex, srcProfileIndex);
+        }
+        else if (value == 1) {
+            copyControlRateProfile(dstProfileIndex, srcProfileIndex);
+        }
+        break;
+
 #if defined(GPS) || defined(MAG)
     case MSP_SET_HEADING:
         magHold = sbufReadU16(src);
@@ -1571,7 +1601,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         currentPidProfile->yawRateAccelLimit = sbufReadU16(src);
         if (sbufBytesRemaining(src) >= 2) {
             currentPidProfile->levelAngleLimit = sbufReadU8(src);
-            currentPidProfile->levelSensitivity = sbufReadU8(src);
+            sbufReadU8(src); // was pidProfile.levelSensitivity
         }
         if (sbufBytesRemaining(src) >= 4) {
             currentPidProfile->itermThrottleThreshold = sbufReadU16(src);

src/main/fc/fc_msp_box.c

@@ -76,7 +76,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXCAMERA1, "CAMERA CONTROL 1", 32},
     { BOXCAMERA2, "CAMERA CONTROL 2", 33},
     { BOXCAMERA3, "CAMERA CONTROL 3", 34 },
-    { BOXDSHOTREVERSE, "DSHOT REVERSE MOTORS", 35 },
+    { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH", 35 },
     { BOXPREARM, "PREARM", 36 },
 };
 
@@ -217,7 +217,7 @@ void initActiveBoxIds(void)
     }
 
     if (isMotorProtocolDshot()) {
-        BME(BOXDSHOTREVERSE);
+        BME(BOXFLIPOVERAFTERCRASH);
     }
 
     if (feature(FEATURE_SERVO_TILT)) {
@@ -290,7 +290,7 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
     const uint64_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON)
         | BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD)
         | BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE)
-        | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXDSHOTREVERSE) | BM(BOX3DDISABLE);
+        | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXFLIPOVERAFTERCRASH) | BM(BOX3DDISABLE);
     STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes);
     for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
         if ((rcModeCopyMask & BM(i))    // mode copy is enabled

src/main/fc/fc_rc.c

@@ -23,14 +23,11 @@
 
 #include "build/debug.h"
 
-#include "common/maths.h"
 #include "common/axis.h"
+#include "common/maths.h"
 #include "common/utils.h"
-#include "common/filter.h"
 
 #include "config/feature.h"
-#include "config/parameter_group.h"
-#include "config/parameter_group_ids.h"
 
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
@@ -40,33 +37,36 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/pid.h"
 #include "rx/rx.h"
 
 #include "scheduler/scheduler.h"
 
 #include "sensors/battery.h"
 
-#include "flight/failsafe.h"
-#include "flight/imu.h"
-#include "flight/pid.h"
-#include "flight/mixer.h"
 
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
 static float throttlePIDAttenuation;
 
-float getSetpointRate(int axis) {
+float getSetpointRate(int axis)
+{
     return setpointRate[axis];
 }
 
-float getRcDeflection(int axis) {
+float getRcDeflection(int axis)
+{
     return rcDeflection[axis];
 }
 
-float getRcDeflectionAbs(int axis) {
+float getRcDeflectionAbs(int axis)
+{
     return rcDeflectionAbs[axis];
 }
 
-float getThrottlePIDAttenuation(void) {
+float getThrottlePIDAttenuation(void)
+{
     return throttlePIDAttenuation;
 }
 
@@ -75,10 +75,8 @@ static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for
 
 void generateThrottleCurve(void)
 {
-    uint8_t i;
-
-    for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
-        int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
+    for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
+        const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
         uint8_t y = 1;
         if (tmp > 0)
             y = 100 - currentControlRateProfile->thrMid8;
@@ -89,7 +87,7 @@ void generateThrottleCurve(void)
     }
 }
 
-int16_t rcLookupThrottle(int32_t tmp)
+static int16_t rcLookupThrottle(int32_t tmp)
 {
     const int32_t tmp2 = tmp / 100;
     // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
@@ -114,6 +112,7 @@ static void calculateSetpointRate(int axis)
         rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f);
     }
 
+    // scale rcCommandf to range [-1.0, 1.0]
     float rcCommandf = rcCommand[axis] / 500.0f;
     rcDeflection[axis] = rcCommandf;
     const float rcCommandfAbs = ABS(rcCommandf);
@@ -135,7 +134,8 @@ static void calculateSetpointRate(int axis)
     setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
 }
 
-static void scaleRcCommandToFpvCamAngle(void) {
+static void scaleRcCommandToFpvCamAngle(void)
+{
     //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
     static uint8_t lastFpvCamAngleDegrees = 0;
     static float cosFactor = 1.0;
@@ -156,23 +156,27 @@ static void scaleRcCommandToFpvCamAngle(void) {
 #define THROTTLE_BUFFER_MAX 20
 #define THROTTLE_DELTA_MS 100
 
- static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
+static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
+{
     static int index;
     static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
+
     const int rxRefreshRateMs = rxRefreshRate / 1000;
     const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
     const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold;
 
     rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
-    if (index >= indexMax)
+    if (index >= indexMax) {
         index = 0;
+    }
 
     const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
 
-    if (ABS(rcCommandSpeed) > throttleVelocityThreshold)
+    if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
         pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
-    else
+    } else {
         pidSetItermAccelerator(1.0f);
+    }
 }
 
 void processRcCommand(void)
@@ -181,10 +185,6 @@ void processRcCommand(void)
     static float rcStepSize[4] = { 0, 0, 0, 0 };
     static int16_t rcInterpolationStepCount;
     static uint16_t currentRxRefreshRate;
-    const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
-    uint16_t rxRefreshRate;
-    bool readyToCalculateRate = false;
-    uint8_t readyToCalculateRateAxisCnt = 0;
 
     if (isRXDataNew) {
         currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
@@ -193,19 +193,24 @@ void processRcCommand(void)
         }
     }
 
+    const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
+    uint16_t rxRefreshRate;
+    bool readyToCalculateRate = false;
+    uint8_t readyToCalculateRateAxisCnt = 0;
+
     if (rxConfig()->rcInterpolation) {
          // Set RC refresh rate for sampling and channels to filter
         switch (rxConfig()->rcInterpolation) {
-            case(RC_SMOOTHING_AUTO):
-                rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
-                break;
-            case(RC_SMOOTHING_MANUAL):
-                rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
-                break;
-            case(RC_SMOOTHING_OFF):
-            case(RC_SMOOTHING_DEFAULT):
-            default:
-                rxRefreshRate = rxGetRefreshRate();
+        case RC_SMOOTHING_AUTO:
+            rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
+            break;
+        case RC_SMOOTHING_MANUAL:
+            rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
+            break;
+        case RC_SMOOTHING_OFF:
+        case RC_SMOOTHING_DEFAULT:
+        default:
+            rxRefreshRate = rxGetRefreshRate();
         }
 
         if (isRXDataNew && rxRefreshRate > 0) {
@@ -239,19 +244,22 @@ void processRcCommand(void)
     }
 
     if (readyToCalculateRate || isRXDataNew) {
-        if (isRXDataNew)
+        if (isRXDataNew) {
             readyToCalculateRateAxisCnt = FD_YAW;
+        }
 
-        for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
+        for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) {
             calculateSetpointRate(axis);
+        }
 
         if (debugMode == DEBUG_RC_INTERPOLATION) {
             debug[2] = rcInterpolationStepCount;
             debug[3] = setpointRate[0];
         }
         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
-        if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
+        if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
             scaleRcCommandToFpvCamAngle();
+        }
 
         isRXDataNew = false;
     }
@@ -329,7 +337,8 @@ void updateRcCommands(void)
     }
 }
 
-void resetYawAxis(void) {
+void resetYawAxis(void)
+{
     rcCommand[YAW] = 0;
     setpointRate[YAW] = 0;
 }

src/main/fc/rc_controls.c

@@ -57,6 +57,7 @@
 #include "sensors/acceleration.h"
 
 #include "rx/rx.h"
+#include "scheduler/scheduler.h"
 
 #include "flight/pid.h"
 #include "flight/navigation.h"
@@ -118,11 +119,22 @@ throttleStatus_e calculateThrottleStatus(void)
     return THROTTLE_HIGH;
 }
 
+#define ARM_DELAY_MS        500
+#define STICK_DELAY_MS      50
+#define STICK_AUTOREPEAT_MS 250
+#define repeatAfter(t) { \
+    rcDelayMs -= (t); \
+    doNotRepeat = false; \
+}
 void processRcStickPositions(throttleStatus_e throttleStatus)
 {
-    static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
-    static uint8_t rcSticks;            // this hold sticks position for command combos
-    static uint8_t rcDisarmTicks;       // this is an extra guard for disarming through switch to prevent that one frame can disarm it
+    // time the sticks are maintained
+    static int16_t rcDelayMs;
+    // hold sticks position for command combos
+    static uint8_t rcSticks;
+    // an extra guard for disarming through switch to prevent that one frame can disarm it
+    static uint8_t rcDisarmTicks;
+    static bool doNotRepeat;
     uint8_t stTmp = 0;
     int i;
 
@@ -132,7 +144,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
     }
 #endif
 
-    // ------------------ STICKS COMMAND HANDLER --------------------
     // checking sticks positions
     for (i = 0; i < 4; i++) {
         stTmp >>= 2;
@@ -142,15 +153,16 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
             stTmp |= 0x40;  // check for MAX
     }
     if (stTmp == rcSticks) {
-        if (rcDelayCommand < 250)
-            rcDelayCommand++;
-    } else
-        rcDelayCommand = 0;
+        if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000))
+            rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000;
+    } else {
+        rcDelayMs = 0;
+        doNotRepeat = false;
+    }
     rcSticks = stTmp;
 
     // perform actions
     if (!isUsingSticksToArm) {
-
         if (IS_RC_MODE_ACTIVE(BOXARM)) {
             rcDisarmTicks = 0;
             // Arming via ARM BOX
@@ -158,7 +170,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
         } else {
             // Disarming via ARM BOX
             resetArmingDisabled();
-
             if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive()  ) {
                 rcDisarmTicks++;
                 if (rcDisarmTicks > 3) {
@@ -170,31 +181,37 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
                 }
             }
         }
-    }
-
-    if (rcDelayCommand != 20) {
-        return;
-    }
-
-    if (isUsingSticksToArm) {
-        // Disarm on throttle down + yaw
-        if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
+    } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
+        if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
+            doNotRepeat = true;
+            // Disarm on throttle down + yaw
             if (ARMING_FLAG(ARMED))
                 disarm();
             else {
-                beeper(BEEPER_DISARM_REPEAT);    // sound tone while stick held
-                rcDelayCommand = 0;              // reset so disarm tone will repeat
+                beeper(BEEPER_DISARM_REPEAT);     // sound tone while stick held
+                repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
+            }
+        }
+        return;
+    } else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
+        if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
+            doNotRepeat = true;
+            if (!ARMING_FLAG(ARMED)) {
+                // Arm via YAW
+                tryArm();
+            } else {
+                resetArmingDisabled();
             }
         }
-    }
-
-    if (ARMING_FLAG(ARMED)) {
-        // actions during armed
         return;
     }
 
+    if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS) {
+        return;
+    }
+    doNotRepeat = true;
+
     // actions during not armed
-    i = 0;
 
     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
         // GYRO calibration
@@ -221,6 +238,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
     }
 
     // Multiple configuration profiles
+    i = 0;
     if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
         i = 1;
     else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
@@ -236,18 +254,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
         saveConfigAndNotify();
     }
 
-    if (isUsingSticksToArm) {
-
-        if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
-            // Arm via YAW
-            tryArm();
-
-            return;
-        } else {
-            resetArmingDisabled();
-        }
-    }
-
     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
         // Calibrating Acc
         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
@@ -283,7 +289,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
     }
     if (shouldApplyRollAndPitchTrimDelta) {
         applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
-        rcDelayCommand = 0; // allow autorepetition
+        repeatAfter(STICK_AUTOREPEAT_MS);
         return;
     }
 
@@ -325,7 +331,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
         cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
     } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
-   }
+    }
 #endif
 }
 

src/main/fc/rc_modes.h

@@ -57,7 +57,7 @@ typedef enum {
     BOXCAMERA1,
     BOXCAMERA2,
     BOXCAMERA3,
-    BOXDSHOTREVERSE,
+    BOXFLIPOVERAFTERCRASH,
     BOXPREARM,
     CHECKBOX_ITEM_COUNT
 } boxId_e;

src/main/fc/runtime_config.c

@@ -31,8 +31,8 @@ static uint32_t enabledSensors = 0;
 
 #if defined(OSD) || !defined(MINIMAL_CLI)
 const char *armingDisableFlagNames[]= {
-    "NOGYRO", "FAILSAFE", "RX LOSS", "BAD RX", "BOXFAILSAFE",
-    "THROTTLE", "ANGLE", "BOOT GRACE", "NO PREARM", "ARM SWITCH",
+    "NOGYRO", "FAILSAFE", "RXLOSS", "BADRX", "BOXFAILSAFE",
+    "THROTTLE", "ANGLE", "BOOTGRACE", "NOPREARM", "ARMSWITCH",
     "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST"
 };
 #endif

src/main/fc/settings.c

@@ -38,6 +38,7 @@
 
 #include "drivers/light_led.h"
 #include "drivers/camera_control.h"
+#include "drivers/max7456.h"
 
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
@@ -60,6 +61,7 @@
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/osd.h"
+#include "io/vtx_control.h"
 #include "io/vtx_rtc6705.h"
 
 #include "rx/rx.h"
@@ -81,13 +83,15 @@
 // sync with accelerationSensor_e
 const char * const lookupTableAccHardware[] = {
     "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
-    "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20689", "BMI160", "FAKE"
+    "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20649", "ICM20689",
+    "BMI160", "FAKE"
 };
 
 // sync with gyroSensor_e
 const char * const lookupTableGyroHardware[] = {
     "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
-    "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE"
+    "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689",
+    "BMI160", "FAKE"
 };
 
 #if defined(USE_SENSOR_NAMES) || defined(BARO)
@@ -248,6 +252,12 @@ static const char * const lookupTableBusType[] = {
     "NONE", "I2C", "SPI"
 };
 
+#ifdef USE_MAX7456
+static const char * const lookupTableMax7456Clock[] = {
+    "HALF", "DEFAULT", "FULL"
+};
+#endif
+
 const lookupTableEntry_t lookupTables[] = {
     { lookupTableOffOn, sizeof(lookupTableOffOn) / sizeof(char *) },
     { lookupTableUnit, sizeof(lookupTableUnit) / sizeof(char *) },
@@ -294,19 +304,25 @@ const lookupTableEntry_t lookupTables[] = {
     { lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) },
 #endif
     { lookupTableBusType, sizeof(lookupTableBusType) / sizeof(char *) },
+#ifdef USE_MAX7456
+    { lookupTableMax7456Clock, sizeof(lookupTableMax7456Clock) / sizeof(char *) },
+#endif
 };
 
 const clivalue_t valueTable[] = {
 // PG_GYRO_CONFIG
     { "align_gyro",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) },
     { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf) },
+#if defined(USE_GYRO_SPI_ICM20649)
+    { "gyro_high_range",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
+#endif
     { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) },
     { "gyro_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_type) },
     { "gyro_lowpass_hz",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz) },
     { "gyro_notch1_hz",             VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) },
-    { "gyro_notch1_cutoff",         VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
+    { "gyro_notch1_cutoff",         VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
     { "gyro_notch2_hz",             VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
-    { "gyro_notch2_cutoff",         VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
+    { "gyro_notch2_cutoff",         VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
 #if defined(GYRO_USES_SPI)
 #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
@@ -320,6 +336,9 @@ const clivalue_t valueTable[] = {
 // PG_ACCELEROMETER_CONFIG
     { "align_acc",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
     { "acc_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },
+#if defined(USE_GYRO_SPI_ICM20649)
+    { "acc_high_range",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_high_fsr) },
+#endif
     { "acc_lpf_hz",                 VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
     { "acc_trim_pitch",             VAR_INT16  | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) },
     { "acc_trim_roll",              VAR_INT16  | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) },
@@ -366,7 +385,7 @@ const clivalue_t valueTable[] = {
 #endif
 #ifdef USE_SPEKTRUM_BIND
     { "spektrum_sat_bind",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) },
-    { "spektrum_sat_bind_autoreset",VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) },
+    { "spektrum_sat_bind_autoreset",VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) },
 #endif
     { "airmode_start_throttle",     VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) },
     { "rx_min_usec",                VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) },
@@ -385,7 +404,7 @@ const clivalue_t valueTable[] = {
 
 // PG_BLACKBOX_CONFIG
 #ifdef BLACKBOX
-    { "blackbox_p_denom",           VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_denom) },
+    { "blackbox_p_ratio",           VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_denom) },
     { "blackbox_device",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_DEVICE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, device) },
     { "blackbox_on_motor_test",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, on_motor_test) },
     { "blackbox_record_acc",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, record_acc) },
@@ -460,7 +479,7 @@ const clivalue_t valueTable[] = {
 
 // PG_BEEPER_CONFIG
 #ifdef USE_DSHOT
-    { "beeper_dshot",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotForward) },
+    { "beeper_dshot_beacon_tone",   VAR_UINT8  | MASTER_VALUE, .config.minmax = {0, DSHOT_CMD_BEACON5 }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotBeaconTone) },
 #endif
 #endif
 
@@ -549,7 +568,7 @@ const clivalue_t valueTable[] = {
     { "dterm_lowpass_type",         VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) },
     { "dterm_lowpass",              VAR_INT16  | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz) },
     { "dterm_notch_hz",             VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) },
-    { "dterm_notch_cutoff",         VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
+    { "dterm_notch_cutoff",         VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
     { "vbat_pid_gain",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) },
     { "pid_at_min_throttle",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
     { "anti_gravity_threshold",     VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
@@ -596,8 +615,7 @@ const clivalue_t valueTable[] = {
     { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) },
     { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) },
 
-    { "level_sensitivity",          VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelSensitivity) },
-    { "level_limit",                VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 10, 120 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) },
+    { "level_limit",                VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) },
 
     { "horizon_tilt_effect",        VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0,  250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },
     { "horizon_tilt_expert_mode",   VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) },
@@ -716,13 +734,18 @@ const clivalue_t valueTable[] = {
 #endif
     { "pwr_on_arm_grace",           VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, powerOnArmingGraceTime) },
 
-// PG_VTX_CONFIG
+// PG_VTX_RTC6705_CONFIG
 #ifdef VTX_RTC6705
     { "vtx_band",                   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, band) },
     { "vtx_channel",                VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, channel) },
     { "vtx_power",                  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, RTC6705_POWER_COUNT - 1 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, power) },
 #endif
 
+// PG_VTX_CONFIG
+#if defined(VTX_CONTROL) && defined(VTX_COMMON)
+    { "vtx_halfduplex",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, halfDuplex) },
+#endif
+
 // PG_VCD_CONFIG
 #ifdef USE_MAX7456
     { "vcd_video_system",           VAR_UINT8   | MASTER_VALUE, .config.minmax = { 0, 2 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, video_system) },
@@ -730,6 +753,11 @@ const clivalue_t valueTable[] = {
     { "vcd_v_offset",               VAR_INT8    | MASTER_VALUE, .config.minmax = { -15, 16 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, v_offset) },
 #endif
 
+// PG_MAX7456_CONFIG
+#ifdef USE_MAX7456
+    { "max7456_clock",              VAR_UINT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAX7456_CLOCK }, PG_MAX7456_CONFIG, offsetof(max7456Config_t, clockConfig) },
+#endif
+
 // PG_DISPLAY_PORT_MSP_CONFIG
 #ifdef USE_MSP_DISPLAYPORT
     { "displayport_msp_col_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) },
@@ -766,6 +794,7 @@ const clivalue_t valueTable[] = {
     { "camera_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CAMERA_CONTROL_MODE }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, mode) },
     { "camera_control_ref_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 200, 400 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, refVoltage) },
     { "camera_control_key_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, keyDelayMs) },
+    { "camera_control_internal_resistance", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 1000 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, internalResistance) },
 #endif
 };
 

src/main/fc/settings.h

@@ -17,6 +17,10 @@
 
 #pragma once
 
+#include <stdint.h>
+#include <stdbool.h>
+#include "config/parameter_group.h"
+
 
 typedef enum {
     TABLE_OFF_ON = 0,
@@ -64,6 +68,9 @@ typedef enum {
     TABLE_CAMERA_CONTROL_MODE,
 #endif
     TABLE_BUS_TYPE,
+#ifdef USE_MAX7456
+    TABLE_MAX7456_CLOCK,
+#endif
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
 
@@ -119,7 +126,7 @@ typedef union {
     cliArrayLengthConfig_t array;
 } cliValueConfig_t;
 
-typedef struct {
+typedef struct clivalue_s {
     const char *name;
     const uint8_t type; // see cliValueFlag_e
     const cliValueConfig_t config;

src/main/flight/failsafe.h

@@ -80,7 +80,7 @@ void failsafeReset(void);
 void failsafeStartMonitoring(void);
 void failsafeUpdateState(void);
 
-failsafePhase_e failsafePhase();
+failsafePhase_e failsafePhase(void);
 bool failsafeIsMonitoring(void);
 bool failsafeIsActive(void);
 bool failsafeIsReceivingRxData(void);

src/main/flight/mixer.c

@@ -23,6 +23,7 @@
 #include "platform.h"
 
 #include "build/build_config.h"
+#include "build/debug.h"
 
 #include "common/axis.h"
 #include "common/filter.h"
@@ -43,6 +44,7 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 #include "fc/fc_core.h"
+#include "fc/fc_rc.h"
 
 #include "flight/failsafe.h"
 #include "flight/imu.h"
@@ -102,12 +104,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
 
-#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
-// (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
-#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
-#define EXTERNAL_CONVERSION_MIN_VALUE 1000
-#define EXTERNAL_CONVERSION_MAX_VALUE 2000
-#define EXTERNAL_CONVERSION_3D_MID_VALUE 1500
+#define PWM_RANGE_MID 1500
 
 #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight
 
@@ -312,8 +309,11 @@ const mixer_t mixers[] = {
 };
 #endif // !USE_QUAD_MIXER_ONLY
 
-static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
 float motorOutputHigh, motorOutputLow;
+
+static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
+static uint16_t rcCommand3dDeadBandLow;
+static uint16_t rcCommand3dDeadBandHigh;
 static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
 
 uint8_t getMotorCount(void)
@@ -356,7 +356,9 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
 
 // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
 // DSHOT scaling is done to the actual dshot range
-void initEscEndpoints(void) {
+void initEscEndpoints(void)
+{
+    // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
     switch (motorConfig()->dev.motorPwmProtocol) {
 #ifdef USE_DSHOT
     case PWM_TYPE_PROSHOT1000:
@@ -365,10 +367,11 @@ void initEscEndpoints(void) {
     case PWM_TYPE_DSHOT300:
     case PWM_TYPE_DSHOT150:
         disarmMotorOutput = DSHOT_DISARM_COMMAND;
-        if (feature(FEATURE_3D))
+        if (feature(FEATURE_3D)) {
             motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
-        else
+        } else {
             motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
+        }
         motorOutputHigh = DSHOT_MAX_THROTTLE;
         deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
         deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
@@ -376,8 +379,13 @@ void initEscEndpoints(void) {
         break;
 #endif
     default:
-        disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
-        motorOutputLow = motorConfig()->minthrottle;
+        if (feature(FEATURE_3D)) {
+            disarmMotorOutput = flight3DConfig()->neutral3d;
+            motorOutputLow = PWM_RANGE_MIN;
+        } else {
+            disarmMotorOutput = motorConfig()->mincommand;
+            motorOutputLow = motorConfig()->minthrottle;
+        }
         motorOutputHigh = motorConfig()->maxthrottle;
         deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
         deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
@@ -385,9 +393,13 @@ void initEscEndpoints(void) {
         break;
     }
 
-    rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);
-    rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle;
-    rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
+    rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck;
+
+    rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
+    rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;
+
+    rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
+    rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
 }
 
 void mixerInit(mixerMode_e mixerMode)
@@ -514,53 +526,112 @@ void stopPwmAllMotors(void)
     delayMicroseconds(1500);
 }
 
-float throttle = 0;
-float motorOutputMin, motorOutputMax;
-bool mixerInversion = false;
-float motorOutputRange;
+static float throttle = 0;
+static float motorOutputMin;
+static float motorRangeMin;
+static float motorRangeMax;
+static float motorOutputRange;
+static int8_t motorOutputMixSign;
 
 void calculateThrottleAndCurrentMotorEndpoints(void)
 {
-    static uint16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
+    static uint16_t rcThrottlePrevious = 0;   // Store the last throttle direction for deadband transitions
     float currentThrottleInputRange = 0;
 
     if(feature(FEATURE_3D)) {
-        if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
+        if (!ARMING_FLAG(ARMED)) {
+            rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
+        }
 
-        if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) {
-            motorOutputMax = deadbandMotor3dLow;
-            motorOutputMin = motorOutputLow;
-            throttlePrevious = rcCommand[THROTTLE];                //3D Mode Throttle Fix #3696
-            throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; //3D Mode Throttle Fix #3696
+        if(rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) {
+            // INVERTED
+            motorRangeMin = motorOutputLow;
+            motorRangeMax = deadbandMotor3dLow;
+            if(isMotorProtocolDshot()) {
+                motorOutputMin = motorOutputLow;
+                motorOutputRange = deadbandMotor3dLow - motorOutputLow;
+            } else {
+                motorOutputMin = deadbandMotor3dLow;
+                motorOutputRange = motorOutputLow - deadbandMotor3dLow;
+            }
+            motorOutputMixSign = -1;
+            rcThrottlePrevious = rcCommand[THROTTLE];
+            throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
             currentThrottleInputRange = rcCommandThrottleRange3dLow;
-            if(isMotorProtocolDshot()) mixerInversion = true;
-        } else if(rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
-            motorOutputMax = motorOutputHigh;
+        } else if(rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) {
+            // NORMAL
+            motorRangeMin = deadbandMotor3dHigh;
+            motorRangeMax = motorOutputHigh;
             motorOutputMin = deadbandMotor3dHigh;
-            throttlePrevious = rcCommand[THROTTLE];
-            throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
+            motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
+            motorOutputMixSign = 1;
+            rcThrottlePrevious = rcCommand[THROTTLE];
+            throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
-        } else if((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) {
-            motorOutputMax = deadbandMotor3dLow;
-            motorOutputMin = motorOutputLow;
-            throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
+        } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow)) {
+            // INVERTED_TO_DEADBAND
+            motorRangeMin = motorOutputLow;
+            motorRangeMax = deadbandMotor3dLow;
+            if(isMotorProtocolDshot()) {
+                motorOutputMin = motorOutputLow;
+                motorOutputRange = deadbandMotor3dLow - motorOutputLow;
+            } else {
+                motorOutputMin = deadbandMotor3dLow;
+                motorOutputRange = motorOutputLow - deadbandMotor3dLow;
+            }
+            motorOutputMixSign = -1;
+            throttle = 0;
             currentThrottleInputRange = rcCommandThrottleRange3dLow;
-            if(isMotorProtocolDshot()) mixerInversion = true;
         } else {
-            motorOutputMax = motorOutputHigh;
+            // NORMAL_TO_DEADBAND
+            motorRangeMin = deadbandMotor3dHigh;
+            motorRangeMax = motorOutputHigh;
             motorOutputMin = deadbandMotor3dHigh;
+            motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
+            motorOutputMixSign = 1;
             throttle = 0;
             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
         }
     } else {
         throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
         currentThrottleInputRange = rcCommandThrottleRange;
+        motorRangeMin = motorOutputLow;
+        motorRangeMax = motorOutputHigh;
         motorOutputMin = motorOutputLow;
-        motorOutputMax = motorOutputHigh;
+        motorOutputRange = motorOutputHigh - motorOutputLow;
+        motorOutputMixSign = 1;
     }
 
     throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
-    motorOutputRange = motorOutputMax - motorOutputMin;
+}
+
+#define CRASH_FLIP_DEADBAND 20
+
+static void applyFlipOverAfterCrashModeToMotors(void)
+{
+    float motorMix[MAX_SUPPORTED_MOTORS];
+
+    if (ARMING_FLAG(ARMED)) {
+        for (int i = 0; i < motorCount; i++) {
+            if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) {
+                motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * -1;
+            } else {
+                motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * -1;
+            }
+
+            // Apply the mix to motor endpoints
+            float motorOutput =  motorOutputMin + motorOutputRange * motorMix[i];
+            //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
+            motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND ) ? disarmMotorOutput : motorOutput - CRASH_FLIP_DEADBAND;
+
+            motor[i] = motorOutput;
+        }
+    } else {
+        // Disarmed mode
+        for (int i = 0; i < motorCount; i++) {
+            motor[i] = motor_disarmed[i];
+        }
+    }
 }
 
 static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
@@ -568,21 +639,16 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
     for (uint32_t i = 0; i < motorCount; i++) {
-        float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle));
-
-        // Dshot works exactly opposite in lower 3D section.
-        if (mixerInversion) {
-            motorOutput = motorOutputMin + (motorOutputMax - motorOutput);
-        }
+        float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle));
 
         if (failsafeIsActive()) {
             if (isMotorProtocolDshot()) {
-                motorOutput = (motorOutput < motorOutputMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
+                motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
             }
 
-            motorOutput = constrain(motorOutput, disarmMotorOutput, motorOutputMax);
+            motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax);
         } else {
-            motorOutput = constrain(motorOutput, motorOutputMin, motorOutputMax);
+            motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
         }
 
         // Motor stop handling
@@ -604,6 +670,11 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
 
 void mixTable(uint8_t vbatPidCompensation)
 {
+    if (isFlipOverAfterCrashMode()) {
+        applyFlipOverAfterCrashModeToMotors();        
+        return;
+    }
+
     // Find min and max throttle based on conditions. Throttle has to be known before mixing
     calculateThrottleAndCurrentMotorEndpoints();
 
@@ -668,44 +739,62 @@ void mixTable(uint8_t vbatPidCompensation)
 
 float convertExternalToMotor(uint16_t externalValue)
 {
-    uint16_t motorValue = externalValue;
+    uint16_t motorValue;
+    switch ((int)isMotorProtocolDshot()) {
 #ifdef USE_DSHOT
-    if (isMotorProtocolDshot()) {
-        // Add 1 to the value, otherwise throttle tops out at 2046
-        motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR + 1, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
+    case true:
+        externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
 
         if (feature(FEATURE_3D)) {
-            if (externalValue == EXTERNAL_CONVERSION_3D_MID_VALUE) {
+            if (externalValue == PWM_RANGE_MID) {
                 motorValue = DSHOT_DISARM_COMMAND;
-            } else if (motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) {
-                // Add 1 to the value, otherwise throttle tops out at 2046
-                motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) + 1;
+            } else if (externalValue < PWM_RANGE_MID) {
+                motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE);
+            } else {
+                motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
             }
+        } else {
+            motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_DISARM_COMMAND : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
         }
-    }
+
+        break;
+    case false:
 #endif
+    default:
+        motorValue = externalValue;
+
+        break;
+    }
 
     return (float)motorValue;
 }
 
 uint16_t convertMotorToExternal(float motorValue)
 {
-    uint16_t externalValue = lrintf(motorValue);
+    uint16_t externalValue;
+    switch ((int)isMotorProtocolDshot()) {
 #ifdef USE_DSHOT
-    if (isMotorProtocolDshot()) {
-        if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) {
-            // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor()
-            motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) - 1;
+    case true:
+        if (feature(FEATURE_3D)) {
+            if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) {
+                externalValue = PWM_RANGE_MID;
+            } else if (motorValue <= DSHOT_3D_DEADBAND_LOW) {
+                externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW, PWM_RANGE_MID - 1, PWM_RANGE_MIN);
+            } else {
+                externalValue = scaleRange(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX);
+            }
+        } else {
+            externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
         }
 
-        // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor()
-        externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain(((motorValue - 1)/ EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE);
-
-        if (feature(FEATURE_3D) && motorValue == DSHOT_DISARM_COMMAND) {
-            externalValue = EXTERNAL_CONVERSION_3D_MID_VALUE;
-        }
-    }
+        break;
+    case false:
 #endif
+    default:
+        externalValue = motorValue;
+
+        break;
+    }
 
     return externalValue;
 }

src/main/flight/mixer.h

@@ -105,6 +105,7 @@ PG_DECLARE(motorConfig_t, motorConfig);
 extern const mixer_t mixers[];
 extern float motor[MAX_SUPPORTED_MOTORS];
 extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
+extern float motorOutputHigh, motorOutputLow;
 struct rxConfig_s;
 
 uint8_t getMotorCount(void);

src/main/flight/pid.c

@@ -69,7 +69,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
     .pid_process_denom = PID_PROCESS_DENOM_DEFAULT
 );
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 1);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
@@ -98,9 +98,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .vbatPidCompensation = 0,
         .pidAtMinThrottle = PID_STABILISATION_ON,
         .levelAngleLimit = 55,
-        .levelSensitivity = 55,
         .setpointRelaxRatio = 100,
-        .dtermSetpointWeight = 60,
+        .dtermSetpointWeight = 0,
         .yawRateAccelLimit = 100,
         .rateAccelLimit = 0,
         .itermThrottleThreshold = 350,
@@ -127,7 +126,7 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
     }
 }
 
-void pidSetTargetLooptime(uint32_t pidLooptime)
+static void pidSetTargetLooptime(uint32_t pidLooptime)
 {
     targetPidLooptime = pidLooptime;
     dT = (float)targetPidLooptime * 0.000001f;
@@ -171,9 +170,17 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 {
     BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
 
+    if (targetPidLooptime == 0) {
+        // no looptime set, so set all the filters to null
+        dtermNotchFilterApplyFn = nullFilterApply;
+        dtermLpfApplyFn = nullFilterApply;
+        ptermYawFilterApplyFn = nullFilterApply;
+        return;
+    }
+
     const uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed
 
-    float dTermNotchHz;
+    uint16_t dTermNotchHz;
     if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
         dTermNotchHz = pidProfile->dterm_notch_hz;
     } else {
@@ -184,8 +191,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
         }
     }
 
-    static biquadFilter_t biquadFilterNotch[2];
-    if (dTermNotchHz) {
+    if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
+        static biquadFilter_t biquadFilterNotch[2];
         dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
         const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
         for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
@@ -200,8 +207,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) {
         dtermLpfApplyFn = nullFilterApply;
     } else {
-        memset(&dtermFilterLpfUnion, 0, sizeof(dtermFilterLpfUnion));
-
         switch (pidProfile->dterm_filter_type) {
         default:
             dtermLpfApplyFn = nullFilterApply;
@@ -295,6 +300,16 @@ void pidInit(const pidProfile_t *pidProfile)
     pidInitMixer(pidProfile);
 }
 
+
+void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
+{
+    if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1)
+        && dstPidProfileIndex != srcPidProfileIndex
+    ) {
+        memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
+    }
+}
+
 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
 static float calcHorizonLevelStrength(void)
 {
@@ -353,25 +368,27 @@ static float calcHorizonLevelStrength(void)
 
 static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
     // calculate error angle and limit the angle to the max inclination
-    float angle = pidProfile->levelSensitivity * getRcDeflection(axis);
+    // rcDeflection is in range [-1.0, 1.0]
+    float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
 #ifdef GPS
     angle += GPS_angle[axis];
 #endif
     angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
     const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
     if (FLIGHT_MODE(ANGLE_MODE)) {
-        // ANGLE mode - control is angle based, so control loop is needed
+        // ANGLE mode - control is angle based
         currentPidSetpoint = errorAngle * levelGain;
     } else {
-        // HORIZON mode - direct sticks control is applied to rate PID
-        // mix up angle error to desired AngleRate to add a little auto-level feel
+        // HORIZON mode - mix of ANGLE and ACRO modes
+        // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
         const float horizonLevelStrength = calcHorizonLevelStrength();
         currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
     }
     return currentPidSetpoint;
 }
 
-static float accelerationLimit(int axis, float currentPidSetpoint) {
+static float accelerationLimit(int axis, float currentPidSetpoint)
+{
     static float previousSetpoint[3];
     const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
 
@@ -488,7 +505,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered);
 
             float dynC = 0;
-            if ( (pidProfile->setpointRelaxRatio < 100) && (!flightModeFlags) ) {
+            if ( (pidProfile->dtermSetpointWeight > 0) && (!flightModeFlags) ) {
                 dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
             }
             const float rD = dynC * currentPidSetpoint - gyroRateFiltered;    // cr - y

src/main/flight/pid.h

@@ -85,7 +85,6 @@ typedef struct pidProfile_s {
     uint8_t vbatPidCompensation;            // Scale PIDsum to battery voltage
     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
     uint8_t levelAngleLimit;                // Max angle in degrees in level mode
-    uint8_t levelSensitivity;               // Angle mode sensitivity reflected in degrees assuming user using full stick
 
     uint8_t horizon_tilt_effect;            // inclination factor for Horizon mode
     uint8_t horizon_tilt_expert_mode;       // OFF or ON
@@ -129,10 +128,10 @@ extern uint8_t PIDweight[3];
 
 void pidResetErrorGyroState(void);
 void pidStabilisationState(pidStabilisationState_e pidControllerState);
-void pidSetTargetLooptime(uint32_t pidLooptime);
 void pidSetItermAccelerator(float newItermAccelerator);
 void pidInitFilters(const pidProfile_t *pidProfile);
 void pidInitConfig(const pidProfile_t *pidProfile);
 void pidInit(const pidProfile_t *pidProfile);
+void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
 
 #endif

src/main/io/asyncfatfs/asyncfatfs.c

@@ -539,7 +539,7 @@ static bool afatfs_fileIsBusy(afatfsFilePtr_t file)
  *
  * Note that this is the same as the number of clusters in an AFATFS supercluster.
  */
-static uint32_t afatfs_fatEntriesPerSector()
+static uint32_t afatfs_fatEntriesPerSector(void)
 {
     return afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT32 ? AFATFS_FAT32_FAT_ENTRIES_PER_SECTOR : AFATFS_FAT16_FAT_ENTRIES_PER_SECTOR;
 }
@@ -548,7 +548,7 @@ static uint32_t afatfs_fatEntriesPerSector()
  * Size of a FAT cluster in bytes
  */
 ONLY_EXPOSE_FOR_TESTING
-uint32_t afatfs_clusterSize()
+uint32_t afatfs_clusterSize(void)
 {
     return afatfs.sectorsPerCluster * AFATFS_SECTOR_SIZE;
 }
@@ -806,7 +806,7 @@ static int afatfs_allocateCacheSector(uint32_t sectorIndex)
 /**
  * Attempt to flush dirty cache pages out to the sdcard, returning true if all flushable data has been flushed.
  */
-bool afatfs_flush()
+bool afatfs_flush(void)
 {
     if (afatfs.cacheDirtyEntries > 0) {
         // Flush the oldest flushable sector
@@ -836,7 +836,7 @@ bool afatfs_flush()
 /**
  * Returns true if either the freefile or the regular cluster pool has been exhausted during a previous write operation.
  */
-bool afatfs_isFull()
+bool afatfs_isFull(void)
 {
     return afatfs.filesystemFull;
 }
@@ -1618,7 +1618,7 @@ static afatfsOperationStatus_e afatfs_appendRegularFreeCluster(afatfsFilePtr_t f
  * Size of a AFATFS supercluster in bytes
  */
 ONLY_EXPOSE_FOR_TESTING
-uint32_t afatfs_superClusterSize()
+uint32_t afatfs_superClusterSize(void)
 {
     return afatfs_fatEntriesPerSector() * afatfs_clusterSize();
 }
@@ -2364,7 +2364,7 @@ static afatfsOperationStatus_e afatfs_allocateDirectoryEntry(afatfsFilePtr_t dir
  * Return a pointer to a free entry in the open files table (a file whose type is "NONE"). You should initialize
  * the file afterwards with afatfs_initFileHandle().
  */
-static afatfsFilePtr_t afatfs_allocateFileHandle()
+static afatfsFilePtr_t afatfs_allocateFileHandle(void)
 {
     for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) {
         if (afatfs.openFiles[i].type == AFATFS_FILE_TYPE_NONE) {
@@ -3211,7 +3211,7 @@ static void afatfs_fileOperationContinue(afatfsFile_t *file)
 /**
  * Check files for pending operations and execute them.
  */
-static void afatfs_fileOperationsPoll()
+static void afatfs_fileOperationsPoll(void)
 {
     afatfs_fileOperationContinue(&afatfs.currentDirectory);
 
@@ -3229,7 +3229,7 @@ static void afatfs_fileOperationsPoll()
 /**
  * Return the available size of the freefile (used for files in contiguous append mode)
  */
-uint32_t afatfs_getContiguousFreeSpace()
+uint32_t afatfs_getContiguousFreeSpace(void)
 {
     return afatfs.freeFile.logicalSize;
 }
@@ -3238,7 +3238,7 @@ uint32_t afatfs_getContiguousFreeSpace()
  * Call to set up the initial state for finding the largest block of free space on the device whose corresponding FAT
  * sectors are themselves entirely free space (so the free space has dedicated FAT sectors of its own).
  */
-static void afatfs_findLargestContiguousFreeBlockBegin()
+static void afatfs_findLargestContiguousFreeBlockBegin(void)
 {
     // The first FAT sector has two reserved entries, so it isn't eligible for this search. Start at the next FAT sector.
     afatfs.initState.freeSpaceSearch.candidateStart = afatfs_fatEntriesPerSector();
@@ -3256,7 +3256,7 @@ static void afatfs_findLargestContiguousFreeBlockBegin()
  *     AFATFS_OPERATION_SUCCESS - When the search has finished and afatfs.initState.freeSpaceSearch has been updated with the details of the best gap.
  *     AFATFS_OPERATION_FAILURE - When a read error occured
  */
-static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue()
+static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue(void)
 {
     afatfsFreeSpaceSearch_t *opState = &afatfs.initState.freeSpaceSearch;
     uint32_t fatEntriesPerSector = afatfs_fatEntriesPerSector();
@@ -3361,7 +3361,7 @@ static void afatfs_introspecLogCreated(afatfsFile_t *file)
 
 #endif
 
-static void afatfs_initContinue()
+static void afatfs_initContinue(void)
 {
 #ifdef AFATFS_USE_FREEFILE
     afatfsOperationStatus_e status;
@@ -3491,7 +3491,7 @@ static void afatfs_initContinue()
  * Check to see if there are any pending operations on the filesystem and perform a little work (without waiting on the
  * sdcard). You must call this periodically.
  */
-void afatfs_poll()
+void afatfs_poll(void)
 {
     // Only attempt to continue FS operations if the card is present & ready, otherwise we would just be wasting time
     if (sdcard_poll()) {
@@ -3554,17 +3554,17 @@ void afatfs_sdcardProfilerCallback(sdcardBlockOperation_e operation, uint32_t bl
 
 #endif
 
-afatfsFilesystemState_e afatfs_getFilesystemState()
+afatfsFilesystemState_e afatfs_getFilesystemState(void)
 {
     return afatfs.filesystemState;
 }
 
-afatfsError_e afatfs_getLastError()
+afatfsError_e afatfs_getLastError(void)
 {
     return afatfs.lastError;
 }
 
-void afatfs_init()
+void afatfs_init(void)
 {
     afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_INITIALIZATION;
     afatfs.initPhase = AFATFS_INITIALIZATION_READ_MBR;
@@ -3646,7 +3646,7 @@ bool afatfs_destroy(bool dirty)
 /**
  * Get a pessimistic estimate of the amount of buffer space that we have available to write to immediately.
  */
-uint32_t afatfs_getFreeBufferSpace()
+uint32_t afatfs_getFreeBufferSpace(void)
 {
     uint32_t result = 0;
     for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) {

src/main/io/asyncfatfs/asyncfatfs.h

@@ -58,7 +58,7 @@ typedef enum {
 } afatfsSeek_e;
 
 typedef void (*afatfsFileCallback_t)(afatfsFilePtr_t file);
-typedef void (*afatfsCallback_t)();
+typedef void (*afatfsCallback_t)(void);
 
 bool afatfs_fopen(const char *filename, const char *mode, afatfsFileCallback_t complete);
 bool afatfs_ftruncate(afatfsFilePtr_t file, afatfsFileCallback_t callback);
@@ -79,14 +79,14 @@ void afatfs_findFirst(afatfsFilePtr_t directory, afatfsFinder_t *finder);
 afatfsOperationStatus_e afatfs_findNext(afatfsFilePtr_t directory, afatfsFinder_t *finder, fatDirectoryEntry_t **dirEntry);
 void afatfs_findLast(afatfsFilePtr_t directory);
 
-bool afatfs_flush();
-void afatfs_init();
+bool afatfs_flush(void);
+void afatfs_init(void);
 bool afatfs_destroy(bool dirty);
-void afatfs_poll();
+void afatfs_poll(void);
 
-uint32_t afatfs_getFreeBufferSpace();
-uint32_t afatfs_getContiguousFreeSpace();
-bool afatfs_isFull();
+uint32_t afatfs_getFreeBufferSpace(void);
+uint32_t afatfs_getContiguousFreeSpace(void);
+bool afatfs_isFull(void);
 
-afatfsFilesystemState_e afatfs_getFilesystemState();
-afatfsError_e afatfs_getLastError();
+afatfsFilesystemState_e afatfs_getFilesystemState(void);
+afatfsError_e afatfs_getLastError(void);

src/main/io/beeper.c

@@ -84,6 +84,11 @@ PG_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig,
 #define BEEPER_COMMAND_STOP   0xFF
 
 #ifdef BEEPER
+PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1);
+PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig,
+    .dshotBeaconTone = 0
+);
+
 /* Beeper Sound Sequences: (Square wave generation)
  * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from
  * start when 0xFF stops the sound when it's completed.
@@ -224,7 +229,7 @@ void beeper(beeperMode_e mode)
     if (
         mode == BEEPER_SILENCE || (
             (getBeeperOffMask() & (1 << (BEEPER_USB - 1)))
-            && (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && (getBatteryCellCount() == 0))
+            && getBatteryState() == BATTERY_NOT_PRESENT
         )
     ) {
         beeperSilence();
@@ -363,8 +368,13 @@ void beeperUpdate(timeUs_t currentTimeUs)
     }
 
     #ifdef USE_DSHOT
-    if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) {
-        pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3);
+    if (!areMotorsRunning() && beeperConfig()->dshotBeaconTone && (beeperConfig()->dshotBeaconTone <= DSHOT_CMD_BEACON5) && currentBeeperEntry->mode == BEEPER_RX_SET) {
+        pwmDisableMotors();
+        delay(1);
+
+        pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone);
+
+        pwmEnableMotors();
     }
     #endif
 

src/main/io/beeper.h

@@ -51,7 +51,7 @@ typedef enum {
 typedef struct beeperConfig_s {
     uint32_t beeper_off_flags;
     uint32_t preferred_beeper_off_flags;
-    bool dshotForward;
+    uint8_t dshotBeaconTone;
 } beeperConfig_t;
 
 #ifdef BEEPER

src/main/io/dashboard.c

@@ -181,7 +181,7 @@ static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent)
 }
 
 #if 0
-static void fillScreenWithCharacters()
+static void fillScreenWithCharacters(void)
 {
     for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
         for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
@@ -241,7 +241,7 @@ static void updateFailsafeStatus(void)
     i2c_OLED_send_char(bus, failsafeIndicator);
 }
 
-static void showTitle()
+static void showTitle(void)
 {
     i2c_OLED_set_line(bus, 0);
     i2c_OLED_send_string(bus, pageState.page->title);
@@ -346,7 +346,7 @@ static void showProfilePage(void)
 #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
 
 #ifdef GPS
-static void showGpsPage()
+static void showGpsPage(void)
 {
     if (!feature(FEATURE_GPS)) {
         pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;

src/main/io/flashfs.c

@@ -52,12 +52,12 @@ static uint8_t bufferHead = 0, bufferTail = 0;
 // The position of the buffer's tail in the overall flash address space:
 static uint32_t tailAddress = 0;
 
-static void flashfsClearBuffer()
+static void flashfsClearBuffer(void)
 {
     bufferTail = bufferHead = 0;
 }
 
-static bool flashfsBufferIsEmpty()
+static bool flashfsBufferIsEmpty(void)
 {
     return bufferTail == bufferHead;
 }
@@ -67,7 +67,7 @@ static void flashfsSetTailAddress(uint32_t address)
     tailAddress = address;
 }
 
-void flashfsEraseCompletely()
+void flashfsEraseCompletely(void)
 {
     m25p16_eraseCompletely();
 
@@ -106,17 +106,17 @@ void flashfsEraseRange(uint32_t start, uint32_t end)
 /**
  * Return true if the flash is not currently occupied with an operation.
  */
-bool flashfsIsReady()
+bool flashfsIsReady(void)
 {
     return m25p16_isReady();
 }
 
-uint32_t flashfsGetSize()
+uint32_t flashfsGetSize(void)
 {
     return m25p16_getGeometry()->totalSize;
 }
 
-static uint32_t flashfsTransmitBufferUsed()
+static uint32_t flashfsTransmitBufferUsed(void)
 {
     if (bufferHead >= bufferTail)
         return bufferHead - bufferTail;
@@ -127,7 +127,7 @@ static uint32_t flashfsTransmitBufferUsed()
 /**
  * Get the size of the largest single write that flashfs could ever accept without blocking or data loss.
  */
-uint32_t flashfsGetWriteBufferSize()
+uint32_t flashfsGetWriteBufferSize(void)
 {
     return FLASHFS_WRITE_BUFFER_USABLE;
 }
@@ -135,12 +135,12 @@ uint32_t flashfsGetWriteBufferSize()
 /**
  * Get the number of bytes that can currently be written to flashfs without any blocking or data loss.
  */
-uint32_t flashfsGetWriteBufferFreeSpace()
+uint32_t flashfsGetWriteBufferFreeSpace(void)
 {
     return flashfsGetWriteBufferSize() - flashfsTransmitBufferUsed();
 }
 
-const flashGeometry_t* flashfsGetGeometry()
+const flashGeometry_t* flashfsGetGeometry(void)
 {
     return m25p16_getGeometry();
 }
@@ -270,7 +270,7 @@ static void flashfsGetDirtyDataBuffers(uint8_t const *buffers[], uint32_t buffer
 /**
  * Get the current offset of the file pointer within the volume.
  */
-uint32_t flashfsGetOffset()
+uint32_t flashfsGetOffset(void)
 {
     uint8_t const * buffers[2];
     uint32_t bufferSizes[2];
@@ -305,7 +305,7 @@ static void flashfsAdvanceTailInBuffer(uint32_t delta)
  * Returns true if all data in the buffer has been flushed to the device, or false if
  * there is still data to be written (call flush again later).
  */
-bool flashfsFlushAsync()
+bool flashfsFlushAsync(void)
 {
     if (flashfsBufferIsEmpty()) {
         return true; // Nothing to flush
@@ -328,7 +328,7 @@ bool flashfsFlushAsync()
  * The flash will still be busy some time after this sync completes, but space will
  * be freed up to accept more writes in the write buffer.
  */
-void flashfsFlushSync()
+void flashfsFlushSync(void)
 {
     if (flashfsBufferIsEmpty()) {
         return; // Nothing to flush
@@ -484,7 +484,7 @@ int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len)
 /**
  * Find the offset of the start of the free space on the device (or the size of the device if it is full).
  */
-int flashfsIdentifyStartOfFreeSpace()
+int flashfsIdentifyStartOfFreeSpace(void)
 {
     /* Find the start of the free space on the device by examining the beginning of blocks with a binary search,
      * looking for ones that appear to be erased. We can achieve this with good accuracy because an erased block
@@ -553,14 +553,15 @@ int flashfsIdentifyStartOfFreeSpace()
 /**
  * Returns true if the file pointer is at the end of the device.
  */
-bool flashfsIsEOF() {
+bool flashfsIsEOF(void)
+{
     return tailAddress >= flashfsGetSize();
 }
 
 /**
  * Call after initializing the flash chip in order to set up the filesystem.
  */
-void flashfsInit()
+void flashfsInit(void)
 {
     // If we have a flash chip present at all
     if (flashfsGetSize() > 0) {

src/main/io/flashfs.h

@@ -23,16 +23,16 @@
 // Automatically trigger a flush when this much data is in the buffer
 #define FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN 64
 
-void flashfsEraseCompletely();
+void flashfsEraseCompletely(void);
 void flashfsEraseRange(uint32_t start, uint32_t end);
 
-uint32_t flashfsGetSize();
-uint32_t flashfsGetOffset();
-uint32_t flashfsGetWriteBufferFreeSpace();
-uint32_t flashfsGetWriteBufferSize();
-int flashfsIdentifyStartOfFreeSpace();
+uint32_t flashfsGetSize(void);
+uint32_t flashfsGetOffset(void);
+uint32_t flashfsGetWriteBufferFreeSpace(void);
+uint32_t flashfsGetWriteBufferSize(void);
+int flashfsIdentifyStartOfFreeSpace(void);
 struct flashGeometry_s;
-const struct flashGeometry_s* flashfsGetGeometry();
+const struct flashGeometry_s* flashfsGetGeometry(void);
 
 void flashfsSeekAbs(uint32_t offset);
 void flashfsSeekRel(int32_t offset);
@@ -42,10 +42,10 @@ void flashfsWrite(const uint8_t *data, unsigned int len, bool sync);
 
 int flashfsReadAbs(uint32_t offset, uint8_t *data, unsigned int len);
 
-bool flashfsFlushAsync();
-void flashfsFlushSync();
+bool flashfsFlushAsync(void);
+void flashfsFlushSync(void);
 
-void flashfsInit();
+void flashfsInit(void);
 
-bool flashfsIsReady();
-bool flashfsIsEOF();
+bool flashfsIsReady(void);
+bool flashfsIsEOF(void);

src/main/io/ledstrip.c

@@ -441,22 +441,35 @@ static const struct {
     {0,             LED_MODE_ORIENTATION},
 };
 
-static void applyLedFixedLayers()
+static void applyLedFixedLayers(void)
 {
     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
         hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);
-        hsvColor_t nextColor = *getSC(LED_SCOLOR_BACKGROUND); //next color above the one selected, or color 0 if your are at the maximum
-        hsvColor_t previousColor = *getSC(LED_SCOLOR_BACKGROUND); //Previous color to the one selected, modulo color count
 
         int fn = ledGetFunction(ledConfig);
-        int hOffset = HSV_HUE_MAX;
+        int hOffset = HSV_HUE_MAX + 1;
 
         switch (fn) {
         case LED_FUNCTION_COLOR:
             color = ledStripConfig()->colors[ledGetColor(ledConfig)];
-            nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
-            previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
+
+            hsvColor_t nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
+            hsvColor_t previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
+
+            if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {   //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor
+            	int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2;
+                if (auxInput < centerPWM) {
+                    color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h);
+                    color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s);
+                    color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v);
+                } else {
+                    color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h);
+                    color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s);
+                    color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v);
+                }
+            }
+
             break;
 
         case LED_FUNCTION_FLIGHT_MODE:
@@ -489,22 +502,10 @@ static void applyLedFixedLayers()
             break;
         }
 
-        if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE))   //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor
-    {
-        int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2;
-            if (auxInput < centerPWM)
-                {
-                    color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h);
-                    color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s);
-                    color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v);
-                }
-            else
-                {
-                    color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h);
-                    color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s);
-                    color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v);
-                }
-       }
+        if ((fn != LED_FUNCTION_COLOR) && ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {
+            hOffset += scaleRange(auxInput, PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1);
+        }
+
         color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);
         setLedHsv(ledIndex, &color);
     }
@@ -1145,7 +1146,7 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex)
     return true;
 }
 
-void ledStripInit()
+void ledStripInit(void)
 {
     colors = ledStripConfigMutable()->colors;
     modeColors = ledStripConfig()->modeColors;

src/main/io/osd.c

@@ -163,7 +163,7 @@ static escSensorData_t *escData;
 /**
  * Gets the correct altitude symbol for the current unit system
  */
-static char osdGetMetersToSelectedUnitSymbol()
+static char osdGetMetersToSelectedUnitSymbol(void)
 {
     switch (osdConfig()->units) {
     case OSD_UNIT_IMPERIAL:
@@ -572,7 +572,7 @@ static void osdDrawSingleElement(uint8_t item)
         }
 
     case OSD_POWER:
-        tfp_sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000);
+        tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000);
         break;
 
     case OSD_PIDRATE_PROFILE:

src/main/io/rcsplit.c

@@ -83,7 +83,7 @@ static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
     serialWriteBuf(rcSplitSerialPort, uart_buffer, 5);
 }
 
-static void rcSplitProcessMode()
+static void rcSplitProcessMode(void)
 {
     // if the device not ready, do not handle any mode change event
     if (RCSPLIT_STATE_IS_READY != cameraState)

src/main/io/serial.c

@@ -119,15 +119,6 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig)
 
     serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
 
-#if defined(USE_VCP) && defined(USE_MSP_UART)
-    if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) {
-        serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1);
-        if (uart1Config) {
-            uart1Config->functionMask = FUNCTION_MSP;
-        }
-    }
-#endif
-
 #ifdef SERIALRX_UART
     serialPortConfig_t *serialRxUartConfig = serialFindPortConfiguration(SERIALRX_UART);
     if (serialRxUartConfig) {
@@ -142,6 +133,15 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig)
     }
 #endif
 
+#if defined(USE_VCP) && defined(USE_MSP_UART)
+    if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) {
+        serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1);
+        if (uart1Config) {
+            uart1Config->functionMask = FUNCTION_MSP;
+        }
+    }
+#endif
+
     serialConfig->reboot_character = 'R';
     serialConfig->serial_update_rate_hz = 100;
 }

src/main/io/vtx_control.c

@@ -16,9 +16,13 @@
  */
 
 
-// Get target build configuration
+#include <stdbool.h>
+#include <stdint.h>
+
 #include "platform.h"
 
+#if defined(VTX_CONTROL) && defined(VTX_COMMON)
+
 #include "common/maths.h"
 
 #include "config/config_eeprom.h"
@@ -38,9 +42,13 @@
 #include "io/vtx_control.h"
 
 
-#if defined(VTX_CONTROL) && defined(VTX_COMMON)
 
-PG_REGISTER(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
+
+PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
+//    .vtxChannelActivationConditions = { 0 },
+    .halfDuplex = true
+);
 
 static uint8_t locked = 0;
 
@@ -171,7 +179,6 @@ void vtxCyclePower(const uint8_t powerStep)
 void handleVTXControlButton(void)
 {
 #if defined(VTX_RTC6705) && defined(BUTTON_A_PIN)
-    bool buttonHeld;
     bool buttonWasPressed = false;
     uint32_t start = millis();
     uint32_t ledToggleAt = start;
@@ -179,6 +186,7 @@ void handleVTXControlButton(void)
     uint8_t flashesDone = 0;
 
     uint8_t actionCounter = 0;
+    bool buttonHeld;
     while ((buttonHeld = buttonAPressed())) {
         uint32_t end = millis();
 
@@ -227,21 +235,20 @@ void handleVTXControlButton(void)
     LED1_OFF;
 
     switch (actionCounter) {
-        case 4:
-            vtxCycleBandOrChannel(0, +1);
-            break;
-        case 3:
-            vtxCycleBandOrChannel(+1, 0);
-            break;
-        case 2:
-            vtxCyclePower(+1);
-            break;
-        case 1:
-            saveConfigAndNotify();
-            break;
+    case 4:
+        vtxCycleBandOrChannel(0, +1);
+        break;
+    case 3:
+        vtxCycleBandOrChannel(+1, 0);
+        break;
+    case 2:
+        vtxCyclePower(+1);
+        break;
+    case 1:
+        saveConfigAndNotify();
+        break;
     }
 #endif
 }
 
 #endif
-