diff --git a/.gitignore b/.gitignore index ef7592b64..d5ba321dd 100644 --- a/.gitignore +++ b/.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 -/.vscode/ +# artefacts for Visual Studio Code +/.vscode -# Artefacts for CLion -/cmake-build-debug/ +# artefacts for CLion +/cmake-build-debug /CMakeLists.txt diff --git a/.travis.yml b/.travis.yml index d22f1ef08..4c9ca9872 100644 --- a/.travis.yml +++ b/.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 diff --git a/Makefile b/Makefile index c8deb61ba..35c83fa98 100644 --- a/Makefile +++ b/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: diff --git a/README.md b/README.md index 06b2741d0..2fa791e7e 100644 --- a/README.md +++ b/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. diff --git a/make/tools.mk b/make/tools.mk index c3f251404..86f3fbeeb 100644 --- a/make/tools.mk +++ b/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)) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 0964acca9..6df56ffef 100644 --- a/src/main/cms/cms.c +++ b/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); diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 35e80f206..971deffc0 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/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.writeFn = mpu6000SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + 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.writeFn = mpu6500SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + 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.slowreadFn = mpu9250SpiSlowReadRegister; - gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister; - gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiWriteRegister; 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.writeFn = icm20689SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + 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.writeFn = bmi160SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 07910797a..2da4ce109 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/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; diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index 835ab25b2..ee3077421 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/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); - spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response - DISABLE_BMI160(acc->bus.spi.csnPin); + IOLo(acc->bus.spi.csnPin); + spiTransfer(acc->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response + 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); - spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response - DISABLE_BMI160(gyro->bus.spi.csnPin); + IOLo(gyro->bus.spi.csnPin); + spiTransfer(gyro->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response + 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]); diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.h b/src/main/drivers/accgyro/accgyro_spi_bmi160.h index 1c51c23c9..ad5c6e1e2 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.h +++ b/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); diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index cb73bf431..762ac1a61 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/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); - - icm20689SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); - if (tmp == ICM20689_WHO_AM_I_CONST) { + const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I); + if (whoAmI == 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) diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.h b/src/main/drivers/accgyro/accgyro_spi_icm20689.h index 9eeb0c24a..164f51dec 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.h +++ b/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); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 3ce84e4a5..94a9e2e56 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/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); - if (in == MPU6000_WHO_AM_I_CONST) { + const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I); + 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) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - return true; + switch (productID) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + case MPU6000ES_REV_D6: + case MPU6000ES_REV_D7: + case MPU6000ES_REV_D8: + case MPU6000_REV_D6: + case MPU6000_REV_D7: + case MPU6000_REV_D8: + case MPU6000_REV_D9: + case MPU6000_REV_D10: + 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; diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h index 7eb9b6918..8435cebad 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h +++ b/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); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 1afecf509..8f2f4db21 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/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; - mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); + const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I); 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); } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro/accgyro_spi_mpu6500.h index a23d2607c..40cef8b3c 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h +++ b/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); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 7467bf8a6..1df220046 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/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) -#define ENABLE_MPU9250(spiCsnPin) IOLo(spiCsnPin) +bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + 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); - 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_ACCEL_CONFIG, INV_FSR_8G << 3); + 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); - - mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); + const uint8_t in = spiReadRegister(bus, MPU_RA_WHO_AM_I); 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; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index 4518bd8e2..a9d9b1b0d 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/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 mpu9250SpiReadRegister(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); +bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index e8c143601..a1367d598 100644 --- a/src/main/drivers/bus.h +++ b/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 { diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index abec322e8..009a424bc 100644 --- a/src/main/drivers/bus_spi.c +++ b/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 diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index c9a1597a6..3f3a50704 100644 --- a/src/main/drivers/bus_spi.h +++ b/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]; diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 470da18d4..3dde1f4b7 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/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; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 80c847ff8..981d8e392 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/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 mpu9250SpiWriteRegister mpu6500SpiWriteRegister -#define mpu9250SpiReadRegister mpu6500SpiReadRegister +#define mpu9250SpiWriteRegisterVerify mpu6500SpiWriteRegDelayed +static bool mpu6500SpiWriteRegDelayed(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); + 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 - verifympu9250SpiWriteRegister(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_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + 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 - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - verifympu9250SpiWriteRegister(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_ADDR, addr_); // set I2C slave address for write + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value + 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 - verifympu9250SpiWriteRegister(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_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + 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) - - verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); + mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); delay(10); - - verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz delay(10); - - verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); +#endif #endif // check for AK8963 diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7d7ed4fc0..d065eefce 100644 --- a/src/main/drivers/pwm_output.c +++ b/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; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 34c56c69d..f0ef307c2 100644 --- a/src/main/drivers/pwm_output.h +++ b/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 diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index 863e21e8e..f922c8350 100644 --- a/src/main/drivers/rx_pwm.c +++ b/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); diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index 7bde2a94c..aeb6906a1 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/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) { diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index eee2069d2..b27a917d9 100644 --- a/src/main/drivers/system.c +++ b/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 diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index b898c9901..4a5b83535 100644 --- a/src/main/drivers/system.h +++ b/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 diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 00cf4d8bc..1127a1c45 100644 --- a/src/main/drivers/timer.h +++ b/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[]; diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 9c4a4fcb3..1b26e691a 100644 --- a/src/main/drivers/transponder_ir.c +++ b/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; } diff --git a/src/main/drivers/transponder_ir.h b/src/main/drivers/transponder_ir.h index 7f4979e9f..fe7416a7c 100644 --- a/src/main/drivers/transponder_ir.h +++ b/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); diff --git a/src/main/fc/.cli.c.swo b/src/main/fc/.cli.c.swo new file mode 100644 index 000000000..df03206f5 Binary files /dev/null and b/src/main/fc/.cli.c.swo differ diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 919396970..750a8f13e 100755 --- a/src/main/fc/cli.c +++ b/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) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 50e126a39..3aa78b71d 100644 --- a/src/main/fc/fc_core.c +++ b/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) { - ENABLE_ARMING_FLAG(OK_TO_ARM); + if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { + 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)) { - warningLedFlash(); - DISABLE_ARMING_FLAG(OK_TO_ARM); + if (averageSystemLoadPercent > 100) { + setArmingDisabled(ARMING_DISABLED_LOAD); } else { - if (ARMING_FLAG(OK_TO_ARM)) { - warningLedDisable(); - } else { - warningLedFlash(); - } + unsetArmingDisabled(ARMING_DISABLED_LOAD); + } + + 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)) { - //TODO: Use BOXDSHOTREVERSE here - if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - reverseMotors = false; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); - } - } else { - reverseMotors = true; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); - } + if (!feature(FEATURE_3D)) { + //TODO: Use BOXDSHOTREVERSE here + if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = false; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); + } + } else { + reverseMotors = true; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); } } + } #endif - ENABLE_ARMING_FLAG(ARMED); - ENABLE_ARMING_FLAG(WAS_EVER_ARMED); - headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); + ENABLE_ARMING_FLAG(ARMED); + ENABLE_ARMING_FLAG(WAS_EVER_ARMED); + 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) - beeper(BEEPER_ARMING_GPS_FIX); - else - beeper(BEEPER_ARMING); -#else + if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { + beeper(BEEPER_ARMING_GPS_FIX); + } 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 diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index baba693d5..8bc8b4253 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -40,12 +40,12 @@ union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); void handleInflightCalibrationStickPosition(); -void mwDisarm(void); -void mwArm(void); +void disarm(void); +void tryArm(void); void processRx(timeUs_t currentTimeUs); -void updateLEDs(void); +void updateArmingStatus(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); -bool isMotorsReversed(void); \ No newline at end of file +bool isMotorsReversed(void); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index cfc9c5bcc..c76f936b2 100644 --- a/src/main/fc/fc_init.c +++ b/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. - failureMode(FAILURE_MISSING_ACC); + // if gyro was not detected due to whatever reason, notify and don't arm. + 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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 53774dc22..97a562c85 100644 --- a/src/main/fc/fc_msp.c +++ b/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)); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 013189292..5e1bb3b3b 100644 --- a/src/main/fc/fc_tasks.c +++ b/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); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 259cab256..f90445703 100644 --- a/src/main/fc/rc_controls.c +++ b/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) { - if (ARMING_FLAG(OK_TO_ARM)) { - mwArm(); - } - } + tryArm(); } 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; } } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 889f612e1..17bd29105 100644 --- a/src/main/fc/runtime_config.c +++ b/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. diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 6311050c9..a54f287bd 100644 --- a/src/main/fc/runtime_config.h +++ b/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), - PREVENT_ARMING = (1 << 1), - ARMED = (1 << 2), - WAS_EVER_ARMED = (1 << 3) + ARMED = (1 << 0), + WAS_EVER_ARMED = (1 << 1) } 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), diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index e29d9ff5e..8f3a028ec 100644 --- a/src/main/fc/settings.c +++ b/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 *) }, diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 52ddb0a3f..a9f05e580 100644 --- a/src/main/fc/settings.h +++ b/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; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a9c9e9c87..445d260c4 100644 --- a/src/main/flight/failsafe.c +++ b/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 - mwDisarm(); + setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link + 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; } diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index c34b66819..d107939d0 100644 --- a/src/main/flight/navigation.c +++ b/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) { diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 040a2e584..d3c926929 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/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; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 6baa47362..4bd3b4eb6 100644 --- a/src/main/io/ledstrip.c +++ b/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); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 60c572b4c..afb3ea9d7 100755 --- a/src/main/io/osd.c +++ b/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 } diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index dc29ad571..0559dbf75 100644 --- a/src/main/io/transponder_ir.c +++ b/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, - .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 -); +void pgResetFn_transponderConfig(transponderConfig_t *transponderConfig) +{ + RESET_CONFIG_2(transponderConfig_t, transponderConfig, + .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; } diff --git a/src/main/io/transponder_ir.h b/src/main/io/transponder_ir.h index 155d78eb9..2b67ac078 100644 --- a/src/main/io/transponder_ir.h +++ b/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 { diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index f73e2f2a8..a3a176373 100644 --- a/src/main/rx/spektrum.c +++ b/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; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 5f173eaf8..10c164d15 100644 --- a/src/main/sensors/acceleration.c +++ b/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; diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index e75e2a7b9..338dc5022 100644 --- a/src/main/target/CJMCU/target.h +++ b/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 diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index ca870492e..9c93858bd 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/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)) - mwDisarm(); - ENABLE_ARMING_FLAG(PREVENT_ARMING); + if (ARMING_FLAG(ARMED)) { + disarm(); + } + 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(); diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 8edd69b2f..759594ea5 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/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_RX DMA1_Stream0 -#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER +//#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 +//#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 +//#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 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 4be0de971..0b81c2959 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/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 VBAT_ADC_PIN PC2 - -//#define RSSI_ADC_PIN PA0 +#define CURRENT_METER_ADC_PIN PC1 // Direct from CRNT pad (part of onboard sensor for Pro) +#define VBAT_ADC_PIN PC2 // 11:1 (10K + 1K) divider +#define RSSI_ADC_PIN PA0 // Direct from RSSI pad #define TRANSPONDER diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index efe3e1865..1e573f77f 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -22,7 +22,6 @@ #include #include -#include #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 uint64_t last_realtime = 0; // in uS - static struct timespec last_ts; // last packet + static double last_timestamp = 0; // in seconds + static uint64_t last_realtime = 0; // in uS + static struct timespec last_ts; // last packet - struct timespec now_ts; - clock_gettime(CLOCK_MONOTONIC, &now_ts); + struct timespec now_ts; + clock_gettime(CLOCK_MONOTONIC, &now_ts); - const uint64_t realtime_now = micros64_real(); - if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout - last_timestamp = pkt->timestamp; - last_realtime = realtime_now; - sendMotorUpdate(); - return; - } + const uint64_t realtime_now = micros64_real(); + if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout + last_timestamp = pkt->timestamp; + last_realtime = realtime_now; + sendMotorUpdate(); + return; + } - const double deltaSim = pkt->timestamp - last_timestamp; // in seconds - if(deltaSim < 0) { // don't use old packet - return; - } + const double deltaSim = pkt->timestamp - last_timestamp; // in seconds + if(deltaSim < 0) { // don't use old packet + return; + } - int16_t x,y,z; - x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE; - y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE; - z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE; - fakeAccSet(fakeAccDev, x, y, z); + int16_t x,y,z; + x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE; + y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE; + z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE; + 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; - y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG; - z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG; - fakeGyroSet(fakeGyroDev, x, y, z); + x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG; + y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG; + z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG; + 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 - double qw = pkt->imu_orientation_quat[0]; - double qx = pkt->imu_orientation_quat[1]; - double qy = pkt->imu_orientation_quat[2]; - double qz = pkt->imu_orientation_quat[3]; - double ysqr = qy * qy; - double xf, yf, zf; + // set from Euler + double qw = pkt->imu_orientation_quat[0]; + double qx = pkt->imu_orientation_quat[1]; + double qy = pkt->imu_orientation_quat[2]; + double qz = pkt->imu_orientation_quat[3]; + double ysqr = qy * qy; + double xf, yf, zf; - // roll (x-axis rotation) - double t0 = +2.0 * (qw * qx + qy * qz); - double t1 = +1.0 - 2.0 * (qx * qx + ysqr); - xf = atan2(t0, t1) * RAD2DEG; + // roll (x-axis rotation) + double t0 = +2.0 * (qw * qx + qy * qz); + double t1 = +1.0 - 2.0 * (qx * qx + ysqr); + xf = atan2(t0, t1) * RAD2DEG; - // pitch (y-axis rotation) - double t2 = +2.0 * (qw * qy - qz * qx); - t2 = t2 > 1.0 ? 1.0 : t2; - t2 = t2 < -1.0 ? -1.0 : t2; - yf = asin(t2) * RAD2DEG; // from wiki + // pitch (y-axis rotation) + double t2 = +2.0 * (qw * qy - qz * qx); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + yf = asin(t2) * RAD2DEG; // from wiki - // yaw (z-axis rotation) - double t3 = +2.0 * (qw * qz + qx * qy); - double t4 = +1.0 - 2.0 * (ysqr + qz * qz); - zf = atan2(t3, t4) * RAD2DEG; - imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!! + // yaw (z-axis rotation) + double t3 = +2.0 * (qw * qz + qx * qy); + double t4 = +1.0 - 2.0 * (ysqr + qz * qz); + zf = atan2(t3, t4) * RAD2DEG; + 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); - imuUpdateAttitude(micros()); + imuSetHasNewData(deltaSim*1e6); + 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; - timeval_sub(&out_ts, &now_ts, &last_ts); - simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec); - } + struct timespec out_ts; + timeval_sub(&out_ts, &now_ts, &last_ts); + 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_realtime = micros64_real(); + last_timestamp = pkt->timestamp; + last_realtime = micros64_real(); - last_ts.tv_sec = now_ts.tv_sec; - last_ts.tv_nsec = now_ts.tv_nsec; + last_ts.tv_sec = now_ts.tv_sec; + 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); - int n = 0; + UNUSED(data); + int n = 0; - while (workerRunning) { - n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); - if(n == sizeof(fdm_packet)) { + while (workerRunning) { + n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); + if(n == sizeof(fdm_packet)) { // printf("[data]new fdm %d\n", n); - updateState(&fdmPkt); - } - } + updateState(&fdmPkt); + } + } - printf("udpThread end!!\n"); - return NULL; + printf("udpThread end!!\n"); + return NULL; } static void* tcpThread(void* data) { - UNUSED(data); + UNUSED(data); - dyad_init(); - dyad_setTickInterval(0.2f); - dyad_setUpdateTimeout(0.5f); + dyad_init(); + dyad_setTickInterval(0.2f); + dyad_setUpdateTimeout(0.5f); - while (workerRunning) { - dyad_update(); - } + while (workerRunning) { + dyad_update(); + } - dyad_shutdown(); - printf("tcpThread end!!\n"); - return NULL; + dyad_shutdown(); + printf("tcpThread end!!\n"); + return NULL; } // system void systemInit(void) { - int ret; + int ret; - clock_gettime(CLOCK_MONOTONIC, &start_time); - printf("[system]Init...\n"); + clock_gettime(CLOCK_MONOTONIC, &start_time); + printf("[system]Init...\n"); - SystemCoreClock = 500 * 1e6; // fake 500MHz - FLASH_Unlock(); + SystemCoreClock = 500 * 1e6; // fake 500MHz + FLASH_Unlock(); - if (pthread_mutex_init(&updateLock, NULL) != 0) { - printf("Create updateLock error!\n"); - exit(1); - } + if (pthread_mutex_init(&updateLock, NULL) != 0) { + printf("Create updateLock error!\n"); + exit(1); + } - if (pthread_mutex_init(&mainLoopLock, NULL) != 0) { - printf("Create mainLoopLock error!\n"); - exit(1); - } + if (pthread_mutex_init(&mainLoopLock, NULL) != 0) { + printf("Create mainLoopLock error!\n"); + exit(1); + } - ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL); - if(ret != 0) { - printf("Create tcpWorker error!\n"); - exit(1); - } + ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL); + if(ret != 0) { + printf("Create tcpWorker error!\n"); + exit(1); + } - ret = udpInit(&pwmLink, "127.0.0.1", 9002, false); - printf("init PwnOut UDP link...%d\n", ret); + ret = udpInit(&pwmLink, "127.0.0.1", 9002, false); + printf("init PwnOut UDP link...%d\n", ret); - ret = udpInit(&stateLink, NULL, 9003, true); - printf("start UDP server...%d\n", ret); + ret = udpInit(&stateLink, NULL, 9003, true); + printf("start UDP server...%d\n", ret); - ret = pthread_create(&udpWorker, NULL, udpThread, NULL); - if(ret != 0) { - printf("Create udpWorker error!\n"); - exit(1); - } + ret = pthread_create(&udpWorker, NULL, udpThread, NULL); + if(ret != 0) { + printf("Create udpWorker error!\n"); + exit(1); + } - // serial can't been slow down - rescheduleTask(TASK_SERIAL, 1); + // serial can't been slow down + rescheduleTask(TASK_SERIAL, 1); } void systemReset(void){ - printf("[system]Reset!\n"); - workerRunning = false; - pthread_join(tcpWorker, NULL); - pthread_join(udpWorker, NULL); - exit(0); + printf("[system]Reset!\n"); + workerRunning = false; + pthread_join(tcpWorker, NULL); + pthread_join(udpWorker, NULL); + exit(0); } void systemResetToBootloader(void) { - printf("[system]ResetToBootloader!\n"); - workerRunning = false; - pthread_join(tcpWorker, NULL); - pthread_join(udpWorker, NULL); - exit(0); + printf("[system]ResetToBootloader!\n"); + workerRunning = false; + pthread_join(tcpWorker, NULL); + pthread_join(udpWorker, NULL); + exit(0); } // drivers/light_led.c void ledInit(const statusLedConfig_t *statusLedConfig) { - UNUSED(statusLedConfig); - 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); + UNUSED(statusLedConfig); + 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 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; - clock_gettime(CLOCK_MONOTONIC, &ts); - return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec); + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec); } + uint64_t micros64_real() { - struct timespec 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))); + struct timespec 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))); } + uint64_t millis64_real() { - struct timespec 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))); + struct timespec 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))); } uint64_t micros64() { - static uint64_t last = 0; - static uint64_t out = 0; - uint64_t now = nanos64_real(); + static uint64_t last = 0; + static uint64_t out = 0; + uint64_t now = nanos64_real(); - out += (now - last) * simRate; - last = now; + out += (now - last) * simRate; + last = now; - return out*1e-3; + return out*1e-3; // return micros64_real(); } + uint64_t millis64() { - static uint64_t last = 0; - static uint64_t out = 0; - uint64_t now = nanos64_real(); + static uint64_t last = 0; + static uint64_t out = 0; + uint64_t now = nanos64_real(); - out += (now - last) * simRate; - last = now; + out += (now - last) * simRate; + 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) { - microsleep(1000); - } +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) { + 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 ns_carry = 0; - // Perform the carry for the later subtraction by updating y. - if (x->tv_nsec < y->tv_nsec) { - int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1; - ns_carry += 1000000000 * nsec; - s_carry += nsec; - } + unsigned int s_carry = 0; + unsigned int ns_carry = 0; + // Perform the carry for the later subtraction by updating y. + if (x->tv_nsec < y->tv_nsec) { + int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1; + ns_carry += 1000000000 * nsec; + s_carry += nsec; + } - // Compute the time remaining to wait. tv_usec is certainly positive. - result->tv_sec = x->tv_sec - y->tv_sec - s_carry; - result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry; + // Compute the time remaining to wait. tv_usec is certainly positive. + result->tv_sec = x->tv_sec - y->tv_sec - s_carry; + result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry; - // Return 1 if result is negative. - return x->tv_sec < y->tv_sec; + // Return 1 if result is negative. + 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(motorCount); + UNUSED(motorConfig); + UNUSED(motorCount); - idlePulse = _idlePulse; + idlePulse = _idlePulse; - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { - motors[motorIndex].enabled = true; - } - pwmMotorsEnabled = true; + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + motors[motorIndex].enabled = true; + } + pwmMotorsEnabled = true; } + void servoDevInit(const servoDevConfig_t *servoConfig) { - UNUSED(servoConfig); - for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { - servos[servoIndex].enabled = true; - } + UNUSED(servoConfig); + for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { + 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); - pwmMotorsEnabled = false; + UNUSED(motorCount); + pwmMotorsEnabled = false; } + void pwmCompleteMotorUpdate(uint8_t motorCount) { - UNUSED(motorCount); - // send to simulator - // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0] + UNUSED(motorCount); + // send to simulator + // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0] - double outScale = 1000.0; - if (feature(FEATURE_3D)) { - outScale = 500.0; - } + double outScale = 1000.0; + if (feature(FEATURE_3D)) { + outScale = 500.0; + } - pwmPkt.motor_speed[3] = motorsPwm[0] / outScale; - pwmPkt.motor_speed[0] = motorsPwm[1] / outScale; - pwmPkt.motor_speed[1] = motorsPwm[2] / outScale; - pwmPkt.motor_speed[2] = motorsPwm[3] / outScale; + pwmPkt.motor_speed[3] = motorsPwm[0] / outScale; + pwmPkt.motor_speed[0] = motorsPwm[1] / outScale; + pwmPkt.motor_speed[1] = motorsPwm[2] / outScale; + pwmPkt.motor_speed[2] = motorsPwm[3] / outScale; - // get one "fdm_packet" can only send one "servo_packet"!! - if(pthread_mutex_trylock(&updateLock) != 0) return; - udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); + // get one "fdm_packet" can only send one "servo_packet"!! + if(pthread_mutex_trylock(&updateLock) != 0) return; + 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); - return 0; + UNUSED(channel); + return 0; } // stack part @@ -438,62 +460,73 @@ char _estack; char _Min_Stack_Size; // fake EEPROM -extern uint8_t __config_start; -extern uint8_t __config_end; +uint8_t __config_start; +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) { - printf("[FLASH_Unlock] eepromFd != NULL\n"); - return; - } + if (eepromFd != NULL) { + printf("[FLASH_Unlock] eepromFd != NULL\n"); + return; + } - // open or create - eepromFd = fopen(EEPROM_FILENAME,"r+"); - if (eepromFd != NULL) { - // obtain file size: - fseek(eepromFd , 0 , SEEK_END); - long lSize = ftell(eepromFd); - rewind(eepromFd); + // open or create + eepromFd = fopen(EEPROM_FILENAME,"r+"); + if (eepromFd != NULL) { + // obtain file size: + fseek(eepromFd , 0 , SEEK_END); + long lSize = ftell(eepromFd); + rewind(eepromFd); - printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start)); - for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) { - int c = fgetc(eepromFd); - if(c == EOF) break; - eeprom[i] = (uint8_t)c; - } - } else { - eepromFd = fopen(EEPROM_FILENAME, "w+"); - fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd); - } + 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++) { + int c = fgetc(eepromFd); + if(c == EOF) break; + eeprom[i] = (uint8_t)c; + } + } else { + eepromFd = fopen(EEPROM_FILENAME, "w+"); + fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd); + } } void FLASH_Lock(void) { - // flush & close - if (eepromFd != NULL) { - const uint8_t *eeprom = &__config_start; - fseek(eepromFd, 0, SEEK_SET); - fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd); - fclose(eepromFd); - eepromFd = NULL; - } + // flush & close + if (eepromFd != NULL) { + const uint8_t *eeprom = &__config_start; + fseek(eepromFd, 0, SEEK_SET); + fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd); + fclose(eepromFd); + 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)) { - *((uint32_t*)addr) = Data; - printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr)); - } else { - printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr); - } - return FLASH_COMPLETE; + if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) { + *((uint32_t*)addr) = Data; + printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr)); + } else { + printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr); + } + return FLASH_COMPLETE; } +void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig) +{ + UNUSED(pSerialPinConfig); + printf("uartPinConfigure"); +} +void spektrumBind(rxConfig_t *rxConfig) +{ + UNUSED(rxConfig); + printf("spektrumBind"); +} diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 0065aa4d3..14fc279bd 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/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 ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define USE_I2C #define USE_I2C_DEVICE_1 diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index a99bbb0e3..5ce914c9e 100644 --- a/src/main/target/common_fc_pre.h +++ b/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 diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 08f85752c..3f06e0039 100644 --- a/src/main/telemetry/smartport.c +++ b/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; diff --git a/src/test/Makefile b/src/test/Makefile index 4776ac9df..a30ab5cfd 100644 --- a/src/test/Makefile +++ b/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 diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index a61f2a4df..c133cde63 100644 --- a/src/test/unit/cms_unittest.cc +++ b/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); } } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 5edb8c45d..219db6cbc 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/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); } } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 058cf2131..7bb59caaf 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/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; } + } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 57c94cab6..de1524b11 100644 --- a/src/test/unit/osd_unittest.cc +++ b/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); } } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 5cf1c57e6..75f8e8358 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/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 mwDisarm(void) {} +void tryArm(void) {} +void disarm(void) {} void dashboardDisablePageCycling() {} void dashboardEnablePageCycling() {}