Merge remote-tracking branch 'betaflight/master' into bfdev-configurable-spi-phase-1

.gitignore

@@ -19,20 +19,21 @@ startup_stm32f10x_md_gcc.s
 docs/Manual.pdf
 README.pdf
 
-# artifacts of top-level Makefile
+# artefacts of top-level Makefile
 /downloads
 /tools
 /build
 # local changes only
 make/local.mk
 
+# artefacts for VisualGDB (running in Visual Studio)
 mcu.mak
 mcu.mak.old
 stm32.mak
 
-# Artefacts for Visual Studio Code
+# artefacts for Visual Studio Code
-/.vscode/
+/.vscode
 
-# Artefacts for CLion
+# artefacts for CLion
-/cmake-build-debug/
+/cmake-build-debug
 /CMakeLists.txt

.travis.yml

@@ -47,7 +47,7 @@ install:
   - make arm_sdk_install
 
 before_script:
-  - tools/gcc-arm-none-eabi-6-2017-q1-update/bin/arm-none-eabi-gcc --version
+  - tools/gcc-arm-none-eabi-6-2017-q2-update/bin/arm-none-eabi-gcc --version
   - clang --version
   - clang++ --version
   - gcc --version

Makefile

@@ -1348,22 +1348,20 @@ targets-group-rest: $(GROUP_OTHER_TARGETS)
 
 
 $(VALID_TARGETS):
-		$(V0) echo "" && \
+		$(V0) @echo "" && \
 		echo "Building $@" && \
 		time $(MAKE) binary hex TARGET=$@ && \
 		echo "Building $@ succeeded."
 
-
-
 CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) )
 TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) )
 
 ## clean             : clean up temporary / machine-generated files
 clean:
-	$(V0) echo "Cleaning $(TARGET)"
+	$(V0) @echo "Cleaning $(TARGET)"
 	$(V0) rm -f $(CLEAN_ARTIFACTS)
 	$(V0) rm -rf $(OBJECT_DIR)/$(TARGET)
-	$(V0) echo "Cleaning $(TARGET) succeeded."
+	$(V0) @echo "Cleaning $(TARGET) succeeded."
 
 ## clean_test        : clean up temporary / machine-generated files (tests)
 clean_test:

README.md

@@ -1,4 +1,6 @@
-![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067)
+![BetaFlight Notice, version 3.2 will be the last version of Betaflight to support STM32F1 based flight controllers, this includes NAZE, CC3D (original) and CJMCU like flight controllers](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf3_2_notice.png)
+
+![BetaFlight](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf_logo.png)
 
 Betaflight is flight controller software (firmware) used to fly multi-rotor craft and fixed wing craft.
 

make/tools.mk

@@ -14,14 +14,14 @@
 ##############################
 
 # Set up ARM (STM32) SDK
-ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q1-update
+ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q2-update
 # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion)
 GCC_REQUIRED_VERSION ?= 6.3.1
 
 ## arm_sdk_install   : Install Arm SDK
 .PHONY: arm_sdk_install
 
-ARM_SDK_URL_BASE  := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6_1-2017q1/gcc-arm-none-eabi-6-2017-q1-update
+ARM_SDK_URL_BASE  := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2017q2/gcc-arm-none-eabi-6-2017-q2-update
 
 # source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads
 ifdef LINUX
@@ -33,7 +33,7 @@ ifdef MACOSX
 endif
 
 ifdef WINDOWS
-  ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-win32-zip.zip
+  ARM_SDK_URL  := $(ARM_SDK_URL_BASE)-win32.zip
 endif
 
 ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))

src/main/cms/cms.c

@@ -574,7 +574,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
             return;
         cmsInMenu = true;
         currentCtx = (cmsCtx_t){ &menuMain, 0, 0 };
-        DISABLE_ARMING_FLAG(OK_TO_ARM);
+        setArmingDisabled(ARMING_DISABLED_CMS_MENU);
     } else {
         // Switch display
         displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
@@ -642,7 +642,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
         systemReset();
     }
 
-    ENABLE_ARMING_FLAG(OK_TO_ARM);
+    unsetArmingDisabled(ARMING_DISABLED_CMS_MENU);
 
     return 0;
 }
@@ -872,7 +872,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
 
 uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeatCount)
 {
-    uint16_t ret;
+    uint16_t ret = 0;
 
     for (int i = 0 ; i < repeatCount ; i++) {
         ret = cmsHandleKey(pDisplay, key);

src/main/drivers/accgyro/accgyro_mpu.c

@@ -30,6 +30,7 @@
 #include "common/utils.h"
 
 #include "drivers/bus_i2c.h"
+#include "drivers/bus_spi.h"
 #include "drivers/exti.h"
 #include "drivers/io.h"
 #include "drivers/nvic.h"
@@ -233,62 +234,65 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
 {
     UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
 #ifdef USE_GYRO_SPI_MPU6000
+    spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE);
 #ifdef MPU6000_CS_PIN
     gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
 #endif
     if (mpu6000SpiDetect(&gyro->bus)) {
         gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
         gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
-        gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
+        gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
-        gyro->mpuConfiguration.writeFn = mpu6000SpiWriteRegister;
+        gyro->mpuConfiguration.writeFn = spiWriteRegister;
         return true;
     }
 #endif
 
 #ifdef USE_GYRO_SPI_MPU6500
+    spiBusSetInstance(&gyro->bus, MPU6500_SPI_INSTANCE);
     gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
     const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus);
     // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
     if (mpu6500Sensor != MPU_NONE) {
         gyro->mpuDetectionResult.sensor = mpu6500Sensor;
         gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
-        gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
+        gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
-        gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
+        gyro->mpuConfiguration.writeFn = spiWriteRegister;
         return true;
     }
 #endif
 
 #ifdef  USE_GYRO_SPI_MPU9250
+    spiBusSetInstance(&gyro->bus, MPU9250_SPI_INSTANCE);
     gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin;
     if (mpu9250SpiDetect(&gyro->bus)) {
         gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
         gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
-        gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
+        gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
-        gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister;
+        gyro->mpuConfiguration.writeFn = spiWriteRegister;
-        gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister;
-        gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
         gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
         return true;
     }
 #endif
 
 #ifdef USE_GYRO_SPI_ICM20689
+    spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE);
     gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin;
     if (icm20689SpiDetect(&gyro->bus)) {
         gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
         gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
-        gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
+        gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
-        gyro->mpuConfiguration.writeFn = icm20689SpiWriteRegister;
+        gyro->mpuConfiguration.writeFn = spiWriteRegister;
         return true;
     }
 #endif
 
 #ifdef USE_ACCGYRO_BMI160
+    spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE);
     gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin;
     if (bmi160Detect(&gyro->bus)) {
         gyro->mpuDetectionResult.sensor = BMI_160_SPI;
-        gyro->mpuConfiguration.readFn = bmi160SpiReadRegister;
+        gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
-        gyro->mpuConfiguration.writeFn = bmi160SpiWriteRegister;
+        gyro->mpuConfiguration.writeFn = spiWriteRegister;
         return true;
     }
 #endif

src/main/drivers/accgyro/accgyro_mpu.h

@@ -127,15 +127,13 @@
 
 typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
 typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
-typedef void(*mpuResetFnPtr)(void);
+typedef void (*mpuResetFnPtr)(void);
 
 extern mpuResetFnPtr mpuResetFn;
 
 typedef struct mpuConfiguration_s {
     mpuReadRegisterFnPtr readFn;
     mpuWriteRegisterFnPtr writeFn;
-    mpuReadRegisterFnPtr slowreadFn;
-    mpuWriteRegisterFnPtr verifywriteFn;
     mpuResetFnPtr resetFn;
     uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
 } mpuConfiguration_t;

src/main/drivers/accgyro/accgyro_spi_bmi160.c

@@ -90,29 +90,27 @@ static volatile bool bmi160ExtiInitDone = false;
 //! Private functions
 static int32_t BMI160_Config(const busDevice_t *bus);
 static int32_t BMI160_do_foc(const busDevice_t *bus);
-static uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg);
 static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
 
-#define DISABLE_BMI160(spiCsnPin)       IOHi(spiCsnPin)
-#define ENABLE_BMI160(spiCsnPin)        IOLo(spiCsnPin)
-
 
 bool bmi160Detect(const busDevice_t *bus)
 {
-    if (BMI160Detected)
+    if (BMI160Detected) {
         return true;
+    }
+
     IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
     IOHi(bus->spi.csnPin);
 
-    spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR);
+    spiSetDivisor(bus->spi.instance, BMI160_SPI_DIVISOR);
 
     /* Read this address to acticate SPI (see p. 84) */
-    BMI160_ReadReg(bus, 0x7F);
+    spiReadRegister(bus, 0x7F);
     delay(10); // Give SPI some time to start up
 
     /* Check the chip ID */
-    if (BMI160_ReadReg(bus, BMI160_REG_CHIPID) != 0xd1){
+    if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1){
         return false;
     }
 
@@ -127,8 +125,9 @@ bool bmi160Detect(const busDevice_t *bus)
  */
 static void BMI160_Init(const busDevice_t *bus)
 {
-    if (BMI160InitDone || !BMI160Detected)
+    if (BMI160InitDone || !BMI160Detected) {
         return;
+    }
 
     /* Configure the BMI160 Sensor */
     if (BMI160_Config(bus) != 0){
@@ -164,7 +163,7 @@ static int32_t BMI160_Config(const busDevice_t *bus)
     delay(5); // can take up to 3.8ms
 
     // Verify that normal power mode was entered
-    uint8_t pmu_status = BMI160_ReadReg(bus, BMI160_REG_PMU_STAT);
+    uint8_t pmu_status = spiReadRegister(bus, BMI160_REG_PMU_STAT);
     if ((pmu_status & 0x3C) != 0x14){
         return -3;
     }
@@ -193,7 +192,7 @@ static int32_t BMI160_Config(const busDevice_t *bus)
     delay(1);
 
     // Enable offset compensation
-    uint8_t val = BMI160_ReadReg(bus, BMI160_REG_OFFSET_0);
+    uint8_t val = spiReadRegister(bus, BMI160_REG_OFFSET_0);
     if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){
         return -7;
     }
@@ -234,7 +233,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
 
     // Wait for FOC to complete
     for (int i=0; i<50; i++) {
-        val = BMI160_ReadReg(bus, BMI160_REG_STATUS);
+        val = spiReadRegister(bus, BMI160_REG_STATUS);
         if (val & BMI160_REG_STATUS_FOC_RDY) {
             break;
         }
@@ -245,7 +244,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
     }
 
     // Program NVM
-    val = BMI160_ReadReg(bus, BMI160_REG_CONF);
+    val = spiReadRegister(bus, BMI160_REG_CONF);
     if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) {
         return -4;
     }
@@ -256,7 +255,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
 
     // Wait for NVM programming to complete
     for (int i=0; i<50; i++) {
-        val = BMI160_ReadReg(bus, BMI160_REG_STATUS);
+        val = spiReadRegister(bus, BMI160_REG_STATUS);
         if (val & BMI160_REG_STATUS_NVM_RDY) {
             break;
         }
@@ -269,41 +268,6 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
     return 0;
 }
 
-/**
- * @brief Read a register from BMI160
- * @returns The register value
- * @param reg[in] Register address to be read
- */
-uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg)
-{
-    uint8_t data;
-
-    ENABLE_BMI160(bus->spi.csnPin);
-
-    spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte
-    spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1);   // receive response
-
-    DISABLE_BMI160(bus->spi.csnPin);
-
-    return data;
-}
-
-bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
-{
-    ENABLE_BMI160(bus->spi.csnPin);
-    spiTransferByte(BMI160_SPI_INSTANCE, reg | 0x80); // read transaction
-    spiTransfer(BMI160_SPI_INSTANCE, data, NULL, length);
-    ENABLE_BMI160(bus->spi.csnPin);
-
-    return true;
-}
-
-/**
- * @brief Writes one byte to the BMI160 register
- * \param[in] reg Register address
- * \param[in] data Byte to write
- * @returns 0 when success
- */
 static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data)
 {
     ENABLE_BMI160(bus->spi.csnPin);
@@ -316,11 +280,6 @@ static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data
     return 0;
 }
 
-bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    return BMI160_WriteReg(bus, reg, data);
-}
-
 extiCallbackRec_t bmi160IntCallbackRec;
 
 void bmi160ExtiHandler(extiCallbackRec_t *cb)
@@ -366,9 +325,9 @@ bool bmi160AccRead(accDev_t *acc)
     uint8_t bmi160_rec_buf[BUFFER_SIZE];
     uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
 
-    ENABLE_BMI160(acc->bus.spi.csnPin);
+    IOLo(acc->bus.spi.csnPin);
-    spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE);   // receive response
+    spiTransfer(acc->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE);   // receive response
-    DISABLE_BMI160(acc->bus.spi.csnPin);
+    IOHi(acc->bus.spi.csnPin);
 
     acc->ADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_XOUT_L]);
     acc->ADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_YOUT_L]);
@@ -394,9 +353,9 @@ bool bmi160GyroRead(gyroDev_t *gyro)
     uint8_t bmi160_rec_buf[BUFFER_SIZE];
     uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
 
-    ENABLE_BMI160(gyro->bus.spi.csnPin);
+    IOLo(gyro->bus.spi.csnPin);
-    spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE);   // receive response
+    spiTransfer(gyro->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE);   // receive response
-    DISABLE_BMI160(gyro->bus.spi.csnPin);
+    IOHi(gyro->bus.spi.csnPin);
 
     gyro->gyroADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_XOUT_L]);
     gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_YOUT_L]);

src/main/drivers/accgyro/accgyro_spi_bmi160.h

@@ -68,9 +68,6 @@ enum bmi160_gyro_range {
     BMI160_RANGE_2000DPS = 0x00,
 };
 
-bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
-
 bool bmi160Detect(const busDevice_t *bus);
 bool bmi160SpiAccDetect(accDev_t *acc);
 bool bmi160SpiGyroDetect(gyroDev_t *gyro);

src/main/drivers/accgyro/accgyro_spi_icm20689.c

@@ -36,29 +36,6 @@
 #include "accgyro_mpu.h"
 #include "accgyro_spi_icm20689.h"
 
-#define DISABLE_ICM20689(spiCsnPin)       IOHi(spiCsnPin)
-#define ENABLE_ICM20689(spiCsnPin)        IOLo(spiCsnPin)
-
-bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    ENABLE_ICM20689(bus->spi.csnPin);
-    spiTransferByte(ICM20689_SPI_INSTANCE, reg);
-    spiTransferByte(ICM20689_SPI_INSTANCE, data);
-    DISABLE_ICM20689(bus->spi.csnPin);
-
-    return true;
-}
-
-bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
-{
-    ENABLE_ICM20689(bus->spi.csnPin);
-    spiTransferByte(ICM20689_SPI_INSTANCE, reg | 0x80); // read transaction
-    spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length);
-    DISABLE_ICM20689(bus->spi.csnPin);
-
-    return true;
-}
-
 static void icm20689SpiInit(const busDevice_t *bus)
 {
     static bool hardwareInitialised = false;
@@ -71,27 +48,24 @@ static void icm20689SpiInit(const busDevice_t *bus)
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
     IOHi(bus->spi.csnPin);
 
-    spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);
 
     hardwareInitialised = true;
 }
 
 bool icm20689SpiDetect(const busDevice_t *bus)
 {
-    uint8_t tmp;
-    uint8_t attemptsRemaining = 20;
-
     icm20689SpiInit(bus);
 
-    spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed
 
-    icm20689SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
+    spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
 
+    uint8_t attemptsRemaining = 20;
     do {
         delay(150);
-
+        const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I);
-        icm20689SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
+        if (whoAmI == ICM20689_WHO_AM_I_CONST) {
-        if (tmp == ICM20689_WHO_AM_I_CONST) {
             break;
         }
         if (!attemptsRemaining) {
@@ -99,7 +73,7 @@ bool icm20689SpiDetect(const busDevice_t *bus)
         }
     } while (attemptsRemaining--);
 
-    spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);
 
     return true;
 
@@ -126,7 +100,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
 {
     mpuGyroInit(gyro);
 
-    spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
 
     gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
     delay(100);
@@ -156,7 +130,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
     gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
 #endif
 
-    spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_STANDARD);
 }
 
 bool icm20689SpiGyroDetect(gyroDev_t *gyro)

src/main/drivers/accgyro/accgyro_spi_icm20689.h

@@ -31,6 +31,3 @@ bool icm20689SpiDetect(const busDevice_t *bus);
 
 bool icm20689SpiAccDetect(accDev_t *acc);
 bool icm20689SpiGyroDetect(gyroDev_t *gyro);
-
-bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

src/main/drivers/accgyro/accgyro_spi_mpu6000.c

@@ -99,42 +99,19 @@ static bool mpuSpi6000InitDone = false;
 #define MPU6000_REV_D9 0x59
 #define MPU6000_REV_D10 0x5A
 
-#define DISABLE_MPU6000(spiCsnPin)       IOHi(spiCsnPin)
-#define ENABLE_MPU6000(spiCsnPin)        IOLo(spiCsnPin)
-
-bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    ENABLE_MPU6000(bus->spi.csnPin);
-    spiTransferByte(MPU6000_SPI_INSTANCE, reg);
-    spiTransferByte(MPU6000_SPI_INSTANCE, data);
-    DISABLE_MPU6000(bus->spi.csnPin);
-
-    return true;
-}
-
-bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
-{
-    ENABLE_MPU6000(bus->spi.csnPin);
-    spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
-    spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
-    DISABLE_MPU6000(bus->spi.csnPin);
-
-    return true;
-}
-
 void mpu6000SpiGyroInit(gyroDev_t *gyro)
 {
     mpuGyroInit(gyro);
 
     mpu6000AccAndGyroInit(gyro);
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
 
     // Accel and Gyro DLPF Setting
-    mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
+    spiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
     delayMicroseconds(1);
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);  // 18 MHz SPI clock
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);  // 18 MHz SPI clock
 
     mpuGyroRead(gyro);
 
@@ -150,22 +127,20 @@ void mpu6000SpiAccInit(accDev_t *acc)
 
 bool mpu6000SpiDetect(const busDevice_t *bus)
 {
-    uint8_t in;
-    uint8_t attemptsRemaining = 5;
-
     IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
     IOHi(bus->spi.csnPin);
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON);
 
-    mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
+    spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
 
+    uint8_t attemptsRemaining = 5;
     do {
         delay(150);
 
-        mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
+        const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I);
-        if (in == MPU6000_WHO_AM_I_CONST) {
+        if (whoAmI == MPU6000_WHO_AM_I_CONST) {
             break;
         }
         if (!attemptsRemaining) {
@@ -173,25 +148,25 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
         }
     } while (attemptsRemaining--);
 
-    mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
+    const uint8_t productID = spiReadRegister(bus, MPU_RA_PRODUCT_ID);
 
     /* look for a product ID we recognise */
 
     // verify product revision
-    switch (in) {
+    switch (productID) {
-        case MPU6000ES_REV_C4:
+    case MPU6000ES_REV_C4:
-        case MPU6000ES_REV_C5:
+    case MPU6000ES_REV_C5:
-        case MPU6000_REV_C4:
+    case MPU6000_REV_C4:
-        case MPU6000_REV_C5:
+    case MPU6000_REV_C5:
-        case MPU6000ES_REV_D6:
+    case MPU6000ES_REV_D6:
-        case MPU6000ES_REV_D7:
+    case MPU6000ES_REV_D7:
-        case MPU6000ES_REV_D8:
+    case MPU6000ES_REV_D8:
-        case MPU6000_REV_D6:
+    case MPU6000_REV_D6:
-        case MPU6000_REV_D7:
+    case MPU6000_REV_D7:
-        case MPU6000_REV_D8:
+    case MPU6000_REV_D8:
-        case MPU6000_REV_D9:
+    case MPU6000_REV_D9:
-        case MPU6000_REV_D10:
+    case MPU6000_REV_D10:
-            return true;
+        return true;
     }
 
     return false;
@@ -203,48 +178,48 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
         return;
     }
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
 
     // Device Reset
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
+    spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
     delay(150);
 
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
+    spiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
     delay(150);
 
     // Clock Source PPL with Z axis gyro reference
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
+    spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
     delayMicroseconds(15);
 
     // Disable Primary I2C Interface
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
+    spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
     delayMicroseconds(15);
 
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
+    spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
     delayMicroseconds(15);
 
     // Accel Sample Rate 1kHz
     // Gyroscope Output Rate =  1kHz when the DLPF is enabled
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
+    spiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
     delayMicroseconds(15);
 
     // Gyro +/- 1000 DPS Full Scale
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
+    spiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
     delayMicroseconds(15);
 
     // Accel +/- 8 G Full Scale
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
+    spiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
     delayMicroseconds(15);
 
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR
+    spiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR
     delayMicroseconds(15);
 
 #ifdef USE_MPU_DATA_READY_SIGNAL
-    mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
+    spiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
     delayMicroseconds(15);
 #endif
 
-    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
     delayMicroseconds(1);
 
     mpuSpi6000InitDone = true;

src/main/drivers/accgyro/accgyro_spi_mpu6000.h

@@ -21,6 +21,3 @@ bool mpu6000SpiDetect(const busDevice_t *bus);
 
 bool mpu6000SpiAccDetect(accDev_t *acc);
 bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
-
-bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

src/main/drivers/accgyro/accgyro_spi_mpu6500.c

@@ -34,39 +34,8 @@
 #include "accgyro_mpu6500.h"
 #include "accgyro_spi_mpu6500.h"
 
-#define DISABLE_MPU6500(spiCsnPin)       IOHi(spiCsnPin)
-#define ENABLE_MPU6500(spiCsnPin)        IOLo(spiCsnPin)
-
 #define BIT_SLEEP                   0x40
 
-bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    ENABLE_MPU6500(bus->spi.csnPin);
-    spiTransferByte(MPU6500_SPI_INSTANCE, reg);
-    spiTransferByte(MPU6500_SPI_INSTANCE, data);
-    DISABLE_MPU6500(bus->spi.csnPin);
-
-    return true;
-}
-
-bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    mpu6500SpiWriteRegister(bus, reg, data);
-    delayMicroseconds(10);
-
-    return true;
-}
-
-bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
-{
-    ENABLE_MPU6500(bus->spi.csnPin);
-    spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
-    spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
-    DISABLE_MPU6500(bus->spi.csnPin);
-
-    return true;
-}
-
 static void mpu6500SpiInit(const busDevice_t *bus)
 {
     static bool hardwareInitialised = false;
@@ -79,7 +48,7 @@ static void mpu6500SpiInit(const busDevice_t *bus)
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
     IOHi(bus->spi.csnPin);
 
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST);
 
     hardwareInitialised = true;
 }
@@ -88,11 +57,10 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus)
 {
     mpu6500SpiInit(bus);
 
-    uint8_t tmp;
+    const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I);
-    mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
 
     uint8_t mpuDetected = MPU_NONE;
-    switch (tmp) {
+    switch (whoAmI) {
     case MPU6500_WHO_AM_I_CONST:
         mpuDetected = MPU_65xx_SPI;
         break;
@@ -122,16 +90,16 @@ void mpu6500SpiAccInit(accDev_t *acc)
 
 void mpu6500SpiGyroInit(gyroDev_t *gyro)
 {
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_SLOW);
     delayMicroseconds(1);
 
     mpu6500GyroInit(gyro);
 
     // Disable Primary I2C Interface
-    mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
+    spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
     delay(100);
 
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
     delayMicroseconds(1);
 }
 

src/main/drivers/accgyro/accgyro_spi_mpu6500.h

@@ -24,9 +24,5 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus);
 bool mpu6500SpiAccDetect(accDev_t *acc);
 bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
 
-bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
-
 void mpu6500SpiGyroInit(gyroDev_t *gyro);
 void mpu6500SpiAccInit(accDev_t *acc);

src/main/drivers/accgyro/accgyro_spi_mpu9250.c

@@ -50,8 +50,29 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
 
 static bool mpuSpi9250InitDone = false;
 
-#define DISABLE_MPU9250(spiCsnPin)       IOHi(spiCsnPin)
+bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
-#define ENABLE_MPU9250(spiCsnPin)        IOLo(spiCsnPin)
+{
+    IOLo(bus->spi.csnPin);
+    delayMicroseconds(1);
+    spiTransferByte(bus->spi.instance, reg);
+    spiTransferByte(bus->spi.instance, data);
+    IOHi(bus->spi.csnPin);
+    delayMicroseconds(1);
+
+    return true;
+}
+
+static bool mpu9250SpiSlowReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
+{
+    IOLo(bus->spi.csnPin);
+    delayMicroseconds(1);
+    spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
+    spiTransfer(bus->spi.instance, data, NULL, length);
+    IOHi(bus->spi.csnPin);
+    delayMicroseconds(1);
+
+    return true;
+}
 
 void mpu9250SpiResetGyro(void)
 {
@@ -63,49 +84,15 @@ void mpu9250SpiResetGyro(void)
 #endif
 }
 
-bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    ENABLE_MPU9250(bus->spi.csnPin);
-    delayMicroseconds(1);
-    spiTransferByte(MPU9250_SPI_INSTANCE, reg);
-    spiTransferByte(MPU9250_SPI_INSTANCE, data);
-    DISABLE_MPU9250(bus->spi.csnPin);
-    delayMicroseconds(1);
-
-    return true;
-}
-
-bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
-{
-    ENABLE_MPU9250(bus->spi.csnPin);
-    spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
-    spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
-    DISABLE_MPU9250(bus->spi.csnPin);
-
-    return true;
-}
-
-bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
-{
-    ENABLE_MPU9250(bus->spi.csnPin);
-    delayMicroseconds(1);
-    spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
-    spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
-    DISABLE_MPU9250(bus->spi.csnPin);
-    delayMicroseconds(1);
-
-    return true;
-}
-
 void mpu9250SpiGyroInit(gyroDev_t *gyro)
 {
     mpuGyroInit(gyro);
 
     mpu9250AccAndGyroInit(gyro);
 
-    spiResetErrorCounter(MPU9250_SPI_INSTANCE);
+    spiResetErrorCounter(gyro->bus.spi.instance);
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
 
     mpuGyroRead(gyro);
 
@@ -120,16 +107,15 @@ void mpu9250SpiAccInit(accDev_t *acc)
     acc->acc_1G = 512 * 8;
 }
 
-bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
+bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data)
 {
-    uint8_t in;
-    uint8_t attemptsRemaining = 20;
-
     mpu9250SpiWriteRegister(bus, reg, data);
     delayMicroseconds(100);
 
+    uint8_t attemptsRemaining = 20;
     do {
-        mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
+        uint8_t in;
+        mpu9250SpiSlowReadRegisterBuffer(bus, reg, 1, &in);
         if (in == data) {
             return true;
         } else {
@@ -147,54 +133,51 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
         return;
     }
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
 
     mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
     delay(50);
 
-    verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
 
     //Fchoice_b defaults to 00 which makes fchoice 11
     const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
-    verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
 
     if (gyro->lpf == 4) {
-        verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
+        mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
     } else if (gyro->lpf < 4) {
-        verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
+        mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
     } else if (gyro->lpf > 4) {
-        verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
+        mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
     }
 
-    verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
 
-    verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
-    verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
 
 #if defined(USE_MPU_DATA_READY_SIGNAL)
-    verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
 #endif
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
 
     mpuSpi9250InitDone = true; //init done
 }
 
 bool mpu9250SpiDetect(const busDevice_t *bus)
 {
-    uint8_t in;
-    uint8_t attemptsRemaining = 20;
-
     IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed
     mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 
+    uint8_t attemptsRemaining = 20;
     do {
         delay(150);
-
+        const uint8_t in = spiReadRegister(bus, MPU_RA_WHO_AM_I);
-        mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
         if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
             break;
         }
@@ -203,7 +186,7 @@ bool mpu9250SpiDetect(const busDevice_t *bus)
         }
     } while (attemptsRemaining--);
 
-    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST);
 
     return true;
 }

src/main/drivers/accgyro/accgyro_spi_mpu9250.h

@@ -34,6 +34,5 @@ bool mpu9250SpiAccDetect(accDev_t *acc);
 bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
 
 bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
+bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
+bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
-bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

src/main/drivers/bus.h

@@ -25,6 +25,9 @@
 typedef union busDevice_u {
     struct deviceSpi_s {
         SPI_TypeDef *instance;
+#if defined(USE_HAL_DRIVER)
+        SPI_HandleTypeDef* handle; // cached here for efficiency
+#endif
         IO_t csnPin;
     } spi;
     struct deviceI2C_s {

src/main/drivers/bus_spi.c

@@ -23,6 +23,7 @@
 
 #ifdef USE_SPI
 
+#include "drivers/bus.h"
 #include "drivers/bus_spi.h"
 #include "drivers/bus_spi_impl.h"
 #include "drivers/exti.h"
@@ -266,4 +267,40 @@ void spiResetErrorCounter(SPI_TypeDef *instance)
     if (device != SPIINVALID)
         spiDevice[device].errorCount = 0;
 }
+
+bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
+{
+    IOLo(bus->spi.csnPin);
+    spiTransferByte(bus->spi.instance, reg);
+    spiTransferByte(bus->spi.instance, data);
+    IOHi(bus->spi.csnPin);
+
+    return true;
+}
+
+bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
+{
+    IOLo(bus->spi.csnPin);
+    spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
+    spiTransfer(bus->spi.instance, data, NULL, length);
+    IOHi(bus->spi.csnPin);
+
+    return true;
+}
+
+uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg)
+{
+    uint8_t data;
+    IOLo(bus->spi.csnPin);
+    spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
+    spiTransfer(bus->spi.instance, &data, NULL, 1);
+    IOHi(bus->spi.csnPin);
+
+    return data;
+}
+
+void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance)
+{
+    bus->spi.instance = instance;
+}
 #endif

src/main/drivers/bus_spi.h

@@ -18,7 +18,8 @@
 #pragma once
 
 #include "drivers/io_types.h"
-#include "rcc_types.h"
+#include "drivers/bus.h"
+#include "drivers/rcc_types.h"
 
 #if defined(STM32F4) || defined(STM32F3)
 #define SPI_IO_AF_CFG      IO_CONFIG(GPIO_Mode_AF,  GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL)
@@ -97,6 +98,11 @@ SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance);
 DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size);
 #endif
 
+bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
+bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
+uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg);
+void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance);
+
 typedef struct spiPinConfig_s {
     ioTag_t ioTagSck[SPIDEV_COUNT];
     ioTag_t ioTagMiso[SPIDEV_COUNT];

src/main/drivers/bus_spi_hal.c

@@ -32,6 +32,50 @@
 
 spiDevice_t spiDevice[SPIDEV_COUNT];
 
+#ifndef SPI2_SCK_PIN
+#define SPI2_NSS_PIN    PB12
+#define SPI2_SCK_PIN    PB13
+#define SPI2_MISO_PIN   PB14
+#define SPI2_MOSI_PIN   PB15
+#endif
+
+#ifndef SPI3_SCK_PIN
+#define SPI3_NSS_PIN    PA15
+#define SPI3_SCK_PIN    PB3
+#define SPI3_MISO_PIN   PB4
+#define SPI3_MOSI_PIN   PB5
+#endif
+
+#ifndef SPI4_SCK_PIN
+#define SPI4_NSS_PIN    PA15
+#define SPI4_SCK_PIN    PB3
+#define SPI4_MISO_PIN   PB4
+#define SPI4_MOSI_PIN   PB5
+#endif
+
+#ifndef SPI1_NSS_PIN
+#define SPI1_NSS_PIN NONE
+#endif
+#ifndef SPI2_NSS_PIN
+#define SPI2_NSS_PIN NONE
+#endif
+#ifndef SPI3_NSS_PIN
+#define SPI3_NSS_PIN NONE
+#endif
+#ifndef SPI4_NSS_PIN
+#define SPI4_NSS_PIN NONE
+#endif
+
+#define SPI_DEFAULT_TIMEOUT 10
+
+
+static spiDevice_t spiHardwareMap[] = {
+    { .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER },
+    { .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER },
+    { .dev = SPI3, .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
+    { .dev = SPI4, .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER }
+};
+
 SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
 {
 #ifdef USE_SPI_DEVICE_1
@@ -212,13 +256,6 @@ uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance)
     return spiDevice[device].errorCount;
 }
 
-// return uint8_t value or -1 when failure
-uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in)
-{
-    spiTransfer(instance, &in, &in, 1);
-    return in;
-}
-
 /**
  * Return true if the bus is currently in the middle of a transmission.
  */
@@ -236,8 +273,6 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
     SPIDevice device = spiDeviceByInstance(instance);
     HAL_StatusTypeDef status;
 
-#define SPI_DEFAULT_TIMEOUT 10
-
     if(!out) // Tx only
     {
         status = HAL_SPI_Transmit(&spiDevice[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT);
@@ -257,6 +292,32 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
     return true;
 }
 
+static bool spiBusReadBuffer(const busDevice_t *bus, uint8_t *out, int len)
+{
+    const HAL_StatusTypeDef status = HAL_SPI_Receive(bus->spi.handle, out, len, SPI_DEFAULT_TIMEOUT);
+    if (status != HAL_OK) {
+        spiTimeoutUserCallback(bus->spi.instance);
+    }
+    return true;
+}
+
+// return uint8_t value or -1 when failure
+uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in)
+{
+    spiTransfer(instance, &in, &in, 1);
+    return in;
+}
+
+// return uint8_t value or -1 when failure
+static uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t in)
+{
+    const HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(bus->spi.handle, &in, &in, 1, SPI_DEFAULT_TIMEOUT);
+    if (status != HAL_OK) {
+        spiTimeoutUserCallback(bus->spi.instance);
+    }
+    return in;
+}
+
 void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
 {
     SPIDevice device = spiDeviceByInstance(instance);
@@ -292,6 +353,43 @@ void spiResetErrorCounter(SPI_TypeDef *instance)
         spiDevice[device].errorCount = 0;
 }
 
+bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
+{
+    IOLo(bus->spi.csnPin);
+    spiBusTransferByte(bus, reg);
+    spiBusTransferByte(bus, data);
+    IOHi(bus->spi.csnPin);
+
+    return true;
+}
+
+bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
+{
+    IOLo(bus->spi.csnPin);
+    spiBusTransferByte(bus, reg | 0x80); // read transaction
+    spiBusReadBuffer(bus, data, length);
+    IOHi(bus->spi.csnPin);
+
+    return true;
+}
+
+uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg)
+{
+    uint8_t data;
+    IOLo(bus->spi.csnPin);
+    spiBusTransferByte(bus, reg | 0x80); // read transaction
+    spiBusReadBuffer(bus, &data, 1);
+    IOHi(bus->spi.csnPin);
+
+    return data;
+}
+
+void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance)
+{
+    bus->spi.instance = instance;
+    bus->spi.handle = spiHandleByInstance(instance);
+}
+
 void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor)
 {
     SPIDevice device = descriptor->userParam;

src/main/drivers/compass/compass_ak8963.c

@@ -28,8 +28,10 @@
 #include "common/maths.h"
 #include "common/utils.h"
 
+#include "drivers/bus.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/bus_spi.h"
+#include "drivers/io.h"
 #include "drivers/sensor.h"
 #include "drivers/time.h"
 
@@ -82,15 +84,25 @@
 
 static float magGain[3] = { 1.0f, 1.0f, 1.0f };
 
+#if defined(USE_SPI)
 static busDevice_t *bus = NULL; // HACK
+#endif
 
 // FIXME pretend we have real MPU9250 support
 // Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same.
 #if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE)
 #define MPU9250_SPI_INSTANCE
-#define verifympu9250SpiWriteRegister mpu6500SpiWriteRegisterDelayed
+#define mpu9250SpiWriteRegisterVerify mpu6500SpiWriteRegDelayed
-#define mpu9250SpiWriteRegister mpu6500SpiWriteRegister
+static bool mpu6500SpiWriteRegDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
-#define mpu9250SpiReadRegister mpu6500SpiReadRegister
+{
+    IOLo(bus->spi.csnPin);
+    spiTransferByte(bus->spi.instance, reg);
+    spiTransferByte(bus->spi.instance, data);
+    IOHi(bus->spi.csnPin);
+    delayMicroseconds(10);
+
+    return true;
+}
 #endif
 
 #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
@@ -111,22 +123,22 @@ static queuedReadState_t queuedRead = { false, 0, 0};
 
 static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
 {
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG);   // set I2C slave address for read
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG);   // set I2C slave address for read
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_);                 // set I2C slave register
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_);                 // set I2C slave register
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80);         // read number of bytes
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80);         // read number of bytes
     delay(10);
     __disable_irq();
-    bool ack = mpu9250SpiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf);    // read I2C
+    bool ack = spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf);    // read I2C
     __enable_irq();
     return ack;
 }
 
 static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
 {
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_);               // set I2C slave address for write
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_);               // set I2C slave address for write
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_);                 // set I2C slave register
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_);                 // set I2C slave register
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_DO, data);                  // set I2C salve value
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_DO, data);                  // set I2C salve value
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, 0x81);                // write 1 byte
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, 0x81);                // write 1 byte
     return true;
 }
 
@@ -138,9 +150,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
 
     queuedRead.len = len_;
 
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG);   // set I2C slave address for read
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG);   // set I2C slave address for read
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_);                 // set I2C slave register
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_);                 // set I2C slave register
-    verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80);         // read number of bytes
+    mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80);         // read number of bytes
 
     queuedRead.readStartedAt = micros();
     queuedRead.waiting = true;
@@ -175,7 +187,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
 
     queuedRead.waiting = false;
 
-    mpu9250SpiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf);               // read I2C buffer
+    spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf);               // read I2C buffer
     return true;
 }
 #else
@@ -315,19 +327,21 @@ bool ak8963Detect(magDev_t *mag)
 {
     uint8_t sig = 0;
 
+#if defined(USE_SPI)
     bus = &mag->bus;
+#if defined(MPU6500_SPI_INSTANCE)
+    spiBusSetInstance(&mag->bus, MPU6500_SPI_INSTANCE);
+#elif defined(MPU9250_SPI_INSTANCE)
+    spiBusSetInstance(&mag->bus, MPU9250_SPI_INSTANCE);
 
-#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
     // initialze I2C master via SPI bus (MPU9250)
-
+    mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
-    verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
     delay(10);
-
+    mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D);              // I2C multi-master / 400kHz
-    verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D);              // I2C multi-master / 400kHz
     delay(10);
-
+    mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30);                 // I2C master mode, SPI mode only
-    verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30);                 // I2C master mode, SPI mode only
     delay(10);
+#endif
 #endif
 
     // check for AK8963

src/main/drivers/pwm_output.c

@@ -42,7 +42,7 @@ static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
 
 #ifdef BEEPER
 static pwmOutputPort_t beeperPwm;
-static uint16_t freqBeep=0;
+static uint16_t freqBeep = 0;
 #endif
 
 bool pwmMotorsEnabled = false;
@@ -313,7 +313,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
 #endif
 
         /* standard PWM outputs */
-        const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen));
+        const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen * 2));
 
         const uint32_t clock = timerClock(timerHardware->tim);
         /* used to find the desired timer frequency for max resolution */
@@ -456,7 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
             /* flag failure and disable ability to arm */
             break;
         }
-        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_HZ, PWM_TIMER_HZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
+        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
         servos[servoIndex].enabled = true;
     }
 }
@@ -469,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep)
         if(!beeperPwm.io)
             return;
         if(onoffBeep == true) {
-            *beeperPwm.ccr = (PWM_TIMER_HZ / freqBeep) / 2;
+            *beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
             beeperPwm.enabled = true;
         } else {
             *beeperPwm.ccr = 0;
@@ -495,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency)
             IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
 #endif
             freqBeep = frequency;
-            pwmOutConfig(&beeperPwm, timer, PWM_TIMER_HZ, PWM_TIMER_HZ / freqBeep, (PWM_TIMER_HZ / freqBeep) / 2, 0);
+            pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
         }
         *beeperPwm.ccr = 0;
         beeperPwm.enabled = false;

src/main/drivers/pwm_output.h

@@ -77,7 +77,7 @@ typedef enum {
     PWM_TYPE_MAX
 } motorPwmProtocolTypes_e;
 
-#define PWM_TIMER_HZ          MHZ_TO_HZ(1)
+#define PWM_TIMER_1MHZ        MHZ_TO_HZ(1)
 
 #ifdef USE_DSHOT
 #define MAX_DMA_TIMERS        8

src/main/drivers/rx_pwm.c

@@ -391,7 +391,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
         IOConfigGPIO(io, IOCFG_AF_PP);
 #endif
 
-        timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ);
+        timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ);
         timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
         timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
         timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
@@ -448,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig)
     IOConfigGPIO(io, IOCFG_AF_PP);
 #endif
 
-    timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ);
+    timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ);
     timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
     timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
     timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);

src/main/drivers/serial_uart_stm32f7xx.c

@@ -341,9 +341,14 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
     IO_t rxIO = IOGetByTag(uartdev->rx);
 
     if ((options & SERIAL_BIDIR) && txIO) {
-        // XXX BIDIR_PP handling is missing
+        ioConfig_t ioCfg = IO_CONFIG(
+            ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD,
+            GPIO_SPEED_FREQ_HIGH,
+            ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PULLDOWN : GPIO_PULLUP
+        );
+
         IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-        IOConfigGPIOAF(txIO, IOCFG_AF_PP, hardware->af);
+        IOConfigGPIOAF(txIO, ioCfg, hardware->af);
     }
     else {
         if ((mode & MODE_TX) && txIO) {

src/main/drivers/system.c

@@ -164,9 +164,8 @@ void delay(uint32_t ms)
 #define SHORT_FLASH_DURATION 50
 #define CODE_FLASH_DURATION 250
 
-void failureMode(failureMode_e mode)
+void failureLedCode(failureMode_e mode, int codeRepeatsRemaining)
 {
-    int codeRepeatsRemaining = 10;
     int codeFlashesRemaining;
     int shortFlashesRemaining;
 
@@ -201,6 +200,12 @@ void failureMode(failureMode_e mode)
         delay(1000);
     }
 
+}
+
+void failureMode(failureMode_e mode)
+{
+    failureLedCode(mode, 10);
+
 #ifdef DEBUG
     systemReset();
 #else

src/main/drivers/system.h

@@ -33,6 +33,7 @@ typedef enum {
 } failureMode_e;
 
 // failure
+void failureLedCode(failureMode_e mode, int repeatCount);
 void failureMode(failureMode_e mode);
 
 // bootloader/IAP

src/main/drivers/timer.h

@@ -134,7 +134,7 @@ typedef enum {
 #define HARDWARE_TIMER_DEFINITION_COUNT 14
 #endif
 
-#define MHZ_TO_HZ(x) (x * 1000000)
+#define MHZ_TO_HZ(x) ((x) * 1000000)
 
 extern const timerHardware_t timerHardware[];
 extern const timerDef_t timerDefinitions[];

src/main/drivers/transponder_ir.c

@@ -152,16 +152,8 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
     DMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
 }
 
-bool transponderIrInit(const transponderProvider_e provider)
+bool transponderIrInit(const ioTag_t ioTag, const transponderProvider_e provider)
 {
-    ioTag_t ioTag = IO_TAG_NONE;
-    for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
-        if (timerHardware[i].usageFlags & TIM_USE_TRANSPONDER) {
-            ioTag = timerHardware[i].tag;
-            break;
-        }
-    }
-
     if (!ioTag) {
         return false;
     }

src/main/drivers/transponder_ir.h

@@ -113,7 +113,7 @@ struct transponderVTable {
     void (*updateTransponderDMABuffer)(transponder_t *transponder, const uint8_t* transponderData);
 };
 
-bool transponderIrInit(const transponderProvider_e provider);
+bool transponderIrInit(const ioTag_t ioTag, const transponderProvider_e provider);
 void transponderIrDisable(void);
 
 void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder);

src/main/fc/cli.c

@@ -72,6 +72,7 @@ extern uint8_t __config_end;
 #include "drivers/sonar_hcsr04.h"
 #include "drivers/stack_check.h"
 #include "drivers/system.h"
+#include "drivers/transponder_ir.h"
 #include "drivers/time.h"
 #include "drivers/timer.h"
 #include "drivers/vcd.h"
@@ -105,6 +106,7 @@ extern uint8_t __config_end;
 #include "io/ledstrip.h"
 #include "io/osd.h"
 #include "io/serial.h"
+#include "io/transponder_ir.h"
 #include "io/vtx_rtc6705.h"
 #include "io/vtx_control.h"
 
@@ -183,18 +185,18 @@ static const char * const sensorTypeNames[] = {
 
 #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
 
-// sync with gyroSensor_e
-static const char * const gyroNames[] = {
-    "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
-    "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE"
-};
-
 static const char * const *sensorHardwareNames[] = {
-    gyroNames, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware
+    lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware
-
 };
 #endif // USE_SENSOR_NAMES
 
+#ifndef MINIMAL_CLI
+static const char *armingDisableFlagNames[] = {
+    "NOGYRO", "FAILSAFE", "BOXFAILSAFE", "THROTTLE",
+    "ANGLE", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST"
+};
+#endif
+
 static void cliPrint(const char *str)
 {
     while (*str) {
@@ -1165,7 +1167,7 @@ static void cliRxRange(char *cmdline)
         ptr = cmdline;
         i = atoi(ptr);
         if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) {
-            int rangeMin, rangeMax;
+            int rangeMin = 0, rangeMax = 0;
 
             ptr = nextArg(ptr);
             if (ptr) {
@@ -2712,7 +2714,18 @@ static void cliStatus(char *cmdline)
     const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
     cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
             constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate);
-
+#ifdef MINIMAL_CLI
+    cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags());
+#else
+    cliPrint("Arming disable flags:");
+    uint16_t flags = getArmingDisableFlags();
+    while (flags) {
+        int bitpos = ffs(flags) - 1;
+        flags &= ~(1 << bitpos);
+        cliPrintf(" %s", armingDisableFlagNames[bitpos]);
+    }
+    cliPrintLinefeed();
+#endif
 }
 
 #ifndef SKIP_TASK_STATISTICS
@@ -2833,6 +2846,9 @@ const cliResourceValue_t resourceTable[] = {
     { OWNER_RX_BIND,       PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_pin_override_ioTag), 0 },
     { OWNER_RX_BIND_PLUG,  PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_plug_ioTag), 0 },
 #endif
+#ifdef TRANSPONDER
+    { OWNER_TRANSPONDER,   PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 },
+#endif
 #ifdef USE_SPI
     { OWNER_SPI_SCK,       PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagSck[0]), SPIDEV_COUNT },
     { OWNER_SPI_MISO,      PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagMiso[0]), SPIDEV_COUNT },
@@ -3500,7 +3516,7 @@ void cliEnter(serialPort_t *serialPort)
 #endif
     cliPrompt();
 
-    ENABLE_ARMING_FLAG(PREVENT_ARMING);
+    setArmingDisabled(ARMING_DISABLED_CLI);
 }
 
 void cliInit(const serialConfig_t *serialConfig)

src/main/fc/fc_core.c

@@ -128,7 +128,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
     saveConfigAndNotify();
 }
 
-bool isCalibrating()
+static bool isCalibrating()
 {
 #ifdef BARO
     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
@@ -141,35 +141,52 @@ bool isCalibrating()
     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 }
 
-void updateLEDs(void)
+void updateArmingStatus(void)
 {
     if (ARMING_FLAG(ARMED)) {
         LED0_ON;
     } else {
-        if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) {
+        if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
-            ENABLE_ARMING_FLAG(OK_TO_ARM);
+            setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
+        } else {
+            unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
+        }
+
+        if (calculateThrottleStatus() != THROTTLE_LOW) {
+            setArmingDisabled(ARMING_DISABLED_THROTTLE);
+        } else {
+            unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
         }
 
         if (!STATE(SMALL_ANGLE)) {
-            DISABLE_ARMING_FLAG(OK_TO_ARM);
+            setArmingDisabled(ARMING_DISABLED_ANGLE);
+        } else {
+            unsetArmingDisabled(ARMING_DISABLED_ANGLE);
         }
 
-        if (isCalibrating() || (averageSystemLoadPercent > 100)) {
+        if (averageSystemLoadPercent > 100) {
-            warningLedFlash();
+            setArmingDisabled(ARMING_DISABLED_LOAD);
-            DISABLE_ARMING_FLAG(OK_TO_ARM);
         } else {
-            if (ARMING_FLAG(OK_TO_ARM)) {
+            unsetArmingDisabled(ARMING_DISABLED_LOAD);
-                warningLedDisable();
+        }
-            } else {
+
-                warningLedFlash();
+        if (isCalibrating()) {
-            }
+            setArmingDisabled(ARMING_DISABLED_CALIBRATING);
+        } else {
+            unsetArmingDisabled(ARMING_DISABLED_CALIBRATING);
+        }
+
+        if (isArmingDisabled()) {
+            warningLedFlash();
+        } else {
+            warningLedDisable();
         }
 
         warningLedUpdate();
     }
 }
 
-void mwDisarm(void)
+void disarm(void)
 {
     armingCalibrationWasInitialised = false;
 
@@ -186,7 +203,7 @@ void mwDisarm(void)
     }
 }
 
-void mwArm(void)
+void tryArm(void)
 {
     static bool firstArmingCalibrationWasCompleted;
 
@@ -196,51 +213,47 @@ void mwArm(void)
         firstArmingCalibrationWasCompleted = true;
     }
 
-    if (!isGyroCalibrationComplete()) return;  // prevent arming before gyro is calibrated
+    updateArmingStatus();
 
-    if (ARMING_FLAG(OK_TO_ARM)) {
+    if (!isArmingDisabled()) {
         if (ARMING_FLAG(ARMED)) {
             return;
         }
-        if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
-            return;
-        }
-        if (!ARMING_FLAG(PREVENT_ARMING)) {
 #ifdef USE_DSHOT
-            if (!feature(FEATURE_3D)) {
+        if (!feature(FEATURE_3D)) {
-                //TODO: Use BOXDSHOTREVERSE here
+            //TODO: Use BOXDSHOTREVERSE here
-                if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
+            if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
-                    reverseMotors = false;
+                reverseMotors = false;
-                    for (unsigned index = 0; index < getMotorCount(); index++) {
+                for (unsigned index = 0; index < getMotorCount(); index++) {
-                        pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
+                    pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
-                    }
+                }
-                } else {
+            } else {
-                    reverseMotors = true;
+                reverseMotors = true;
-                    for (unsigned index = 0; index < getMotorCount(); index++) {
+                for (unsigned index = 0; index < getMotorCount(); index++) {
-                        pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
+                    pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
-                    }
                 }
             }
+	}
 #endif
 
-            ENABLE_ARMING_FLAG(ARMED);
+        ENABLE_ARMING_FLAG(ARMED);
-            ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
+        ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
-            headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
+        headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
-            disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
+        disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 
-            //beep to indicate arming
+        //beep to indicate arming
 #ifdef GPS
-            if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5)
+        if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
-                beeper(BEEPER_ARMING_GPS_FIX);
+            beeper(BEEPER_ARMING_GPS_FIX);
-            else
+        } else {
-                beeper(BEEPER_ARMING);
-#else
             beeper(BEEPER_ARMING);
+        }
+#else
+        beeper(BEEPER_ARMING);
 #endif
 
-            return;
+        return;
-        }
     }
 
     if (!ARMING_FLAG(ARMED)) {
@@ -315,7 +328,7 @@ void processRx(timeUs_t currentTimeUs)
     // in 3D mode, we need to be able to disarm by switch at any time
     if (feature(FEATURE_3D)) {
         if (!IS_RC_MODE_ACTIVE(BOXARM))
-            mwDisarm();
+            disarm();
     }
 
     updateRSSI(currentTimeUs);
@@ -364,7 +377,7 @@ void processRx(timeUs_t currentTimeUs)
                     && (int32_t)(disarmAt - millis()) < 0
                 ) {
                     // auto-disarm configured and delay is over
-                    mwDisarm();
+                    disarm();
                     armedBeeperOn = false;
                 } else {
                     // still armed; do warning beeps while armed

src/main/fc/fc_core.h

@@ -40,11 +40,11 @@ union rollAndPitchTrims_u;
 void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
 void handleInflightCalibrationStickPosition();
 
-void mwDisarm(void);
+void disarm(void);
-void mwArm(void);
+void tryArm(void);
 
 void processRx(timeUs_t currentTimeUs);
-void updateLEDs(void);
+void updateArmingStatus(void);
 void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);

src/main/fc/fc_init.c

@@ -315,7 +315,7 @@ void init(void)
     }
 #endif
 
-#if defined(USE_SPEKTRUM_BIND) && !defined(SITL)
+#if defined(USE_SPEKTRUM_BIND)
     if (feature(FEATURE_RX_SERIAL)) {
         switch (rxConfig()->serialrx_provider) {
         case SERIALRX_SPEKTRUM1024:
@@ -338,7 +338,7 @@ void init(void)
     busSwitchInit();
 #endif
 
-#if defined(USE_UART) && !defined(SITL)
+#if defined(USE_UART)
     uartPinConfigure(serialPinConfig());
 #endif
 
@@ -384,12 +384,12 @@ void init(void)
     if (0) {}
 #if defined(USE_PPM)
     else if (feature(FEATURE_RX_PPM)) {
-          ppmRxInit(ppmConfig());
+        ppmRxInit(ppmConfig());
     }
 #endif
 #if defined(USE_PWM)
     else if (feature(FEATURE_RX_PARALLEL_PWM)) {
-          pwmRxInit(pwmConfig());
+        pwmRxInit(pwmConfig());
     }
 #endif
 
@@ -480,8 +480,9 @@ void init(void)
     initBoardAlignment(boardAlignment());
 
     if (!sensorsAutodetect()) {
-        // if gyro was not detected due to whatever reason, we give up now.
+        // if gyro was not detected due to whatever reason, notify and don't arm.
-        failureMode(FAILURE_MISSING_ACC);
+        failureLedCode(FAILURE_MISSING_ACC, 2);
+        setArmingDisabled(ARMING_DISABLED_NO_GYRO);
     }
 
     systemState |= SYSTEM_STATE_SENSORS_READY;
@@ -665,7 +666,6 @@ void init(void)
     timerStart();
 
     ENABLE_STATE(SMALL_ANGLE);
-    DISABLE_ARMING_FLAG(PREVENT_ARMING);
 
 #ifdef SOFTSERIAL_LOOPBACK
     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly

src/main/fc/fc_msp.c

@@ -912,7 +912,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 #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);
+            sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4 | sensors(SENSOR_GYRO) << 5);
             sbufWriteData(dst, &flightModeFlags, 4);        // unconditional part of flags, first 32 bits
             sbufWriteU8(dst, getCurrentPidProfileIndex());
             sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));

src/main/fc/fc_tasks.c

@@ -151,7 +151,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
     updateRcCommands();
 #endif
-    updateLEDs();
+    updateArmingStatus();
 
 #ifdef BARO
     if (sensors(SENSOR_BARO)) {
@@ -259,8 +259,11 @@ void osdSlaveTasksInit(void)
 void fcTasksInit(void)
 {
     schedulerInit();
-    rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
+
-    setTaskEnabled(TASK_GYROPID, true);
+    if (sensors(SENSOR_GYRO)) {
+        rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
+        setTaskEnabled(TASK_GYROPID, true);
+    }
 
     if (sensors(SENSOR_ACC)) {
         setTaskEnabled(TASK_ACCEL, true);

src/main/fc/rc_controls.c

@@ -144,11 +144,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
         if (IS_RC_MODE_ACTIVE(BOXARM)) {
             rcDisarmTicks = 0;
             // Arming via ARM BOX
-            if (throttleStatus == THROTTLE_LOW) {
+            tryArm();
-                if (ARMING_FLAG(OK_TO_ARM)) {
-                    mwArm();
-                }
-            }
         } else {
             // Disarming via ARM BOX
 
@@ -156,9 +152,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
                 rcDisarmTicks++;
                 if (rcDisarmTicks > 3) {
                     if (armingConfig()->disarm_kill_switch) {
-                        mwDisarm();
+                        disarm();
                     } else if (throttleStatus == THROTTLE_LOW) {
-                        mwDisarm();
+                        disarm();
                     }
                 }
             }
@@ -173,7 +169,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
         // Disarm on throttle down + yaw
         if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
             if (ARMING_FLAG(ARMED))
-                mwDisarm();
+                disarm();
             else {
                 beeper(BEEPER_DISARM_REPEAT);    // sound tone while stick held
                 rcDelayCommand = 0;              // reset so disarm tone will repeat
@@ -233,7 +229,8 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 
         if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
             // Arm via YAW
-            mwArm();
+            tryArm();
+
             return;
         }
     }

src/main/fc/runtime_config.c

@@ -29,6 +29,28 @@ uint16_t flightModeFlags = 0;
 
 static uint32_t enabledSensors = 0;
 
+static armingDisableFlags_e armingDisableFlags = 0;
+
+void setArmingDisabled(armingDisableFlags_e flag)
+{
+    armingDisableFlags = armingDisableFlags | flag;
+}
+
+void unsetArmingDisabled(armingDisableFlags_e flag)
+{
+    armingDisableFlags = armingDisableFlags & ~flag;
+}
+
+bool isArmingDisabled()
+{
+    return armingDisableFlags;
+}
+
+armingDisableFlags_e getArmingDisableFlags(void)
+{
+    return armingDisableFlags;
+}
+
 /**
  * Enables the given flight mode.  A beep is sounded if the flight mode
  * has changed.  Returns the new 'flightModeFlags' value.

src/main/fc/runtime_config.h

@@ -19,10 +19,8 @@
 
 // FIXME some of these are flight modes, some of these are general status indicators
 typedef enum {
-    OK_TO_ARM       = (1 << 0),
+    ARMED           = (1 << 0),
-    PREVENT_ARMING  = (1 << 1),
+    WAS_EVER_ARMED  = (1 << 1)
-    ARMED           = (1 << 2),
-    WAS_EVER_ARMED  = (1 << 3)
 } armingFlag_e;
 
 extern uint8_t armingFlags;
@@ -31,6 +29,29 @@ extern uint8_t armingFlags;
 #define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask))
 #define ARMING_FLAG(mask) (armingFlags & (mask))
 
+/*
+ * Arming disable flags are listed in the order of criticalness.
+ * (Beeper code can notify the most critical reason.)
+ */
+typedef enum {
+    ARMING_DISABLED_NO_GYRO     = (1 << 0),
+    ARMING_DISABLED_FAILSAFE    = (1 << 1),
+    ARMING_DISABLED_BOXFAILSAFE = (1 << 2),
+    ARMING_DISABLED_THROTTLE    = (1 << 3),
+    ARMING_DISABLED_ANGLE       = (1 << 4),
+    ARMING_DISABLED_LOAD        = (1 << 5),
+    ARMING_DISABLED_CALIBRATING = (1 << 6),
+    ARMING_DISABLED_CLI         = (1 << 7),
+    ARMING_DISABLED_CMS_MENU    = (1 << 8),
+    ARMING_DISABLED_OSD_MENU    = (1 << 9),
+    ARMING_DISABLED_BST         = (1 << 10),
+} armingDisableFlags_e;
+
+void setArmingDisabled(armingDisableFlags_e flag);
+void unsetArmingDisabled(armingDisableFlags_e flag);
+bool isArmingDisabled(void);
+armingDisableFlags_e getArmingDisableFlags(void);
+
 typedef enum {
     ANGLE_MODE      = (1 << 0),
     HORIZON_MODE    = (1 << 1),

src/main/fc/settings.c

@@ -74,13 +74,19 @@
 #include "telemetry/frsky.h"
 #include "telemetry/telemetry.h"
 
-
 // Sensor names (used in lookup tables for *_hardware settings and in status command output)
 // sync with accelerationSensor_e
 const char * const lookupTableAccHardware[] = {
     "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
     "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20689", "BMI160", "FAKE"
 };
+
+// sync with gyroSensor_e
+const char * const lookupTableGyroHardware[] = {
+    "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
+    "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE"
+};
+
 #if defined(USE_SENSOR_NAMES) || defined(BARO)
 // sync with baroSensor_e
 const char * const lookupTableBaroHardware[] = {
@@ -272,6 +278,7 @@ const lookupTableEntry_t lookupTables[] = {
     { lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) },
 #endif
     { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
+    { lookupTableGyroHardware, sizeof(lookupTableGyroHardware) / sizeof(char *) },
     { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) },
 #ifdef BARO
     { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },

src/main/fc/settings.h

@@ -41,6 +41,7 @@ typedef enum {
     TABLE_RX_SPI,
 #endif
     TABLE_GYRO_LPF,
+    TABLE_GYRO_HARDWARE,
     TABLE_ACC_HARDWARE,
 #ifdef BARO
     TABLE_BARO_HARDWARE,
@@ -130,6 +131,8 @@ extern const uint16_t valueTableEntryCount;
 extern const clivalue_t valueTable[];
 //extern const uint8_t lookupTablesEntryCount;
 
+extern const char * const lookupTableGyroHardware[];
+
 extern const char * const lookupTableAccHardware[];
 //extern const uint8_t lookupTableAccHardwareEntryCount;
 

src/main/flight/failsafe.c

@@ -30,6 +30,7 @@
 #include "drivers/time.h"
 
 #include "fc/config.h"
+#include "fc/fc_core.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -261,8 +262,8 @@ void failsafeUpdateState(void)
                 break;
 
             case FAILSAFE_LANDED:
-                ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link
+                setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
-                mwDisarm();
+                disarm();
                 failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
                 failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
                 reprocessState = true;
@@ -274,7 +275,7 @@ void failsafeUpdateState(void)
                     if (millis() > failsafeState.receivingRxDataPeriod) {
                         // rx link is good now, when arming via ARM switch, it must be OFF first
                         if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
-                            DISABLE_ARMING_FLAG(PREVENT_ARMING);
+                            unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
                             failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                             reprocessState = true;
                         }

src/main/flight/navigation.c

@@ -107,19 +107,22 @@ void navigationInit(void)
 #define GPS_FILTERING              1    // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
 #define GPS_LOW_SPEED_D_FILTER     1    // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
 
-static bool check_missed_wp(void);
 static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
 //static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
 static void GPS_calc_longitude_scaling(int32_t lat);
 static void GPS_calc_velocity(void);
 static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
+
+#ifdef USE_NAV
+static bool check_missed_wp(void);
 static void GPS_calc_poshold(void);
 static void GPS_calc_nav_rate(uint16_t max_speed);
 static void GPS_update_crosstrack(void);
 static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
+static int32_t wrap_36000(int32_t angle);
+#endif
 
 static int32_t wrap_18000(int32_t error);
-static int32_t wrap_36000(int32_t angle);
 
 typedef struct {
     int16_t last_velocity;
@@ -134,7 +137,6 @@ typedef struct {
 
 static PID_PARAM posholdPID_PARAM;
 static PID_PARAM poshold_ratePID_PARAM;
-static PID_PARAM navPID_PARAM;
 
 typedef struct {
     float integrator;          // integrator value
@@ -146,6 +148,9 @@ typedef struct {
 
 static PID posholdPID[2];
 static PID poshold_ratePID[2];
+
+#ifdef USE_NAV
+static PID_PARAM navPID_PARAM;
 static PID navPID[2];
 
 static int32_t get_P(int32_t error, PID_PARAM *pid)
@@ -176,6 +181,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
     // add in derivative component
     return pid_param->kD * pid->derivative;
 }
+#endif
 
 static void reset_PID(PID *pid)
 {
@@ -197,11 +203,15 @@ static void reset_PID(PID *pid)
 static float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
 static int16_t actual_speed[2] = { 0, 0 };
 static float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
+static int32_t error[2];
 
+#ifdef USE_NAV
 // The difference between the desired rate of travel and the actual rate of travel
 // updated after GPS read - 5-10hz
 static int16_t rate_error[2];
-static int32_t error[2];
+// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
+static int16_t crosstrack_error;
+#endif
 
 // Currently used WP
 static int32_t GPS_WP[2];
@@ -217,8 +227,6 @@ static int32_t target_bearing;
 // deg * 100, The original angle to the next_WP when the next_WP was set
 // Also used to check when we pass a WP
 static int32_t original_target_bearing;
-// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
-static int16_t crosstrack_error;
 ////////////////////////////////////////////////////////////////////////////////
 // The location of the copter in relation to home, updated every GPS read (1deg - 100)
 //static int32_t home_to_copter_bearing;
@@ -265,10 +273,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
 
 void onGpsNewData(void)
 {
-    int axis;
     static uint32_t nav_loopTimer;
-    uint16_t speed;
-
 
     if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
         return;
@@ -283,7 +288,7 @@ void onGpsNewData(void)
     // Apply moving average filter to GPS data
 #if defined(GPS_FILTERING)
     GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
-    for (axis = 0; axis < 2; axis++) {
+    for (int axis = 0; axis < 2; axis++) {
         GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
         GPS_degree[axis] = GPS_read[axis] / 10000000;   // get the degree to assure the sum fits to the int32_t
 
@@ -321,6 +326,7 @@ void onGpsNewData(void)
     // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
     GPS_calc_velocity();
 
+#ifdef USE_NAV
     if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
         // we are navigating
 
@@ -328,6 +334,7 @@ void onGpsNewData(void)
         GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
         GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
 
+        uint16_t speed;
         switch (nav_mode) {
         case NAV_MODE_POSHOLD:
             // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
@@ -360,6 +367,7 @@ void onGpsNewData(void)
             break;
         }
     }                   //end of gps calcs
+#endif
 }
 
 void GPS_reset_home_position(void)
@@ -385,7 +393,9 @@ void GPS_reset_nav(void)
         nav[i] = 0;
         reset_PID(&posholdPID[i]);
         reset_PID(&poshold_ratePID[i]);
+#ifdef USE_NAV
         reset_PID(&navPID[i]);
+#endif
     }
 }
 
@@ -401,10 +411,12 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
     poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
     poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 
+#ifdef USE_NAV
     navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
     navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f;
     navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
     navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
+#endif
 }
 
 // OK here is the onboard GPS code
@@ -442,6 +454,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
     waypoint_speed_gov = navigationConfig()->nav_speed_min;
 }
 
+#ifdef USE_NAV
 ////////////////////////////////////////////////////////////////////////////////////
 // Check if we missed the destination somehow
 //
@@ -452,6 +465,7 @@ static bool check_missed_wp(void)
     temp = wrap_18000(temp);
     return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
 }
+#endif
 
 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
 #define TAN_89_99_DEGREES 5729.57795f
@@ -522,6 +536,7 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in
     error[LAT] = *target_lat - *gps_lat;        // Y Error
 }
 
+#ifdef USE_NAV
 ////////////////////////////////////////////////////////////////////////////////////
 // Calculate nav_lat and nav_lon from the x and y error and the speed
 //
@@ -627,6 +642,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
     }
     return max_speed;
 }
+#endif
 
 ////////////////////////////////////////////////////////////////////////////////////
 // Utilities
@@ -640,6 +656,7 @@ static int32_t wrap_18000(int32_t error)
     return error;
 }
 
+#ifdef USE_NAV
 static int32_t wrap_36000(int32_t angle)
 {
     if (angle > 36000)
@@ -648,6 +665,7 @@ static int32_t wrap_36000(int32_t angle)
         angle += 36000;
     return angle;
 }
+#endif
 
 void updateGpsStateForHomeAndHoldMode(void)
 {

src/main/io/asyncfatfs/asyncfatfs.c

@@ -1639,7 +1639,7 @@ static afatfsOperationStatus_e afatfs_appendSuperclusterContinue(afatfsFile_t *f
 {
     afatfsAppendSupercluster_t *opState = &file->operation.state.appendSupercluster;
 
-    afatfsOperationStatus_e status;
+    afatfsOperationStatus_e status = AFATFS_OPERATION_FAILURE;
 
     doMore:
     switch (opState->phase) {
@@ -2387,7 +2387,7 @@ static afatfsFilePtr_t afatfs_allocateFileHandle()
 static afatfsOperationStatus_e afatfs_ftruncateContinue(afatfsFilePtr_t file, bool markDeleted)
 {
     afatfsTruncateFile_t *opState = &file->operation.state.truncateFile;
-    afatfsOperationStatus_e status;
+    afatfsOperationStatus_e status = AFATFS_OPERATION_FAILURE;
 
 #ifdef AFATFS_USE_FREEFILE
     uint32_t oldFreeFileStart, freeFileGrow;

src/main/io/ledstrip.c

@@ -541,7 +541,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
                 warningFlags |= 1 << WARNING_LOW_BATTERY;
             if (feature(FEATURE_FAILSAFE) && failsafeIsActive())
                 warningFlags |= 1 << WARNING_FAILSAFE;
-            if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM))
+            if (!ARMING_FLAG(ARMED) && isArmingDisabled())
                 warningFlags |= 1 << WARNING_ARMING_DISABLED;
         }
         *timer += HZ_TO_US(10);

src/main/io/osd.c

@@ -1143,7 +1143,9 @@ void osdUpdate(timeUs_t currentTimeUs)
 #ifdef CMS
     // do not allow ARM if we are in menu
     if (displayIsGrabbed(osdDisplayPort)) {
-        DISABLE_ARMING_FLAG(OK_TO_ARM);
+        setArmingDisabled(ARMING_DISABLED_OSD_MENU);
+    } else {
+        unsetArmingDisabled(ARMING_DISABLED_OSD_MENU);
     }
 #endif
 }

src/main/io/transponder_ir.c

@@ -26,9 +26,11 @@
 #ifdef TRANSPONDER
 #include "build/build_config.h"
 
+#include "config/config_reset.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
+#include "drivers/timer.h"
 #include "drivers/transponder_ir.h"
 #include "drivers/system.h"
 #include "drivers/usb_io.h"
@@ -37,13 +39,24 @@
 
 #include "io/transponder_ir.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0);
+PG_REGISTER_WITH_RESET_FN(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0);
 
-PG_RESET_TEMPLATE(transponderConfig_t, transponderConfig,
+void pgResetFn_transponderConfig(transponderConfig_t *transponderConfig)
-    .provider = TRANSPONDER_ILAP,
+{
-    .reserved = 0,
+    RESET_CONFIG_2(transponderConfig_t, transponderConfig,
-    .data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0x0, 0x0, 0x0 }, // Note, this is NOT a valid transponder code, it's just for testing production hardware
+        .provider = TRANSPONDER_ILAP,
-);
+        .reserved = 0,
+        .data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0x0, 0x0, 0x0 }, // Note, this is NOT a valid transponder code, it's just for testing production hardware
+        .ioTag = IO_TAG_NONE
+    );
+
+    for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
+        if (timerHardware[i].usageFlags & TIM_USE_TRANSPONDER) {
+            transponderConfig->ioTag = timerHardware[i].tag;
+            break;
+        }
+    }
+}
 
 static bool transponderInitialised = false;
 static bool transponderRepeat = false;
@@ -95,7 +108,7 @@ void transponderUpdate(timeUs_t currentTimeUs)
 
 void transponderInit(void)
 {
-    transponderInitialised = transponderIrInit(transponderConfig()->provider);
+    transponderInitialised = transponderIrInit(transponderConfig()->ioTag, transponderConfig()->provider);
     if (!transponderInitialised) {
         return;
     }

src/main/io/transponder_ir.h

@@ -24,6 +24,7 @@ typedef struct transponderConfig_s {
     transponderProvider_e provider;
     uint8_t reserved;
     uint8_t data[9];
+    ioTag_t ioTag;
 } transponderConfig_t;
 
 typedef struct transponderRequirement_s {

src/main/rx/spektrum.c

@@ -212,7 +212,7 @@ void spektrumBind(rxConfig_t *rxConfig)
     }
 
     // Determine a pin to use
-    ioTag_t bindPin;
+    ioTag_t bindPin = IO_TAG_NONE;
 
     if (rxConfig->spektrum_bind_pin_override_ioTag) {
         bindPin = rxConfig->spektrum_bind_pin_override_ioTag;

src/main/sensors/acceleration.c

@@ -120,7 +120,7 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
 
 bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 {
-    accelerationSensor_e accHardware;
+    accelerationSensor_e accHardware = ACC_NONE;
 
 #ifdef USE_ACC_ADXL345
     drv_adxl345_config_t acc_params;

src/main/target/CJMCU/target.h

@@ -76,7 +76,7 @@
 #define USE_RX_NRF24
 #define USE_RX_CX10
 #define USE_RX_H8_3D
-#define USE_RX_INAV
+//#define USE_RX_INAV // Temporary disabled to make some room in flash
 #define USE_RX_SYMA
 #define USE_RX_V202
 //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -596,12 +596,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             isRebootScheduled = true;
             break;
         case BST_DISARM:
-            if (ARMING_FLAG(ARMED))
+            if (ARMING_FLAG(ARMED)) {
-                    mwDisarm();
+                    disarm();
-            ENABLE_ARMING_FLAG(PREVENT_ARMING);
+            }
+            setArmingDisabled(ARMING_DISABLED_BST);
             break;
         case BST_ENABLE_ARM:
-                DISABLE_ARMING_FLAG(PREVENT_ARMING);
+            unsetArmingDisabled(ARMING_DISABLED_BST);
             break;
         case BST_SET_DEADBAND:
             rcControlsConfigMutable()->alt_hold_deadband = bstRead8();

src/main/target/KAKUTEF4/target.h

@@ -62,16 +62,16 @@
 #define MAX7456_SPI_INSTANCE    SPI3
 #define MAX7456_SPI_CS_PIN      PB14
 
-#define MAX7456_DMA_CHANNEL_TX              DMA1_Stream5
+//#define MAX7456_DMA_CHANNEL_TX              DMA1_Stream5
-#define MAX7456_DMA_CHANNEL_RX              DMA1_Stream0
+//#define MAX7456_DMA_CHANNEL_RX              DMA1_Stream0
-#define MAX7456_DMA_IRQ_HANDLER_ID          DMA1_ST0_HANDLER
+//#define MAX7456_DMA_IRQ_HANDLER_ID          DMA1_ST0_HANDLER
 
 #define M25P16_CS_PIN           PB3
 #define M25P16_SPI_INSTANCE     SPI3
 
 #define USE_FLASHFS
 #define USE_FLASH_M25P16
-#define USE_FLASH_TOOLS
+//#define USE_FLASH_TOOLS
 
 #define USE_VCP
 #define VBUS_SENSING_PIN        PA8

src/main/target/OMNIBUSF4/target.h

@@ -178,10 +178,9 @@
 #define OLED_I2C_INSTANCE              (I2CDEV_3)
 
 #define USE_ADC
-#define CURRENT_METER_ADC_PIN   PC1
+#define CURRENT_METER_ADC_PIN   PC1  // Direct from CRNT pad (part of onboard sensor for Pro)
-#define VBAT_ADC_PIN            PC2
+#define VBAT_ADC_PIN            PC2  // 11:1 (10K + 1K) divider
-
+#define RSSI_ADC_PIN            PA0  // Direct from RSSI pad
-//#define RSSI_ADC_PIN          PA0
 
 #define TRANSPONDER
 

src/main/target/SITL/target.c

@@ -22,7 +22,6 @@
 
 #include <errno.h>
 #include <time.h>
-#include <pthread.h>
 
 #include "drivers/io.h"
 #include "drivers/dma.h"
@@ -43,6 +42,8 @@ const timerHardware_t timerHardware[1]; // unused
 #include "fc/config.h"
 #include "scheduler/scheduler.h"
 
+#include "rx/rx.h"
+
 #include "dyad.h"
 #include "target/SITL/udplink.h"
 
@@ -60,259 +61,270 @@ static pthread_mutex_t mainLoopLock;
 int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y);
 
 int lockMainPID(void) {
-	return pthread_mutex_trylock(&mainLoopLock);
+    return pthread_mutex_trylock(&mainLoopLock);
 }
 
 #define RAD2DEG (180.0 / M_PI)
 #define ACC_SCALE (256 / 9.80665)
 #define GYRO_SCALE (16.4)
 void sendMotorUpdate() {
-	udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
+    udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
 }
 void updateState(const fdm_packet* pkt) {
-	static double last_timestamp = 0; // in seconds
+    static double last_timestamp = 0; // in seconds
-	static uint64_t last_realtime = 0; // in uS
+    static uint64_t last_realtime = 0; // in uS
-	static struct timespec last_ts; // last packet
+    static struct timespec last_ts; // last packet
 
-	struct timespec now_ts;
+    struct timespec now_ts;
-	clock_gettime(CLOCK_MONOTONIC, &now_ts);
+    clock_gettime(CLOCK_MONOTONIC, &now_ts);
 
-	const uint64_t realtime_now = micros64_real();
+    const uint64_t realtime_now = micros64_real();
-	if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
+    if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
-		last_timestamp = pkt->timestamp;
+        last_timestamp = pkt->timestamp;
-		last_realtime = realtime_now;
+        last_realtime = realtime_now;
-		sendMotorUpdate();
+        sendMotorUpdate();
-		return;
+        return;
-	}
+    }
 
-	const double deltaSim = pkt->timestamp - last_timestamp;  // in seconds
+    const double deltaSim = pkt->timestamp - last_timestamp;  // in seconds
-	if(deltaSim < 0) { // don't use old packet
+    if(deltaSim < 0) { // don't use old packet
-		return;
+        return;
-	}
+    }
 
-	int16_t x,y,z;
+    int16_t x,y,z;
-	x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
+    x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
-	y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
+    y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
-	z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
+    z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
-	fakeAccSet(fakeAccDev, x, y, z);
+    fakeAccSet(fakeAccDev, x, y, z);
 //	printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
 
-	x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
+    x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
-	y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
+    y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
-	z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
+    z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
-	fakeGyroSet(fakeGyroDev, x, y, z);
+    fakeGyroSet(fakeGyroDev, x, y, z);
 //	printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
 
 #if defined(SKIP_IMU_CALC)
 #if defined(SET_IMU_FROM_EULER)
-	// set from Euler
+    // set from Euler
-	double qw = pkt->imu_orientation_quat[0];
+    double qw = pkt->imu_orientation_quat[0];
-	double qx = pkt->imu_orientation_quat[1];
+    double qx = pkt->imu_orientation_quat[1];
-	double qy = pkt->imu_orientation_quat[2];
+    double qy = pkt->imu_orientation_quat[2];
-	double qz = pkt->imu_orientation_quat[3];
+    double qz = pkt->imu_orientation_quat[3];
-	double ysqr = qy * qy;
+    double ysqr = qy * qy;
-	double xf, yf, zf;
+    double xf, yf, zf;
 
-	// roll (x-axis rotation)
+    // roll (x-axis rotation)
-	double t0 = +2.0 * (qw * qx + qy * qz);
+    double t0 = +2.0 * (qw * qx + qy * qz);
-	double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
+    double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
-	xf = atan2(t0, t1) * RAD2DEG;
+    xf = atan2(t0, t1) * RAD2DEG;
 
-	// pitch (y-axis rotation)
+    // pitch (y-axis rotation)
-	double t2 = +2.0 * (qw * qy - qz * qx);
+    double t2 = +2.0 * (qw * qy - qz * qx);
-	t2 = t2 > 1.0 ? 1.0 : t2;
+    t2 = t2 > 1.0 ? 1.0 : t2;
-	t2 = t2 < -1.0 ? -1.0 : t2;
+    t2 = t2 < -1.0 ? -1.0 : t2;
-	yf = asin(t2) * RAD2DEG; // from wiki
+    yf = asin(t2) * RAD2DEG; // from wiki
 
-	// yaw (z-axis rotation)
+    // yaw (z-axis rotation)
-	double t3 = +2.0 * (qw * qz + qx * qy);
+    double t3 = +2.0 * (qw * qz + qx * qy);
-	double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
+    double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
-	zf = atan2(t3, t4) * RAD2DEG;
+    zf = atan2(t3, t4) * RAD2DEG;
-	imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
+    imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
 #else
-	imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
+    imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
 #endif
 #endif
 
 #if defined(SIMULATOR_IMU_SYNC)
-	imuSetHasNewData(deltaSim*1e6);
+    imuSetHasNewData(deltaSim*1e6);
-	imuUpdateAttitude(micros());
+    imuUpdateAttitude(micros());
 #endif
 
 
-	if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
+    if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
 //		simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
-		struct timespec out_ts;
+        struct timespec out_ts;
-		timeval_sub(&out_ts, &now_ts, &last_ts);
+        timeval_sub(&out_ts, &now_ts, &last_ts);
-		simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
+        simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
-	}
+    }
 //	printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6);
 
-	last_timestamp = pkt->timestamp;
+    last_timestamp = pkt->timestamp;
-	last_realtime = micros64_real();
+    last_realtime = micros64_real();
 
-	last_ts.tv_sec = now_ts.tv_sec;
+    last_ts.tv_sec = now_ts.tv_sec;
-	last_ts.tv_nsec = now_ts.tv_nsec;
+    last_ts.tv_nsec = now_ts.tv_nsec;
 
-	pthread_mutex_unlock(&updateLock); // can send PWM output now
+    pthread_mutex_unlock(&updateLock); // can send PWM output now
 
 #if defined(SIMULATOR_GYROPID_SYNC)
-	pthread_mutex_unlock(&mainLoopLock); // can run main loop
+    pthread_mutex_unlock(&mainLoopLock); // can run main loop
 #endif
 }
 
 static void* udpThread(void* data) {
-	UNUSED(data);
+    UNUSED(data);
-	int n = 0;
+    int n = 0;
 
-	while (workerRunning) {
+    while (workerRunning) {
-		n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
+        n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
-		if(n == sizeof(fdm_packet)) {
+        if(n == sizeof(fdm_packet)) {
 //			printf("[data]new fdm %d\n", n);
-			updateState(&fdmPkt);
+            updateState(&fdmPkt);
-		}
+        }
-	}
+    }
 
-	printf("udpThread end!!\n");
+    printf("udpThread end!!\n");
-	return NULL;
+    return NULL;
 }
 
 static void* tcpThread(void* data) {
-	UNUSED(data);
+    UNUSED(data);
 
-	dyad_init();
+    dyad_init();
-	dyad_setTickInterval(0.2f);
+    dyad_setTickInterval(0.2f);
-	dyad_setUpdateTimeout(0.5f);
+    dyad_setUpdateTimeout(0.5f);
 
-	while (workerRunning) {
+    while (workerRunning) {
-		dyad_update();
+        dyad_update();
-	}
+    }
 
-	dyad_shutdown();
+    dyad_shutdown();
-	printf("tcpThread end!!\n");
+    printf("tcpThread end!!\n");
-	return NULL;
+    return NULL;
 }
 
 // system
 void systemInit(void) {
-	int ret;
+    int ret;
 
-	clock_gettime(CLOCK_MONOTONIC, &start_time);
+    clock_gettime(CLOCK_MONOTONIC, &start_time);
-	printf("[system]Init...\n");
+    printf("[system]Init...\n");
 
-	SystemCoreClock = 500 * 1e6; // fake 500MHz
+    SystemCoreClock = 500 * 1e6; // fake 500MHz
-	FLASH_Unlock();
+    FLASH_Unlock();
 
-	if (pthread_mutex_init(&updateLock, NULL) != 0) {
+    if (pthread_mutex_init(&updateLock, NULL) != 0) {
-		printf("Create updateLock error!\n");
+        printf("Create updateLock error!\n");
-		exit(1);
+        exit(1);
-	}
+    }
 
-	if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
+    if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
-		printf("Create mainLoopLock error!\n");
+        printf("Create mainLoopLock error!\n");
-		exit(1);
+        exit(1);
-	}
+    }
 
-	ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
+    ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
-	if(ret != 0) {
+    if(ret != 0) {
-		printf("Create tcpWorker error!\n");
+        printf("Create tcpWorker error!\n");
-		exit(1);
+        exit(1);
-	}
+    }
 
-	ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
+    ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
-	printf("init PwnOut UDP link...%d\n", ret);
+    printf("init PwnOut UDP link...%d\n", ret);
 
-	ret = udpInit(&stateLink, NULL, 9003, true);
+    ret = udpInit(&stateLink, NULL, 9003, true);
-	printf("start UDP server...%d\n", ret);
+    printf("start UDP server...%d\n", ret);
 
-	ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
+    ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
-	if(ret != 0) {
+    if(ret != 0) {
-		printf("Create udpWorker error!\n");
+        printf("Create udpWorker error!\n");
-		exit(1);
+        exit(1);
-	}
+    }
 
-	// serial can't been slow down
+    // serial can't been slow down
-	rescheduleTask(TASK_SERIAL, 1);
+    rescheduleTask(TASK_SERIAL, 1);
 }
 
 void systemReset(void){
-	printf("[system]Reset!\n");
+    printf("[system]Reset!\n");
-	workerRunning = false;
+    workerRunning = false;
-	pthread_join(tcpWorker, NULL);
+    pthread_join(tcpWorker, NULL);
-	pthread_join(udpWorker, NULL);
+    pthread_join(udpWorker, NULL);
-	exit(0);
+    exit(0);
 }
 void systemResetToBootloader(void) {
-	printf("[system]ResetToBootloader!\n");
+    printf("[system]ResetToBootloader!\n");
-	workerRunning = false;
+    workerRunning = false;
-	pthread_join(tcpWorker, NULL);
+    pthread_join(tcpWorker, NULL);
-	pthread_join(udpWorker, NULL);
+    pthread_join(udpWorker, NULL);
-	exit(0);
+    exit(0);
 }
 
 // drivers/light_led.c
 void ledInit(const statusLedConfig_t *statusLedConfig) {
-	UNUSED(statusLedConfig);
+    UNUSED(statusLedConfig);
-	printf("[led]Init...\n");
+    printf("[led]Init...\n");
-}
-void timerInit(void) {
-	printf("[timer]Init...\n");
-}
-void timerStart(void) {
-}
-void failureMode(failureMode_e mode) {
-	printf("[failureMode]!!! %d\n", mode);
-	while(1);
 }
 
+void timerInit(void) {
+    printf("[timer]Init...\n");
+}
+
+void timerStart(void) {
+}
+
+void failureMode(failureMode_e mode) {
+    printf("[failureMode]!!! %d\n", mode);
+    while(1);
+}
+
+void failureLedCode(failureMode_e mode, int repeatCount)
+{
+    UNUSED(repeatCount);
+    printf("Failure LED flash for: [failureMode]!!! %d\n", mode);	
+}
 
 // Time part
 // Thanks ArduPilot
 uint64_t nanos64_real() {
-	struct timespec ts;
+    struct timespec ts;
-	clock_gettime(CLOCK_MONOTONIC, &ts);
+    clock_gettime(CLOCK_MONOTONIC, &ts);
-	return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec);
+    return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec);
 }
+
 uint64_t micros64_real() {
-	struct timespec ts;
+    struct timespec ts;
-	clock_gettime(CLOCK_MONOTONIC, &ts);
+    clock_gettime(CLOCK_MONOTONIC, &ts);
-	return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
+    return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
 }
+
 uint64_t millis64_real() {
-	struct timespec ts;
+    struct timespec ts;
-	clock_gettime(CLOCK_MONOTONIC, &ts);
+    clock_gettime(CLOCK_MONOTONIC, &ts);
-	return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
+    return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
 }
 
 uint64_t micros64() {
-	static uint64_t last = 0;
+    static uint64_t last = 0;
-	static uint64_t out = 0;
+    static uint64_t out = 0;
-	uint64_t now = nanos64_real();
+    uint64_t now = nanos64_real();
 
-	out += (now - last) * simRate;
+    out += (now - last) * simRate;
-	last = now;
+    last = now;
 
-	return out*1e-3;
+    return out*1e-3;
 //	return micros64_real();
 }
+
 uint64_t millis64() {
-	static uint64_t last = 0;
+    static uint64_t last = 0;
-	static uint64_t out = 0;
+    static uint64_t out = 0;
-	uint64_t now = nanos64_real();
+    uint64_t now = nanos64_real();
 
-	out += (now - last) * simRate;
+    out += (now - last) * simRate;
-	last = now;
+    last = now;
 
-	return out*1e-6;
+    return out*1e-6;
 //	return millis64_real();
 }
 
 uint32_t micros(void) {
-	return micros64() & 0xFFFFFFFF;
+    return micros64() & 0xFFFFFFFF;
 }
 
 uint32_t millis(void) {
-	return millis64() & 0xFFFFFFFF;
+    return millis64() & 0xFFFFFFFF;
 }
 
 void microsleep(uint32_t usec) {
@@ -321,18 +333,21 @@ void microsleep(uint32_t usec) {
     ts.tv_nsec = usec*1000UL;
     while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
 }
-void delayMicroseconds(uint32_t us) {
-	microsleep(us / simRate);
-}
-void delayMicroseconds_real(uint32_t us) {
-	microsleep(us);
-}
-void delay(uint32_t ms) {
-	uint64_t start = millis64();
 
-	while ((millis64() - start) < ms) {
+void delayMicroseconds(uint32_t us) {
-		microsleep(1000);
+    microsleep(us / simRate);
-	}
+}
+
+void delayMicroseconds_real(uint32_t us) {
+    microsleep(us);
+}
+
+void delay(uint32_t ms) {
+    uint64_t start = millis64();
+
+    while ((millis64() - start) < ms) {
+        microsleep(1000);
+    }
 }
 
 // Subtract the ‘struct timespec’ values X and Y,  storing the result in RESULT.
@@ -340,21 +355,21 @@ void delay(uint32_t ms) {
 // result = x - y
 // from: http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
 int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y) {
-	unsigned int s_carry = 0;
+    unsigned int s_carry = 0;
-	unsigned int ns_carry = 0;
+    unsigned int ns_carry = 0;
-	// Perform the carry for the later subtraction by updating y.
+    // Perform the carry for the later subtraction by updating y.
-	if (x->tv_nsec < y->tv_nsec) {
+    if (x->tv_nsec < y->tv_nsec) {
-		int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
+        int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
-		ns_carry += 1000000000 * nsec;
+        ns_carry += 1000000000 * nsec;
-		s_carry += nsec;
+        s_carry += nsec;
-	}
+    }
 
-	// Compute the time remaining to wait. tv_usec is certainly positive.
+    // Compute the time remaining to wait. tv_usec is certainly positive.
-	result->tv_sec = x->tv_sec - y->tv_sec - s_carry;
+    result->tv_sec = x->tv_sec - y->tv_sec - s_carry;
-	result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry;
+    result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry;
 
-	// Return 1 if result is negative.
+    // Return 1 if result is negative.
-	return x->tv_sec < y->tv_sec;
+    return x->tv_sec < y->tv_sec;
 }
 
 
@@ -369,68 +384,75 @@ static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
 static int16_t idlePulse;
 
 void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
-	UNUSED(motorConfig);
+    UNUSED(motorConfig);
-	UNUSED(motorCount);
+    UNUSED(motorCount);
 
-	idlePulse = _idlePulse;
+    idlePulse = _idlePulse;
 
-	for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
+    for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
-		motors[motorIndex].enabled = true;
+        motors[motorIndex].enabled = true;
-	}
+    }
-	pwmMotorsEnabled = true;
+    pwmMotorsEnabled = true;
 }
+
 void servoDevInit(const servoDevConfig_t *servoConfig) {
-	UNUSED(servoConfig);
+    UNUSED(servoConfig);
-	for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
+    for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
-		servos[servoIndex].enabled = true;
+        servos[servoIndex].enabled = true;
-	}
+    }
 }
 
 pwmOutputPort_t *pwmGetMotors(void) {
-	return motors;
+    return motors;
 }
+
 bool pwmAreMotorsEnabled(void) {
-	return pwmMotorsEnabled;
+    return pwmMotorsEnabled;
 }
+
 bool isMotorProtocolDshot(void)
 {
     return false;
 }
+
 void pwmWriteMotor(uint8_t index, float value) {
-	motorsPwm[index] = value - idlePulse;
+    motorsPwm[index] = value - idlePulse;
 }
+
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
-	UNUSED(motorCount);
+    UNUSED(motorCount);
-	pwmMotorsEnabled = false;
+    pwmMotorsEnabled = false;
 }
+
 void pwmCompleteMotorUpdate(uint8_t motorCount) {
-	UNUSED(motorCount);
+    UNUSED(motorCount);
-	// send to simulator
+    // send to simulator
-	// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
+    // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
 
-	double outScale = 1000.0;
+    double outScale = 1000.0;
-	if (feature(FEATURE_3D)) {
+    if (feature(FEATURE_3D)) {
-		outScale = 500.0;
+        outScale = 500.0;
-	}
+    }
 
-	pwmPkt.motor_speed[3] = motorsPwm[0] / outScale;
+    pwmPkt.motor_speed[3] = motorsPwm[0] / outScale;
-	pwmPkt.motor_speed[0] = motorsPwm[1] / outScale;
+    pwmPkt.motor_speed[0] = motorsPwm[1] / outScale;
-	pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
+    pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
-	pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
+    pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
 
-	// get one "fdm_packet" can only send one "servo_packet"!!
+    // get one "fdm_packet" can only send one "servo_packet"!!
-	if(pthread_mutex_trylock(&updateLock) != 0) return;
+    if(pthread_mutex_trylock(&updateLock) != 0) return;
-	udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
+    udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
 //	printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
 }
+
 void pwmWriteServo(uint8_t index, float value) {
-	servosPwm[index] = value;
+    servosPwm[index] = value;
 }
 
 // ADC part
 uint16_t adcGetChannel(uint8_t channel) {
-	UNUSED(channel);
+    UNUSED(channel);
-	return 0;
+    return 0;
 }
 
 // stack part
@@ -438,62 +460,73 @@ char _estack;
 char _Min_Stack_Size;
 
 // fake EEPROM
-extern uint8_t __config_start;
+uint8_t __config_start;
-extern uint8_t __config_end;
+uint8_t __config_end;
 static FILE *eepromFd = NULL;
 
 void FLASH_Unlock(void) {
-	uint8_t * const eeprom = &__config_start;
+    uint8_t * const eeprom = &__config_start;
 
-	if (eepromFd != NULL) {
+    if (eepromFd != NULL) {
-		printf("[FLASH_Unlock] eepromFd != NULL\n");
+        printf("[FLASH_Unlock] eepromFd != NULL\n");
-		return;
+        return;
-	}
+    }
 
-	// open or create
+    // open or create
-	eepromFd = fopen(EEPROM_FILENAME,"r+");
+    eepromFd = fopen(EEPROM_FILENAME,"r+");
-	if (eepromFd != NULL) {
+    if (eepromFd != NULL) {
-		// obtain file size:
+        // obtain file size:
-		fseek(eepromFd , 0 , SEEK_END);
+        fseek(eepromFd , 0 , SEEK_END);
-		long lSize = ftell(eepromFd);
+        long lSize = ftell(eepromFd);
-		rewind(eepromFd);
+        rewind(eepromFd);
 
-		printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start));
+        printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (long)(&__config_end - &__config_start));
-		for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
+        for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
-		        int c = fgetc(eepromFd);
+                int c = fgetc(eepromFd);
-			if(c == EOF) break;
+            if(c == EOF) break;
-			eeprom[i] = (uint8_t)c;
+            eeprom[i] = (uint8_t)c;
-		}
+        }
-	} else {
+    } else {
-		eepromFd = fopen(EEPROM_FILENAME, "w+");
+        eepromFd = fopen(EEPROM_FILENAME, "w+");
-		fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
+        fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
-	}
+    }
 }
 
 void FLASH_Lock(void) {
-	// flush & close
+    // flush & close
-	if (eepromFd != NULL) {
+    if (eepromFd != NULL) {
-		const uint8_t *eeprom = &__config_start;
+        const uint8_t *eeprom = &__config_start;
-		fseek(eepromFd, 0, SEEK_SET);
+        fseek(eepromFd, 0, SEEK_SET);
-		fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
+        fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
-		fclose(eepromFd);
+        fclose(eepromFd);
-		eepromFd = NULL;
+        eepromFd = NULL;
-	}
+    }
 }
 
 FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) {
-	UNUSED(Page_Address);
+    UNUSED(Page_Address);
 //	printf("[FLASH_ErasePage]%x\n", Page_Address);
-	return FLASH_COMPLETE;
+    return FLASH_COMPLETE;
 }
+
 FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) {
-	if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
+    if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
-		*((uint32_t*)addr) = Data;
+        *((uint32_t*)addr) = Data;
-		printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
+        printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
-	} else {
+    } else {
-	        printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
+            printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
-	}
+    }
-	return FLASH_COMPLETE;
+    return FLASH_COMPLETE;
 }
 
+void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig)
+{
+    UNUSED(pSerialPinConfig);
+    printf("uartPinConfigure");
+}
 
+void spektrumBind(rxConfig_t *rxConfig)
+{
+    UNUSED(rxConfig);
+    printf("spektrumBind");
+}

src/main/target/SPRACINGF4EVO/target.h

@@ -84,9 +84,8 @@
 #define UART5_TX_PIN            PC12
 #define UART5_RX_PIN            PD2
 
-// TODO
+#define USE_ESCSERIAL
-// #define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
-// #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
 
 #define USE_I2C
 #define USE_I2C_DEVICE_1

src/main/target/common_fc_pre.h

@@ -115,6 +115,7 @@
 #define TELEMETRY_SRXL
 #define USE_DASHBOARD
 #define USE_MSP_DISPLAYPORT
+#define USE_RCSPLIT
 #define USE_RX_MSP
 #define USE_SERIALRX_JETIEXBUS
 #define USE_SENSOR_NAMES
@@ -131,10 +132,8 @@
 #endif
 
 #if (FLASH_SIZE > 256)
-// Temporarily moved this here because of overflowing flash size on F3
+// Temporarily moved GPS here because of overflowing flash size on F3
 #define GPS
-
+#define USE_NAV
 #define USE_UNCOMMON_MIXERS
 #endif
-
-#define USE_RCSPLIT

src/main/telemetry/smartport.c

@@ -698,10 +698,11 @@ void handleSmartPortTelemetry(void)
                 // the Taranis seems to be able to fit 5 digits on the screen
                 // the Taranis seems to consider this number a signed 16 bit integer
 
-                if (ARMING_FLAG(OK_TO_ARM))
+                if (!isArmingDisabled()) {
                     tmpi += 1;
-                if (ARMING_FLAG(PREVENT_ARMING))
+                } else {
                     tmpi += 2;
+                }
                 if (ARMING_FLAG(ARMED))
                     tmpi += 4;
 

src/test/Makefile

@@ -70,6 +70,7 @@ encoding_unittest_SRC := \
 flight_failsafe_unittest_SRC := \
 		$(USER_DIR)/common/bitarray.c \
 		$(USER_DIR)/fc/rc_modes.c \
+		$(USER_DIR)/fc/runtime_config.c \
 		$(USER_DIR)/flight/failsafe.c
 
 

src/test/unit/cms_unittest.cc

@@ -30,6 +30,7 @@ extern "C" {
     #include "target.h"
     #include "cms/cms.h"
     #include "cms/cms_types.h"
+    #include "fc/runtime_config.h"
     void cmsMenuOpen(void);
     long cmsMenuBack(displayPort_t *pDisplay);
     uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key);
@@ -141,4 +142,6 @@ void saveConfigAndNotify(void) {}
 void stopMotors(void) {}
 void stopPwmAllMotors(void) {}
 void systemReset(void) {}
+void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
+void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
 }

src/test/unit/flight_failsafe_unittest.cc

@@ -49,7 +49,6 @@ extern "C" {
 #include "gtest/gtest.h"
 
 uint32_t testFeatureMask = 0;
-uint16_t flightModeFlags = 0;
 uint16_t testMinThrottle = 0;
 throttleStatus_e throttleStatus = THROTTLE_HIGH;
 
@@ -203,7 +202,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
     EXPECT_EQ(true, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
-    EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_TRUE(isArmingDisabled());
 
     // given
     failsafeOnValidDataFailed();                    // set last invalid sample at current time
@@ -217,7 +216,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
     EXPECT_EQ(true, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
-    EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_TRUE(isArmingDisabled());
 
     // given
     sysTickUptime += PERIOD_OF_30_SECONDS + 1;      // adjust time to point just past the required additional recovery time
@@ -230,7 +229,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
     EXPECT_EQ(false, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
-    EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_FALSE(isArmingDisabled());
 }
 
 /****************************************************************************************/
@@ -269,7 +268,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
     EXPECT_EQ(true, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
-    EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_TRUE(isArmingDisabled());
 
     // given
     failsafeOnValidDataFailed();                    // set last invalid sample at current time
@@ -283,7 +282,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
     EXPECT_EQ(true, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
-    EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_TRUE(isArmingDisabled());
 
     // given
     sysTickUptime += PERIOD_OF_3_SECONDS + 1;       // adjust time to point just past the required additional recovery time
@@ -296,7 +295,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
     EXPECT_EQ(false, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
-    EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_FALSE(isArmingDisabled());
 }
 
 /****************************************************************************************/
@@ -325,7 +324,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
 
     // then
     EXPECT_EQ(true, failsafeIsActive());
-    EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_TRUE(isArmingDisabled());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 
@@ -342,7 +341,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
 
     // then
     EXPECT_EQ(true, failsafeIsActive());
-    EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_TRUE(isArmingDisabled());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
     EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
 
@@ -357,7 +356,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
     EXPECT_EQ(false, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));  // disarm not called repeatedly.
-    EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_FALSE(isArmingDisabled());
 }
 
 /****************************************************************************************/
@@ -406,14 +405,13 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
     EXPECT_EQ(false, failsafeIsActive());
     EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
     EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
-    EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
+    EXPECT_FALSE(isArmingDisabled());
 }
 
 // STUBS
 
 extern "C" {
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
-uint8_t armingFlags;
 float rcCommand[4];
 int16_t debug[DEBUG16_VALUE_COUNT];
 bool isUsingSticksToArm = true;
@@ -437,7 +435,7 @@ bool feature(uint32_t mask) {
     return (mask & testFeatureMask);
 }
 
-void mwDisarm(void) {
+void disarm(void) {
     callCounts[COUNTER_MW_DISARM]++;
 }
 
@@ -445,18 +443,6 @@ void beeper(beeperMode_e mode) {
     UNUSED(mode);
 }
 
-uint16_t enableFlightMode(flightModeFlags_e mask)
-{
-    flightModeFlags |= (mask);
-    return flightModeFlags;
-}
-
-uint16_t disableFlightMode(flightModeFlags_e mask)
-{
-    flightModeFlags &= ~(mask);
-    return flightModeFlags;
-}
-
 uint16_t getCurrentMinthrottle(void)
 {
     return testMinThrottle;
@@ -467,4 +453,5 @@ bool isUsingSticksForArming(void)
     return isUsingSticksToArm;
 }
 
+void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
 }

src/test/unit/ledstrip_unittest.cc

@@ -382,4 +382,6 @@ bool sensors(uint32_t mask)
 
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {};
 
+bool isArmingDisabled(void) { return false; }
+
 }

src/test/unit/osd_unittest.cc

@@ -574,4 +574,7 @@ extern "C" {
         UNUSED(pDisplay);
         return false;
     }
+
+    void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
+    void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
 }

src/test/unit/rc_controls_unittest.cc

@@ -679,8 +679,8 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
 void handleInflightCalibrationStickPosition(void) {}
 bool feature(uint32_t) { return false;}
 bool sensors(uint32_t) { return false;}
-void mwArm(void) {}
+void tryArm(void) {}
-void mwDisarm(void) {}
+void disarm(void) {}
 void dashboardDisablePageCycling() {}
 void dashboardEnablePageCycling() {}