Rebased onto master

Makefile

@@ -665,6 +665,7 @@ COMMON_SRC = \
             build/version.c \
             $(TARGET_DIR_SRC) \
             main.c \
+            common/bitarray.c \
             common/encoding.c \
             common/filter.c \
             common/maths.c \
@@ -677,6 +678,7 @@ COMMON_SRC = \
             config/config_streamer.c \
             drivers/adc.c \
             drivers/buf_writer.c \
+            drivers/bus_i2c_config.c \
             drivers/bus_i2c_soft.c \
             drivers/bus_spi.c \
             drivers/bus_spi_soft.c \
@@ -735,6 +737,7 @@ FC_SRC = \
             fc/fc_rc.c \
             fc/rc_adjustments.c \
             fc/rc_controls.c \
+            fc/rc_modes.c \
             fc/cli.c \
             fc/settings.c \
             flight/altitude.c \
@@ -896,7 +899,11 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             io/osd_slave.c
 
 SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
+            drivers/bus_i2c_config.c \
             drivers/serial_escserial.c \
+            drivers/serial_pinconfig.c \
+            drivers/serial_uart_init.c \
+            drivers/serial_uart_pinconfig.c \
             drivers/vtx_rtc6705_soft_spi.c \
             drivers/vtx_rtc6705.c \
             drivers/vtx_common.c \
@@ -907,9 +914,6 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             config/feature.c \
             config/parameter_group.c \
             config/config_streamer.c \
-            drivers/serial_pinconfig.c \
-            drivers/serial_uart_init.c \
-            drivers/serial_uart_pinconfig.c \
             io/serial_4way.c \
             io/serial_4way_avrootloader.c \
             io/serial_4way_stk500v2.c \
@@ -1014,7 +1018,8 @@ STM32F7xx_COMMON_SRC = \
             drivers/serial_uart_stm32f7xx.c \
             drivers/serial_uart_hal.c
 
-F7EXCLUDES = drivers/bus_spi.c \
+F7EXCLUDES = \
+            drivers/bus_spi.c \
             drivers/bus_i2c.c \
             drivers/timer.c \
             drivers/serial_uart.c
@@ -1023,6 +1028,7 @@ SITLEXCLUDES = \
             drivers/adc.c \
             drivers/bus_spi.c \
             drivers/bus_i2c.c \
+            drivers/bus_i2c_config.c \
             drivers/dma.c \
             drivers/pwm_output.c \
             drivers/timer.c \

src/main/blackbox/blackbox.c

@@ -47,6 +47,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"
@@ -322,10 +323,10 @@ typedef struct blackboxSlowState_s {
 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 
 //From mixer.c:
-extern uint16_t motorOutputHigh, motorOutputLow;
+extern float motorOutputHigh, motorOutputLow;
 
 //From rc_controls.c
-extern uint32_t rcModeActivationMask;
+extern boxBitmask_t rcModeActivationMask;
 
 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
 
@@ -753,7 +754,7 @@ static void writeSlowFrame(void)
  */
 static void loadSlowState(blackboxSlowState_t *slow)
 {
-    slow->flightModeFlags = rcModeActivationMask; //was flightModeFlags;
+    memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
     slow->stateFlags = stateFlags;
     slow->failsafePhase = failsafePhase();
     slow->rxSignalReceived = rxIsReceivingSignal();
@@ -861,7 +862,7 @@ static void blackboxStart(void)
      */
     blackboxBuildConditionCache();
 
-    blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
+    blackboxModeActivationConditionPresent = isModeActivationConditionPresent(BOXBLACKBOX);
 
     blackboxResetIterationTimers();
 
@@ -870,7 +871,7 @@ static void blackboxStart(void)
      * it finally plays the beep for this arming event.
      */
     blackboxLastArmingBeep = getArmingBeepTimeMicros();
-    blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status
+    memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
 
     blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
 }
@@ -1187,6 +1188,9 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v
  */
 static bool blackboxWriteSysinfo(void)
 {
+    const uint16_t motorOutputLowInt = lrintf(motorOutputLow);
+    const uint16_t motorOutputHighInt = lrintf(motorOutputHigh);
+
     // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line)
     if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) {
         return false;
@@ -1202,7 +1206,7 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d",                      motorConfig()->minthrottle);
         BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d",                      motorConfig()->maxthrottle);
         BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x",                     castFloatBytesToInt(1.0f));
-        BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d",                   motorOutputLow,motorOutputHigh);
+        BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d",                   motorOutputLowInt,motorOutputHighInt);
         BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u",                           acc.dev.acc_1G);
 
         BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
@@ -1383,10 +1387,10 @@ static void blackboxCheckAndLogFlightMode(void)
     flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
 
     // Use != so that we can still detect a change if the counter wraps
-    if (rcModeActivationMask != blackboxLastFlightModeFlags) {
+    if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
         eventData.lastFlags = blackboxLastFlightModeFlags;
-        blackboxLastFlightModeFlags = rcModeActivationMask;
+        memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
-        eventData.flags = rcModeActivationMask;
+        memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
 
         blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData);
     }

src/main/cms/cms_menu_osd.c

@@ -137,7 +137,7 @@ OSD_Entry menuOsdActiveElemsEntries[] =
     {"YAW PID",            OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_YAW_PIDS], 0},
     {"PROFILES",           OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PIDRATE_PROFILE], 0},
     {"DEBUG",              OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_DEBUG], 0},
-    {"BATT WARN",          OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_MAIN_BATT_WARNING], 0},
+    {"WARNINGS",           OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_WARNINGS], 0},
     {"DISARMED",           OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_DISARMED], 0},
     {"PIT ANG",            OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_ANGLE], 0},
     {"ROL ANG",            OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_ANGLE], 0},

src/main/common/bitarray.c

@@ -15,34 +15,25 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <stdbool.h>
 #include <stdint.h>
+#include <stdbool.h>
 
-#include "platform.h"
+#include "bitarray.h"
-#include "drivers/bus_i2c.h"
-#include "drivers/bus_spi.h"
-#include "hardware_revision.h"
 
-void targetBusInit(void)
+#define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op (1 << ((bit) % (sizeof((array)[0]) * 8))))
+
+bool bitArrayGet(const void *array, unsigned bit)
 {
-    #ifdef USE_SPI
+    return BITARRAY_BIT_OP((uint32_t*)array, bit, &);
-    #ifdef USE_SPI_DEVICE_1
+}
-        spiInit(SPIDEV_1);
+
-    #endif
+void bitArraySet(void *array, unsigned bit)
-    #ifdef USE_SPI_DEVICE_2
+{
-        spiInit(SPIDEV_2);
+    BITARRAY_BIT_OP((uint32_t*)array, bit, |=);
-    #endif
+}
-    #ifdef USE_SPI_DEVICE_3
+
-        if (hardwareRevision == AFF3_REV_2) {
+void bitArrayClr(void *array, unsigned bit)
-            spiInit(SPIDEV_3);
+{
-        }
+    BITARRAY_BIT_OP((uint32_t*)array, bit, &=~);
-    #endif
+}
-    #ifdef USE_SPI_DEVICE_4
-        spiInit(SPIDEV_4);
-    #endif
-    #endif
 
-    #ifdef USE_I2C
-        i2cInit(I2C_DEVICE);
-    #endif
-}

src/main/common/bitarray.h

@@ -0,0 +1,20 @@
+/*
+ * 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/>.
+ */
+
+bool bitArrayGet(const void *array, unsigned bit);
+void bitArraySet(void *array, unsigned bit);
+void bitArrayClr(void *array, unsigned bit);

src/main/common/utils.h

@@ -20,6 +20,8 @@
 #include <stddef.h>
 #include <stdint.h>
 
+#define NOOP do {} while (0)
+
 #define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0]))
 #define ARRAYEND(x) (&(x)[ARRAYLEN(x)])
 

src/main/config/config_eeprom.c

@@ -38,10 +38,7 @@ static uint16_t eepromConfigSize;
 
 typedef enum {
     CR_CLASSICATION_SYSTEM   = 0,
-    CR_CLASSICATION_PROFILE1 = 1,
+    CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_SYSTEM,
-    CR_CLASSICATION_PROFILE2 = 2,
-    CR_CLASSICATION_PROFILE3 = 3,
-    CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_PROFILE3,
 } configRecordFlags_e;
 
 #define CR_CLASSIFICATION_MASK  (0x3)
@@ -178,23 +175,12 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla
 bool loadEEPROM(void)
 {
     PG_FOREACH(reg) {
-        configRecordFlags_e cls_start, cls_end;
+        const configRecord_t *rec = findEEPROM(reg, CR_CLASSICATION_SYSTEM);
-        if (pgIsSystem(reg)) {
+        if (rec) {
-            cls_start = CR_CLASSICATION_SYSTEM;
+            // config from EEPROM is available, use it to initialize PG. pgLoad will handle version mismatch
-            cls_end = CR_CLASSICATION_SYSTEM;
+            pgLoad(reg, rec->pg, rec->size - offsetof(configRecord_t, pg), rec->version);
         } else {
-            cls_start = CR_CLASSICATION_PROFILE1;
+            pgReset(reg);
-            cls_end = CR_CLASSICATION_PROFILE_LAST;
-        }
-        for (configRecordFlags_e cls = cls_start; cls <= cls_end; cls++) {
-            int profileIndex = cls - cls_start;
-            const configRecord_t *rec = findEEPROM(reg, cls);
-            if (rec) {
-                // config from EEPROM is available, use it to initialize PG. pgLoad will handle version mismatch
-                pgLoad(reg, profileIndex, rec->pg, rec->size - offsetof(configRecord_t, pg), rec->version);
-            } else {
-                pgReset(reg, profileIndex);
-            }
         }
     }
     return true;
@@ -225,25 +211,11 @@ static bool writeSettingsToEEPROM(void)
             .flags = 0
         };
 
-        if (pgIsSystem(reg)) {
+        record.flags |= CR_CLASSICATION_SYSTEM;
-            // write the only instance
+        config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
-            record.flags |= CR_CLASSICATION_SYSTEM;
+        crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record));
-            config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
+        config_streamer_write(&streamer, reg->address, regSize);
-            crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record));
+        crc = crc16_ccitt_update(crc, reg->address, regSize);
-            config_streamer_write(&streamer, reg->address, regSize);
-            crc = crc16_ccitt_update(crc, reg->address, regSize);
-        } else {
-            // write one instance for each profile
-            for (uint8_t profileIndex = 0; profileIndex < PG_PROFILE_COUNT; profileIndex++) {
-                record.flags = 0;
-                record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
-                config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
-                crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record));
-                const uint8_t *address = reg->address + (regSize * profileIndex);
-                config_streamer_write(&streamer, address, regSize);
-                crc = crc16_ccitt_update(crc, address, regSize);
-            }
-        }
     }
 
     configFooter_t footer = {

src/main/config/parameter_group.c

@@ -34,15 +34,9 @@ const pgRegistry_t* pgFind(pgn_t pgn)
     return NULL;
 }
 
-static uint8_t *pgOffset(const pgRegistry_t* reg, uint8_t profileIndex)
+static uint8_t *pgOffset(const pgRegistry_t* reg)
 {
-    const uint16_t regSize = pgSize(reg);
+    return reg->address;
-
-    uint8_t *base = reg->address;
-    if (!pgIsSystem(reg)) {
-        base += (regSize * profileIndex);
-    }
-    return base;
 }
 
 static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base)
@@ -59,18 +53,9 @@ static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base)
     }
 }
 
-void pgReset(const pgRegistry_t* reg, int profileIndex)
+void pgReset(const pgRegistry_t* reg)
 {
-    pgResetInstance(reg, pgOffset(reg, profileIndex));
+    pgResetInstance(reg, pgOffset(reg));
-}
-
-void pgResetCurrent(const pgRegistry_t *reg)
-{
-    if (pgIsSystem(reg)) {
-        pgResetInstance(reg, reg->address);
-    } else {
-        pgResetInstance(reg, *reg->ptr);
-    }
 }
 
 bool pgResetCopy(void *copy, pgn_t pgn)
@@ -83,44 +68,26 @@ bool pgResetCopy(void *copy, pgn_t pgn)
     return false;
 }
 
-void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int size, int version)
+void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version)
 {
-    pgResetInstance(reg, pgOffset(reg, profileIndex));
+    pgResetInstance(reg, pgOffset(reg));
     // restore only matching version, keep defaults otherwise
     if (version == pgVersion(reg)) {
         const int take = MIN(size, pgSize(reg));
-        memcpy(pgOffset(reg, profileIndex), from, take);
+        memcpy(pgOffset(reg), from, take);
     }
 }
 
-int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex)
+int pgStore(const pgRegistry_t* reg, void *to, int size)
 {
     const int take = MIN(size, pgSize(reg));
-    memcpy(to, pgOffset(reg, profileIndex), take);
+    memcpy(to, pgOffset(reg), take);
     return take;
 }
 
-
+void pgResetAll()
-void pgResetAll(int profileCount)
 {
     PG_FOREACH(reg) {
-        if (pgIsSystem(reg)) {
+        pgReset(reg);
-            pgReset(reg, 0);
-        } else {
-            // reset one instance for each profile
-            for (int profileIndex = 0; profileIndex < profileCount; profileIndex++) {
-                pgReset(reg, profileIndex);
-            }
-        }
-    }
-}
-
-void pgActivateProfile(int profileIndex)
-{
-    PG_FOREACH(reg) {
-        if (!pgIsSystem(reg)) {
-            uint8_t *ptr = pgOffset(reg, profileIndex);
-            *(reg->ptr) = ptr;
-        }
     }
 }

src/main/config/parameter_group.h

@@ -34,14 +34,9 @@ typedef enum {
     PGR_PGN_MASK =          0x0fff,
     PGR_PGN_VERSION_MASK =  0xf000,
     PGR_SIZE_MASK =         0x0fff,
-    PGR_SIZE_SYSTEM_FLAG =  0x0000, // documentary
+    PGR_SIZE_SYSTEM_FLAG =  0x0000 // documentary
-    PGR_SIZE_PROFILE_FLAG = 0x8000  // start using flags from the top bit down
 } pgRegistryInternal_e;
 
-enum {
-    PG_PROFILE_COUNT = 3
-};
-
 // function that resets a single parameter group instance
 typedef void (pgResetFunc)(void * /* base */, int /* size */);
 
@@ -60,8 +55,6 @@ typedef struct pgRegistry_s {
 static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_MASK;}
 static inline uint8_t pgVersion(const pgRegistry_t* reg) {return (uint8_t)(reg->pgn >> 12);}
 static inline uint16_t pgSize(const pgRegistry_t* reg) {return reg->size & PGR_SIZE_MASK;}
-static inline uint16_t pgIsSystem(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == 0;}
-static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == PGR_SIZE_PROFILE_FLAG;}
 
 #define PG_PACKED __attribute__((packed))
 
@@ -89,19 +82,11 @@ extern const uint8_t __pg_resetdata_end[];
 #define PG_FOREACH(_name) \
     for (const pgRegistry_t *(_name) = __pg_registry_start; (_name) < __pg_registry_end; _name++)
 
-#define PG_FOREACH_PROFILE(_name)                                    \
-    PG_FOREACH(_name)                                                \
-        if(pgIsSystem(_name)) \
-            continue;                                                \
-        else                                                         \
-            /**/
-
 // Reset configuration to default (by name)
-// Only current profile is reset for profile based configs
+#define PG_RESET(_name)                                         \
-#define PG_RESET_CURRENT(_name)                                         \
     do {                                                                \
         extern const pgRegistry_t _name ##_Registry;                    \
-        pgResetCurrent(&_name ## _Registry);                            \
+        pgReset(&_name ## _Registry);                            \
     } while(0)                                                          \
     /**/
 
@@ -124,15 +109,6 @@ extern const uint8_t __pg_resetdata_end[];
     struct _dummy                                                       \
     /**/
 
-// Declare profile config
-#define PG_DECLARE_PROFILE(_type, _name)                                \
-    extern _type *_name ## _ProfileCurrent;                             \
-    static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \
-    static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \
-    struct _dummy                                                       \
-    /**/
-
-
 // Register system config
 #define PG_REGISTER_I(_type, _name, _pgn, _version, _reset)             \
     _type _name ## _System;                                             \
@@ -195,43 +171,6 @@ extern const uint8_t __pg_resetdata_end[];
     /**/
 #endif
 
-#ifdef UNIT_TEST
-# define _PG_PROFILE_CURRENT_DECL(_type, _name)                  \
-    _type *_name ## _ProfileCurrent = &_name ## _Storage[0];
-#else
-# define _PG_PROFILE_CURRENT_DECL(_type, _name)  \
-    _type *_name ## _ProfileCurrent;
-#endif
-
-// register profile config
-#define PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, _reset)     \
-    STATIC_UNIT_TESTED _type _name ## _Storage[PG_PROFILE_COUNT];       \
-    _PG_PROFILE_CURRENT_DECL(_type, _name)                              \
-    extern const pgRegistry_t _name ## _Registry;                       \
-    const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = {    \
-        .pgn = _pgn | (_version << 12),                                 \
-        .size = sizeof(_type) | PGR_SIZE_PROFILE_FLAG,                  \
-        .address = (uint8_t*)&_name ## _Storage,                        \
-        .ptr = (uint8_t **)&_name ## _ProfileCurrent,                   \
-        _reset,                                                         \
-    }                                                                   \
-    /**/
-
-#define PG_REGISTER_PROFILE(_type, _name, _pgn, _version)               \
-    PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \
-    /**/
-
-#define PG_REGISTER_PROFILE_WITH_RESET_FN(_type, _name, _pgn, _version) \
-    extern void pgResetFn_ ## _name(_type *);                           \
-    PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
-    /**/
-
-#define PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \
-    extern const _type pgResetTemplate_ ## _name;                       \
-    PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
-    /**/
-
-
 // Emit reset defaults for config.
 // Config must be registered with PG_REGISTER_<xxx>_WITH_RESET_TEMPLATE macro
 #define PG_RESET_TEMPLATE(_type, _name, ...)                            \
@@ -245,10 +184,8 @@ extern const uint8_t __pg_resetdata_end[];
 
 const pgRegistry_t* pgFind(pgn_t pgn);
 
-void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int size, int version);
+void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version);
-int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex);
+int pgStore(const pgRegistry_t* reg, void *to, int size);
-void pgResetAll(int profileCount);
+void pgResetAll();
-void pgResetCurrent(const pgRegistry_t *reg);
 bool pgResetCopy(void *copy, pgn_t pgn);
-void pgReset(const pgRegistry_t* reg, int profileIndex);
+void pgReset(const pgRegistry_t* reg);
-void pgActivateProfile(int profileIndex);

src/main/config/parameter_group_ids.h

@@ -106,7 +106,8 @@
 #define PG_VTX_CONFIG 515
 #define PG_SONAR_CONFIG 516
 #define PG_ESC_SENSOR_CONFIG 517
-#define PG_BETAFLIGHT_END 517
+#define PG_I2C_CONFIG 518
+#define PG_BETAFLIGHT_END 518
 
 
 // OSD configuration (subject to change)

src/main/drivers/bus.h

@@ -0,0 +1,24 @@
+/*
+ * 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 "platform.h"
+
+#ifdef TARGET_BUS_INIT
+void targetBusInit(void);
+#endif

src/main/drivers/bus_i2c.h

@@ -19,14 +19,10 @@
 
 #include "platform.h"
 
+#include "config/parameter_group.h"
 #include "drivers/io_types.h"
 #include "drivers/rcc_types.h"
 
-#define I2C_SHORT_TIMEOUT            ((uint32_t)0x1000)
-#define I2C_LONG_TIMEOUT             ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
-#define I2C_DEFAULT_TIMEOUT          I2C_SHORT_TIMEOUT
-
-
 #ifndef I2C_DEVICE
 #define I2C_DEVICE I2CINVALID
 #endif
@@ -36,39 +32,27 @@ typedef enum I2CDevice {
     I2CDEV_1   = 0,
     I2CDEV_2,
     I2CDEV_3,
-#ifdef USE_I2C_DEVICE_4
     I2CDEV_4,
-#endif
-    I2CDEV_COUNT
 } I2CDevice;
 
-typedef struct i2cDevice_s {
+#if defined(STM32F1) || defined(STM32F3)
-    I2C_TypeDef *dev;
+#define I2CDEV_COUNT 2
-    ioTag_t scl;
+#elif defined(STM32F4)
-    ioTag_t sda;
+#define I2CDEV_COUNT 3
-    rccPeriphTag_t rcc;
+#elif defined(STM32F7)
-    bool overClock;
+#define I2CDEV_COUNT 4
-#if !defined(STM32F303xC)
+#else
-    uint8_t ev_irq;
+#define I2CDEV_COUNT 4
-    uint8_t er_irq;
 #endif
-#if defined(STM32F7)
-    uint8_t af;
-#endif
-} i2cDevice_t;
 
-typedef struct i2cState_s {
+typedef struct i2cConfig_s {
-    volatile bool error;
+    ioTag_t ioTagScl[I2CDEV_COUNT];
-    volatile bool busy;
+    ioTag_t ioTagSda[I2CDEV_COUNT];
-    volatile uint8_t addr;
+    bool overClock[I2CDEV_COUNT];
-    volatile uint8_t reg;
+    bool pullUp[I2CDEV_COUNT];
-    volatile uint8_t bytes;
+} i2cConfig_t;
-    volatile uint8_t writing;
-    volatile uint8_t reading;
-    volatile uint8_t* write_p;
-    volatile uint8_t* read_p;
-} i2cState_t;
 
+void i2cHardwareConfigure(void);
 void i2cInit(I2CDevice device);
 bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data);
 bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data);

src/main/drivers/bus_i2c_config.c

@@ -0,0 +1,262 @@
+/*
+ * 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/>.
+ */
+
+/*
+ * Created by jflyper
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "drivers/io.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_impl.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#if defined(USE_I2C) && !defined(SOFT_I2C)
+
+#ifdef I2C_FULL_RECONFIGURABILITY
+#if I2CDEV_COUNT >= 1
+#ifndef I2C1_SCL
+#define I2C1_SCL NONE
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA NONE
+#endif
+#endif
+
+#if I2CDEV_COUNT >= 2
+#ifndef I2C2_SCL
+#define I2C2_SCL NONE
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA NONE
+#endif
+#endif
+
+#if I2CDEV_COUNT >= 3
+#ifndef I2C3_SCL
+#define I2C3_SCL NONE
+#endif
+#ifndef I2C3_SDA
+#define I2C3_SDA NONE
+#endif
+#endif
+
+#if I2CDEV_COUNT >= 4
+#ifndef I2C4_SCL
+#define I2C4_SCL NONE
+#endif
+#ifndef I2C4_SDA
+#define I2C4_SDA NONE
+#endif
+#endif
+
+#else // I2C_FULL_RECONFIGURABILITY
+
+// Backward compatibility for exisiting targets
+
+#ifdef STM32F1
+#ifndef I2C1_SCL
+#define I2C1_SCL PB8
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB9
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PB10
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PB11
+#endif
+#endif // STM32F1
+
+#ifdef STM32F3
+#ifndef I2C1_SCL
+#define I2C1_SCL PB6
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB7
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PA9
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PA10
+#endif
+#endif // STM32F3
+
+#ifdef STM32F4
+#ifndef I2C1_SCL
+#define I2C1_SCL PB6
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB7
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PB10
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PB11
+#endif
+#ifndef I2C3_SCL
+#define I2C3_SCL PA8
+#endif
+#ifndef I2C3_SDA
+#define I2C3_SDA PC9
+#endif
+#endif // STM32F4
+
+#ifdef STM32F7
+#ifndef I2C1_SCL
+#define I2C1_SCL PB6
+#endif
+#ifndef I2C1_SDA
+#define I2C1_SDA PB7
+#endif
+#ifndef I2C2_SCL
+#define I2C2_SCL PB10
+#endif
+#ifndef I2C2_SDA
+#define I2C2_SDA PB11
+#endif
+#ifndef I2C3_SCL
+#define I2C3_SCL PA8
+#endif
+#ifndef I2C3_SDA
+#define I2C3_SDA PB4
+#endif
+#ifndef I2C4_SCL
+#define I2C4_SCL PD12
+#endif
+#ifndef I2C4_SDA
+#define I2C4_SDA PD13
+#endif
+#endif // STM32F7
+
+#endif // I2C_FULL_RECONFIGURABILITY
+
+// Backward compatibility for overclocking and internal pullup.
+// These will eventually be configurable through PG-based configurator
+// (and/or probably through some cli extension).
+
+#ifndef I2C1_OVERCLOCK
+#define I2C1_OVERCLOCK false
+#endif
+#ifndef I2C2_OVERCLOCK
+#define I2C2_OVERCLOCK false
+#endif
+#ifndef I2C3_OVERCLOCK
+#define I2C3_OVERCLOCK false
+#endif
+#ifndef I2C4_OVERCLOCK
+#define I2C4_OVERCLOCK false
+#endif
+
+// Default values for internal pullup
+
+#if defined(USE_I2C_PULLUP)
+#define I2C1_PULLUP true
+#define I2C2_PULLUP true
+#define I2C3_PULLUP true
+#define I2C4_PULLUP true
+#else
+#define I2C1_PULLUP false
+#define I2C2_PULLUP false
+#define I2C3_PULLUP false
+#define I2C4_PULLUP false
+#endif
+
+typedef struct i2cDefaultConfig_s {
+    I2CDevice device;
+    ioTag_t ioTagScl, ioTagSda;
+    bool overClock;
+    bool pullUp;
+} i2cDefaultConfig_t;
+
+static const i2cDefaultConfig_t i2cDefaultConfig[] = {
+#ifdef USE_I2C_DEVICE_1
+    { I2CDEV_1, IO_TAG(I2C1_SCL), IO_TAG(I2C1_SDA), I2C1_OVERCLOCK, I2C1_PULLUP },
+#endif
+#ifdef USE_I2C_DEVICE_2
+    { I2CDEV_2, IO_TAG(I2C2_SCL), IO_TAG(I2C2_SDA), I2C2_OVERCLOCK, I2C2_PULLUP },
+#endif
+#ifdef USE_I2C_DEVICE_3
+    { I2CDEV_3, IO_TAG(I2C3_SCL), IO_TAG(I2C3_SDA), I2C3_OVERCLOCK, I2C3_PULLUP },
+#endif
+#ifdef USE_I2C_DEVICE_4
+    { I2CDEV_4, IO_TAG(I2C4_SCL), IO_TAG(I2C4_SDA), I2C4_OVERCLOCK, I2C4_PULLUP },
+#endif
+};
+
+PG_DECLARE(i2cConfig_t, i2cConfig);
+PG_REGISTER_WITH_RESET_FN(i2cConfig_t, i2cConfig, PG_I2C_CONFIG, 0);
+
+void pgResetFn_i2cConfig(i2cConfig_t *i2cConfig)
+{
+    memset(i2cConfig, 0, sizeof(*i2cConfig));
+
+    for (size_t index = 0 ; index < ARRAYLEN(i2cDefaultConfig) ; index++) {
+        const i2cDefaultConfig_t *defconf = &i2cDefaultConfig[index];
+        i2cConfig->ioTagScl[defconf->device] = defconf->ioTagScl;
+        i2cConfig->ioTagSda[defconf->device] = defconf->ioTagSda;
+        i2cConfig->overClock[defconf->device] = defconf->overClock;
+        i2cConfig->pullUp[defconf->device] = defconf->pullUp;
+    }
+}
+
+void i2cHardwareConfigure(void)
+{
+    const i2cConfig_t *pConfig = i2cConfig();
+
+    for (int index = 0 ; index < I2CDEV_COUNT ; index++) {
+        const i2cHardware_t *hardware = &i2cHardware[index];
+
+        if (!hardware->reg) {
+            continue;
+        }
+
+        I2CDevice device = hardware->device;
+        i2cDevice_t *pDev = &i2cDevice[device];
+
+        memset(pDev, 0, sizeof(*pDev));
+
+        for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) {
+            if (pConfig->ioTagScl[device] == hardware->sclPins[pindex])
+              pDev->scl = IOGetByTag(pConfig->ioTagScl[device]);
+            if (pConfig->ioTagSda[device] == hardware->sdaPins[pindex])
+              pDev->sda = IOGetByTag(pConfig->ioTagSda[device]);
+        }
+
+        if (pDev->scl && pDev->sda) {
+            pDev->hardware = hardware;
+            pDev->reg = hardware->reg;
+            pDev->overClock = pConfig->overClock[device];
+            pDev->pullUp = pConfig->pullUp[device];
+        }
+    }
+}
+
+#endif // defined(USE_I2C) && !defined(USE_SOFT_I2C)

src/main/drivers/bus_i2c_hal.c

@@ -17,114 +17,118 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include <platform.h>
 
-#include "drivers/bus_i2c.h"
 #include "drivers/io.h"
+#include "drivers/io_impl.h"
 #include "drivers/nvic.h"
 #include "drivers/time.h"
+#include "drivers/rcc.h"
 
-#include "io_impl.h"
+#include "drivers/bus_i2c.h"
-#include "rcc.h"
+#include "drivers/bus_i2c_impl.h"
 
-
+#if defined(USE_I2C) && !defined(SOFT_I2C)
-#if !defined(SOFT_I2C) && defined(USE_I2C)
 
 #define CLOCKSPEED 800000    // i2c clockspeed 400kHz default (conform specs), 800kHz  and  1200kHz (Betaflight default)
 
 static void i2cUnstick(IO_t scl, IO_t sda);
 
-#if defined(USE_I2C_PULLUP)
+#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
-#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
+#define IOCFG_I2C    IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
-#else
-#define IOCFG_I2C IOCFG_AF_OD
-#endif
 
-#ifndef I2C1_SCL
+#define GPIO_AF4_I2C GPIO_AF4_I2C1
-#define I2C1_SCL PB6
-#endif
 
-#ifndef I2C1_SDA
+const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
-#define I2C1_SDA PB7
+#ifdef USE_I2C_DEVICE_1
+    {
+        .device = I2CDEV_1,
+        .reg = I2C1,
+        .sclPins = { DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) },
+        .sdaPins = { DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) },
+        .rcc = RCC_APB1(I2C1),
+        .ev_irq = I2C1_EV_IRQn,
+        .er_irq = I2C1_ER_IRQn,
+    },
 #endif
-
+#ifdef USE_I2C_DEVICE_2
-#ifndef I2C2_SCL
+    {
-#define I2C2_SCL PB10
+        .device = I2CDEV_2,
+        .reg = I2C2,
+        .sclPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PF1) },
+        .sdaPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PF0) },
+        .rcc = RCC_APB1(I2C2),
+        .ev_irq = I2C2_EV_IRQn,
+        .er_irq = I2C2_ER_IRQn,
+    },
 #endif
-#ifndef I2C2_SDA
+#ifdef USE_I2C_DEVICE_3
-#define I2C2_SDA PB11
+    {   
+        .device = I2CDEV_3,
+        .reg = I2C3, 
+        .sclPins = { DEFIO_TAG_E(PA8) },
+        .sdaPins = { DEFIO_TAG_E(PC9) },
+        .rcc = RCC_APB1(I2C3),
+        .ev_irq = I2C3_EV_IRQn,
+        .er_irq = I2C3_ER_IRQn,
+    },
 #endif
-
+#ifdef USE_I2C_DEVICE_4
-#ifndef I2C3_SCL
+    {
-#define I2C3_SCL PA8
+        .device = I2CDEV_4,
-#endif
+        .reg = I2C4,
-#ifndef I2C3_SDA
+        .sclPins = { DEFIO_TAG_E(PD12), DEFIO_TAG_E(PF14) },
-#define I2C3_SDA PB4
+        .sdaPins = { DEFIO_TAG_E(PD13), DEFIO_TAG_E(PF15) },
-#endif
+        .rcc = RCC_APB1(I2C4),
-
+        .ev_irq = I2C4_EV_IRQn,
-#if defined(USE_I2C_DEVICE_4)
+        .er_irq = I2C4_ER_IRQn,
-#ifndef I2C4_SCL
+    },
-#define I2C4_SCL PD12
-#endif
-#ifndef I2C4_SDA
-#define I2C4_SDA PD13
-#endif
-#endif
-
-static i2cDevice_t i2cHardwareMap[] = {
-    { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn, .af = GPIO_AF4_I2C1 },
-    { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 },
-    { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 },
-#if defined(USE_I2C_DEVICE_4)
-    { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1(I2C4), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
 #endif
 };
 
-typedef struct{
+i2cDevice_t i2cDevice[I2CDEV_COUNT];
-    I2C_HandleTypeDef Handle;
-}i2cHandle_t;
-static i2cHandle_t i2cHandle[I2CDEV_COUNT];
 
 void I2C1_ER_IRQHandler(void)
 {
-    HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_1].Handle);
+    HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_1].handle);
 }
 
 void I2C1_EV_IRQHandler(void)
 {
-    HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_1].Handle);
+    HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_1].handle);
 }
 
 void I2C2_ER_IRQHandler(void)
 {
-    HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_2].Handle);
+    HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_2].handle);
 }
 
 void I2C2_EV_IRQHandler(void)
 {
-    HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_2].Handle);
+    HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_2].handle);
 }
 
 void I2C3_ER_IRQHandler(void)
 {
-    HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_3].Handle);
+    HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_3].handle);
 }
 
 void I2C3_EV_IRQHandler(void)
 {
-    HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_3].Handle);
+    HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_3].handle);
 }
 
 #ifdef USE_I2C_DEVICE_4
 void I2C4_ER_IRQHandler(void)
 {
-    HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_4].Handle);
+    HAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_4].handle);
 }
 
 void I2C4_EV_IRQHandler(void)
 {
-    HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_4].Handle);
+    HAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_4].handle);
 }
 #endif
 
@@ -147,12 +151,22 @@ static bool i2cHandleHardwareFailure(I2CDevice device)
 
 bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
 {
+    if (device == I2CINVALID || device > I2CDEV_COUNT) {
+        return false;
+    }
+
+    I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle;
+
+    if (!pHandle->Instance) {
+        return false;
+    }
+
     HAL_StatusTypeDef status;
 
     if(reg_ == 0xFF)
-        status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle,addr_ << 1,data, len_, I2C_DEFAULT_TIMEOUT);
+        status = HAL_I2C_Master_Transmit(pHandle ,addr_ << 1, data, len_, I2C_DEFAULT_TIMEOUT);
     else
-        status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT);
+        status = HAL_I2C_Mem_Write(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT);
 
     if(status != HAL_OK)
         return i2cHandleHardwareFailure(device);
@@ -167,12 +181,22 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
 
 bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
 {
+    if (device == I2CINVALID || device > I2CDEV_COUNT) {
+        return false;
+    }
+
+    I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle;
+
+    if (!pHandle->Instance) {
+        return false;
+    }
+
     HAL_StatusTypeDef status;
 
     if(reg_ == 0xFF)
-        status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT);
+        status = HAL_I2C_Master_Receive(pHandle ,addr_ << 1, buf, len, I2C_DEFAULT_TIMEOUT);
     else
-        status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT);
+        status = HAL_I2C_Mem_Read(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT);
 
     if(status != HAL_OK)
         return i2cHandleHardwareFailure(device);
@@ -182,87 +206,72 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t
 
 void i2cInit(I2CDevice device)
 {
-    /*## Configure the I2C clock source. The clock is derived from the SYSCLK #*/
+    if (device == I2CINVALID) {
-//    RCC_PeriphCLKInitTypeDef  RCC_PeriphCLKInitStruct;
+        return;
-//    RCC_PeriphCLKInitStruct.PeriphClockSelection = i2cHardwareMap[device].clk;
-//    RCC_PeriphCLKInitStruct.I2c1ClockSelection = i2cHardwareMap[device].clk_src;
-//    HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphCLKInitStruct);
-
-    switch (device) {
-    case I2CDEV_1:
-        __HAL_RCC_I2C1_CLK_ENABLE();
-        break;
-    case I2CDEV_2:
-        __HAL_RCC_I2C2_CLK_ENABLE();
-        break;
-    case I2CDEV_3:
-        __HAL_RCC_I2C3_CLK_ENABLE();
-        break;
-#ifdef USE_I2C_DEVICE_4
-    case I2CDEV_4:
-        __HAL_RCC_I2C4_CLK_ENABLE();
-        break;
-#endif
-    default:
-        break;
     }
-        if (device == I2CINVALID)
-            return;
 
-        i2cDevice_t *i2c;
+    i2cDevice_t *pDev = &i2cDevice[device];
-        i2c = &(i2cHardwareMap[device]);
 
-        //I2C_InitTypeDef i2cInit;
+    const i2cHardware_t *hardware = pDev->hardware;
 
-        IO_t scl = IOGetByTag(i2c->scl);
+    if (!hardware) {
-        IO_t sda = IOGetByTag(i2c->sda);
+        return;
+    }
 
-        IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
+    IO_t scl = pDev->scl;
-        IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
+    IO_t sda = pDev->sda;
 
-        // Enable RCC
+    IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
-        RCC_ClockCmd(i2c->rcc, ENABLE);
+    IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
 
+    // Enable RCC
+    RCC_ClockCmd(hardware->rcc, ENABLE);
 
-        i2cUnstick(scl, sda);
+    i2cUnstick(scl, sda);
+
+    // Init pins
+#ifdef STM32F7
+    IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C);
+    IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C);
+#else
+    IOConfigGPIO(scl, IOCFG_AF_OD);
+    IOConfigGPIO(sda, IOCFG_AF_OD);
+#endif
 
-        // Init pins
-    #ifdef STM32F7
-        IOConfigGPIOAF(scl, IOCFG_I2C, i2c->af);
-        IOConfigGPIOAF(sda, IOCFG_I2C, i2c->af);
-    #else
-        IOConfigGPIO(scl, IOCFG_AF_OD);
-        IOConfigGPIO(sda, IOCFG_AF_OD);
-    #endif
     // Init I2C peripheral
 
-    i2cHandle[device].Handle.Instance             = i2cHardwareMap[device].dev;
+    I2C_HandleTypeDef *pHandle = &pDev->handle;
+
+    memset(pHandle, 0, sizeof(*pHandle));
+
+    pHandle->Instance = pDev->hardware->reg;
+
     /// TODO: HAL check if I2C timing is correct
 
-    if (i2c->overClock) {
+    if (pDev->overClock) {
         // 800khz Maximum speed tested on various boards without issues
-        i2cHandle[device].Handle.Init.Timing          = 0x00500D1D;
+        pHandle->Init.Timing = 0x00500D1D;
     } else {
-        //i2cHandle[device].Handle.Init.Timing          = 0x00500B6A;
+        pHandle->Init.Timing = 0x00500C6F;
-        i2cHandle[device].Handle.Init.Timing          = 0x00500C6F;
     }
-    //i2cHandle[device].Handle.Init.Timing          = 0x00D00E28; /* (Rise time = 120ns, Fall time = 25ns) */
-    i2cHandle[device].Handle.Init.OwnAddress1     = 0x0;
-    i2cHandle[device].Handle.Init.AddressingMode  = I2C_ADDRESSINGMODE_7BIT;
-    i2cHandle[device].Handle.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
-    i2cHandle[device].Handle.Init.OwnAddress2     = 0x0;
-    i2cHandle[device].Handle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
-    i2cHandle[device].Handle.Init.NoStretchMode   = I2C_NOSTRETCH_DISABLE;
 
+    pHandle->Init.OwnAddress1 = 0x0;
+    pHandle->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    pHandle->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    pHandle->Init.OwnAddress2 = 0x0;
+    pHandle->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    pHandle->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
 
-    HAL_I2C_Init(&i2cHandle[device].Handle);
+    HAL_I2C_Init(pHandle);
-    /* Enable the Analog I2C Filter */
-    HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE);
 
-    HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+    // Enable the Analog I2C Filter
-    HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq);
+    HAL_I2CEx_ConfigAnalogFilter(pHandle, I2C_ANALOGFILTER_ENABLE);
-    HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+
-    HAL_NVIC_EnableIRQ(i2cHardwareMap[device].ev_irq);
+    // Setup interrupt handlers
+    HAL_NVIC_SetPriority(hardware->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+    HAL_NVIC_EnableIRQ(hardware->er_irq);
+    HAL_NVIC_SetPriority(hardware->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+    HAL_NVIC_EnableIRQ(hardware->ev_irq);
 }
 
 uint16_t i2cGetErrorCounter(void)

src/main/drivers/bus_i2c_impl.h

@@ -0,0 +1,76 @@
+/*
+ * 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 "platform.h"
+
+#include "drivers/io_types.h"
+#include "drivers/rcc_types.h"
+
+#define I2C_SHORT_TIMEOUT            ((uint32_t)0x1000)
+#define I2C_LONG_TIMEOUT             ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
+#define I2C_DEFAULT_TIMEOUT          I2C_SHORT_TIMEOUT
+
+#define I2C_PIN_SEL_MAX 3
+
+typedef struct i2cHardware_s {
+    I2CDevice device;
+    I2C_TypeDef *reg;
+    ioTag_t sclPins[I2C_PIN_SEL_MAX];
+    ioTag_t sdaPins[I2C_PIN_SEL_MAX];
+    rccPeriphTag_t rcc;
+#if !defined(STM32F303xC)
+    uint8_t ev_irq;
+    uint8_t er_irq;
+#endif
+} i2cHardware_t;
+
+extern const i2cHardware_t i2cHardware[];
+
+#if defined(STM32F1) || defined(STM32F4)
+typedef struct i2cState_s {
+    volatile bool error;
+    volatile bool busy;
+    volatile uint8_t addr;
+    volatile uint8_t reg;
+    volatile uint8_t bytes;
+    volatile uint8_t writing;
+    volatile uint8_t reading;
+    volatile uint8_t* write_p;
+    volatile uint8_t* read_p;
+} i2cState_t;
+#endif
+
+typedef struct i2cDevice_s {
+    const i2cHardware_t *hardware;
+    I2C_TypeDef *reg;
+    IO_t scl;
+    IO_t sda;
+    bool overClock;
+    bool pullUp;
+
+    // MCU/Driver dependent member follows
+#if defined(STM32F1) || defined(STM32F4)
+    i2cState_t state;
+#endif
+#ifdef USE_HAL_DRIVER
+    I2C_HandleTypeDef handle;
+#endif
+} i2cDevice_t;
+
+extern i2cDevice_t i2cDevice[];

src/main/drivers/bus_i2c_stm32f10x.c

@@ -18,18 +18,19 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <platform.h>
 
 #include "drivers/io.h"
 #include "drivers/time.h"
+#include "drivers/nvic.h"
+#include "drivers/rcc.h"
 
 #include "drivers/bus_i2c.h"
-#include "drivers/nvic.h"
+#include "drivers/bus_i2c_impl.h"
-#include "io_impl.h"
-#include "rcc.h"
 
-#ifndef SOFT_I2C
+#if defined(USE_I2C) && !defined(SOFT_I2C)
 
 #define CLOCKSPEED 800000    // i2c clockspeed 400kHz default (conform specs), 800kHz  and  1200kHz (Betaflight default)
 
@@ -40,65 +41,52 @@ static void i2cUnstick(IO_t scl, IO_t sda);
 #define GPIO_AF_I2C GPIO_AF_I2C1
 
 #ifdef STM32F4
-
+#define IOCFG_I2C_PU IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP)
-#if defined(USE_I2C_PULLUP)
+#define IOCFG_I2C    IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)
-#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP)
+#else // STM32F4
-#else
-#define IOCFG_I2C IOCFG_AF_OD
-#endif
-
-#ifndef I2C1_SCL
-#define I2C1_SCL PB8
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA PB9
-#endif
-
-#else
-
-#ifndef I2C1_SCL
-#define I2C1_SCL PB6
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA PB7
-#endif
 #define IOCFG_I2C   IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz)
-
 #endif
 
-#ifndef I2C2_SCL
+const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
-#define I2C2_SCL PB10
+#ifdef USE_I2C_DEVICE_1
+    {
+        .device = I2CDEV_1,
+        .reg = I2C1,
+        .sclPins = { DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) },
+        .sdaPins = { DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) },
+        .rcc = RCC_APB1(I2C1),
+        .ev_irq = I2C1_EV_IRQn,
+        .er_irq = I2C1_ER_IRQn,
+    },
 #endif
-
+#ifdef USE_I2C_DEVICE_2
-#ifndef I2C2_SDA
+    {
-#define I2C2_SDA PB11
+        .device = I2CDEV_2,
+        .reg = I2C2,
+        .sclPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PF1) },
+        .sdaPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PF0) },
+        .rcc = RCC_APB1(I2C2),
+        .ev_irq = I2C2_EV_IRQn,
+        .er_irq = I2C2_ER_IRQn,
+    },
 #endif
-
+#ifdef USE_I2C_DEVICE_3
-#ifdef STM32F4
+    {
-#ifndef I2C3_SCL
+        .device = I2CDEV_3,
-#define I2C3_SCL PA8
+        .reg = I2C3,
-#endif
+        .sclPins = { DEFIO_TAG_E(PA8) },
-#ifndef I2C3_SDA
+        .sdaPins = { DEFIO_TAG_E(PC9) },
-#define I2C3_SDA PC9
+        .rcc = RCC_APB1(I2C3),
-#endif
+        .ev_irq = I2C3_EV_IRQn,
-#endif
+        .er_irq = I2C3_ER_IRQn,
-
+    },
-static i2cDevice_t i2cHardwareMap[] = {
-    { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn },
-    { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn },
-#ifdef STM32F4
-    { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn }
 #endif
 };
 
+i2cDevice_t i2cDevice[I2CDEV_COUNT];
+
 static volatile uint16_t i2cErrorCount = 0;
 
-static i2cState_t i2cState[] = {
-    { false, false, 0, 0, 0, 0, 0, 0, 0 },
-    { false, false, 0, 0, 0, 0, 0, 0, 0 },
-    { false, false, 0, 0, 0, 0, 0, 0, 0 }
-};
-
 void I2C1_ER_IRQHandler(void) {
     i2c_er_handler(I2CDEV_1);
 }
@@ -135,18 +123,19 @@ static bool i2cHandleHardwareFailure(I2CDevice device)
 
 bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
 {
-
+    if (device == I2CINVALID || device > I2CDEV_COUNT) {
-    if (device == I2CINVALID)
         return false;
+    }
 
+    I2C_TypeDef *I2Cx = i2cDevice[device].reg;
+
+    if (!I2Cx) {
+        return false;
+    }
+
+    i2cState_t *state = &i2cDevice[device].state;
     uint32_t timeout = I2C_DEFAULT_TIMEOUT;
 
-    I2C_TypeDef *I2Cx;
-    I2Cx = i2cHardwareMap[device].dev;
-
-    i2cState_t *state;
-    state = &(i2cState[device]);
-
     state->addr = addr_ << 1;
     state->reg = reg_;
     state->writing = 1;
@@ -182,17 +171,19 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
 
 bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
 {
-    if (device == I2CINVALID)
+    if (device == I2CINVALID || device > I2CDEV_COUNT) {
         return false;
+    }
 
+    I2C_TypeDef *I2Cx = i2cDevice[device].reg;
+
+    if (!I2Cx) {
+        return false;
+    }
+
+    i2cState_t *state = &i2cDevice[device].state;
     uint32_t timeout = I2C_DEFAULT_TIMEOUT;
 
-    I2C_TypeDef *I2Cx;
-    I2Cx = i2cHardwareMap[device].dev;
-
-    i2cState_t *state;
-    state = &(i2cState[device]);
-
     state->addr = addr_ << 1;
     state->reg = reg_;
     state->writing = 0;
@@ -223,11 +214,9 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t
 
 static void i2c_er_handler(I2CDevice device) {
 
-    I2C_TypeDef *I2Cx;
+    I2C_TypeDef *I2Cx = i2cDevice[device].hardware->reg;
-    I2Cx = i2cHardwareMap[device].dev;
 
-    i2cState_t *state;
+    i2cState_t *state = &i2cDevice[device].state;
-    state = &(i2cState[device]);
 
     // Read the I2C1 status register
     volatile uint32_t SR1Register = I2Cx->SR1;
@@ -258,11 +247,9 @@ static void i2c_er_handler(I2CDevice device) {
 
 void i2c_ev_handler(I2CDevice device) {
 
-    I2C_TypeDef *I2Cx;
+    I2C_TypeDef *I2Cx = i2cDevice[device].hardware->reg;
-    I2Cx = i2cHardwareMap[device].dev;
 
-    i2cState_t *state;
+    i2cState_t *state = &i2cDevice[device].state;
-    state = &(i2cState[device]);
 
     static uint8_t subaddress_sent, final_stop;                                 // flag to indicate if subaddess sent, flag to indicate final bus condition
     static int8_t index;                                                        // index is signed -1 == send the subaddress
@@ -378,65 +365,72 @@ void i2cInit(I2CDevice device)
     if (device == I2CINVALID)
         return;
 
-    i2cDevice_t *i2c;
+    i2cDevice_t *pDev = &i2cDevice[device];
-    i2c = &(i2cHardwareMap[device]);
+    const i2cHardware_t *hw = pDev->hardware; 
+
+    if (!hw) {
+        return;
+    }
+
+    I2C_TypeDef *I2Cx = hw->reg;
+
+    memset(&pDev->state, 0, sizeof(pDev->state));
 
     NVIC_InitTypeDef nvic;
     I2C_InitTypeDef i2cInit;
 
-    IO_t scl = IOGetByTag(i2c->scl);
+    IO_t scl = pDev->scl;
-    IO_t sda = IOGetByTag(i2c->sda);
+    IO_t sda = pDev->sda;
 
     IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
     IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
 
     // Enable RCC
-    RCC_ClockCmd(i2c->rcc, ENABLE);
+    RCC_ClockCmd(hw->rcc, ENABLE);
 
-    I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE);
+    I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);
 
     i2cUnstick(scl, sda);
 
     // Init pins
 #ifdef STM32F4
-    IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C);
+    IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_I2C);
-    IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_I2C);
+    IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_I2C);
 #else
     IOConfigGPIO(scl, IOCFG_I2C);
     IOConfigGPIO(sda, IOCFG_I2C);
 #endif
 
-    I2C_DeInit(i2c->dev);
+    I2C_DeInit(I2Cx);
     I2C_StructInit(&i2cInit);
 
-    I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE);               // Disable EVT and ERR interrupts - they are enabled by the first request
+    I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE);               // Disable EVT and ERR interrupts - they are enabled by the first request
     i2cInit.I2C_Mode = I2C_Mode_I2C;
     i2cInit.I2C_DutyCycle = I2C_DutyCycle_2;
     i2cInit.I2C_OwnAddress1 = 0;
     i2cInit.I2C_Ack = I2C_Ack_Enable;
     i2cInit.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
 
-    if (i2c->overClock) {
+    if (pDev->overClock) {
         i2cInit.I2C_ClockSpeed = 800000; // 800khz Maximum speed tested on various boards without issues
     } else {
         i2cInit.I2C_ClockSpeed = 400000; // 400khz Operation according specs
     }
 
-    I2C_Cmd(i2c->dev, ENABLE);
+    I2C_Cmd(I2Cx, ENABLE);
-    I2C_Init(i2c->dev, &i2cInit);
+    I2C_Init(I2Cx, &i2cInit);
-
-    I2C_StretchClockCmd(i2c->dev, ENABLE);
 
+    I2C_StretchClockCmd(I2Cx, ENABLE);
 
     // I2C ER Interrupt
-    nvic.NVIC_IRQChannel = i2c->er_irq;
+    nvic.NVIC_IRQChannel = hw->er_irq;
     nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER);
     nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER);
     nvic.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&nvic);
 
     // I2C EV Interrupt
-    nvic.NVIC_IRQChannel = i2c->ev_irq;
+    nvic.NVIC_IRQChannel = hw->ev_irq;
     nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV);
     nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV);
     NVIC_Init(&nvic);

src/main/drivers/bus_i2c_stm32f30x.c

@@ -17,54 +17,57 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include <platform.h>
 
+#include "build/debug.h"
+
 #include "drivers/system.h"
 #include "drivers/io.h"
-#include "io_impl.h"
+#include "drivers/io_impl.h"
-#include "rcc.h"
+#include "drivers/rcc.h"
 
 #include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_impl.h"
 
-#ifndef SOFT_I2C
+#if defined(USE_I2C) && !defined(SOFT_I2C)
 
-#if defined(USE_I2C_PULLUP)
+#define IOCFG_I2C_PU IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP)
-#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP)
+#define IOCFG_I2C    IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL)
-#else
-#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL)
-#endif
 
 #define I2C_HIGHSPEED_TIMING  0x00500E30  // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4.
 #define I2C_STANDARD_TIMING   0x00E0257A  // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
 
-#define I2C_SHORT_TIMEOUT   ((uint32_t)0x1000)
-#define I2C_LONG_TIMEOUT    ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
 #define I2C_GPIO_AF         GPIO_AF_4
 
-#ifndef I2C1_SCL
-#define I2C1_SCL PB6
-#endif
-#ifndef I2C1_SDA
-#define I2C1_SDA PB7
-#endif
-#ifndef I2C2_SCL
-#define I2C2_SCL PF4
-#endif
-#ifndef I2C2_SDA
-#define I2C2_SDA PA10
-#endif
-
 static uint32_t i2cTimeout;
 
 static volatile uint16_t i2cErrorCount = 0;
-//static volatile uint16_t i2c2ErrorCount = 0;
 
-static i2cDevice_t i2cHardwareMap[] = {
+const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
-    { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK },
+#ifdef USE_I2C_DEVICE_1
-    { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK }
+    {
+        .device = I2CDEV_1,
+        .reg = I2C1,
+        .sclPins = { DEFIO_TAG_E(PA15), DEFIO_TAG_E(PB6), DEFIO_TAG_E(PB8) },
+        .sdaPins = { DEFIO_TAG_E(PA14), DEFIO_TAG_E(PB7), DEFIO_TAG_E(PB9) },
+        .rcc = RCC_APB1(I2C1),
+    },
+#endif
+#ifdef USE_I2C_DEVICE_2
+    {
+        .device = I2CDEV_2,
+        .reg = I2C2,
+        .sclPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PF6) },
+        .sdaPins = { DEFIO_TAG_E(PA10) },
+        .rcc = RCC_APB1(I2C2),
+    },
+#endif
 };
 
+i2cDevice_t i2cDevice[I2CDEV_COUNT];
+
 ///////////////////////////////////////////////////////////////////////////////
 // I2C TimeoutUserCallback
 ///////////////////////////////////////////////////////////////////////////////
@@ -77,24 +80,30 @@ uint32_t i2cTimeoutUserCallback(void)
 
 void i2cInit(I2CDevice device)
 {
+    if (device == I2CINVALID || device > I2CDEV_COUNT) {
+        return;
+    }
 
-    i2cDevice_t *i2c;
+    i2cDevice_t *pDev = &i2cDevice[device]; 
-    i2c = &(i2cHardwareMap[device]);
+    const i2cHardware_t *hw = pDev->hardware;
 
-    I2C_TypeDef *I2Cx;
+    if (!hw) {
-    I2Cx = i2c->dev;
+        return;
+    }
 
-    IO_t scl = IOGetByTag(i2c->scl);
+    I2C_TypeDef *I2Cx = pDev->reg;
-    IO_t sda = IOGetByTag(i2c->sda);
 
-    RCC_ClockCmd(i2c->rcc, ENABLE);
+    IO_t scl = pDev->scl;
+    IO_t sda = pDev->sda;
+
+    RCC_ClockCmd(hw->rcc, ENABLE);
     RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK);
 
     IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
-    IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_4);
+    IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_4);
 
     IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
-    IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_4);
+    IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF_4);
 
     I2C_InitTypeDef i2cInit = {
         .I2C_Mode = I2C_Mode_I2C,
@@ -103,7 +112,7 @@ void i2cInit(I2CDevice device)
         .I2C_OwnAddress1 = 0x00,
         .I2C_Ack = I2C_Ack_Enable,
         .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
-        .I2C_Timing = (i2c->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING)
+        .I2C_Timing = (pDev->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING)
     };
 
     I2C_Init(I2Cx, &i2cInit);
@@ -120,10 +129,17 @@ uint16_t i2cGetErrorCounter(void)
 
 bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data)
 {
-    addr_ <<= 1;
+    if (device == I2CINVALID || device > I2CDEV_COUNT) {
+        return false;
+    }
 
-    I2C_TypeDef *I2Cx;
+    I2C_TypeDef *I2Cx = i2cDevice[device].reg;
-    I2Cx = i2cHardwareMap[device].dev;
+
+    if (!I2Cx) {
+        return false;
+    }
+
+    addr_ <<= 1;
 
     /* Test on BUSY Flag */
     i2cTimeout = I2C_LONG_TIMEOUT;
@@ -186,10 +202,17 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data)
 
 bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf)
 {
-    addr_ <<= 1;
+    if (device == I2CINVALID || device > I2CDEV_COUNT) {
+        return false;
+    }
 
-    I2C_TypeDef *I2Cx;
+    I2C_TypeDef *I2Cx = i2cDevice[device].reg;
-    I2Cx = i2cHardwareMap[device].dev;
+
+    if (!I2Cx) {
+        return false;
+    }
+
+    addr_ <<= 1;
 
     /* Test on BUSY Flag */
     i2cTimeout = I2C_LONG_TIMEOUT;

src/main/drivers/display_ug2864hsweg01.c

@@ -24,6 +24,8 @@
 
 #include "display_ug2864hsweg01.h"
 
+#ifdef USE_I2C_OLED_DISPLAY
+
 #if !defined(OLED_I2C_INSTANCE)
 #if defined(I2C_DEVICE)
 #define OLED_I2C_INSTANCE I2C_DEVICE
@@ -288,3 +290,4 @@ bool ug2864hsweg01InitI2C(void)
 
     return true;
 }
+#endif // USE_I2C_OLED_DISPLAY

src/main/drivers/light_led.c

@@ -17,24 +17,49 @@
 
 #include "platform.h"
 
+#include "config/parameter_group_ids.h"
+
 #include "drivers/io.h"
 #include "io_impl.h"
 
 #include "light_led.h"
 
-static IO_t leds[LED_NUMBER];
+PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0);
-static uint8_t ledPolarity = 0;
+
+static IO_t leds[STATUS_LED_NUMBER];
+static uint8_t ledInversion = 0;
+
+void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig)
+{
+#ifdef LED0_PIN
+    statusLedConfig->ioTags[0] = IO_TAG(LED0_PIN);
+#endif
+#ifdef LED1_PIN
+    statusLedConfig->ioTags[1] = IO_TAG(LED1_PIN);
+#endif
+#ifdef LED2_PIN
+    statusLedConfig->ioTags[2] = IO_TAG(LED2_PIN);
+#endif
+
+    statusLedConfig->inversion = 0
+#ifdef LED0_INVERTED
+    | BIT(0)
+#endif
+#ifdef LED1_INVERTED
+    | BIT(1)
+#endif
+#ifdef LED2_INVERTED
+    | BIT(2)
+#endif
+    ;
+}
 
 void ledInit(const statusLedConfig_t *statusLedConfig)
 {
-    LED0_OFF;
+    ledInversion = statusLedConfig->inversion;
-    LED1_OFF;
+    for (int i = 0; i < STATUS_LED_NUMBER; i++) {
-    LED2_OFF;
+        if (statusLedConfig->ioTags[i]) {
-
+            leds[i] = IOGetByTag(statusLedConfig->ioTags[i]);
-    ledPolarity = statusLedConfig->polarity;
-    for (int i = 0; i < LED_NUMBER; i++) {
-        if (statusLedConfig->ledTags[i]) {
-            leds[i] = IOGetByTag(statusLedConfig->ledTags[i]);
             IOInit(leds[i], OWNER_LED, RESOURCE_INDEX(i));
             IOConfigGPIO(leds[i], IOCFG_OUT_PP);
         } else {
@@ -54,6 +79,6 @@ void ledToggle(int led)
 
 void ledSet(int led, bool on)
 {
-    const bool inverted = (1 << (led)) & ledPolarity;
+    const bool inverted = (1 << (led)) & ledInversion;
     IOWrite(leds[led], on ? inverted : !inverted);
 }

src/main/drivers/light_led.h

@@ -19,47 +19,48 @@
 
 #include "config/parameter_group.h"
 #include "drivers/io_types.h"
+#include "common/utils.h"
 
-#define LED_NUMBER 3
+#define STATUS_LED_NUMBER 3
 
 typedef struct statusLedConfig_s {
-    ioTag_t ledTags[LED_NUMBER];
+    ioTag_t ioTags[STATUS_LED_NUMBER];
-    uint8_t polarity;
+    uint8_t inversion;
 } statusLedConfig_t;
 
 PG_DECLARE(statusLedConfig_t, statusLedConfig);
 
 // Helpful macros
-#ifdef LED0
+#if defined(UNIT_TEST) || defined(USE_FAKE_LED)
-# define LED0_TOGGLE              ledToggle(0)
-# define LED0_OFF                 ledSet(0, false)
-# define LED0_ON                  ledSet(0, true)
-#else
-# define LED0_TOGGLE              do {} while(0)
-# define LED0_OFF                 do {} while(0)
-# define LED0_ON                  do {} while(0)
-#endif
 
-#ifdef LED1
+#define LED0_TOGGLE              NOOP
-# define LED1_TOGGLE              ledToggle(1)
+#define LED0_OFF                 NOOP
-# define LED1_OFF                 ledSet(1, false)
+#define LED0_ON                  NOOP
-# define LED1_ON                  ledSet(1, true)
+
-#else
+#define LED1_TOGGLE              NOOP
-# define LED1_TOGGLE              do {} while(0)
+#define LED1_OFF                 NOOP
-# define LED1_OFF                 do {} while(0)
+#define LED1_ON                  NOOP
-# define LED1_ON                  do {} while(0)
+
-#endif
+#define LED2_TOGGLE              NOOP
+#define LED2_OFF                 NOOP
+#define LED2_ON                  NOOP
 
-#ifdef LED2
-# define LED2_TOGGLE              ledToggle(2)
-# define LED2_OFF                 ledSet(2, false)
-# define LED2_ON                  ledSet(2, true)
 #else
-# define LED2_TOGGLE              do {} while(0)
+
-# define LED2_OFF                 do {} while(0)
+#define LED0_TOGGLE              ledToggle(0)
-# define LED2_ON                  do {} while(0)
+#define LED0_OFF                 ledSet(0, false)
-#endif
+#define LED0_ON                  ledSet(0, true)
+
+#define LED1_TOGGLE              ledToggle(1)
+#define LED1_OFF                 ledSet(1, false)
+#define LED1_ON                  ledSet(1, true)
+
+#define LED2_TOGGLE              ledToggle(2)
+#define LED2_OFF                 ledSet(2, false)
+#define LED2_ON                  ledSet(2, true)
 
 void ledInit(const statusLedConfig_t *statusLedConfig);
 void ledToggle(int led);
 void ledSet(int led, bool state);
+
+#endif

src/main/drivers/pwm_output.c

@@ -125,40 +125,42 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
     *port->ccr = 0;
 }
 
-static void pwmWriteUnused(uint8_t index, uint16_t value)
+static void pwmWriteUnused(uint8_t index, float value)
 {
     UNUSED(index);
     UNUSED(value);
 }
 
-static void pwmWriteBrushed(uint8_t index, uint16_t value)
+static void pwmWriteBrushed(uint8_t index, float value)
 {
-    *motors[index].ccr = (value - 1000) * motors[index].period / 1000;
+    *motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000);
 }
 
-static void pwmWriteStandard(uint8_t index, uint16_t value)
+static void pwmWriteStandard(uint8_t index, float value)
 {
-    *motors[index].ccr = value;
+    *motors[index].ccr = lrintf(value);
 }
 
-static void pwmWriteOneShot125(uint8_t index, uint16_t value)
+static void pwmWriteOneShot125(uint8_t index, float value)
 {
-    *motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
+    *motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f);
 }
 
-static void pwmWriteOneShot42(uint8_t index, uint16_t value)
+static void pwmWriteOneShot42(uint8_t index, float value)
 {
-    *motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
+    *motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f);
 }
 
-static void pwmWriteMultiShot(uint8_t index, uint16_t value)
+static void pwmWriteMultiShot(uint8_t index, float value)
 {
-    *motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
+    *motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
 }
 
-void pwmWriteMotor(uint8_t index, uint16_t value)
+void pwmWriteMotor(uint8_t index, float value)
 {
-    pwmWritePtr(index, value);
+    if (pwmMotorsEnabled) {
+        pwmWritePtr(index, value);
+    }    
 }
 
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
@@ -245,11 +247,16 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
         idlePulse = 0;
         break;
 #ifdef USE_DSHOT
+    case PWM_TYPE_PROSHOT1000:
+        pwmWritePtr = pwmWriteProShot;
+        pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
+        isDigital = true;
+        break;
     case PWM_TYPE_DSHOT1200:
     case PWM_TYPE_DSHOT600:
     case PWM_TYPE_DSHOT300:
     case PWM_TYPE_DSHOT150:
-        pwmWritePtr = pwmWriteDigital;
+        pwmWritePtr = pwmWriteDshot;
         pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
         isDigital = true;
         break;
@@ -320,15 +327,17 @@ pwmOutputPort_t *pwmGetMotors(void)
 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
 {
     switch (pwmProtocolType) {
-        case(PWM_TYPE_DSHOT1200):
+    case(PWM_TYPE_PROSHOT1000):
-            return MOTOR_DSHOT1200_MHZ * 1000000;
+        return MOTOR_PROSHOT1000_MHZ * 1000000;
-        case(PWM_TYPE_DSHOT600):
+    case(PWM_TYPE_DSHOT1200):
-            return MOTOR_DSHOT600_MHZ * 1000000;
+        return MOTOR_DSHOT1200_MHZ * 1000000;
-        case(PWM_TYPE_DSHOT300):
+    case(PWM_TYPE_DSHOT600):
-            return MOTOR_DSHOT300_MHZ * 1000000;
+        return MOTOR_DSHOT600_MHZ * 1000000;
-        default:
+    case(PWM_TYPE_DSHOT300):
-        case(PWM_TYPE_DSHOT150):
+        return MOTOR_DSHOT300_MHZ * 1000000;
-            return MOTOR_DSHOT150_MHZ * 1000000;
+    default:
+    case(PWM_TYPE_DSHOT150):
+        return MOTOR_DSHOT150_MHZ * 1000000;
     }
 }
 
@@ -338,11 +347,21 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
         motorDmaOutput_t *const motor = getMotorDmaOutput(index);
 
         unsigned repeats;
-        if ((command >= 7 && command <= 10) || command == 12) {
+        switch (command) {
+        case DSHOT_CMD_SPIN_DIRECTION_1:
+        case DSHOT_CMD_SPIN_DIRECTION_2:
+        case DSHOT_CMD_3D_MODE_OFF:
+        case DSHOT_CMD_3D_MODE_ON:
+        case DSHOT_CMD_SAVE_SETTINGS:
+        case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
+        case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
             repeats = 10;
-        } else {
+            break;
+        default:
             repeats = 1;
+            break;
         }
+
         for (; repeats; repeats--) {
             motor->requestTelemetry = true;
             pwmWritePtr(index, command);
@@ -355,10 +374,10 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
 #endif
 
 #ifdef USE_SERVOS
-void pwmWriteServo(uint8_t index, uint16_t value)
+void pwmWriteServo(uint8_t index, float value)
 {
     if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
-        *servos[index].ccr = value;
+        *servos[index].ccr = lrintf(value);
     }
 }
 

src/main/drivers/pwm_output.h

@@ -28,6 +28,26 @@
 #define MAX_SUPPORTED_SERVOS 8
 #endif
 
+typedef enum {
+    DSHOT_CMD_MOTOR_STOP = 0,
+    DSHOT_CMD_BEEP1,
+    DSHOT_CMD_BEEP2,
+    DSHOT_CMD_BEEP3,
+    DSHOT_CMD_BEEP4,
+    DSHOT_CMD_BEEP5,
+    DSHOT_CMD_ESC_INFO,
+    DSHOT_CMD_SPIN_DIRECTION_1,
+    DSHOT_CMD_SPIN_DIRECTION_2,
+    DSHOT_CMD_3D_MODE_OFF,
+    DSHOT_CMD_3D_MODE_ON, 
+    DSHOT_CMD_SETTINGS_REQUEST,
+    DSHOT_CMD_SAVE_SETTINGS, 
+    DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, //Blheli_S only command
+    DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,  //Blheli_S only command
+    DSHOT_CMD_MAX = 47
+} dshotCommands_e;
+
+
 typedef enum {
     PWM_TYPE_STANDARD = 0,
     PWM_TYPE_ONESHOT125,
@@ -39,6 +59,7 @@ typedef enum {
     PWM_TYPE_DSHOT300,
     PWM_TYPE_DSHOT600,
     PWM_TYPE_DSHOT1200,
+    PWM_TYPE_PROSHOT1000,
 #endif
     PWM_TYPE_MAX
 } motorPwmProtocolTypes_e;
@@ -56,6 +77,11 @@ typedef enum {
 #define MOTOR_BIT_0           7
 #define MOTOR_BIT_1           14
 #define MOTOR_BITLENGTH       19
+
+#define MOTOR_PROSHOT1000_MHZ        24 
+#define PROSHOT_BASE_SYMBOL          24 // 1uS
+#define PROSHOT_BIT_WIDTH            3
+#define MOTOR_NIBBLE_LENGTH_PROSHOT  96 // 4uS
 #endif
 
 #if defined(STM32F40_41xxx) // must be multiples of timer clock
@@ -75,7 +101,8 @@ typedef enum {
 #define PWM_BRUSHED_TIMER_MHZ 24
 #endif
 
-#define MOTOR_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
+#define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
+#define PROSHOT_DMA_BUFFER_SIZE 6/* resolution + frame reset (2us) */
 
 typedef struct {
     TIM_TypeDef *timer;
@@ -89,9 +116,9 @@ typedef struct {
     uint16_t timerDmaSource;
     volatile bool requestTelemetry;
 #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
-    uint32_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];
+    uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
 #else
-    uint8_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];
+    uint8_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
 #endif
 #if defined(STM32F7)
     TIM_HandleTypeDef TimHandle;
@@ -104,7 +131,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
 extern bool pwmMotorsEnabled;
 
 struct timerHardware_s;
-typedef void(*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
+typedef void(*pwmWriteFuncPtr)(uint8_t index, float value);  // function pointer used to write motors
 typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount);   // function pointer used after motors are written
 
 typedef struct {
@@ -140,7 +167,8 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
 #ifdef USE_DSHOT
 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
 void pwmWriteDshotCommand(uint8_t index, uint8_t command);
-void pwmWriteDigital(uint8_t index, uint16_t value);
+void pwmWriteProShot(uint8_t index, float value);
+void pwmWriteDshot(uint8_t index, float value);
 void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
 #endif
@@ -151,11 +179,11 @@ void pwmToggleBeeper(void);
 void beeperPwmInit(IO_t io, uint16_t frequency);
 #endif
 
-void pwmWriteMotor(uint8_t index, uint16_t value);
+void pwmWriteMotor(uint8_t index, float value);
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
 void pwmCompleteMotorUpdate(uint8_t motorCount);
 
-void pwmWriteServo(uint8_t index, uint16_t value);
+void pwmWriteServo(uint8_t index, float value);
 
 pwmOutputPort_t *pwmGetMotors(void);
 bool pwmIsSynced(void);

src/main/drivers/pwm_output_dshot.c

@@ -54,12 +54,9 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
     return dmaMotorTimerCount-1;
 }
 
-void pwmWriteDigital(uint8_t index, uint16_t value)
+void pwmWriteDshot(uint8_t index, float value)
 {
-
+    const uint16_t digitalValue = lrintf(value);
-    if (!pwmMotorsEnabled) {
-        return;
-    }
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
@@ -67,7 +64,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
         return;
     }
 
-    uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
+    uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
     motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 
     // compute checksum
@@ -86,7 +83,41 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
         packet <<= 1;
     }
 
-    DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, MOTOR_DMA_BUFFER_SIZE);
+    DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE);
+    DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
+}
+
+void pwmWriteProShot(uint8_t index, float value)
+{
+    const uint16_t digitalValue = lrintf(value);
+
+    motorDmaOutput_t * const motor = &dmaMotors[index];
+
+    if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
+        return;
+    }
+
+    uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
+    motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
+
+    // compute checksum
+    int csum = 0;
+    int csum_data = packet;
+    for (int i = 0; i < 3; i++) {
+        csum ^=  csum_data;   // xor data by nibbles
+        csum_data >>= 4;
+    }
+    csum &= 0xf;
+    // append checksum
+    packet = (packet << 4) | csum;
+
+    // generate pulses for Proshot
+    for (int i = 0; i < 4; i++) {
+        motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH;  // Most significant nibble first
+        packet <<= 4;	// Shift 4 bits
+    }
+
+    DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, PROSHOT_DMA_BUFFER_SIZE);
     DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
 }
 
@@ -139,7 +170,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
         TIM_Cmd(timer, DISABLE);
 
         TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((timerClock(timer) / getDshotHz(pwmProtocolType)) - 1);
-        TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH;
+        TIM_TimeBaseStructure.TIM_Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH;
         TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
         TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
@@ -205,7 +236,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
     DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 #endif
     DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware);
-    DMA_InitStructure.DMA_BufferSize = MOTOR_DMA_BUFFER_SIZE;
+    DMA_InitStructure.DMA_BufferSize = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
     DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
     DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
     DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;

src/main/drivers/pwm_output_dshot_hal.c

@@ -49,11 +49,9 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
     return dmaMotorTimerCount - 1;
 }
 
-void pwmWriteDigital(uint8_t index, uint16_t value)
+void pwmWriteDshot(uint8_t index, float value)
 {
-    if (!pwmMotorsEnabled) {
+    const uint16_t digitalValue = lrintf(value);
-        return;
-    }
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
@@ -61,7 +59,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
         return;
     }
 
-    uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
+    uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
     motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
 
     // compute checksum
@@ -81,12 +79,55 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
     }
 
     if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
-        if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) {
+        if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
             /* Starting PWM generation Error */
             return;
         }
     } else {
-        if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) {
+        if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
+            /* Starting PWM generation Error */
+            return;
+        }
+    }
+}
+
+void pwmWriteProShot(uint8_t index, float value)
+{
+    const uint16_t digitalValue = lrintf(value);
+
+    motorDmaOutput_t * const motor = &dmaMotors[index];
+
+    if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
+        return;
+    }
+
+    uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
+    motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
+
+    // compute checksum
+    int csum = 0;
+    int csum_data = packet;
+    for (int i = 0; i < 3; i++) {
+        csum ^=  csum_data;   // xor data by nibbles
+        csum_data >>= 4;
+    }
+    csum &= 0xf;
+    // append checksum
+    packet = (packet << 4) | csum;
+
+    // generate pulses for Proshot
+    for (int i = 0; i < 4; i++) {
+        motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH;  // Most significant nibble first
+        packet <<= 4;	// Shift 4 bits
+    }
+
+    if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+        if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
+            /* Starting PWM generation Error */
+            return;
+        }
+    } else {
+        if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
             /* Starting PWM generation Error */
             return;
         }
@@ -122,8 +163,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
     RCC_ClockCmd(timerRCC(timer), ENABLE);
 
     motor->TimHandle.Instance = timerHardware->tim;
-    motor->TimHandle.Init.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1;;
+    motor->TimHandle.Init.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1;
-    motor->TimHandle.Init.Period = MOTOR_BITLENGTH;
+    motor->TimHandle.Init.Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH;
     motor->TimHandle.Init.RepetitionCounter = 0;
     motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
     motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;

src/main/drivers/serial_uart_hal.c

@@ -58,6 +58,8 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) {
     }
 }
 
+// XXX uartReconfigure does not handle resource management properly.
+
 void uartReconfigure(uartPort_t *uartPort)
 {
     /*RCC_PeriphCLKInitTypeDef RCC_PeriphClkInit;

src/main/drivers/serial_uart_stm32f30x.c

@@ -184,7 +184,7 @@ void serialUARTInitIO(IO_t txIO, IO_t rxIO, portMode_t mode, portOptions_t optio
             ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP
         );
 
-        IOInit(txIO, OWNER_SERIAL_TX, index);
+        IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(index));
         IOConfigGPIOAF(txIO, ioCfg, af);
 
         if (!(options & SERIAL_INVERTED))
@@ -192,12 +192,12 @@ void serialUARTInitIO(IO_t txIO, IO_t rxIO, portMode_t mode, portOptions_t optio
     } else {
         ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP);
         if ((mode & MODE_TX) && txIO) {
-            IOInit(txIO, OWNER_SERIAL_TX, index);
+            IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(index));
             IOConfigGPIOAF(txIO, ioCfg, af);
         }
 
         if ((mode & MODE_RX) && rxIO) {
-            IOInit(rxIO, OWNER_SERIAL_RX, index);
+            IOInit(rxIO, OWNER_SERIAL_RX, RESOURCE_INDEX(index));
             IOConfigGPIOAF(rxIO, ioCfg, af);
         }
     }

src/main/drivers/system_stm32f10x.c

@@ -83,7 +83,7 @@ void systemInit(void)
     // Turn on clocks for stuff we use
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
 
-    // cache RCC->CSR value to use it in isMPUSoftreset() and others
+    // cache RCC->CSR value to use it in isMPUSoftReset() and others
     cachedRccCsrValue = RCC->CSR;
     RCC_ClearFlag();
 

src/main/drivers/system_stm32f30x.c

@@ -90,7 +90,7 @@ void systemInit(void)
     // Configure NVIC preempt/priority groups
     NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
 
-    // cache RCC->CSR value to use it in isMPUSoftreset() and others
+    // cache RCC->CSR value to use it in isMPUSoftReset() and others
     cachedRccCsrValue = RCC->CSR;
     RCC_ClearFlag();
 

src/main/drivers/system_stm32f4xx.c

@@ -152,7 +152,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void)
 
 bool isMPUSoftReset(void)
 {
-    if (RCC->CSR & RCC_CSR_SFTRSTF)
+    if (cachedRccCsrValue & RCC_CSR_SFTRSTF)
         return true;
     else
         return false;
@@ -167,7 +167,7 @@ void systemInit(void)
     // Configure NVIC preempt/priority groups
     NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
 
-    // cache RCC->CSR value to use it in isMPUSoftreset() and others
+    // cache RCC->CSR value to use it in isMPUSoftReset() and others
     cachedRccCsrValue = RCC->CSR;
 
     /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */

src/main/drivers/system_stm32f7xx.c

@@ -149,7 +149,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void)
 
 bool isMPUSoftReset(void)
 {
-    if (RCC->CSR & RCC_CSR_SFTRSTF)
+    if (cachedRccCsrValue & RCC_CSR_SFTRSTF)
         return true;
     else
         return false;
@@ -164,7 +164,7 @@ void systemInit(void)
     // Configure NVIC preempt/priority groups
     HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITY_GROUPING);
 
-    // cache RCC->CSR value to use it in isMPUSoftreset() and others
+    // cache RCC->CSR value to use it in isMPUSoftReset() and others
     cachedRccCsrValue = RCC->CSR;
 
     /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */

src/main/fc/cli.c

@@ -75,6 +75,7 @@ extern uint8_t __config_end;
 #include "drivers/time.h"
 #include "drivers/timer.h"
 #include "drivers/vcd.h"
+#include "drivers/light_led.h"
 
 #include "fc/settings.h"
 #include "fc/cli.h"
@@ -126,10 +127,20 @@ extern uint8_t __config_end;
 
 
 static serialPort_t *cliPort;
-static bufWriter_t *cliWriter;
-static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128];
 
-static char cliBuffer[64];
+#ifdef STM32F1
+#define CLI_IN_BUFFER_SIZE 128
+#define CLI_OUT_BUFFER_SIZE 64
+#else
+// Space required to set array parameters
+#define CLI_IN_BUFFER_SIZE 256
+#define CLI_OUT_BUFFER_SIZE 256
+#endif
+
+static bufWriter_t *cliWriter;
+static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_IN_BUFFER_SIZE];
+
+static char cliBuffer[CLI_OUT_BUFFER_SIZE];
 static uint32_t bufferIndex = 0;
 
 static bool configIsInCopy = false;
@@ -295,33 +306,44 @@ static void cliPrintLinef(const char *format, ...)
 
 static void printValuePointer(const clivalue_t *var, const void *valuePointer, bool full)
 {
-    int value = 0;
+    if ((var->type & VALUE_MODE_MASK) == MODE_ARRAY) {
+        for (int i = 0; i < var->config.array.length; i++) {
+            uint8_t value = ((uint8_t *)valuePointer)[i];
 
-    switch (var->type & VALUE_TYPE_MASK) {
+            cliPrintf("%d", value);
-    case VAR_UINT8:
+            if (i < var->config.array.length - 1) {
-        value = *(uint8_t *)valuePointer;
+                cliPrint(",");
-        break;
+            }
-
+        }
-    case VAR_INT8:
+    } else {
-        value = *(int8_t *)valuePointer;
+        int value = 0;
-        break;
+
-
+        switch (var->type & VALUE_TYPE_MASK) {
-    case VAR_UINT16:
+        case VAR_UINT8:
-    case VAR_INT16:
+            value = *(uint8_t *)valuePointer;
-        value = *(int16_t *)valuePointer;
+            break;
-        break;
+
-    }
+        case VAR_INT8:
-
+            value = *(int8_t *)valuePointer;
-    switch(var->type & VALUE_MODE_MASK) {
+            break;
-    case MODE_DIRECT:
+
-        cliPrintf("%d", value);
+        case VAR_UINT16:
-        if (full) {
+        case VAR_INT16:
-            cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max);
+            value = *(int16_t *)valuePointer;
+            break;
+        }
+
+        switch(var->type & VALUE_MODE_MASK) {
+        case MODE_DIRECT:
+            cliPrintf("%d", value);
+            if (full) {
+                cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max);
+            }
+            break;
+        case MODE_LOOKUP:
+            cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]);
+            break;
         }
-        break;
-    case MODE_LOOKUP:
-        cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]);
-        break;
     }
 }
 
@@ -379,14 +401,15 @@ static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask)
     const char *defaultFormat = "#set %s = ";
     const int valueOffset = getValueOffset(value);
     const bool equalsDefault = valuePtrEqualsDefault(value->type, pg->copy + valueOffset, pg->address + valueOffset);
+
     if (((dumpMask & DO_DIFF) == 0) || !equalsDefault) {
         if (dumpMask & SHOW_DEFAULTS && !equalsDefault) {
             cliPrintf(defaultFormat, value->name);
-            printValuePointer(value, (uint8_t*)pg->address + valueOffset, 0);
+            printValuePointer(value, (uint8_t*)pg->address + valueOffset, false);
             cliPrintLinefeed();
         }
         cliPrintf(format, value->name);
-        printValuePointer(value, pg->copy + valueOffset, 0);
+        printValuePointer(value, pg->copy + valueOffset, false);
         cliPrintLinefeed();
     }
 }
@@ -426,26 +449,31 @@ static void cliPrintVarRange(const clivalue_t *var)
         }
         cliPrintLinefeed();
     }
+    break;
+    case (MODE_ARRAY): {
+        cliPrintLinef("Array length: %d", var->config.array.length);
+    }
+
     break;
     }
 }
 
-static void cliSetVar(const clivalue_t *var, const cliVar_t value)
+static void cliSetVar(const clivalue_t *var, const int16_t value)
 {
     void *ptr = getValuePointer(var);
 
     switch (var->type & VALUE_TYPE_MASK) {
     case VAR_UINT8:
-        *(uint8_t *)ptr = value.uint8;
+        *(uint8_t *)ptr = value;
         break;
 
     case VAR_INT8:
-        *(int8_t *)ptr = value.int8;
+        *(int8_t *)ptr = value;
         break;
 
     case VAR_UINT16:
     case VAR_INT16:
-        *(int16_t *)ptr = value.int16;
+        *(int16_t *)ptr = value;
         break;
     }
 }
@@ -2509,18 +2537,34 @@ static void cliGet(char *cmdline)
     cliPrintLine("Invalid name");
 }
 
+static char *skipSpace(char *buffer)
+{
+    while (*(buffer) == ' ') {
+        buffer++;
+    }
+
+    return buffer;
+}
+
+static uint8_t getWordLength(char *bufBegin, char *bufEnd)
+{
+    while (*(bufEnd - 1) == ' ') {
+        bufEnd--;
+    }
+
+    return bufEnd - bufBegin;
+}
+
 static void cliSet(char *cmdline)
 {
-    uint32_t len;
+    const uint32_t len = strlen(cmdline);
-    const clivalue_t *val;
+    char *eqptr;
-    char *eqptr = NULL;
-
-    len = strlen(cmdline);
 
     if (len == 0 || (len == 1 && cmdline[0] == '*')) {
         cliPrintLine("Current settings: ");
+
         for (uint32_t i = 0; i < valueTableEntryCount; i++) {
-            val = &valueTable[i];
+            const clivalue_t *val = &valueTable[i];
             cliPrintf("%s = ", valueTable[i].name);
             cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
             cliPrintLinefeed();
@@ -2528,52 +2572,81 @@ static void cliSet(char *cmdline)
     } else if ((eqptr = strstr(cmdline, "=")) != NULL) {
         // has equals
 
-        char *lastNonSpaceCharacter = eqptr;
+        uint8_t variableNameLength = getWordLength(cmdline, eqptr);
-        while (*(lastNonSpaceCharacter - 1) == ' ') {
-            lastNonSpaceCharacter--;
-        }
-        uint8_t variableNameLength = lastNonSpaceCharacter - cmdline;
 
         // skip the '=' and any ' ' characters
         eqptr++;
-        while (*(eqptr) == ' ') {
+        eqptr = skipSpace(eqptr);
-            eqptr++;
-        }
 
         for (uint32_t i = 0; i < valueTableEntryCount; i++) {
-            val = &valueTable[i];
+            const clivalue_t *val = &valueTable[i];
             // ensure exact match when setting to prevent setting variables with shorter names
             if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) {
 
-                bool changeValue = false;
+                bool valueChanged = false;
-                cliVar_t value  = { .int16 = 0 };
+                int16_t value  = 0;
                 switch (valueTable[i].type & VALUE_MODE_MASK) {
                     case MODE_DIRECT: {
-                            value.int16 = atoi(eqptr);
+                        int16_t value = atoi(eqptr);
 
-                            if (value.int16 >= valueTable[i].config.minmax.min && value.int16 <= valueTable[i].config.minmax.max) {
+                        if (value >= valueTable[i].config.minmax.min && value <= valueTable[i].config.minmax.max) {
-                                changeValue = true;
+                            cliSetVar(val, value);
-                            }
+                            valueChanged = true;
                         }
-                        break;
+                    }
+
+                    break;
                     case MODE_LOOKUP: {
-                            const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex];
+                        const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex];
-                            bool matched = false;
+                        bool matched = false;
-                            for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) {
+                        for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) {
-                                matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0;
+                            matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0;
 
-                                if (matched) {
+                            if (matched) {
-                                    value.int16 = tableValueIndex;
+                                value = tableValueIndex;
-                                    changeValue = true;
+
-                                }
+                                cliSetVar(val, value);
+                                valueChanged = true;
                             }
                         }
-                        break;
+                    }
+
+                    break;
+                    case MODE_ARRAY: {
+                        const uint8_t arrayLength = valueTable[i].config.array.length;
+                        char *valPtr = eqptr;
+                        uint8_t array[256];
+                        char curVal[4];
+                        for (int i = 0; i < arrayLength; i++) {
+                            valPtr = skipSpace(valPtr);
+                            char *valEnd = strstr(valPtr, ",");
+                            if ((valEnd != NULL) && (i < arrayLength - 1)) {
+                                uint8_t varLength = getWordLength(valPtr, valEnd);
+                                if (varLength <= 3) {
+                                    strncpy(curVal, valPtr, getWordLength(valPtr, valEnd));
+                                    curVal[varLength] = '\0';
+                                    array[i] = (uint8_t)atoi((const char *)curVal);
+                                    valPtr = valEnd + 1;
+                                } else {
+                                    break;
+                                }
+                            } else if ((valEnd == NULL) && (i == arrayLength - 1)) {
+                                array[i] = atoi(valPtr); 
+
+                                uint8_t *ptr = getValuePointer(val);
+                                memcpy(ptr, array, arrayLength);
+                                valueChanged = true;
+                            } else {
+                                break;
+                            }
+                        }
+                    }
+
+                    break;
+
                 }
 
-                if (changeValue) {
+                if (valueChanged) {
-                    cliSetVar(val, value);
-
                     cliPrintf("%s set to ", valueTable[i].name);
                     cliPrintVar(val, 0);
                 } else {
@@ -2984,11 +3057,7 @@ static void backupConfigs(void)
 {
     // make copies of configs to do differencing
     PG_FOREACH(pg) {
-        if (pgIsProfile(pg)) {
+        memcpy(pg->copy, pg->address, pg->size);
-            //memcpy(pg->copy, pg->address, pg->size * MAX_PROFILE_COUNT);
-        } else {
-            memcpy(pg->copy, pg->address, pg->size);
-        }
     }
 
     configIsInCopy = true;
@@ -2997,11 +3066,7 @@ static void backupConfigs(void)
 static void restoreConfigs(void)
 {
     PG_FOREACH(pg) {
-        if (pgIsProfile(pg)) {
+        memcpy(pg->address, pg->copy, pg->size);
-            //memcpy(pg->address, pg->copy, pg->size * MAX_PROFILE_COUNT);
-        } else {
-            memcpy(pg->address, pg->copy, pg->size);
-        }
     }
 
     configIsInCopy = false;

src/main/fc/config.c

@@ -148,7 +148,6 @@ PG_REGISTER_WITH_RESET_FN(pwmConfig_t, pwmConfig, PG_PWM_CONFIG, 0);
 #ifdef USE_PPM
 PG_REGISTER_WITH_RESET_FN(ppmConfig_t, ppmConfig, PG_PPM_CONFIG, 0);
 #endif
-PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0);
 
 #ifdef USE_FLASHFS
 PG_REGISTER_WITH_RESET_TEMPLATE(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0);
@@ -255,35 +254,6 @@ void pgResetFn_pwmConfig(pwmConfig_t *pwmConfig)
 #define SECOND_PORT_INDEX 1
 #endif
 
-void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig)
-{
-    for (int i = 0; i < LED_NUMBER; i++) {
-        statusLedConfig->ledTags[i] = IO_TAG_NONE;
-    }
-
-#ifdef LED0
-    statusLedConfig->ledTags[0] = IO_TAG(LED0);
-#endif
-#ifdef LED1
-    statusLedConfig->ledTags[1] = IO_TAG(LED1);
-#endif
-#ifdef LED2
-    statusLedConfig->ledTags[2] = IO_TAG(LED2);
-#endif
-
-    statusLedConfig->polarity = 0
-#ifdef LED0_INVERTED
-    | BIT(0)
-#endif
-#ifdef LED1_INVERTED
-    | BIT(1)
-#endif
-#ifdef LED2_INVERTED
-    | BIT(2)
-#endif
-    ;
-}
-
 #ifndef USE_OSD_SLAVE
 uint8_t getCurrentPidProfileIndex(void)
 {
@@ -312,14 +282,12 @@ uint16_t getCurrentMinthrottle(void)
 
 void resetConfigs(void)
 {
-    pgResetAll(MAX_PROFILE_COUNT);
+    pgResetAll();
 
 #if defined(TARGET_CONFIG)
     targetConfiguration();
 #endif
 
-    pgActivateProfile(0);
-
 #ifndef USE_OSD_SLAVE
     setPidProfile(0);
     setControlRateProfile(0);
@@ -337,7 +305,7 @@ void activateConfig(void)
 
     resetAdjustmentStates();
 
-    useRcControlsConfig(modeActivationConditions(0), currentPidProfile);
+    useRcControlsConfig(currentPidProfile);
     useAdjustmentConfig(currentPidProfile);
 
 #ifdef GPS

src/main/fc/config.h

@@ -27,6 +27,7 @@
 #include "drivers/rx_pwm.h"
 #include "drivers/sdcard.h"
 #include "drivers/serial.h"
+#include "drivers/bus_i2c.h"
 #include "drivers/sound_beeper.h"
 #include "drivers/vcd.h"
 

src/main/fc/fc_core.c

@@ -107,7 +107,7 @@ int16_t magHold;
 int16_t headFreeModeHold;
 
 uint8_t motorControlEnable = false;
-
+static bool reverseMotors;
 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;
@@ -206,6 +206,21 @@ void mwArm(void)
             return;
         }
         if (!ARMING_FLAG(PREVENT_ARMING)) {
+            #ifdef USE_DSHOT
+            //TODO: Use BOXDSHOTREVERSE here
+            if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
+                reverseMotors = false;
+                for (unsigned index = 0; index < getMotorCount(); index++) {
+                    pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
+                }
+            }
+            if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
+                reverseMotors = true;
+                for (unsigned index = 0; index < getMotorCount(); index++) {
+                    pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
+                }
+            }
+            #endif
             ENABLE_ARMING_FLAG(ARMED);
             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
@@ -645,3 +660,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         runTaskMainSubprocesses = true;
     }
 }
+
+bool isMotorsReversed()
+{
+    return reverseMotors;
+}

src/main/fc/fc_core.h

@@ -48,3 +48,4 @@ void updateLEDs(void);
 void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);
+bool isMotorsReversed(void);

src/main/fc/fc_init.c

@@ -54,6 +54,7 @@
 #include "drivers/rx_pwm.h"
 #include "drivers/pwm_output.h"
 #include "drivers/adc.h"
+#include "drivers/bus.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/bus_spi.h"
 #include "drivers/buttons.h"
@@ -135,10 +136,6 @@
 void targetPreInit(void);
 #endif
 
-#ifdef TARGET_BUS_INIT
-void targetBusInit(void);
-#endif
-
 extern uint8_t motorControlEnable;
 
 #ifdef SOFTSERIAL_LOOPBACK
@@ -226,7 +223,9 @@ void init(void)
     targetPreInit();
 #endif
 
+#if !defined(UNIT_TEST) && !defined(USE_FAKE_LED)
     ledInit(statusLedConfig());
+#endif
     LED2_ON;
 
 #ifdef USE_EXTI
@@ -367,6 +366,11 @@ void init(void)
 #endif /* USE_SPI */
 
 #ifdef USE_I2C
+    i2cHardwareConfigure();
+
+    // Note: Unlike UARTs which are configured when client is present,
+    // I2C buses are initialized unconditionally if they are configured.
+
 #ifdef USE_I2C_DEVICE_1
     i2cInit(I2CDEV_1);
 #endif

src/main/fc/fc_msp.c

@@ -29,6 +29,7 @@
 #include "build/version.h"
 
 #include "common/axis.h"
+#include "common/bitarray.h"
 #include "common/color.h"
 #include "common/maths.h"
 #include "common/streambuf.h"
@@ -59,6 +60,8 @@
 #include "fc/fc_msp.h"
 #include "fc/fc_rc.h"
 #include "fc/rc_adjustments.h"
+#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/altitude.h"
@@ -110,12 +113,13 @@
 #endif
 
 #define STATIC_ASSERT(condition, name) \
-    typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
+    typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] __attribute__((unused))
 
 static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 
 #ifndef USE_OSD_SLAVE
+// permanent IDs must uniquely identify BOX meaning, DO NOT REUSE THEM!
 static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXARM, "ARM", 0 },
     { BOXANGLE, "ANGLE", 1 },
@@ -152,9 +156,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
-static uint32_t activeBoxIds;
+
-// check that all boxId_e values fit
+static boxBitmask_t activeBoxIds;
-STATIC_ASSERT(sizeof(activeBoxIds) * 8 >= CHECKBOX_ITEM_COUNT, CHECKBOX_ITEMS_wont_fit_in_activeBoxIds);
 
 static const char pidnames[] =
     "ROLL;"
@@ -276,23 +279,41 @@ const box_t *findBoxByPermanentId(uint8_t permanentId)
     return NULL;
 }
 
-static void serializeBoxNamesReply(sbuf_t *dst)
+static bool activeBoxIdGet(boxId_e boxId)
 {
-    for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) {
+    if(boxId > sizeof(activeBoxIds) * 8)
-        if(activeBoxIds & (1 << id)) {
+        return false;
-            const box_t *box = findBoxByBoxId(id);
+    return bitArrayGet(&activeBoxIds, boxId);
-            sbufWriteString(dst, box->boxName);
-            sbufWriteU8(dst, ';');
-        }
-    }
 }
 
-static void serializeBoxIdsReply(sbuf_t *dst)
+
+// callcack for box serialization
+typedef void serializeBoxFn(sbuf_t *dst, const box_t *box);
+
+static void serializeBoxNameFn(sbuf_t *dst, const box_t *box)
 {
+    sbufWriteString(dst, box->boxName);
+    sbufWriteU8(dst, ';');
+}
+
+static void serializeBoxPermanentIdFn(sbuf_t *dst, const box_t *box)
+{
+    sbufWriteU8(dst, box->permanentId);
+}
+
+// serialize 'page' of boxNames.
+// Each page contains at most 32 boxes
+static void serializeBoxReply(sbuf_t *dst, int page, serializeBoxFn *serializeBox)
+{
+    unsigned boxIdx = 0;
+    unsigned pageStart = page * 32;
+    unsigned pageEnd = pageStart + 32;
     for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) {
-        if(activeBoxIds & (1 << id)) {
+        if (activeBoxIdGet(id)) {
-            const box_t *box = findBoxByBoxId(id);
+            if (boxIdx >= pageStart && boxIdx < pageEnd) {
-            sbufWriteU8(dst, box->permanentId);
+                (*serializeBox)(dst, findBoxByBoxId(id));
+            }
+            boxIdx++;                 // count active boxes
         }
     }
 }
@@ -300,9 +321,11 @@ static void serializeBoxIdsReply(sbuf_t *dst)
 void initActiveBoxIds(void)
 {
     // calculate used boxes based on features and set corresponding activeBoxIds bits
-    uint32_t ena = 0;  // temporary variable to collect result
+    boxBitmask_t ena;  // temporary variable to collect result
+    memset(&ena, 0, sizeof(ena));
+
     // macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only
-#define BME(boxId) do { ena |= (1 << (boxId)); } while(0)
+#define BME(boxId) do { bitArraySet(&ena, boxId); } while(0)
     BME(BOXARM);
 
     if (!feature(FEATURE_AIRMODE)) {
@@ -370,9 +393,8 @@ void initActiveBoxIds(void)
 
     BME(BOXFPVANGLEMIX);
 
-    if (feature(FEATURE_3D)) {
+    //TODO: Split this into BOX3DDISABLESWITCH and BOXDSHOTREVERSE
-        BME(BOX3DDISABLESWITCH);
+    BME(BOX3DDISABLESWITCH);
-    }
 
     if (feature(FEATURE_SERVO_TILT)) {
         BME(BOXCAMSTAB);
@@ -400,63 +422,69 @@ void initActiveBoxIds(void)
 
 #undef BME
     // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
-    for(boxId_e boxId = 0;  boxId < CHECKBOX_ITEM_COUNT; boxId++)
+    for (boxId_e boxId = 0;  boxId < CHECKBOX_ITEM_COUNT; boxId++)
-        if((ena & (1 << boxId))
+        if (bitArrayGet(&ena, boxId)
-           && findBoxByBoxId(boxId) == NULL)
+            && findBoxByBoxId(boxId) == NULL)
-            ena &= ~ (1 << boxId);             // this should not happen, but handle it gracefully
+            bitArrayClr(&ena, boxId);                 // this should not happen, but handle it gracefully
 
-    activeBoxIds = ena;                        // set global variable
+    activeBoxIds = ena;                               // set global variable
 }
 
-static uint32_t packFlightModeFlags(void)
+// pack used flightModeFlags into supplied array
+// returns number of bits used
+static int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
 {
     // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
+    memset(mspFlightModeFlags, 0, sizeof(boxBitmask_t));
 
-    uint32_t boxEnabledMask = 0;      // enabled BOXes, bits indexed by boxId_e
+    // enabled BOXes, bits indexed by boxId_e
+    boxBitmask_t boxEnabledMask;
+    memset(&boxEnabledMask, 0, sizeof(boxEnabledMask));
 
     // enable BOXes dependent on FLIGHT_MODE, use mapping table (from runtime_config.h)
     // flightMode_boxId_map[HORIZON_MODE] == BOXHORIZON
     static const int8_t flightMode_boxId_map[] = FLIGHT_MODE_BOXID_MAP_INITIALIZER;
     flightModeFlags_e flightModeCopyMask = ~0;  // only modes with bit set will be copied
-    for(unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) {
+    for (unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) {
-        if(flightMode_boxId_map[i] != -1        // boxId_e does exist for this FLIGHT_MODE
+        if (flightMode_boxId_map[i] != -1        // boxId_e does exist for this FLIGHT_MODE
            && (flightModeCopyMask & (1 << i))   // this flightmode is copy is enabled
            && FLIGHT_MODE(1 << i)) {            // this flightmode is active
-            boxEnabledMask |= (1 << flightMode_boxId_map[i]);
+            bitArraySet(&boxEnabledMask, flightMode_boxId_map[i]);
         }
     }
 
     // enable BOXes dependent on rcMode bits, indexes are the same.
     // only subset of BOXes depend on rcMode, use mask to select them
-#define BM(x) (1 << (x))
+#define BM(x) (1ULL << (x))
-    const uint32_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON)
+    // limited to 64 BOXes now to keep code simple
+    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);
-    for(unsigned i = 0; i < sizeof(rcModeCopyMask) * 8; i++) {
+    STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes);
-        if((rcModeCopyMask & BM(i))    // mode copy is enabled
+    for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
+        if ((rcModeCopyMask & BM(i))    // mode copy is enabled
            && IS_RC_MODE_ACTIVE(i)) {    // mode is active
-            boxEnabledMask |= (1 << i);
+            bitArraySet(&boxEnabledMask, i);
         }
     }
 #undef BM
     // copy ARM state
-    if(ARMING_FLAG(ARMED))
+    if (ARMING_FLAG(ARMED))
-        boxEnabledMask |= (1 << BOXARM);
+        bitArraySet(&boxEnabledMask, BOXARM);
 
     // map boxId_e enabled bits to MSP status indexes
     // only active boxIds are sent in status over MSP, other bits are not counted
-    uint32_t mspBoxEnabledMask = 0;
     unsigned mspBoxIdx = 0;           // index of active boxId (matches sent permanentId and boxNames)
     for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) {
-        if(activeBoxIds & (1 << boxId)) {
+        if (activeBoxIdGet(boxId)) {
-            if (boxEnabledMask & (1 << boxId))
+            if (bitArrayGet(&boxEnabledMask,  boxId))
-                mspBoxEnabledMask |= 1 << mspBoxIdx;      // box is enabled
+                bitArraySet(mspFlightModeFlags,  mspBoxIdx);      // box is enabled
-            mspBoxIdx++;                                  // box is active, count it
+            mspBoxIdx++;                                          // box is active, count it
         }
     }
-
+    // return count of used bits
-    return mspBoxEnabledMask;
+    return mspBoxIdx;
 }
 
 static void serializeSDCardSummaryReply(sbuf_t *dst)
@@ -831,20 +859,6 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
 
     switch (cmdMSP) {
     case MSP_STATUS_EX:
-        sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
-#ifdef USE_I2C
-        sbufWriteU16(dst, i2cGetErrorCounter());
-#else
-        sbufWriteU16(dst, 0);
-#endif
-        sbufWriteU16(dst, 0); // sensors
-        sbufWriteU32(dst, 0); // flight modes
-        sbufWriteU8(dst, 0); // profile
-        sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
-        sbufWriteU8(dst, 1); // max profiles
-        sbufWriteU8(dst, 0); // control rate profile
-        break;
-
     case MSP_STATUS:
         sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
 #ifdef USE_I2C
@@ -856,7 +870,12 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
         sbufWriteU32(dst, 0); // flight modes
         sbufWriteU8(dst, 0); // profile
         sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
-        sbufWriteU16(dst, 0); // gyro cycle time
+        if (cmdMSP == MSP_STATUS_EX) {
+            sbufWriteU8(dst, 1); // max profiles
+            sbufWriteU8(dst, 0); // control rate profile
+        } else {
+            sbufWriteU16(dst, 0); // gyro cycle time
+        }
         break;
 
     default:
@@ -873,32 +892,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
     switch (cmdMSP) {
     case MSP_STATUS_EX:
-        sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
-#ifdef USE_I2C
-        sbufWriteU16(dst, i2cGetErrorCounter());
-#else
-        sbufWriteU16(dst, 0);
-#endif
-        sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
-        sbufWriteU32(dst, packFlightModeFlags());
-        sbufWriteU8(dst, getCurrentPidProfileIndex());
-        sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
-        sbufWriteU8(dst, MAX_PROFILE_COUNT);
-        sbufWriteU8(dst, getCurrentControlRateProfileIndex());
-        break;
-
     case MSP_STATUS:
-        sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
+        {
+            boxBitmask_t flightModeFlags;
+            const int flagBits = packFlightModeFlags(&flightModeFlags);
+
+            sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
 #ifdef USE_I2C
-        sbufWriteU16(dst, i2cGetErrorCounter());
+            sbufWriteU16(dst, i2cGetErrorCounter());
 #else
-        sbufWriteU16(dst, 0);
+            sbufWriteU16(dst, 0);
 #endif
-        sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
+            sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
-        sbufWriteU32(dst, packFlightModeFlags());
+            sbufWriteData(dst, &flightModeFlags, 4);        // unconditional part of flags, first 32 bits
-        sbufWriteU8(dst, getCurrentPidProfileIndex());
+            sbufWriteU8(dst, getCurrentPidProfileIndex());
-        sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
+            sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
-        sbufWriteU16(dst, 0); // gyro cycle time
+            if (cmdMSP == MSP_STATUS_EX) {
+                sbufWriteU8(dst, MAX_PROFILE_COUNT);
+                sbufWriteU8(dst, getCurrentControlRateProfileIndex());
+            } else {  // MSP_STATUS
+                sbufWriteU16(dst, 0); // gyro cycle time
+            }
+
+            // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
+            // header is emited even when all bits fit into 32 bits to allow future extension
+            int byteCount = (flagBits - 32 + 7) / 8;        // 32 already stored, round up
+            byteCount = constrain(byteCount, 0, 15);        // limit to 16 bytes (128 bits)
+            sbufWriteU8(dst, byteCount);
+            sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount);
+        }
         break;
 
     case MSP_RAW_IMU:
@@ -1060,14 +1082,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         }
         break;
 
-    case MSP_BOXNAMES:
-        serializeBoxNamesReply(dst);
-        break;
-
-    case MSP_BOXIDS:
-        serializeBoxIdsReply(dst);
-        break;
-
     case MSP_MOTOR_CONFIG:
         sbufWriteU16(dst, motorConfig()->minthrottle);
         sbufWriteU16(dst, motorConfig()->maxthrottle);
@@ -1334,13 +1348,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
                 uint8_t band=0, channel=0;
                 vtxCommonGetBandAndChannel(&band,&channel);
-                
+
                 uint8_t powerIdx=0; // debug
                 vtxCommonGetPowerIndex(&powerIdx);
-                
+
                 uint8_t pitmode=0;
                 vtxCommonGetPitMode(&pitmode);
-                
+
                 sbufWriteU8(dst, deviceType);
                 sbufWriteU8(dst, band);
                 sbufWriteU8(dst, channel);
@@ -1360,6 +1374,29 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
     }
     return true;
 }
+
+static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
+{
+    UNUSED(mspPostProcessFn);
+
+    switch (cmdMSP) {
+    case MSP_BOXNAMES:
+        {
+            const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
+            serializeBoxReply(dst, page, &serializeBoxNameFn);
+        }
+        break;
+    case MSP_BOXIDS:
+        {
+            const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
+            serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
+        }
+        break;
+    default:
+        return MSP_RESULT_CMD_UNKNOWN;
+    }
+    return MSP_RESULT_ACK;
+}
 #endif
 
 #ifdef GPS
@@ -1497,7 +1534,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
                 mac->range.startStep = sbufReadU8(src);
                 mac->range.endStep = sbufReadU8(src);
 
-                useRcControlsConfig(modeActivationConditions(0), currentPidProfile);
+                useRcControlsConfig(currentPidProfile);
             } else {
                 return MSP_RESULT_ERROR;
             }
@@ -1659,7 +1696,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         }*/
         validateAndFixGyroConfig();
 
-        if (sbufBytesRemaining(src)) {        
+        if (sbufBytesRemaining(src)) {
             motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
         }
         break;
@@ -1762,13 +1799,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 
                 if (sbufBytesRemaining(src) < 2)
                     break;
-            
+
                 uint8_t power = sbufReadU8(src);
                 uint8_t current_power = 0;
                 vtxCommonGetPowerIndex(&current_power);
                 if (current_power != power)
                     vtxCommonSetPowerByIndex(power);
-            
+
                 uint8_t pitmode = sbufReadU8(src);
                 uint8_t current_pitmode = 0;
                 vtxCommonGetPitMode(&current_pitmode);
@@ -2178,6 +2215,8 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
 #ifndef USE_OSD_SLAVE
     } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
         ret = MSP_RESULT_ACK;
+    } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
+        /* ret */;
 #endif
 #ifdef USE_OSD_SLAVE
     } else if (mspOsdSlaveProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {

src/main/fc/fc_msp.h

@@ -18,12 +18,12 @@
 #pragma once
 
 #include "msp/msp.h"
-#include "rc_controls.h"
+#include "fc/rc_modes.h"
 
 typedef struct box_e {
     const uint8_t boxId;            // see boxId_e
     const char *boxName;            // GUI-readable box name
-    const uint8_t permanentId;      //
+    const uint8_t permanentId;      // permanent ID used to identify BOX. This ID is unique for one function, DO NOT REUSE IT
 } box_t;
 
 const box_t *findBoxByBoxId(boxId_e boxId);

src/main/fc/fc_rc.c

@@ -37,6 +37,7 @@
 #include "fc/fc_core.h"
 #include "fc/fc_rc.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "rx/rx.h"
@@ -46,6 +47,7 @@
 #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;
@@ -166,7 +168,7 @@ static void scaleRcCommandToFpvCamAngle(void) {
     const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
 
     if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
-        pidSetItermAccelerator(0.001f * currentPidProfile->itermAcceleratorGain);
+        pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
     else
         pidSetItermAccelerator(1.0f);
 }

src/main/fc/rc_adjustments.c

@@ -88,7 +88,7 @@ static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustm
 }
 #endif
 
-static uint8_t adjustmentStateMask = 0;
+STATIC_UNIT_TESTED uint8_t adjustmentStateMask = 0;
 
 #define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
 #define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
@@ -199,9 +199,9 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
 
 #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
 
-static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
+STATIC_UNIT_TESTED adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
 
-static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig)
+STATIC_UNIT_TESTED void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig)
 {
     adjustmentState_t *adjustmentState = &adjustmentStates[index];
 
@@ -386,10 +386,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig)
             continue;
         }
 
-        const int32_t signedDiff = now - adjustmentState->timeoutAt;
+        if (cmp32(now, adjustmentState->timeoutAt) >= 0) {
-        const bool canResetReadyStates = signedDiff >= 0L;
-
-        if (canResetReadyStates) {
             adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
             MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
         }

src/main/fc/rc_adjustments.h

@@ -19,7 +19,7 @@
 
 #include <stdbool.h>
 #include "config/parameter_group.h"
-#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 
 typedef enum {
     ADJUSTMENT_NONE = 0,

src/main/fc/rc_controls.c

@@ -65,8 +65,6 @@ static bool isUsingSticksToArm = true;
 
 float rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
-uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
-
 PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
 
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
@@ -85,15 +83,13 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
     .auto_disarm_delay = 5
 );
 
-PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
-
+PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig,
-bool isAirmodeActive(void) {
+    .deadband3d_low = 1406,
-    return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
+    .deadband3d_high = 1514,
-}
+    .neutral3d = 1460,
-
+    .deadband3d_throttle = 50
-bool isAntiGravityModeActive(void) {
+);
-    return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
-}
 
 bool isUsingSticksForArming(void)
 {
@@ -308,51 +304,13 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 
 }
 
-bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
-{
-    uint8_t index;
-
-    for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
-
-        if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
-            return true;
-        }
-    }
-
-    return false;
-}
-
-bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
-    if (!IS_RANGE_USABLE(range)) {
-        return false;
-    }
-
-    const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
-    return (channelValue >= 900 + (range->startStep * 25) &&
-            channelValue < 900 + (range->endStep * 25));
-}
-
-void updateActivatedModes(void)
-{
-    rcModeActivationMask = 0;
-
-    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index);
-
-        if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
-            ACTIVATE_RC_MODE(modeActivationCondition->modeId);
-        }
-    }
-}
-
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
     return MIN(ABS(rcData[axis] - midrc), 500);
 }
 
-void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse)
+void useRcControlsConfig(pidProfile_t *pidProfileToUse)
 {
     pidProfile = pidProfileToUse;
 
-    isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
+    isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
 }

src/main/fc/rc_controls.h

@@ -21,47 +21,6 @@
 
 #include "config/parameter_group.h"
 
-typedef enum {
-    BOXARM = 0,
-    BOXANGLE,
-    BOXHORIZON,
-    BOXBARO,
-    BOXANTIGRAVITY,
-    BOXMAG,
-    BOXHEADFREE,
-    BOXHEADADJ,
-    BOXCAMSTAB,
-    BOXCAMTRIG,
-    BOXGPSHOME,
-    BOXGPSHOLD,
-    BOXPASSTHRU,
-    BOXBEEPERON,
-    BOXLEDMAX,
-    BOXLEDLOW,
-    BOXLLIGHTS,
-    BOXCALIB,
-    BOXGOV,
-    BOXOSD,
-    BOXTELEMETRY,
-    BOXGTUNE,
-    BOXSONAR,
-    BOXSERVO1,
-    BOXSERVO2,
-    BOXSERVO3,
-    BOXBLACKBOX,
-    BOXFAILSAFE,
-    BOXAIRMODE,
-    BOX3DDISABLESWITCH,
-    BOXFPVANGLEMIX,
-    BOXBLACKBOXERASE,
-    CHECKBOX_ITEM_COUNT
-} boxId_e;
-
-extern uint32_t rcModeActivationMask;
-
-#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask)
-#define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId))
-
 typedef enum rc_alias {
     ROLL = 0,
     PITCH,
@@ -109,17 +68,6 @@ typedef enum {
 #define THR_CE (3 << (2 * THROTTLE))
 #define THR_HI (2 << (2 * THROTTLE))
 
-#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
-
-#define CHANNEL_RANGE_MIN 900
-#define CHANNEL_RANGE_MAX 2100
-
-#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step)
-#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25)
-
-#define MIN_MODE_RANGE_STEP 0
-#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
-
 // Roll/pitch rates are a proportion used for mixing, so it tops out at 1.0:
 #define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX  100
 
@@ -128,29 +76,6 @@ typedef enum {
 
 #define CONTROL_RATE_CONFIG_TPA_MAX              100
 
-// steps are 25 apart
-// a value of 0 corresponds to a channel value of 900 or less
-// a value of 48 corresponds to a channel value of 2100 or more
-// 48 steps between 900 and 1200
-typedef struct channelRange_s {
-    uint8_t startStep;
-    uint8_t endStep;
-} channelRange_t;
-
-typedef struct modeActivationCondition_s {
-    boxId_e modeId;
-    uint8_t auxChannelIndex;
-    channelRange_t range;
-} modeActivationCondition_t;
-
-PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
-
-typedef struct modeActivationProfile_s {
-    modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
-} modeActivationProfile_t;
-
-#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
-
 extern float rcCommand[4];
 
 typedef struct rcControlsConfig_s {
@@ -186,15 +111,9 @@ bool areSticksInApModePosition(uint16_t ap_mode);
 throttleStatus_e calculateThrottleStatus(void);
 void processRcStickPositions(throttleStatus_e throttleStatus);
 
-bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
-void updateActivatedModes(void);
-
-bool isAirmodeActive(void);
-bool isAntiGravityModeActive(void);
-
 bool isUsingSticksForArming(void);
 
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
-bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
 struct pidProfile_s;
-void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse);
+struct modeActivationCondition_s;
+void useRcControlsConfig(struct pidProfile_s *pidProfileToUse);

src/main/fc/rc_modes.c

@@ -0,0 +1,101 @@
+/*
+ * 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 <string.h>
+
+#include "rc_modes.h"
+
+#include "common/bitarray.h"
+#include "common/maths.h"
+
+#include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/config.h"
+#include "fc/rc_controls.h"
+
+#include "rx/rx.h"
+
+boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
+
+PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions,
+                  PG_MODE_ACTIVATION_PROFILE, 0);
+
+
+bool IS_RC_MODE_ACTIVE(boxId_e boxId)
+{
+    return bitArrayGet(&rcModeActivationMask, boxId);
+}
+
+void rcModeUpdate(boxBitmask_t *newState)
+{
+    rcModeActivationMask = *newState;
+}
+
+bool isAirmodeActive(void) {
+    return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
+}
+
+bool isAntiGravityModeActive(void) {
+    return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
+}
+
+bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
+    if (!IS_RANGE_USABLE(range)) {
+        return false;
+    }
+
+    const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
+    return (channelValue >= 900 + (range->startStep * 25) &&
+            channelValue < 900 + (range->endStep * 25));
+}
+
+
+void updateActivatedModes(void)
+{
+    boxBitmask_t newMask;
+    memset(&newMask, 0, sizeof(newMask));
+
+    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index);
+
+        if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
+            boxId_e mode = modeActivationCondition->modeId;
+            if (mode < CHECKBOX_ITEM_COUNT)
+                bitArraySet(&newMask, mode);
+        }
+    }
+    rcModeUpdate(&newMask);
+}
+
+bool isModeActivationConditionPresent(boxId_e modeId)
+{
+    uint8_t index;
+
+    for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index);
+
+        if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
+            return true;
+        }
+    }
+
+    return false;
+}

src/main/fc/rc_modes.h

@@ -0,0 +1,105 @@
+/*
+ * 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 <stdbool.h>
+
+#include "config/parameter_group.h"
+
+typedef enum {
+    BOXARM = 0,
+    BOXANGLE,
+    BOXHORIZON,
+    BOXBARO,
+    BOXANTIGRAVITY,
+    BOXMAG,
+    BOXHEADFREE,
+    BOXHEADADJ,
+    BOXCAMSTAB,
+    BOXCAMTRIG,
+    BOXGPSHOME,
+    BOXGPSHOLD,
+    BOXPASSTHRU,
+    BOXBEEPERON,
+    BOXLEDMAX,
+    BOXLEDLOW,
+    BOXLLIGHTS,
+    BOXCALIB,
+    BOXGOV,
+    BOXOSD,
+    BOXTELEMETRY,
+    BOXGTUNE,
+    BOXSONAR,
+    BOXSERVO1,
+    BOXSERVO2,
+    BOXSERVO3,
+    BOXBLACKBOX,
+    BOXFAILSAFE,
+    BOXAIRMODE,
+    BOX3DDISABLESWITCH,
+    BOXFPVANGLEMIX,
+    BOXBLACKBOXERASE,
+    CHECKBOX_ITEM_COUNT
+} boxId_e;
+
+// type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior
+typedef struct { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } boxBitmask_t;
+
+#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
+
+#define CHANNEL_RANGE_MIN 900
+#define CHANNEL_RANGE_MAX 2100
+
+#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step)
+#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25)
+
+#define MIN_MODE_RANGE_STEP 0
+#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
+
+// steps are 25 apart
+// a value of 0 corresponds to a channel value of 900 or less
+// a value of 48 corresponds to a channel value of 2100 or more
+// 48 steps between 900 and 1200
+typedef struct channelRange_s {
+    uint8_t startStep;
+    uint8_t endStep;
+} channelRange_t;
+
+typedef struct modeActivationCondition_s {
+    boxId_e modeId;
+    uint8_t auxChannelIndex;
+    channelRange_t range;
+} modeActivationCondition_t;
+
+PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
+
+typedef struct modeActivationProfile_s {
+    modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
+} modeActivationProfile_t;
+
+#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
+
+bool IS_RC_MODE_ACTIVE(boxId_e boxId);
+void rcModeUpdate(boxBitmask_t *newState);
+
+bool isAirmodeActive(void);
+bool isAntiGravityModeActive(void);
+
+bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
+void updateActivatedModes(void);
+bool isModeActivationConditionPresent(boxId_e modeId);

src/main/fc/settings.c

@@ -36,10 +36,13 @@
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
+#include "drivers/light_led.h"
+
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
 #include "fc/rc_adjustments.h"
+#include "fc/rc_controls.h"
 #include "fc/settings.h"
 
 #include "flight/altitude.h"
@@ -225,7 +228,7 @@ static const char * const lookupTableSuperExpoYaw[] = {
 static const char * const lookupTablePwmProtocol[] = {
     "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED",
 #ifdef USE_DSHOT
-    "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200"
+    "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "PROSHOT1000"
 #endif
 };
 
@@ -593,7 +596,7 @@ const clivalue_t valueTable[] = {
 // PG_TELEMETRY_CONFIG
 #ifdef TELEMETRY
     { "tlm_switch",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_switch) },
-    { "tlm_inverted",              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },
+    { "tlm_inverted",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },
     { "tlm_halfduplex",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, halfDuplex) },
     { "frsky_default_lat",          VAR_INT16  | MASTER_VALUE, .config.minmax = { -9000, 9000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) },
     { "frsky_default_long",         VAR_INT16  | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLongitude) },
@@ -603,7 +606,7 @@ const clivalue_t valueTable[] = {
     { "hott_alarm_int",             VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 120 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, hottAlarmSoundInterval) },
     { "pid_in_tlm",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pidValuesAsTelemetry) },
 #if defined(TELEMETRY_IBUS)
-    { "report_cell_voltage",   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) },
+    { "report_cell_voltage",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) },
 #endif
 #endif
 
@@ -652,7 +655,7 @@ const clivalue_t valueTable[] = {
     { "osd_debug_pos",              VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DEBUG]) },
     { "osd_power_pos",              VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_POWER]) },
     { "osd_pidrate_profile_pos",    VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PIDRATE_PROFILE]) },
-    { "osd_battery_warning_pos",    VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAIN_BATT_WARNING]) },
+    { "osd_warnings_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_WARNINGS]) },
     { "osd_avg_cell_voltage_pos",   VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_AVG_CELL_VOLTAGE]) },
     { "osd_pit_ang_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PITCH_ANGLE]) },
     { "osd_rol_ang_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_ANGLE]) },
@@ -661,6 +664,8 @@ const clivalue_t valueTable[] = {
     { "osd_disarmed_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DISARMED]) },
     { "osd_nheading_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_HEADING]) },
     { "osd_nvario_pos",             VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_VARIO]) },
+    { "osd_esc_tmp_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_TMP]) },
+    { "osd_esc_rpm_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_RPM]) },
 
     { "osd_stat_max_spd",           VAR_UINT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_MAX_SPEED])},
     { "osd_stat_max_dist",          VAR_UINT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_MAX_DISTANCE])},
@@ -713,6 +718,7 @@ const clivalue_t valueTable[] = {
 #ifdef USE_ESC_SENSOR
     { "esc_sensor_halfduplex",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) },
 #endif
+    { "led_inversion",                  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) },
 };
 
 const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

src/main/fc/settings.h

@@ -69,34 +69,31 @@ typedef struct lookupTableEntry_s {
 
 
 #define VALUE_TYPE_OFFSET 0
-#define VALUE_SECTION_OFFSET 4
+#define VALUE_SECTION_OFFSET 2
-#define VALUE_MODE_OFFSET 6
+#define VALUE_MODE_OFFSET 4
 
 typedef enum {
-    // value type, bits 0-3
+    // value type, bits 0-1
     VAR_UINT8 = (0 << VALUE_TYPE_OFFSET),
     VAR_INT8 = (1 << VALUE_TYPE_OFFSET),
     VAR_UINT16 = (2 << VALUE_TYPE_OFFSET),
     VAR_INT16 = (3 << VALUE_TYPE_OFFSET),
 
-    // value section, bits 4-5
+    // value section, bits 2-3
     MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
     PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET),
-    PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20
+    PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET),
-    // value mode
+
-    MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40
+    // value mode, bits 4-5
-    MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80
+    MODE_DIRECT = (0 << VALUE_MODE_OFFSET),
+    MODE_LOOKUP = (1 << VALUE_MODE_OFFSET),
+    MODE_ARRAY = (2 << VALUE_MODE_OFFSET)
 } cliValueFlag_e;
 
-#define VALUE_TYPE_MASK (0x0F)
-#define VALUE_SECTION_MASK (0x30)
-#define VALUE_MODE_MASK (0xC0)
 
-typedef union {
+#define VALUE_TYPE_MASK (0x03)
-    int8_t int8;
+#define VALUE_SECTION_MASK (0x0c)
-    uint8_t uint8;
+#define VALUE_MODE_MASK (0x30)
-    int16_t int16;
-} cliVar_t;
 
 typedef struct cliMinMaxConfig_s {
     const int16_t min;
@@ -107,9 +104,14 @@ typedef struct cliLookupTableConfig_s {
     const lookupTableIndex_e tableIndex;
 } cliLookupTableConfig_t;
 
+typedef struct cliArrayLengthConfig_s {
+    const uint8_t length;
+} cliArrayLengthConfig_t;
+
 typedef union {
     cliLookupTableConfig_t lookup;
     cliMinMaxConfig_t minmax;
+    cliArrayLengthConfig_t array;
 } cliValueConfig_t;
 
 typedef struct {

src/main/flight/altitude.c

@@ -33,6 +33,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/altitude.h"

src/main/flight/failsafe.c

@@ -31,6 +31,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"

src/main/flight/mixer.c

@@ -40,7 +40,9 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
+#include "fc/fc_core.h"
 
 #include "flight/failsafe.h"
 #include "flight/imu.h"
@@ -51,16 +53,6 @@
 
 #include "sensors/battery.h"
 
-
-PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
-
-PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig,
-    .deadband3d_low = 1406,
-    .deadband3d_high = 1514,
-    .neutral3d = 1460,
-    .deadband3d_throttle = 50
-);
-
 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
 
 #ifndef TARGET_DEFAULT_MIXER
@@ -119,12 +111,11 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR
 
 #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight
 
-
 static uint8_t motorCount;
 static float motorMixRange;
 
-int16_t motor[MAX_SUPPORTED_MOTORS];
+float motor[MAX_SUPPORTED_MOTORS];
-int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
+float motor_disarmed[MAX_SUPPORTED_MOTORS];
 
 mixerMode_e currentMixerMode;
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@@ -321,8 +312,8 @@ const mixer_t mixers[] = {
 };
 #endif // !USE_QUAD_MIXER_ONLY
 
-static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
+static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
-uint16_t motorOutputHigh, motorOutputLow;
+float motorOutputHigh, motorOutputLow;
 static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
 
 uint8_t getMotorCount()
@@ -361,17 +352,18 @@ bool isMotorProtocolDshot(void) {
 #endif
 }
 
-// Add here scaled ESC outputs for digital protol
+// 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) {
 #ifdef USE_DSHOT
     if (isMotorProtocolDshot()) {
         disarmMotorOutput = DSHOT_DISARM_COMMAND;
         if (feature(FEATURE_3D))
-            motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue));
+            motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
         else
-            motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue));
+            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 + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
+        deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
         deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
     } else
 #endif
@@ -518,7 +510,7 @@ void mixTable(pidProfile_t *pidProfile)
     // Scale roll/pitch/yaw uniformly to fit within throttle range
     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
     float throttle = 0, currentThrottleInputRange = 0;
-    uint16_t motorOutputMin, motorOutputMax;
+    float motorOutputMin, motorOutputMax;
     static uint16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
     bool mixerInversion = false;
 
@@ -576,11 +568,14 @@ void mixTable(pidProfile_t *pidProfile)
     float motorMix[MAX_SUPPORTED_MOTORS];
     float motorMixMax = 0, motorMixMin = 0;
     const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed);
+    int motorDirection = GET_DIRECTION(isMotorsReversed());
+        
+        
     for (int i = 0; i < motorCount; i++) {
         float mix =
-            scaledAxisPidRoll  * currentMixer[i].roll +
+            scaledAxisPidRoll  * currentMixer[i].roll  * (motorDirection) +
-            scaledAxisPidPitch * currentMixer[i].pitch +
+            scaledAxisPidPitch * currentMixer[i].pitch * (motorDirection) +
-            scaledAxisPidYaw   * currentMixer[i].yaw * (-yawDirection);
+            scaledAxisPidYaw   * currentMixer[i].yaw * (-yawDirection) * (motorDirection);
 
         if (vbatCompensationFactor > 1.0f) {
             mix *= vbatCompensationFactor;  // Add voltage compensation
@@ -614,7 +609,7 @@ void mixTable(pidProfile_t *pidProfile)
     // 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 + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));
+        float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle));
 
         // Dshot works exactly opposite in lower 3D section.
         if (mixerInversion) {
@@ -648,7 +643,7 @@ void mixTable(pidProfile_t *pidProfile)
     }
 }
 
-uint16_t convertExternalToMotor(uint16_t externalValue)
+float convertExternalToMotor(uint16_t externalValue)
 {
     uint16_t motorValue = externalValue;
 #ifdef USE_DSHOT
@@ -667,12 +662,12 @@ uint16_t convertExternalToMotor(uint16_t externalValue)
     }
 #endif
 
-    return motorValue;
+    return (float)motorValue;
 }
 
-uint16_t convertMotorToExternal(uint16_t motorValue)
+uint16_t convertMotorToExternal(float motorValue)
 {
-    uint16_t externalValue = motorValue;
+    uint16_t externalValue = lrintf(motorValue);
 #ifdef USE_DSHOT
     if (isMotorProtocolDshot()) {
         if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) {

src/main/flight/mixer.h

@@ -118,8 +118,8 @@ PG_DECLARE(motorConfig_t, motorConfig);
 #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
 
 extern const mixer_t mixers[];
-extern int16_t motor[MAX_SUPPORTED_MOTORS];
+extern float motor[MAX_SUPPORTED_MOTORS];
-extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
+extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
 struct rxConfig_s;
 
 uint8_t getMotorCount();
@@ -141,5 +141,5 @@ void stopMotors(void);
 void stopPwmAllMotors(void);
 
 bool isMotorProtocolDshot(void);
-uint16_t convertExternalToMotor(uint16_t externalValue);
+float convertExternalToMotor(uint16_t externalValue);
-uint16_t convertMotorToExternal(uint16_t motorValue);
+uint16_t convertMotorToExternal(float motorValue);

src/main/flight/navigation.c

@@ -37,7 +37,7 @@
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
-#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"

src/main/flight/servos.c

@@ -38,6 +38,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"

src/main/io/beeper.c

@@ -28,6 +28,9 @@
 
 #include "drivers/sound_beeper.h"
 #include "drivers/time.h"
+#include "drivers/pwm_output.h"
+
+#include "flight/mixer.h"
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
@@ -153,7 +156,6 @@ static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2];
 #define BEEPER_CONFIRMATION_BEEP_DURATION 2
 #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20
 
-
 // Beeper off = 0 Beeper on = 1
 static uint8_t beeperIsOn = 0;
 
@@ -338,6 +340,14 @@ void beeperUpdate(timeUs_t currentTimeUs)
         return;
     }
 
+    #ifdef USE_DSHOT
+    if (!ARMING_FLAG(ARMED) && currentBeeperEntry->mode == BEEPER_RX_SET) {  
+        for (unsigned index = 0; index < getMotorCount(); index++) {
+            pwmWriteDshotCommand(index, DSHOT_CMD_BEEP3);
+        }
+    }
+    #endif
+
     if (!beeperIsOn) {
         beeperIsOn = 1;
         if (currentBeeperEntry->sequence[beeperPos] != 0) {

src/main/io/ledstrip.c

@@ -44,6 +44,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"

src/main/io/osd.c

@@ -58,6 +58,7 @@
 #include "drivers/vtx_common.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
+#include "io/beeper.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
 #include "io/osd.h"
@@ -79,6 +80,7 @@
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
+#include "sensors/esc_sensor.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -95,7 +97,8 @@
 
 // Blink control
 
-bool blinkState = true;
+static bool blinkState = true;
+static bool showVisualBeeper = false;
 
 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32];
 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
@@ -154,6 +157,10 @@ static const char compassBar[] = {
 
 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0);
 
+#ifdef USE_ESC_SENSOR
+static escSensorData_t *escData;
+#endif
+
 /**
  * Gets the correct altitude symbol for the current unit system
  */
@@ -518,7 +525,7 @@ static void osdDrawSingleElement(uint8_t item)
             break;
         }
 
-    case OSD_MAIN_BATT_WARNING:
+    case OSD_WARNINGS:
         switch(getBatteryState()) {
         case BATTERY_WARNING:
             tfp_sprintf(buff, "LOW BATTERY");
@@ -529,7 +536,13 @@ static void osdDrawSingleElement(uint8_t item)
             break;
 
         default:
-            return;
+            if (showVisualBeeper) {
+                tfp_sprintf(buff, "  * * * *");
+            } else {
+                return;
+            }
+            break;
+
         }
         break;
 
@@ -605,6 +618,16 @@ static void osdDrawSingleElement(uint8_t item)
             tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
             break;
         }
+#ifdef USE_ESC_SENSOR
+    case OSD_ESC_TMP:
+        buff[0] = SYM_TEMP_C;
+        tfp_sprintf(buff + 1, "%d", escData == NULL ? 0 : escData->temperature);
+        break;
+
+    case OSD_ESC_RPM:
+        tfp_sprintf(buff, "%d", escData == NULL ? 0 : escData->rpm);
+        break;
+#endif
 
     default:
         return;
@@ -613,7 +636,7 @@ static void osdDrawSingleElement(uint8_t item)
     displayWrite(osdDisplayPort, elemPosX + elemOffsetX, elemPosY, buff);
 }
 
-void osdDrawElements(void)
+static void osdDrawElements(void)
 {
     displayClearScreen(osdDisplayPort);
 
@@ -621,13 +644,6 @@ void osdDrawElements(void)
     if (IS_RC_MODE_ACTIVE(BOXOSD))
       return;
 
-#if 0
-    if (currentElement)
-        osdDrawElementPositioningHelp();
-#else
-    if (false)
-        ;
-#endif
 #ifdef CMS
     else if (sensors(SENSOR_ACC) || displayIsGrabbed(osdDisplayPort))
 #else
@@ -654,7 +670,7 @@ void osdDrawElements(void)
     osdDrawSingleElement(OSD_YAW_PIDS);
     osdDrawSingleElement(OSD_POWER);
     osdDrawSingleElement(OSD_PIDRATE_PROFILE);
-    osdDrawSingleElement(OSD_MAIN_BATT_WARNING);
+    osdDrawSingleElement(OSD_WARNINGS);
     osdDrawSingleElement(OSD_AVG_CELL_VOLTAGE);
     osdDrawSingleElement(OSD_DEBUG);
     osdDrawSingleElement(OSD_PITCH_ANGLE);
@@ -681,6 +697,13 @@ void osdDrawElements(void)
         osdDrawSingleElement(OSD_HOME_DIR);
     }
 #endif // GPS
+
+#ifdef USE_ESC_SENSOR
+  if (feature(FEATURE_ESC_SENSOR)) {
+      osdDrawSingleElement(OSD_ESC_TMP);
+      osdDrawSingleElement(OSD_ESC_RPM);
+  }
+#endif
 }
 
 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
@@ -706,7 +729,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[OSD_YAW_PIDS]           = OSD_POS(7, 15)  | VISIBLE_FLAG;
     osdConfig->item_pos[OSD_POWER]              = OSD_POS(1, 10)  | VISIBLE_FLAG;
     osdConfig->item_pos[OSD_PIDRATE_PROFILE]    = OSD_POS(25, 10) | VISIBLE_FLAG;
-    osdConfig->item_pos[OSD_MAIN_BATT_WARNING]  = OSD_POS(9, 10)  | VISIBLE_FLAG;
+    osdConfig->item_pos[OSD_WARNINGS]           = OSD_POS(9, 10)  | VISIBLE_FLAG;
     osdConfig->item_pos[OSD_AVG_CELL_VOLTAGE]   = OSD_POS(12, 2)  | VISIBLE_FLAG;
     osdConfig->item_pos[OSD_DEBUG]              = OSD_POS(1, 0)   | VISIBLE_FLAG;
     osdConfig->item_pos[OSD_PITCH_ANGLE]        = OSD_POS(1, 8)   | VISIBLE_FLAG;
@@ -721,6 +744,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[OSD_DISARMED]           = OSD_POS(10, 4)  | VISIBLE_FLAG;
     osdConfig->item_pos[OSD_NUMERICAL_HEADING]  = OSD_POS(23, 9)  | VISIBLE_FLAG;
     osdConfig->item_pos[OSD_NUMERICAL_VARIO]    = OSD_POS(23, 8)  | VISIBLE_FLAG;
+    osdConfig->item_pos[OSD_ESC_TMP]            = OSD_POS(18, 2)  | VISIBLE_FLAG;
+    osdConfig->item_pos[OSD_ESC_RPM]            = OSD_POS(19, 2)  | VISIBLE_FLAG;
 
     osdConfig->enabled_stats[OSD_STAT_MAX_SPEED]    = true;
     osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY]  = true;
@@ -802,12 +827,12 @@ void osdUpdateAlarms(void)
         CLR_BLINK(OSD_RSSI_VALUE);
 
     if (getBatteryState() == BATTERY_OK) {
+        CLR_BLINK(OSD_WARNINGS);
         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
-        CLR_BLINK(OSD_MAIN_BATT_WARNING);
         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
     } else {
+        SET_BLINK(OSD_WARNINGS);
         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
-        SET_BLINK(OSD_MAIN_BATT_WARNING);
         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
     }
 
@@ -839,7 +864,7 @@ void osdResetAlarms(void)
 {
     CLR_BLINK(OSD_RSSI_VALUE);
     CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
-    CLR_BLINK(OSD_MAIN_BATT_WARNING);
+    CLR_BLINK(OSD_WARNINGS);
     CLR_BLINK(OSD_GPS_SATS);
     CLR_BLINK(OSD_FLYTIME);
     CLR_BLINK(OSD_MAH_DRAWN);
@@ -1064,6 +1089,12 @@ static void osdRefresh(timeUs_t currentTimeUs)
 
     blinkState = (currentTimeUs / 200000) % 2;
 
+#ifdef USE_ESC_SENSOR
+    if (feature(FEATURE_ESC_SENSOR)) {
+        escData = getEscSensorData(ESC_SENSOR_COMBINED);
+    }
+#endif
+
 #ifdef CMS
     if (!displayIsGrabbed(osdDisplayPort)) {
         osdUpdateAlarms();
@@ -1083,6 +1114,11 @@ static void osdRefresh(timeUs_t currentTimeUs)
 void osdUpdate(timeUs_t currentTimeUs)
 {
     static uint32_t counter = 0;
+
+    if (isBeeperOn()) {
+        showVisualBeeper = true;
+    }
+
 #ifdef MAX7456_DMA_CHANNEL_TX
     // don't touch buffers if DMA transaction is in progress
     if (displayIsTransferInProgress(osdDisplayPort)) {
@@ -1108,6 +1144,8 @@ void osdUpdate(timeUs_t currentTimeUs)
 
     if (counter++ % DRAW_FREQ_DENOM == 0) {
         osdRefresh(currentTimeUs);
+
+        showVisualBeeper = false;
     } else { // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
         displayDrawScreen(osdDisplayPort);
     }

src/main/io/osd.h

@@ -48,7 +48,7 @@ typedef enum {
     OSD_YAW_PIDS,
     OSD_POWER,
     OSD_PIDRATE_PROFILE,
-    OSD_MAIN_BATT_WARNING,
+    OSD_WARNINGS,
     OSD_AVG_CELL_VOLTAGE,
     OSD_GPS_LON,
     OSD_GPS_LAT,
@@ -63,6 +63,8 @@ typedef enum {
     OSD_NUMERICAL_HEADING,
     OSD_NUMERICAL_VARIO,
     OSD_COMPASS_BAR,
+    OSD_ESC_TMP,
+    OSD_ESC_RPM,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/io/vtx_control.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 
 #define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT  10
 

src/main/msp/msp.h

@@ -23,7 +23,8 @@
 typedef enum {
     MSP_RESULT_ACK = 1,
     MSP_RESULT_ERROR = -1,
-    MSP_RESULT_NO_REPLY = 0
+    MSP_RESULT_NO_REPLY = 0,
+    MSP_RESULT_CMD_UNKNOWN = -2,   // don't know how to process command, try next handler
 } mspResult_e;
 
 typedef enum {

src/main/osd_slave/osd_slave_init.c

@@ -34,6 +34,7 @@
 #include "config/parameter_group_ids.h"
 
 #include "drivers/adc.h"
+#include "drivers/bus.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/bus_spi.h"
 #include "drivers/dma.h"
@@ -92,10 +93,6 @@
 void targetPreInit(void);
 #endif
 
-#ifdef TARGET_BUS_INIT
-void targetBusInit(void);
-#endif
-
 uint8_t systemState = SYSTEM_STATE_INITIALISING;
 
 void processLoopback(void)
@@ -199,6 +196,11 @@ void init(void)
 #endif /* USE_SPI */
 
 #ifdef USE_I2C
+    i2cHardwareConfigure();
+
+    // Note: Unlike UARTs which are configured when client is present,
+    // I2C buses are initialized unconditionally if they are configured.
+
 #ifdef USE_I2C_DEVICE_1
     i2cInit(I2CDEV_1);
 #endif

src/main/rx/rx.c

@@ -41,6 +41,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 
 #include "flight/failsafe.h"
 
@@ -287,6 +288,7 @@ void rxInit(void)
     rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
 
     // Initialize ARM switch to OFF position when arming via switch is defined
+    // TODO - move to rc_mode.c
     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
         const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(i);
         if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {

src/main/sensors/current.c

@@ -211,7 +211,6 @@ void currentMeterESCReadCombined(currentMeter_t *meter)
     meter->amperageLatest = currentMeterESCState.amperage;
     meter->amperage = currentMeterESCState.amperage;
     meter->mAhDrawn = currentMeterESCState.mAhDrawn;
-    currentMeterReset(meter);
 }
 
 void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter)

src/main/target/AIR32/target.h

@@ -21,7 +21,7 @@
 
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
 
-#define LED0                    PB5 // Blue LED - PB5
+#define LED0_PIN                PB5 // Blue LED - PB5
 
 #define BEEPER                  PA0
 

src/main/target/AIRHEROF3/target.h

@@ -22,8 +22,8 @@
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
 #define CONFIG_PREFER_ACC_ON
 
-#define LED0                    PB3
+#define LED0_PIN                PB3
-#define LED1                    PB4
+#define LED1_PIN                PB4
 
 #define BEEPER                  PA12
 #define BEEPER_INVERTED

src/main/target/ALIENFLIGHTF1/target.h

@@ -22,8 +22,8 @@
 
 #define BRUSHED_ESC_AUTODETECT
 
-#define LED0                    PB3
+#define LED0_PIN                PB3
-#define LED1                    PB4
+#define LED1_PIN                PB4
 
 #define BEEPER                  PA12
 

src/main/target/ALIENFLIGHTF3/config.c

@@ -57,7 +57,7 @@ void targetConfiguration(void)
 {
     /* depending on revision ... depends on the LEDs to be utilised. */
     if (hardwareRevision == AFF3_REV_2) {
-        statusLedConfigMutable()->polarity = 0
+        statusLedConfigMutable()->inversion = 0
 #ifdef LED0_A_INVERTED
             | BIT(0)
 #endif
@@ -69,17 +69,17 @@ void targetConfiguration(void)
 #endif
             ;
 
-        for (int i = 0; i < LED_NUMBER; i++) {
+        for (int i = 0; i < STATUS_LED_NUMBER; i++) {
-            statusLedConfigMutable()->ledTags[i] = IO_TAG_NONE;
+            statusLedConfigMutable()->ioTags[i] = IO_TAG_NONE;
         }
 #ifdef LED0_A
-        statusLedConfigMutable()->ledTags[0] = IO_TAG(LED0_A);
+        statusLedConfigMutable()->ioTags[0] = IO_TAG(LED0_A);
 #endif
 #ifdef LED1_A
-        statusLedConfigMutable()->ledTags[1] = IO_TAG(LED1_A);
+        statusLedConfigMutable()->ioTags[1] = IO_TAG(LED1_A);
 #endif
 #ifdef LED2_A
-        statusLedConfigMutable()->ledTags[2] = IO_TAG(LED2_A);
+        statusLedConfigMutable()->ioTags[2] = IO_TAG(LED2_A);
 #endif
     } else {
         gyroConfigMutable()->gyro_sync_denom = 2;

src/main/target/ALIENFLIGHTF3/target.h

@@ -19,7 +19,6 @@
 
 #define TARGET_BOARD_IDENTIFIER "AFF3" // AlienFlight F3.
 #define TARGET_CONFIG
-#define TARGET_BUS_INIT
 #define REMAP_TIM17_DMA
 
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
@@ -29,8 +28,8 @@
 #define BRUSHED_ESC_AUTODETECT
 
 // LED's V1
-#define LED0                    PB4
+#define LED0_PIN                PB4
-#define LED1                    PB5
+#define LED1_PIN                PB5
 
 // LED's V2
 #define LED0_A                  PB8

src/main/target/ALIENFLIGHTF4/target.h

@@ -25,8 +25,8 @@
 
 #define USBD_PRODUCT_STRING "AlienFlight F4"
 
-#define LED0                    PC12
+#define LED0_PIN                PC12
-#define LED1                    PD2
+#define LED1_PIN                PD2
 
 #define BEEPER                  PC13
 #define BEEPER_INVERTED

src/main/target/ALIENFLIGHTNGF7/target.h

@@ -25,8 +25,8 @@
 
 #define USBD_PRODUCT_STRING "AlienFlightNG F7"
 
-#define LED0                    PC12
+#define LED0_PIN                PC12
-#define LED1                    PD2
+#define LED1_PIN                PD2
 
 #define BEEPER                  PC13
 #define BEEPER_INVERTED

src/main/target/BEEROTORF4/target.h

@@ -20,7 +20,7 @@
 
 #define USBD_PRODUCT_STRING "BeeRotorF4"
 
-#define LED0                    PB4
+#define LED0_PIN                PB4
 
 #define BEEPER                  PB3
 #define BEEPER_INVERTED
@@ -126,6 +126,7 @@
 //#define SPI_RX_CS_PIN           PD2
 
 #define USE_I2C
+#define USE_I2C_DEVICE_1
 #define I2C_DEVICE              (I2CDEV_1)
 #define I2C1_SCL                PB6
 #define I2C1_SDA                PB7

src/main/target/BLUEJAYF4/target.h

@@ -28,9 +28,9 @@
 
 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
 
-#define LED0                    PB6
+#define LED0_PIN                PB6
-#define LED1                    PB5
+#define LED1_PIN                PB5
-#define LED2                    PB4
+#define LED2_PIN                PB4
 
 #define BEEPER                  PC1
 #define BEEPER_OPT              PB7

src/main/target/CC3D/target.h

@@ -17,7 +17,7 @@
 
 #define TARGET_BOARD_IDENTIFIER "CC3D" // CopterControl 3D
 
-#define LED0                    PB3
+#define LED0_PIN                PB3
 
 #define INVERTER_PIN_UART1      PB2 // PB2 (BOOT1) used as inverter select GPIO
 

src/main/target/CHEBUZZF3/target.h

@@ -24,9 +24,9 @@
 #define STM32F3DISCOVERY
 #endif
 
-#define LED0                    PE8 // Blue LEDs - PE8/PE12
+#define LED0_PIN                PE8 // Blue LEDs - PE8/PE12
 #define LED0_INVERTED
-#define LED1                    PE10  // Orange LEDs - PE10/PE14
+#define LED1_PIN                PE10  // Orange LEDs - PE10/PE14
 #define LED1_INVERTED
 
 #define BEEPER                  PE9 // Red LEDs - PE9/PE13

src/main/target/CJMCU/initialisation.c

@@ -19,18 +19,20 @@
 #include <stdint.h>
 
 #include "platform.h"
+#include "drivers/bus.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/bus_spi.h"
 #include "io/serial.h"
 
 void targetBusInit(void)
 {
-    #if defined(USE_SPI) && defined(USE_SPI_DEVICE_1)
+#if defined(USE_SPI) && defined(USE_SPI_DEVICE_1)
-        spiInit(SPIDEV_1);
+    spiInit(SPIDEV_1);
-    #endif
+#endif
 
     if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
         serialRemovePort(SERIAL_PORT_USART3);
+        i2cHardwareConfigure();
         i2cInit(I2C_DEVICE);
     }
-}
+}

src/main/target/CJMCU/target.h

@@ -21,9 +21,9 @@
 #define USE_HARDWARE_REVISION_DETECTION
 #define TARGET_BUS_INIT
 
-#define LED0                    PC14
+#define LED0_PIN                PC14
-#define LED1                    PC13
+#define LED1_PIN                PC13
-#define LED2                    PC15
+#define LED2_PIN                PC15
 
 #undef BEEPER
 

src/main/target/CLRACINGF4/target.h

@@ -24,7 +24,7 @@
 #endif
 
 
-#define LED0                      PB5
+#define LED0_PIN                  PB5
 #define BEEPER                    PB4
 #define BEEPER_INVERTED
 #define BEEPER_PWM_HZ             3800 // Beeper PWM frequency in Hz

src/main/target/CLRACINGF7/target.h

@@ -20,7 +20,7 @@
 
 #define USBD_PRODUCT_STRING "CLRACINGF7"
 
-#define LED0                    PB0
+#define LED0_PIN                PB0
 #define BEEPER                  PB4
 #define BEEPER_INVERTED
 
@@ -87,7 +87,8 @@
 
 #define SERIAL_PORT_COUNT       5
 
-#define USE_I2C
+// XXX To target maintainer: Bus device to configure must be specified.
+//#define USE_I2C
 
 #define USE_SPI
 #define USE_SPI_DEVICE_1

src/main/target/COLIBRI/target.h

@@ -26,8 +26,8 @@
 
 #define TARGET_XTAL_MHZ         16
 
-#define LED0                    PC14
+#define LED0_PIN                PC14
-#define LED1                    PC13
+#define LED1_PIN                PC13
 
 #define BEEPER                  PC5
 

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -139,7 +139,7 @@ static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
 // this is the number of filled indexes in above array
 static uint8_t activeBoxIdCount = 0;
 // from mixer.c
-extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
+extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
 
 // cause reboot after BST processing complete
 static bool isRebootScheduled = false;
@@ -483,7 +483,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
                     mac->range.startStep = bstRead8();
                     mac->range.endStep = bstRead8();
 
-                    useRcControlsConfig(modeActivationConditions(0), currentPidProfile);
+                    useRcControlsConfig(currentPidProfile);
                 } else {
                     ret = BST_FAILED;
                 }

src/main/target/COLIBRI_RACE/target.c

@@ -16,9 +16,11 @@
  */
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #include <platform.h>
 
+#include "drivers/bus_i2c.h"
 #include "drivers/io.h"
 #include "drivers/dma.h"
 #include "drivers/timer.h"
@@ -46,10 +48,12 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
 
 };
 
-
+// XXX Requires some additional work here.
+// XXX Can't do this now without proper semantics about I2C on this target.
 #ifdef USE_BST
 void targetBusInit(void)
 {
+    i2cHardwareConfigure();
     bstInit(BST_DEVICE);
 }
 #endif

src/main/target/COLIBRI_RACE/target.h

@@ -26,9 +26,9 @@
 
 #undef USE_RX_MSP // never used.
 
-#define LED0                    PC15
+#define LED0_PIN                PC15
-#define LED1                    PC14
+#define LED1_PIN                PC14
-#define LED2                    PC13
+#define LED2_PIN                PC13
 
 #define BEEPER                  PB13
 #define BEEPER_INVERTED

src/main/target/CRAZYFLIE2/target.h

@@ -42,9 +42,9 @@
 #define USED_TIMERS             ( TIM_N(2) | TIM_N(4) )
 #endif
 
-#define LED0                    PD2
+#define LED0_PIN                PD2
-#define LED1                    PC0
+#define LED1_PIN                PC0
-#define LED2                    PC3
+#define LED2_PIN                PC3
 
 // Using STM32F405RG, 64 pin package (LQFP64)
 // 16 pins per port, ports A, B, C, and also PD2

src/main/target/DOGE/target.h

@@ -22,11 +22,11 @@
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
 
 // tqfp48 pin 34
-#define LED0                    PA13
+#define LED0_PIN                PA13
 // tqfp48 pin 37
-#define LED1                    PA14
+#define LED1_PIN                PA14
 // tqfp48 pin 38
-#define LED2                    PA15
+#define LED2_PIN                PA15
 
 #define BEEPER                  PB2
 #define BEEPER_INVERTED

src/main/target/ELLE0/target.h

@@ -22,9 +22,9 @@
 
 #define USBD_PRODUCT_STRING "Elle0"
 
-#define LED0                    PA8
+#define LED0_PIN                PA8
-#define LED1                    PB4
+#define LED1_PIN                PB4
-#define LED2                    PC2
+#define LED2_PIN                PC2
 
 // MPU9250 interrupt
 #define USE_EXTI

src/main/target/F4BY/target.h

@@ -20,9 +20,9 @@
 
 #define USBD_PRODUCT_STRING     "Swift-Flyer F4BY"
 
-#define LED0                    PE3 // Blue LED
+#define LED0_PIN                PE3 // Blue LED
-#define LED1                    PE2 // Red LED
+#define LED1_PIN                PE2 // Red LED
-#define LED2                    PE1 // Blue LED
+#define LED2_PIN                PE1 // Blue LED
 
 #define BEEPER                  PE5
 

src/main/target/FF_FORTINIF4/target.h

@@ -19,8 +19,8 @@
 #define TARGET_BOARD_IDENTIFIER "FORT"
 #define USBD_PRODUCT_STRING     "FortiniF4"
 /*--------------LED----------------*/
-#define LED0                    PB5
+#define LED0_PIN                PB5
-#define LED1                    PB6
+#define LED1_PIN                PB6
 #define LED_STRIP
 /*---------------------------------*/
 

src/main/target/FF_PIKOBLX/target.h

@@ -32,8 +32,8 @@
 #define TARGET_CONFIG
 #define BRUSHED_ESC_AUTODETECT
 
-#define LED0                    PB9
+#define LED0_PIN                PB9
-#define LED1                    PB5
+#define LED1_PIN                PB5
 
 #define BEEPER                  PA0
 #define BEEPER_INVERTED

src/main/target/FF_PIKOF4/target.h

@@ -19,8 +19,8 @@
 #define TARGET_BOARD_IDENTIFIER "PIK4"
 #define USBD_PRODUCT_STRING     "PikoF4"
 /*--------------LED----------------*/
-#define LED0                    PA15
+#define LED0_PIN                PA15
-#define LED1                    PB6
+#define LED1_PIN                PB6
 #define LED_STRIP
 /*---------------------------------*/
 

src/main/target/FISHDRONEF4/target.h

@@ -21,8 +21,8 @@
 
 #define USBD_PRODUCT_STRING  "FishDroneF4"
 
-#define LED0                    PC13
+#define LED0_PIN                PC13
-#define LED1                    PC14
+#define LED1_PIN                PC14
 
 #define BEEPER                  PC15
 #define BEEPER_INVERTED

src/main/target/FRSKYF3/target.h

@@ -21,7 +21,7 @@
 #define TARGET_CONFIG
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
 
-#define LED0                    PB3
+#define LED0_PIN                PB3
 #define BEEPER                  PC15
 #define BEEPER_INVERTED
 

src/main/target/FRSKYF4/target.h

@@ -19,7 +19,7 @@
 #define USBD_PRODUCT_STRING "FRSKYF4"
 #define TARGET_CONFIG
 
-#define LED0                    PB5
+#define LED0_PIN                PB5
 #define BEEPER                  PB4
 #define BEEPER_INVERTED
 

src/main/target/FURYF3/target.h

@@ -28,7 +28,7 @@
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
 #define CONFIG_PREFER_ACC_ON
 
-#define LED0                    PC14
+#define LED0_PIN                PC14
 
 #define BEEPER                  PC15
 #define BEEPER_INVERTED
@@ -127,6 +127,7 @@
 #endif
 
 #define USE_I2C
+#define USE_I2C_DEVICE_1
 #define I2C_DEVICE              (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4)
 #define I2C1_SCL            PB8
 #define I2C1_SDA            PB9

src/main/target/FURYF4/target.h

@@ -25,8 +25,8 @@
     #define USBD_PRODUCT_STRING     "FuryF4"
 #endif
 
-#define LED0                    PB5
+#define LED0_PIN                PB5
-#define LED1                    PB4
+#define LED1_PIN                PB4
 
 #define BEEPER                  PA8
 #define BEEPER_INVERTED

src/main/target/FURYF7/target.h

@@ -21,8 +21,8 @@
 
 #define USBD_PRODUCT_STRING "FuryF7"
 
-#define LED0                    PB5
+#define LED0_PIN                PB5
-#define LED1                    PB4
+#define LED1_PIN                PB4
 
 #define BEEPER                  PD10
 #define BEEPER_INVERTED

src/main/target/IMPULSERCF3/target.h

@@ -21,7 +21,7 @@
 
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
 
-#define LED0                    PB7
+#define LED0_PIN                PB7
 
 #define BEEPER                  PC15
 

src/main/target/IRCFUSIONF3/target.h

@@ -21,7 +21,7 @@
 
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
 
-#define LED0                    PB3
+#define LED0_PIN                PB3
 
 #define USE_EXTI
 #define MPU_INT_EXTI            PC13

src/main/target/ISHAPEDF3/target.h

@@ -21,7 +21,7 @@
 
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
 
-#define LED0        PB3
+#define LED0_PIN    PB3
 
 #define BEEPER      PC15
 #define BEEPER_INVERTED

src/main/target/KAKUTEF4/target.h

@@ -20,9 +20,9 @@
 
 #define USBD_PRODUCT_STRING "KakuteF4-V1"
 
-#define LED0                    PB5
+#define LED0_PIN                PB5
-#define LED1                    PB4
+#define LED1_PIN                PB4
-#define LED2                    PB6
+#define LED2_PIN                PB6
 
 #define BEEPER                  PC9
 #define BEEPER_INVERTED

src/main/target/KISSFC/target.h

@@ -29,7 +29,7 @@
 #define ESCSERIAL_TIMER_TX_HARDWARE 6
 #define REMAP_TIM17_DMA
 
-#define LED0                    PB1
+#define LED0_PIN                PB1
 
 #define BEEPER                  PB13
 #define BEEPER_INVERTED