From dda662fa861e682a765f3a13841b9c71a59cf8f5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 23 Jun 2016 08:16:43 +0100 Subject: [PATCH 01/41] Fixed calling of updateRcCommands. --- src/main/mw.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/mw.c b/src/main/mw.c index 5a95f1c924..36dcad2a62 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -694,6 +694,8 @@ void subTaskMainSubprocesses(void) { #endif #if defined(BARO) || defined(SONAR) + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(&masterConfig.airplaneConfig); @@ -874,8 +876,10 @@ void taskUpdateRxMain(void) processRx(); isRXDataNew = true; +#if !defined(BARO) && !defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); +#endif updateLEDs(); #ifdef BARO From 070ea8181693edc7486a5a7af11183625189c178 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 22 Jun 2016 07:50:21 +0200 Subject: [PATCH 02/41] AlienFlight fixes AK8963 driver updates Increase resolution for brushed motors from 250 to 750 steps (@32Khz pwm rate) --- src/main/config/config.c | 8 +- src/main/drivers/compass_ak8963.c | 202 ++++++++++-------------- src/main/drivers/pwm_mapping.h | 2 +- src/main/main.c | 2 +- src/main/sensors/compass.c | 17 +- src/main/sensors/initialisation.c | 4 +- src/main/target/ALIENFLIGHTF3/target.h | 6 +- src/main/target/ALIENFLIGHTF4/target.c | 4 +- src/main/target/ALIENFLIGHTF4/target.h | 6 - src/main/target/ALIENFLIGHTF4/target.mk | 1 - 10 files changed, 101 insertions(+), 151 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index aa88fb0c8e..993b6df831 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -650,12 +650,14 @@ static void resetConf(void) #if defined(ALIENFLIGHT) featureClear(FEATURE_ONESHOT125); -#ifdef ALIENFLIGHTF3 +#ifdef ALIENFLIGHTF1 + masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; +#else masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; +#endif +#ifdef ALIENFLIGHTF3 masterConfig.batteryConfig.vbatscale = 20; masterConfig.mag_hardware = MAG_NONE; // disabled by default -#else - masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; #endif masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 726f4f85a1..7b60fe5c46 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -33,6 +33,7 @@ #include "gpio.h" #include "exti.h" #include "bus_i2c.h" +#include "bus_spi.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" @@ -42,7 +43,9 @@ #include "accgyro.h" #include "accgyro_mpu.h" +#include "accgyro_mpu6500.h" #include "accgyro_spi_mpu6500.h" +#include "accgyro_spi_mpu9250.h" #include "compass_ak8963.h" // This sensor is available in MPU-9250. @@ -83,18 +86,10 @@ #define CNTL_MODE_SELF_TEST 0x08 #define CNTL_MODE_FUSE_ROM 0x0F -typedef bool (*ak8963ReadRegisterFunc)(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf); -typedef bool (*ak8963WriteRegisterFunc)(uint8_t addr_, uint8_t reg_, uint8_t data); - -typedef struct ak8963Configuration_s { - ak8963ReadRegisterFunc read; - ak8963WriteRegisterFunc write; -} ak8963Configuration_t; - -ak8963Configuration_t ak8963config; static float magGain[3] = { 1.0f, 1.0f, 1.0f }; // 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 verifympu9250WriteRegister mpu6500WriteRegister @@ -102,30 +97,7 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f }; #define mpu9250ReadRegister mpu6500ReadRegister #endif -#ifdef USE_SPI -bool ak8963SPIRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) -{ - mpu6500WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - mpu6500WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu6500WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes - delay(8); - __disable_irq(); - mpu6500ReadRegister(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C - __enable_irq(); - return true; -} - -bool ak8963SPIWrite(uint8_t addr_, uint8_t reg_, uint8_t data) -{ - mpu6500WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write - mpu6500WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu6500WriteRegister(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value - mpu6500WriteRegister(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte - return true; -} -#endif - -#ifdef SPRACINGF3EVO +#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) typedef struct queuedReadState_s { bool waiting; @@ -133,9 +105,36 @@ typedef struct queuedReadState_s { uint32_t readStartedAt; // time read was queued in micros. } queuedReadState_t; +typedef enum { + CHECK_STATUS = 0, + WAITING_FOR_STATUS, + WAITING_FOR_DATA +} ak8963ReadState_e; + static queuedReadState_t queuedRead = { false, 0, 0}; -bool ak8963SPIStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) +bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) +{ + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + delay(10); + __disable_irq(); + mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C + __enable_irq(); + return true; +} + +bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) +{ + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value + verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte + return true; +} + +bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) { if (queuedRead.waiting) { return false; @@ -153,7 +152,7 @@ bool ak8963SPIStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) return true; } -static uint32_t ak8963SPIQueuedReadTimeRemaining(void) +static uint32_t ak8963SensorQueuedReadTimeRemaining(void) { if (!queuedRead.waiting) { return 0; @@ -170,9 +169,9 @@ static uint32_t ak8963SPIQueuedReadTimeRemaining(void) return timeRemaining; } -bool ak8963SPICompleteRead(uint8_t *buf) +bool ak8963SensorCompleteRead(uint8_t *buf) { - uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining(); + uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); if (timeRemaining > 0) { delayMicroseconds(timeRemaining); @@ -183,18 +182,15 @@ bool ak8963SPICompleteRead(uint8_t *buf) mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer return true; } - -#endif - -#ifdef USE_I2C -bool c_i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data) +#else +bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { - return i2cWrite(MAG_I2C_INSTANCE, addr_, reg_, data); + return i2cRead(MAG_I2C_INSTANCE, addr_, reg_, len, buf); } -bool c_i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) { - return i2cRead(MAG_I2C_INSTANCE, addr_, reg_, len, buf); + return i2cWrite(MAG_I2C_INSTANCE, addr_, reg_, data); } #endif @@ -203,43 +199,28 @@ bool ak8963Detect(mag_t *mag) bool ack = false; uint8_t sig = 0; -#ifdef USE_I2C - // check for AK8963 on I2C bus - ack = i2cRead(MAG_I2C_INSTANCE, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); +#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) + // initialze I2C master via SPI bus (MPU9250) + + ack = verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR + delay(10); + + ack = verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + delay(10); + + ack = verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + delay(10); +#endif + + // check for AK8963 + ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H' { - ak8963config.read = c_i2cRead; - ak8963config.write = c_i2cWrite; mag->init = ak8963Init; mag->read = ak8963Read; return true; } -#endif - -#ifdef USE_SPI - // check for AK8963 on I2C master via SPI bus (as part of MPU9250) - - ack = mpu6500WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR - delay(10); - - ack = mpu6500WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz - delay(10); - - ack = mpu6500WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only - delay(10); - - ack = ak8963SPIRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); - if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H' - { - ak8963config.read = ak8963SPIRead; - ak8963config.write = ak8963SPIWrite; - mag->init = ak8963Init; - mag->read = ak8963Read; - - return true; - } -#endif return false; } @@ -250,49 +231,43 @@ void ak8963Init() uint8_t calibration[3]; uint8_t status; - ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode + ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode delay(20); - ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode + ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode delay(10); - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, sizeof(calibration), calibration); // Read the x-, y-, and z-axis calibration values + ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, sizeof(calibration), calibration); // Read the x-, y-, and z-axis calibration values delay(10); magGain[X] = (((((float)(int8_t)calibration[X] - 128) / 256) + 1) * 30); magGain[Y] = (((((float)(int8_t)calibration[Y] - 128) / 256) + 1) * 30); magGain[Z] = (((((float)(int8_t)calibration[Z] - 128) / 256) + 1) * 30); - ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading. + ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading. delay(10); // Clear status registers - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &status); - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status); + ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &status); + ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status); // Trigger first measurement -#ifdef SPRACINGF3EVO - ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1); +#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) + ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1); #else - ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); + ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); #endif } -typedef enum { - CHECK_STATUS = 0, - WAITING_FOR_STATUS, - WAITING_FOR_DATA -} ak8963ReadState_e; - bool ak8963Read(int16_t *magData) { - bool ack; + bool ack = false; uint8_t buf[7]; -#ifdef SPRACINGF3EVO +#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) - // we currently need a different approach on the SPRacingF3EVO which has an MPU9250 connected via SPI. - // we cannot use the ak8963SPIRead() method, it is to slow and blocks for far too long. + // we currently need a different approach for the MPU9250 connected via SPI. + // we cannot use the ak8963SensorRead() method for SPI, it is to slow and blocks for far too long. static ak8963ReadState_e state = CHECK_STATUS; @@ -301,17 +276,17 @@ bool ak8963Read(int16_t *magData) restart: switch (state) { case CHECK_STATUS: - ak8963SPIStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1); + ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1); state++; return false; case WAITING_FOR_STATUS: { - uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining(); + uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); if (timeRemaining) { return false; } - ack = ak8963SPICompleteRead(&buf[0]); + ack = ak8963SensorCompleteRead(&buf[0]); uint8_t status = buf[0]; @@ -327,7 +302,7 @@ restart: // read the 6 bytes of data and the status2 register - ak8963SPIStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7); + ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7); state++; @@ -335,31 +310,16 @@ restart: } case WAITING_FOR_DATA: { - uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining(); + uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); if (timeRemaining) { return false; } - ack = ak8963SPICompleteRead(&buf[0]); - - uint8_t status2 = buf[6]; - if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) { - return false; - } - - magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X]; - magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; - magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z]; - - state = CHECK_STATUS; - - return true; + ack = ak8963SensorCompleteRead(&buf[0]); } } - - return false; #else - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &buf[0]); + ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &buf[0]); uint8_t status = buf[0]; @@ -367,8 +327,8 @@ restart: return false; } - ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]); - + ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]); +#endif uint8_t status2 = buf[6]; if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) { return false; @@ -378,6 +338,10 @@ restart: magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z]; - return ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again +#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) + state = CHECK_STATUS; + return true; +#else + return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again #endif } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 60b1462882..14122ac800 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -37,7 +37,7 @@ #define PWM_TIMER_MHZ 1 -#define PWM_BRUSHED_TIMER_MHZ 8 +#define PWM_BRUSHED_TIMER_MHZ 24 #define MULTISHOT_TIMER_MHZ 72 #define ONESHOT42_TIMER_MHZ 24 #define ONESHOT125_TIMER_MHZ 8 diff --git a/src/main/main.c b/src/main/main.c index fbfeb49990..62893465b9 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -730,7 +730,7 @@ void main_init(void) #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); -#ifdef SPRACINGF3EVO +#if defined(USE_SPI) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 40); #endif diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 5df25737b9..7ee14da4c7 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -43,6 +43,7 @@ mag_t mag; // mag access functions extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead. +int16_t magADCRaw[XYZ_AXIS_COUNT]; int32_t magADC[XYZ_AXIS_COUNT]; sensor_align_e magAlign = 0; #ifdef MAG @@ -57,27 +58,19 @@ void compassInit(void) magInit = 1; } -#define COMPASS_UPDATE_FREQUENCY_10HZ (1000 * 100) - void updateCompass(flightDynamicsTrims_t *magZero) { - static uint32_t nextUpdateAt, tCal = 0; + static uint32_t tCal = 0; static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; - int16_t magADCRaw[XYZ_AXIS_COUNT]; uint32_t axis; - if ((int32_t)(currentTime - nextUpdateAt) < 0) - return; - - nextUpdateAt = currentTime + COMPASS_UPDATE_FREQUENCY_10HZ; - mag.read(magADCRaw); - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; // int32_t copy to work with alignSensors(magADC, magADC, magAlign); if (STATE(CALIBRATE_MAG)) { - tCal = nextUpdateAt; + tCal = currentTime; for (axis = 0; axis < 3; axis++) { magZero->raw[axis] = 0; magZeroTempMin.raw[axis] = magADC[axis]; @@ -93,7 +86,7 @@ void updateCompass(flightDynamicsTrims_t *magZero) } if (tCal != 0) { - if ((nextUpdateAt - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions + if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions LED0_TOGGLE; for (axis = 0; axis < 3; axis++) { if (magADC[axis] < magZeroTempMin.raw[axis]) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 5c52c2a825..00093d18e6 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -216,7 +216,7 @@ bool detectGyro(void) ; // fallthrough case GYRO_MPU6500: -#ifdef USE_GYRO_MPU6500 +#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) #ifdef USE_GYRO_SPI_MPU6500 if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) #else @@ -361,7 +361,7 @@ retry: #endif ; // fallthrough case ACC_MPU6500: -#ifdef USE_ACC_MPU6500 +#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) #else diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index e52c98d128..870514892d 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -45,7 +45,6 @@ // Using MPU6050 for the moment. #define GYRO #define USE_GYRO_MPU6050 -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6050_ALIGN CW270_DEG @@ -53,7 +52,6 @@ #define ACC #define USE_ACC_MPU6050 -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 #define ACC_MPU6050_ALIGN CW270_DEG @@ -100,8 +98,8 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) -#define I2C2_SCL_PIN PA9 -#define I2C2_SDA_PIN PA10 +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 // SPI3 // PA15 38 SPI3_NSS diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index 02efa0b5e6..054b27e814 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -76,8 +76,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM1 - PA8 RC1 { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM2 - PB0 RC2 { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM3 - PB1 RC3 - { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM4 - PA14 RC4 - { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM5 - PA15 RC5 + { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM4 - PA14 RC4 + { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM5 - PA15 RC5 { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // PWM6 - PB8 OUT1 { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // PWM7 - PB9 OUT2 diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 88ce6a5791..7b45d957b2 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -45,22 +45,16 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 -#define MPU9250_CS_PIN PA4 -#define MPU9250_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6500 -#define USE_ACC_SPI_MPU9250 #define ACC_MPU6500_ALIGN CW270_DEG -#define ACC_MPU9250_ALIGN CW270_DEG #define GYRO #define USE_GYRO_SPI_MPU6500 -#define USE_GYRO_SPI_MPU9250 #define GYRO_MPU6500_ALIGN CW270_DEG -#define GYRO_MPU9250_ALIGN CW270_DEG #define MAG #define USE_MAG_HMC5883 diff --git a/src/main/target/ALIENFLIGHTF4/target.mk b/src/main/target/ALIENFLIGHTF4/target.mk index 54a4c3108a..910023aa0e 100644 --- a/src/main/target/ALIENFLIGHTF4/target.mk +++ b/src/main/target/ALIENFLIGHTF4/target.mk @@ -4,7 +4,6 @@ FEATURES += SDCARD VCP TARGET_SRC = \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_spi_mpu9250.c \ drivers/barometer_bmp280.c \ drivers/barometer_ms5611.c \ drivers/compass_ak8963.c \ From cac805ef1060195179a7e6a2d39334fdf979b71b Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 23 Jun 2016 19:24:26 +0200 Subject: [PATCH 03/41] Fix Blackbox device assignments --- src/main/config/config.c | 14 ++++---------- src/main/target/ALIENFLIGHTF3/target.h | 4 ++-- src/main/target/ALIENFLIGHTF4/target.h | 6 ++++-- src/main/target/BLUEJAYF4/target.h | 4 ++++ src/main/target/OMNIBUS/target.h | 1 + src/main/target/SINGULARITY/target.h | 6 ++++-- src/main/target/SIRINFPV/target.h | 1 + src/main/target/SPRACINGF3/target.h | 2 ++ src/main/target/SPRACINGF3EVO/target.h | 2 +- src/main/target/SPRACINGF3MINI/target.h | 1 + 10 files changed, 24 insertions(+), 17 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 993b6df831..7239557b3a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,6 +23,8 @@ #include "build_config.h" +#include "blackbox/blackbox_io.h" + #include "common/color.h" #include "common/axis.h" #include "common/maths.h" @@ -614,8 +616,6 @@ static void resetConf(void) masterConfig.vtx_mhz = 5740; //F0 #endif -#ifdef SPRACINGF3 - masterConfig.blackbox_device = 1; #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware @@ -629,12 +629,11 @@ static void resetConf(void) featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = BLACKBOX_DEVICE_SDCARD; #else - masterConfig.blackbox_device = 0; + masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; -#endif // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) @@ -659,13 +658,12 @@ static void resetConf(void) masterConfig.batteryConfig.vbatscale = 20; masterConfig.mag_hardware = MAG_NONE; // disabled by default #endif - masterConfig.rxConfig.serialrx_provider = 1; + masterConfig.rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048; masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; - currentProfile->pidProfile.pidController = 2; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; currentControlRateProfile->rates[FD_PITCH] = 40; @@ -685,10 +683,6 @@ static void resetConf(void) #if defined(SINGULARITY) // alternative defaults settings for SINGULARITY target - masterConfig.blackbox_device = 1; - masterConfig.blackbox_rate_num = 1; - masterConfig.blackbox_rate_denom = 1; - masterConfig.batteryConfig.vbatscale = 77; masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 870514892d..0410f8cbcd 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -137,8 +137,8 @@ #define BINDPLUG_PIN PB12 #define BRUSHED_MOTORS -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES FEATURE_MOTOR_STOP #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 7b45d957b2..fa5f81cfec 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -185,9 +185,11 @@ // Hardware bind plug at PB2 (Pin 28) #define BINDPLUG_PIN PB2 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + #define BRUSHED_MOTORS -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index e22152a86d..1fb646b9c1 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -140,7 +140,11 @@ #define VBAT_ADC_PIN PC3 #define VBAT_ADC_CHANNEL ADC_Channel_13 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + #define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX + #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 9eb39d6dbf..6191839127 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -210,6 +210,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define BUTTONS #define BUTTON_A_PORT GPIOB diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 298d183b42..17af55e114 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -117,8 +117,10 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define SPEKTRUM_BIND // USART2, PA15 diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index c27b1ec689..f6dd3d8884 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -155,6 +155,7 @@ #define USE_SERVOS #define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 929a5f3fd3..04ce25966d 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -148,6 +148,8 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES FEATURE_BLACKBOX diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index d71254270c..e700948b02 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -185,7 +185,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) #define SPEKTRUM_BIND // USART3, diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 7c18fb2356..8db5dbf598 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -193,6 +193,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define BUTTONS #define BUTTON_A_PORT GPIOB From f70f2960a848b09f9c2eba9b5ce48425ce30ba24 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 23 Jun 2016 23:37:44 +0200 Subject: [PATCH 04/41] Remove double parameter // Fix missing parameter --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c3adebf8a2..d50a8caeda 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1259,11 +1259,10 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yaw_lpf_hz); break; case MSP_ADVANCED_TUNING: - headSerialReply(4 * 2 + 2); + headSerialReply(3 * 2 + 2); serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate); serialize16(currentProfile->pidProfile.yawItermIgnoreRate); serialize16(currentProfile->pidProfile.yaw_p_limit); - serialize16(masterConfig.rxConfig.airModeActivateThreshold); serialize8(currentProfile->pidProfile.deltaMethod); serialize8(masterConfig.batteryConfig.vbatPidCompensation); break; @@ -1810,6 +1809,7 @@ static bool processInCommand(void) currentProfile->pidProfile.yawItermIgnoreRate = read16(); currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); + masterConfig.batteryConfig.vbatPidCompensation = read8(); break; case MSP_SET_SPECIAL_PARAMETERS: currentControlRateProfile->rcYawRate8 = read8(); From 737510062d6b560c0502036fdc68ea3ae38f572e Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 09:26:03 +1000 Subject: [PATCH 05/41] Updated 6500 to run at 5.25mhz (rated to 8mhz) --- src/main/drivers/accgyro_spi_mpu6500.c | 4 ---- src/main/drivers/bus_spi.h | 11 ++++------- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 316a296c3a..6f9a25fe79 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -72,11 +72,7 @@ static void mpu6500SpiInit(void) IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI); IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG); -#if defined(STM32F4) - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_SLOW_CLOCK); -#else spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_STANDARD_CLOCK); -#endif hardwareInitialised = true; } diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 1d5eca6bc0..cd250659d8 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -26,31 +26,28 @@ #include "io.h" #include "rcc.h" -#if defined(STM32F40_41xxx) || defined (STM32F411xE) || defined(STM32F303xC) +#if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN) #define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP) #define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) -#elif defined(STM32F10X) +#elif defined(STM32F1) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz) #define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_Out_OD, GPIO_Speed_50MHz) #else #error "Unknown processor" #endif -#if defined(STM32F40_41xxx) || defined (STM32F411xE) +#if defined(STM32F4) #define SPI_SLOW_CLOCK 128 //00.65625 MHz -#define SPI_STANDARD_CLOCK 8 //11.50000 MHz +#define SPI_STANDARD_CLOCK 32 //05.25000 MHz #define SPI_FAST_CLOCK 4 //21.00000 MHz #define SPI_ULTRAFAST_CLOCK 2 //42.00000 MHz - #else - #define SPI_SLOW_CLOCK 128 //00.56250 MHz #define SPI_STANDARD_CLOCK 4 //09.00000 MHz #define SPI_FAST_CLOCK 2 //18.00000 MHz #define SPI_ULTRAFAST_CLOCK 2 //18.00000 MHz - #endif typedef enum SPIDevice { From b8263ec049055c4fe2d989a357bc951c802796d7 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 09:54:45 +1000 Subject: [PATCH 06/41] SPI1 is on APB2 --- src/main/drivers/bus_spi.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index cd250659d8..39074e418d 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -40,7 +40,7 @@ #if defined(STM32F4) #define SPI_SLOW_CLOCK 128 //00.65625 MHz -#define SPI_STANDARD_CLOCK 32 //05.25000 MHz +#define SPI_STANDARD_CLOCK 16 //05.25000 MHz #define SPI_FAST_CLOCK 4 //21.00000 MHz #define SPI_ULTRAFAST_CLOCK 2 //42.00000 MHz #else From 7e6fb8db657e2ffcaa1e8d5946a64a356c9a0ee9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 19 Jun 2016 19:27:45 +1000 Subject: [PATCH 07/41] ADC converted to new IO mapping --- src/main/drivers/adc_impl.h | 22 +++++ src/main/drivers/adc_stm32f10x.c | 97 +++++++++++++---------- src/main/drivers/adc_stm32f30x.c | 92 ++++++++++++--------- src/main/drivers/adc_stm32f4xx.c | 62 +++++++++------ src/main/target/ALIENFLIGHTF3/target.h | 3 +- src/main/target/BLUEJAYF4/target.h | 2 +- src/main/target/CC3D/target.h | 11 +-- src/main/target/CHEBUZZF3/target.h | 12 +-- src/main/target/COLIBRI_RACE/target.h | 12 +-- src/main/target/DOGE/target.h | 6 +- src/main/target/EUSTM32F103RC/target.h | 12 +-- src/main/target/FURYF3/target.h | 9 +-- src/main/target/IRCFUSIONF3/target.h | 12 +-- src/main/target/KISSFC/target.h | 9 +-- src/main/target/LUX_RACE/target.h | 16 ++-- src/main/target/MOTOLAB/target.h | 9 +-- src/main/target/NAZE/target.h | 18 ++--- src/main/target/OLIMEXINO/target.h | 12 +-- src/main/target/OMNIBUS/target.h | 12 +-- src/main/target/PIKOBLX/target.h | 11 +-- src/main/target/PORT103R/target.h | 12 +-- src/main/target/REVO/target.h | 8 +- src/main/target/RMDO/target.h | 11 +-- src/main/target/SINGULARITY/target.h | 3 +- src/main/target/SIRINFPV/target.h | 3 +- src/main/target/SPARKY/target.h | 21 +---- src/main/target/SPRACINGF3/target.h | 11 +-- src/main/target/SPRACINGF3EVO/target.h | 11 +-- src/main/target/SPRACINGF3MINI/target.h | 11 +-- src/main/target/STM32F3DISCOVERY/target.h | 14 ++-- 30 files changed, 263 insertions(+), 281 deletions(-) diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 8007308251..bafff8accb 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -17,5 +17,27 @@ #pragma once +#include "rcc.h" + +typedef enum ADCDevice { + ADCINVALID = -1, + ADCDEV_1 = 0, + ADCDEV_2, + ADCDEV_3, + ADCDEV_MAX = ADCDEV_3, +} ADCDevice; + +typedef struct adcDevice_s { + ADC_TypeDef* ADCx; + rccPeriphTag_t rccADC; + rccPeriphTag_t rccDMA; +#if defined(STM32F4) + DMA_Stream_TypeDef* DMAy_Streamx; +#else + DMA_Channel_TypeDef* DMAy_Channelx; +#endif +} adcDevice_t; + +extern const adcDevice_t adcHardware[]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 11dec5bfc1..8ed04874b4 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -29,17 +29,31 @@ #include "sensor.h" #include "accgyro.h" - #include "adc.h" #include "adc_impl.h" +#include "io.h" +#include "rcc.h" #ifndef ADC_INSTANCE -#define ADC_INSTANCE ADC1 -#define ADC_ABP2_PERIPHERAL RCC_APB2Periph_ADC1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define ADC_DMA_CHANNEL DMA1_Channel1 +#define ADC_INSTANCE ADC1 #endif +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 } +}; + +ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) +{ + if (instance == ADC1) + return ADCDEV_1; + +/* TODO -- ADC2 available on large 10x devices. + if (instance == ADC2) + return ADCDEV_2; +*/ + return ADCINVALID; +} + // Driver for STM32F103CB onboard ADC // // Naze32 @@ -50,27 +64,22 @@ // NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board // - void adcInit(drv_adc_config_t *init) { -#if defined(CJMCU) || defined(CC3D) - UNUSED(init); + +#if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN) + UNUSED(init); #endif - uint8_t i; uint8_t configuredAdcChannels = 0; memset(&adcConfig, 0, sizeof(adcConfig)); - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; - -#ifdef VBAT_ADC_GPIO +#ifdef VBAT_ADC_PIN if (init->enableVBat) { - GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN; - GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; @@ -78,10 +87,10 @@ void adcInit(drv_adc_config_t *init) } #endif -#ifdef RSSI_ADC_GPIO +#ifdef RSSI_ADC_PIN if (init->enableRSSI) { - GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN; - GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; @@ -89,10 +98,10 @@ void adcInit(drv_adc_config_t *init) } #endif -#ifdef EXTERNAL1_ADC_GPIO +#ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { - GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN; - GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; @@ -100,10 +109,10 @@ void adcInit(drv_adc_config_t *init) } #endif -#ifdef CURRENT_METER_ADC_GPIO +#ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { - GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN; - GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; @@ -111,16 +120,22 @@ void adcInit(drv_adc_config_t *init) } #endif + ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); + if (device == ADCINVALID) + return; + + adcDevice_t adc = adcHardware[device]; + RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) - RCC_AHBPeriphClockCmd(ADC_AHB_PERIPHERAL, ENABLE); - RCC_APB2PeriphClockCmd(ADC_ABP2_PERIPHERAL, ENABLE); - + RCC_ClockCmd(adc.rccADC, ENABLE); + RCC_ClockCmd(adc.rccDMA, ENABLE); + // FIXME ADC driver assumes all the GPIO was already placed in 'AIN' mode - DMA_DeInit(ADC_DMA_CHANNEL); + DMA_DeInit(adc.DMAy_Channelx); DMA_InitTypeDef DMA_InitStructure; DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC_INSTANCE->DR; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; DMA_InitStructure.DMA_BufferSize = configuredAdcChannels; @@ -131,8 +146,8 @@ void adcInit(drv_adc_config_t *init) DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(ADC_DMA_CHANNEL, &DMA_InitStructure); - DMA_Cmd(ADC_DMA_CHANNEL, ENABLE); + DMA_Init(adc.DMAy_Channelx, &DMA_InitStructure); + DMA_Cmd(adc.DMAy_Channelx, ENABLE); ADC_InitTypeDef ADC_InitStructure; ADC_StructInit(&ADC_InitStructure); @@ -142,23 +157,23 @@ void adcInit(drv_adc_config_t *init) ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfChannel = configuredAdcChannels; - ADC_Init(ADC_INSTANCE, &ADC_InitStructure); + ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } - ADC_RegularChannelConfig(ADC_INSTANCE, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); + ADC_RegularChannelConfig(adc.ADCx, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); } - ADC_DMACmd(ADC_INSTANCE, ENABLE); - ADC_Cmd(ADC_INSTANCE, ENABLE); + ADC_DMACmd(adc.ADCx, ENABLE); + ADC_Cmd(adc.ADCx, ENABLE); - ADC_ResetCalibration(ADC_INSTANCE); - while(ADC_GetResetCalibrationStatus(ADC_INSTANCE)); - ADC_StartCalibration(ADC_INSTANCE); - while(ADC_GetCalibrationStatus(ADC_INSTANCE)); + ADC_ResetCalibration(adc.ADCx); + while (ADC_GetResetCalibrationStatus(adc.ADCx)); + ADC_StartCalibration(adc.ADCx); + while (ADC_GetCalibrationStatus(adc.ADCx)); - ADC_SoftwareStartConvCmd(ADC_INSTANCE, ENABLE); + ADC_SoftwareStartConvCmd(adc.ADCx, ENABLE); } diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 97f6ee525b..961f81f9f1 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -29,33 +29,44 @@ #include "adc.h" #include "adc_impl.h" +#include "io.h" +#include "rcc.h" #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define ADC_DMA_CHANNEL DMA1_Channel1 #endif +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }, + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } +}; + +ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) +{ + if (instance == ADC1) + return ADCDEV_1; + + if (instance == ADC2) + return ADCDEV_2; + + return ADCINVALID; +} + void adcInit(drv_adc_config_t *init) { UNUSED(init); ADC_InitTypeDef ADC_InitStructure; DMA_InitTypeDef DMA_InitStructure; - GPIO_InitTypeDef GPIO_InitStructure; uint8_t i; uint8_t adcChannelCount = 0; memset(&adcConfig, 0, sizeof(adcConfig)); - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ; - -#ifdef VBAT_ADC_GPIO +#ifdef VBAT_ADC_PIN if (init->enableVBat) { - GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN; - GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; @@ -65,10 +76,10 @@ void adcInit(drv_adc_config_t *init) } #endif -#ifdef RSSI_ADC_GPIO +#ifdef RSSI_ADC_PIN if (init->enableRSSI) { - GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN; - GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; @@ -80,8 +91,8 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN; - GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; @@ -93,8 +104,8 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_GPIO if (init->enableExternal1) { - GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN; - GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure); + IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; @@ -104,13 +115,20 @@ void adcInit(drv_adc_config_t *init) } #endif - RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz - RCC_AHBPeriphClockCmd(ADC_AHB_PERIPHERAL | RCC_AHBPeriph_ADC12, ENABLE); + ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); + if (device == ADCINVALID) + return; + + adcDevice_t adc = adcHardware[device]; - DMA_DeInit(ADC_DMA_CHANNEL); + RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz + RCC_ClockCmd(adc.rccADC, ENABLE); + RCC_ClockCmd(adc.rccDMA, ENABLE); + + DMA_DeInit(adc.DMAy_Channelx); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC_INSTANCE->DR; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; DMA_InitStructure.DMA_BufferSize = adcChannelCount; @@ -122,20 +140,18 @@ void adcInit(drv_adc_config_t *init) DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(ADC_DMA_CHANNEL, &DMA_InitStructure); - - DMA_Cmd(ADC_DMA_CHANNEL, ENABLE); + DMA_Init(adc.DMAy_Channelx, &DMA_InitStructure); + DMA_Cmd(adc.DMAy_Channelx, ENABLE); // calibrate - ADC_VoltageRegulatorCmd(ADC_INSTANCE, ENABLE); + ADC_VoltageRegulatorCmd(adc.ADCx, ENABLE); delay(10); - ADC_SelectCalibrationMode(ADC_INSTANCE, ADC_CalibrationMode_Single); - ADC_StartCalibration(ADC_INSTANCE); - while(ADC_GetCalibrationStatus(ADC_INSTANCE) != RESET); - ADC_VoltageRegulatorCmd(ADC_INSTANCE, DISABLE); - + ADC_SelectCalibrationMode(adc.ADCx, ADC_CalibrationMode_Single); + ADC_StartCalibration(adc.ADCx); + while (ADC_GetCalibrationStatus(adc.ADCx) != RESET); + ADC_VoltageRegulatorCmd(adc.ADCx, DISABLE); ADC_CommonInitTypeDef ADC_CommonInitStructure; @@ -145,7 +161,7 @@ void adcInit(drv_adc_config_t *init) ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1; ADC_CommonInitStructure.ADC_DMAMode = ADC_DMAMode_Circular; ADC_CommonInitStructure.ADC_TwoSamplingDelay = 0; - ADC_CommonInit(ADC_INSTANCE, &ADC_CommonInitStructure); + ADC_CommonInit(adc.ADCx, &ADC_CommonInitStructure); ADC_StructInit(&ADC_InitStructure); @@ -158,24 +174,24 @@ void adcInit(drv_adc_config_t *init) ADC_InitStructure.ADC_AutoInjMode = ADC_AutoInjec_Disable; ADC_InitStructure.ADC_NbrOfRegChannel = adcChannelCount; - ADC_Init(ADC_INSTANCE, &ADC_InitStructure); + ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } - ADC_RegularChannelConfig(ADC_INSTANCE, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); + ADC_RegularChannelConfig(adc.ADCx, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); } - ADC_Cmd(ADC_INSTANCE, ENABLE); + ADC_Cmd(adc.ADCx, ENABLE); - while(!ADC_GetFlagStatus(ADC_INSTANCE, ADC_FLAG_RDY)); + while (!ADC_GetFlagStatus(adc.ADCx, ADC_FLAG_RDY)); - ADC_DMAConfig(ADC_INSTANCE, ADC_DMAMode_Circular); + ADC_DMAConfig(adc.ADCx, ADC_DMAMode_Circular); - ADC_DMACmd(ADC_INSTANCE, ENABLE); + ADC_DMACmd(adc.ADCx, ENABLE); - ADC_StartConversion(ADC_INSTANCE); + ADC_StartConversion(adc.ADCx); } diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 0d051fc118..93cabb4d79 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -23,6 +23,7 @@ #include "system.h" #include "io.h" +#include "rcc.h" #include "sensors/sensors.h" // FIXME dependency into the main code @@ -32,6 +33,25 @@ #include "adc.h" #include "adc_impl.h" +#ifndef ADC_INSTANCE +#define ADC_INSTANCE ADC1 +#endif + +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1 } +}; + +ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) +{ + if (instance == ADC1) + return ADCDEV_1; +/* + if (instance == ADC2) + return ADCDEV_2; +*/ + return ADCINVALID; +} void adcInit(drv_adc_config_t *init) { ADC_InitTypeDef ADC_InitStructure; @@ -92,14 +112,19 @@ void adcInit(drv_adc_config_t *init) //RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); - RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); + ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); + if (device == ADCINVALID) + return; + + adcDevice_t adc = adcHardware[device]; + + RCC_ClockCmd(adc.rccDMA, ENABLE); + RCC_ClockCmd(adc.rccADC, ENABLE); - DMA_DeInit(DMA2_Stream4); + DMA_DeInit(adc.DMAy_Streamx); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR; DMA_InitStructure.DMA_Channel = DMA_Channel_0; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; @@ -110,20 +135,9 @@ void adcInit(drv_adc_config_t *init) DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_Init(DMA2_Stream4, &DMA_InitStructure); + DMA_Init(adc.DMAy_Streamx, &DMA_InitStructure); - DMA_Cmd(DMA2_Stream4, ENABLE); - - // calibrate - - /* - ADC_VoltageRegulatorCmd(ADC1, ENABLE); - delay(10); - ADC_SelectCalibrationMode(ADC1, ADC_CalibrationMode_Single); - ADC_StartCalibration(ADC1); - while(ADC_GetCalibrationStatus(ADC1) != RESET); - ADC_VoltageRegulatorCmd(ADC1, DISABLE); - */ + DMA_Cmd(adc.DMAy_Streamx, ENABLE); ADC_CommonInitTypeDef ADC_CommonInitStructure; @@ -144,19 +158,19 @@ void adcInit(drv_adc_config_t *init) ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group - ADC_Init(ADC1, &ADC_InitStructure); + ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } - ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); + ADC_RegularChannelConfig(adc.ADCx, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); } - ADC_DMARequestAfterLastTransferCmd(ADC1, ENABLE); + ADC_DMARequestAfterLastTransferCmd(adc.ADCx, ENABLE); - ADC_DMACmd(ADC1, ENABLE); - ADC_Cmd(ADC1, ENABLE); + ADC_DMACmd(adc.ADCx, ENABLE); + ADC_Cmd(adc.ADCx, ENABLE); - ADC_SoftwareStartConv(ADC1); + ADC_SoftwareStartConv(adc.ADCx); } diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index e52c98d128..a391414cfd 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -123,8 +123,7 @@ //#define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 // alternative defaults for AlienFlight F3 target diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index e22152a86d..a7d3ed0128 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -133,7 +133,7 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP #define USE_ADC diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 965942e6e6..8e0839b515 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -93,20 +93,17 @@ #define USE_ADC -#define CURRENT_METER_ADC_GPIO GPIOB -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PB1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_PIN PA0 #define VBAT_ADC_CHANNEL ADC_Channel_0 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_0 +#define RSSI_ADC_PIN PB0 #define RSSI_ADC_CHANNEL ADC_Channel_8 #define LED_STRIP -#define LED_STRIP_TIMER TIM3 +#define LED_STRIP_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index d9ee885574..4e5faa607f 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -106,20 +106,16 @@ #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 #define ADC_DMA_CHANNEL DMA1_Channel1 -#define VBAT_ADC_GPIO GPIOC -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_PIN PC0 #define VBAT_ADC_CHANNEL ADC_Channel_6 -#define CURRENT_METER_ADC_GPIO GPIOC -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PC1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 -#define RSSI_ADC_GPIO GPIOC -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PC2 #define RSSI_ADC_CHANNEL ADC_Channel_8 -#define EXTERNAL1_ADC_GPIO GPIOC -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 +#define EXTERNAL1_ADC_PIN PC3 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 // IO - assuming 303 in 64pin package, TODO diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 527ffbbfbf..385a11535e 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -126,20 +126,16 @@ #define ADC_DMA_CHANNEL DMA1_Channel1 #define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_GPIO GPIOC -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_PIN PC0 #define VBAT_ADC_CHANNEL ADC_Channel_6 -#define CURRENT_METER_ADC_GPIO GPIOC -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PC1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 -#define RSSI_ADC_GPIO GPIOC -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PC2 #define RSSI_ADC_CHANNEL ADC_Channel_8 -#define EXTERNAL1_ADC_GPIO GPIOC -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 +#define EXTERNAL1_ADC_PIN PC3 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 #define LED_STRIP diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index b5ed0ec599..b0ed7d9e10 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -145,13 +145,11 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 // tqfp48 pin 14 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 // tqfp48 pin 15 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 // mpu_int definition in sensors/initialisation.c diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 0ff1e2f0b3..05b31ec8c3 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -98,20 +98,16 @@ #define USE_ADC -#define CURRENT_METER_ADC_GPIO GPIOB -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PB1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_4 -#define RSSI_ADC_GPIO GPIOA -#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_PIN PA1 #define RSSI_ADC_CHANNEL ADC_Channel_1 -#define EXTERNAL1_ADC_GPIO GPIOA -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_PIN PA5 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 //#define LED_STRIP diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index afba1e344e..74e54c1f10 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -155,16 +155,13 @@ #define ADC_DMA_CHANNEL DMA1_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_PIN PA0 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define RSSI_ADC_GPIO GPIOA -#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_PIN PA1 #define RSSI_ADC_CHANNEL ADC_Channel_2 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_2 +#define CURRENT_METER_ADC_PIN PA2 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_3 #define LED_STRIP diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 963fc0e6bf..dd60a320a0 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -80,7 +80,6 @@ #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 -#define M25P16_CS_GPIO GPIOB #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 @@ -92,21 +91,18 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE /* diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 211b0ea211..a0ef949db1 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -77,16 +77,13 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 44ef0818c2..d67e64b030 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -93,25 +93,21 @@ #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 #define ADC_DMA_CHANNEL DMA1_Channel1 -#define VBAT_ADC_GPIO GPIOC -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_PIN PC0 #define VBAT_ADC_CHANNEL ADC_Channel_6 -#define CURRENT_METER_ADC_GPIO GPIOC -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PC1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 -#define RSSI_ADC_GPIO GPIOC -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PC2 #define RSSI_ADC_CHANNEL ADC_Channel_8 -#define EXTERNAL1_ADC_GPIO GPIOC -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 +#define EXTERNAL1_ADC_PIN PC3 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 -#define LED_STRIP -#define LED_STRIP_TIMER TIM16 +#define LED_STRIP +#define LED_STRIP_TIMER TIM16 #define WS2811_GPIO GPIOA #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index d4f03bfec8..81fb0b8ec8 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -124,16 +124,13 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_5 +#define VBAT_ADC_PIN PA5 #define VBAT_ADC_CHANNEL ADC_Channel_2 -//#define CURRENT_METER_ADC_GPIO GPIOA -//#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +//#define CURRENT_METER_ADC_PIN PA5 //#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index fffbe42b41..3a59bbb307 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -33,8 +33,8 @@ #define BARO_XCLR_PIN PC13 #define BARO_EOC_PIN PC14 -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 #define USE_EXTI @@ -148,25 +148,21 @@ #define USE_ADC -#define CURRENT_METER_ADC_GPIO GPIOB -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PB1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_4 -#define RSSI_ADC_GPIO GPIOA -#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_PIN PA1 #define RSSI_ADC_CHANNEL ADC_Channel_1 -#define EXTERNAL1_ADC_GPIO GPIOA -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_PIN PA5 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 #define LED_STRIP -#define LED_STRIP_TIMER TIM3 +#define LED_STRIP_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 8a0b35cf2c..d9ff0b9bd5 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -84,20 +84,16 @@ #define USE_ADC -#define CURRENT_METER_ADC_GPIO GPIOB -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PB1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_4 -#define RSSI_ADC_GPIO GPIOA -#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_PIN PA1 #define RSSI_ADC_CHANNEL ADC_Channel_1 -#define EXTERNAL1_ADC_GPIO GPIOA -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_PIN PA5 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 //#define LED_STRIP diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 9eb39d6dbf..b4523502d9 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -160,25 +160,21 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC2 #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 +#define LED_STRIP_TIMER TIM1 #define WS2811_GPIO GPIOA #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 8a25943114..b190deadc1 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -92,21 +92,18 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_2 +#define CURRENT_METER_ADC_PIN PA2 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_3 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_5 +#define VBAT_ADC_PIN PA5 #define VBAT_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #if 1 -#define LED_STRIP_TIMER TIM16 +#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 #define WS2811_GPIO GPIOB diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index a4b4df7a53..4d73c24e86 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -116,20 +116,16 @@ #define USE_ADC -#define CURRENT_METER_ADC_GPIO GPIOB -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PB1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_4 -#define RSSI_ADC_GPIO GPIOA -#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 +#define RSSI_ADC_PIN PA1 #define RSSI_ADC_CHANNEL ADC_Channel_1 -#define EXTERNAL1_ADC_GPIO GPIOA -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5 +#define EXTERNAL1_ADC_PIN PA5 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 //#define LED_STRIP diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 4c3972a69a..49734248be 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -112,11 +112,11 @@ #define CURRENT_METER_ADC_PIN PC1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_11 -#define VBAT_ADC_PIN PC2 -#define VBAT_ADC_CHANNEL ADC_Channel_12 +#define VBAT_ADC_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_Channel_12 -#define RSSI_ADC_GPIO_PIN PA0 -#define RSSI_ADC_CHANNEL ADC_Channel_0 +#define RSSI_ADC_GPIO_PIN PA0 +#define RSSI_ADC_CHANNEL ADC_Channel_0 #define SENSORS_SET (SENSOR_ACC) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 3bf23f6916..4cb76f81ac 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -112,20 +112,17 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 +#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_GPIO GPIOA diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 298d183b42..4ea4fbf1a7 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -97,8 +97,7 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOB -#define VBAT_ADC_GPIO_PIN GPIO_Pin_2 +#define VBAT_ADC_PIN PB2 #define VBAT_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index c27b1ec689..9ebf4d9a7c 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -143,8 +143,7 @@ #define ADC_DMA_CHANNEL DMA1_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_PIN PA0 #define VBAT_ADC_CHANNEL ADC_Channel_1 //#define USE_QUAD_MIXER_ONLY diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 29b764d9f8..c8dfb4a37b 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -87,37 +87,24 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) -#define I2C2_SCL_GPIO GPIOA -#define I2C2_SCL_GPIO_AF GPIO_AF_4 -#define I2C2_SCL_PIN GPIO_Pin_9 -#define I2C2_SCL_PIN_SOURCE GPIO_PinSource9 -#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOA -#define I2C2_SDA_GPIO GPIOA -#define I2C2_SDA_GPIO_AF GPIO_AF_4 -#define I2C2_SDA_PIN GPIO_Pin_10 -#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 -#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA - #define USE_ADC #define ADC_INSTANCE ADC2 #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_7 +#define CURRENT_METER_ADC_PIN PA7 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_4 -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define LED_STRIP #if 1 // LED strip configuration using PWM motor output pin 5. -#define LED_STRIP_TIMER TIM16 +#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 #define WS2811_GPIO GPIOA diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 929a5f3fd3..1614539b4b 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -120,20 +120,17 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 +#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_GPIO GPIOA diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index d71254270c..88ad9d70b2 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -141,20 +141,17 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 +#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_GPIO GPIOA diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 7c18fb2356..2cc9becfa5 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -148,20 +148,17 @@ #define ADC_DMA_CHANNEL DMA2_Channel1 #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define VBAT_ADC_GPIO GPIOA -#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_PIN PA4 #define VBAT_ADC_CHANNEL ADC_Channel_1 -#define CURRENT_METER_ADC_GPIO GPIOA -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 -#define RSSI_ADC_GPIO GPIOB -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PB2 #define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 +#define LED_STRIP_TIMER TIM1 #define WS2811_GPIO GPIOA #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 1ddf2de9a1..9039292a83 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -144,24 +144,20 @@ #define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 #define ADC_DMA_CHANNEL DMA1_Channel1 -#define VBAT_ADC_GPIO GPIOC -#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_PIN PC0 #define VBAT_ADC_CHANNEL ADC_Channel_6 -#define CURRENT_METER_ADC_GPIO GPIOC -#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_PIN PC1 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 -#define RSSI_ADC_GPIO GPIOC -#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_PIN PC2 #define RSSI_ADC_CHANNEL ADC_Channel_8 -#define EXTERNAL1_ADC_GPIO GPIOC -#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 +#define EXTERNAL1_ADC_PIN PC3 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 #define LED_STRIP -#define LED_STRIP_TIMER TIM16 +#define LED_STRIP_TIMER TIM16 #define WS2811_GPIO GPIOB #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB #define WS2811_GPIO_AF GPIO_AF_1 From 71353066b81d8f970d01a75f9643560388a737bf Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 19 Jun 2016 21:45:03 +1000 Subject: [PATCH 08/41] Remove need to specify ADC Channel (only 1 per pin). --- src/main/drivers/adc.c | 8 ++++ src/main/drivers/adc_impl.h | 27 +++++++++++- src/main/drivers/adc_stm32f10x.c | 21 ++++++++-- src/main/drivers/adc_stm32f30x.c | 50 +++++++++++++++++++++-- src/main/drivers/adc_stm32f4xx.c | 48 ++++++++++++++++++---- src/main/drivers/resource.h | 4 +- src/main/drivers/timer.h | 3 +- src/main/io/serial_cli.c | 4 +- src/main/target/ALIENFLIGHTF3/target.h | 5 --- src/main/target/ALIENFLIGHTF4/target.h | 8 ---- src/main/target/BLUEJAYF4/target.h | 1 - src/main/target/CC3D/target.h | 6 --- src/main/target/CHEBUZZF3/target.h | 11 ----- src/main/target/COLIBRI_RACE/target.h | 25 ++---------- src/main/target/DOGE/target.h | 26 +----------- src/main/target/EUSTM32F103RC/target.h | 8 ---- src/main/target/FURYF3/target.h | 11 +---- src/main/target/FURYF4/target.h | 10 +---- src/main/target/IRCFUSIONF3/target.h | 10 ----- src/main/target/KISSFC/target.h | 11 +---- src/main/target/LUX_RACE/target.h | 12 ------ src/main/target/MOTOLAB/target.h | 9 ---- src/main/target/NAZE/target.h | 8 ---- src/main/target/OLIMEXINO/target.h | 8 ---- src/main/target/OMNIBUS/target.h | 10 +---- src/main/target/PIKOBLX/target.h | 9 ---- src/main/target/PORT103R/target.h | 13 ------ src/main/target/REVO/target.h | 16 +++----- src/main/target/REVONANO/target.h | 9 ---- src/main/target/RMDO/target.h | 10 ----- src/main/target/SINGULARITY/target.h | 4 -- src/main/target/SIRINFPV/target.h | 4 -- src/main/target/SPARKY/target.h | 7 ---- src/main/target/SPRACINGF3/target.h | 10 ----- src/main/target/SPRACINGF3EVO/target.h | 9 ---- src/main/target/SPRACINGF3MINI/target.h | 8 ---- src/main/target/STM32F3DISCOVERY/target.h | 11 ----- 37 files changed, 160 insertions(+), 294 deletions(-) diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 37433d1311..672b4c469d 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -33,6 +33,14 @@ adc_config_t adcConfig[ADC_CHANNEL_COUNT]; volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; +uint8_t adcChannelByPin(ioTag_t pin) +{ + for (uint8_t i = 0; i < ARRAYLEN(adcPinMap); i++) { + if (pin == adcPinMap[i].pin) + return adcPinMap[i].channel; + } + return 0; +} uint16_t adcGetChannel(uint8_t channel) { diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index bafff8accb..1d72b4b357 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -17,27 +17,52 @@ #pragma once +#include "io.h" #include "rcc.h" +#if defined(STM32F4) +#define ADC_PIN_MAP_COUNT 16 +#elif defined(STM32F3) +#define ADC_PIN_MAP_COUNT 39 +#else +#define ADC_PIN_MAP_COUNT 10 +#endif + typedef enum ADCDevice { ADCINVALID = -1, ADCDEV_1 = 0, +#if defined(STM32F3) + ADCDEV_2, + ADCDEV_MAX = ADCDEV_2, +#elif defined(STM32F4) ADCDEV_2, ADCDEV_3, - ADCDEV_MAX = ADCDEV_3, + ADCDEV_MAX = ADCDEV_3, +#else + ADCDEV_MAX = ADCDEV_1, +#endif } ADCDevice; +typedef struct adcPinMap_s { + ioTag_t pin; + uint8_t channel; +} adcPinMap_t; + typedef struct adcDevice_s { ADC_TypeDef* ADCx; rccPeriphTag_t rccADC; rccPeriphTag_t rccDMA; #if defined(STM32F4) DMA_Stream_TypeDef* DMAy_Streamx; + uint32_t channel; #else DMA_Channel_TypeDef* DMAy_Channelx; #endif } adcDevice_t; extern const adcDevice_t adcHardware[]; +extern const adcPinMap_t adcPinMap[ADC_PIN_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; + +uint8_t adcChannelByPin(ioTag_t pin); \ No newline at end of file diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 8ed04874b4..2532bf4ad6 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -54,6 +54,19 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) return ADCINVALID; } +const adcPinMap_t adcPinMap[] = { + { .pin = DEFIO_TAG_E__PA0, .channel = ADC_Channel_0 }, // ADC12 + { .pin = DEFIO_TAG_E__PA1, .channel = ADC_Channel_1 }, // ADC12 + { .pin = DEFIO_TAG_E__PA2, .channel = ADC_Channel_2 }, // ADC12 + { .pin = DEFIO_TAG_E__PA3, .channel = ADC_Channel_3 }, // ADC12 + { .pin = DEFIO_TAG_E__PA4, .channel = ADC_Channel_4 }, // ADC12 + { .pin = DEFIO_TAG_E__PA5, .channel = ADC_Channel_5 }, // ADC12 + { .pin = DEFIO_TAG_E__PA6, .channel = ADC_Channel_6 }, // ADC12 + { .pin = DEFIO_TAG_E__PA7, .channel = ADC_Channel_7 }, // ADC12 + { .pin = DEFIO_TAG_E__PB0, .channel = ADC_Channel_8 }, // ADC12 + { .pin = DEFIO_TAG_E__PB1, .channel = ADC_Channel_9 }, // ADC12 +}; + // Driver for STM32F103CB onboard ADC // // Naze32 @@ -80,7 +93,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableVBat) { IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; + adcConfig[ADC_BATTERY].adcChannel = adcChannelByPin(IO_TAG(VBAT_ADC_PIN)); adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; @@ -91,7 +104,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; + adcConfig[ADC_RSSI].adcChannel = adcChannelByPin(IO_TAG(RSSI_ADC_PIN)); adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5; @@ -102,7 +115,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; + adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByPin(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; @@ -113,7 +126,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; + adcConfig[ADC_CURRENT].adcChannel = adcChannelByPin(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 961f81f9f1..b9b4f6f8e1 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -41,6 +41,48 @@ const adcDevice_t adcHardware[] = { { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } }; +const adcPinMap_t adcPinMap[] = { + { .pin = DEFIO_TAG_E__PA0, .channel = ADC_Channel_1 }, // ADC1 + { .pin = DEFIO_TAG_E__PA1, .channel = ADC_Channel_2 }, // ADC1 + { .pin = DEFIO_TAG_E__PA2, .channel = ADC_Channel_3 }, // ADC1 + { .pin = DEFIO_TAG_E__PA3, .channel = ADC_Channel_4 }, // ADC1 + { .pin = DEFIO_TAG_E__PA4, .channel = ADC_Channel_1 }, // ADC2 + { .pin = DEFIO_TAG_E__PA5, .channel = ADC_Channel_2 }, // ADC2 + { .pin = DEFIO_TAG_E__PA6, .channel = ADC_Channel_3 }, // ADC2 + { .pin = DEFIO_TAG_E__PA7, .channel = ADC_Channel_4 }, // ADC2 + { .pin = DEFIO_TAG_E__PB0, .channel = ADC_Channel_12 }, // ADC3 + { .pin = DEFIO_TAG_E__PB1, .channel = ADC_Channel_1 }, // ADC3 + { .pin = DEFIO_TAG_E__PB2, .channel = ADC_Channel_12 }, // ADC2 + { .pin = DEFIO_TAG_E__PB12, .channel = ADC_Channel_3 }, // ADC4 + { .pin = DEFIO_TAG_E__PB13, .channel = ADC_Channel_5 }, // ADC3 + { .pin = DEFIO_TAG_E__PB14, .channel = ADC_Channel_4 }, // ADC4 + { .pin = DEFIO_TAG_E__PB15, .channel = ADC_Channel_5 }, // ADC4 + { .pin = DEFIO_TAG_E__PC0, .channel = ADC_Channel_6 }, // ADC12 + { .pin = DEFIO_TAG_E__PC1, .channel = ADC_Channel_7 }, // ADC12 + { .pin = DEFIO_TAG_E__PC2, .channel = ADC_Channel_8 }, // ADC12 + { .pin = DEFIO_TAG_E__PC3, .channel = ADC_Channel_9 }, // ADC12 + { .pin = DEFIO_TAG_E__PC4, .channel = ADC_Channel_5 }, // ADC2 + { .pin = DEFIO_TAG_E__PC5, .channel = ADC_Channel_11 }, // ADC2 + { .pin = DEFIO_TAG_E__PD8, .channel = ADC_Channel_12 }, // ADC4 + { .pin = DEFIO_TAG_E__PD9, .channel = ADC_Channel_13 }, // ADC4 + { .pin = DEFIO_TAG_E__PD10, .channel = ADC_Channel_7 }, // ADC34 + { .pin = DEFIO_TAG_E__PD11, .channel = ADC_Channel_8 }, // ADC34 + { .pin = DEFIO_TAG_E__PD12, .channel = ADC_Channel_9 }, // ADC34 + { .pin = DEFIO_TAG_E__PD13, .channel = ADC_Channel_10 }, // ADC34 + { .pin = DEFIO_TAG_E__PD14, .channel = ADC_Channel_11 }, // ADC34 + { .pin = DEFIO_TAG_E__PE7, .channel = ADC_Channel_13 }, // ADC3 + { .pin = DEFIO_TAG_E__PE8, .channel = ADC_Channel_6 }, // ADC34 + { .pin = DEFIO_TAG_E__PE9, .channel = ADC_Channel_2 }, // ADC3 + { .pin = DEFIO_TAG_E__PE10, .channel = ADC_Channel_14 }, // ADC3 + { .pin = DEFIO_TAG_E__PE11, .channel = ADC_Channel_15 }, // ADC3 + { .pin = DEFIO_TAG_E__PE12, .channel = ADC_Channel_16 }, // ADC3 + { .pin = DEFIO_TAG_E__PE13, .channel = ADC_Channel_3 }, // ADC3 + { .pin = DEFIO_TAG_E__PE14, .channel = ADC_Channel_1 }, // ADC4 + { .pin = DEFIO_TAG_E__PE15, .channel = ADC_Channel_2 }, // ADC4 + { .pin = DEFIO_TAG_E__PF2, .channel = ADC_Channel_10 }, // ADC12 + { .pin = DEFIO_TAG_E__PF4, .channel = ADC_Channel_5 }, // ADC1 +}; + ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { if (instance == ADC1) @@ -68,7 +110,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; + adcConfig[ADC_BATTERY].adcChannel = adcChannelByPin(IO_TAG(VBAT_ADC_PIN)); adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_BATTERY].enabled = true; @@ -81,7 +123,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; + adcConfig[ADC_RSSI].adcChannel = adcChannelByPin(IO_TAG(RSSI_ADC_PIN)); adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_RSSI].enabled = true; @@ -94,7 +136,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; + adcConfig[ADC_CURRENT].adcChannel = adcChannelByPin(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_CURRENT].enabled = true; @@ -107,7 +149,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; + adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByPin(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_EXTERNAL1].enabled = true; diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 93cabb4d79..44eb3b7c96 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -23,6 +23,7 @@ #include "system.h" #include "io.h" +#include "io_impl.h" #include "rcc.h" #include "sensors/sensors.h" // FIXME dependency into the main code @@ -38,8 +39,38 @@ #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4 }, - //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } +}; + +/* note these could be packed up for saving space */ +const adcPinMap_t adcPinMap[] = { +/* + { .pin = DEFIO_TAG_E__PF3, .channel = ADC_Channel_9 }, + { .pin = DEFIO_TAG_E__PF4, .channel = ADC_Channel_14 }, + { .pin = DEFIO_TAG_E__PF5, .channel = ADC_Channel_15 }, + { .pin = DEFIO_TAG_E__PF6, .channel = ADC_Channel_4 }, + { .pin = DEFIO_TAG_E__PF7, .channel = ADC_Channel_5 }, + { .pin = DEFIO_TAG_E__PF8, .channel = ADC_Channel_6 }, + { .pin = DEFIO_TAG_E__PF9, .channel = ADC_Channel_7 }, + { .pin = DEFIO_TAG_E__PF10, .channel = ADC_Channel_8 }, +*/ + { .pin = DEFIO_TAG_E__PC0, .channel = ADC_Channel_10 }, + { .pin = DEFIO_TAG_E__PC1, .channel = ADC_Channel_11 }, + { .pin = DEFIO_TAG_E__PC2, .channel = ADC_Channel_12 }, + { .pin = DEFIO_TAG_E__PC3, .channel = ADC_Channel_13 }, + { .pin = DEFIO_TAG_E__PC4, .channel = ADC_Channel_14 }, + { .pin = DEFIO_TAG_E__PC5, .channel = ADC_Channel_15 }, + { .pin = DEFIO_TAG_E__PB0, .channel = ADC_Channel_8 }, + { .pin = DEFIO_TAG_E__PB1, .channel = ADC_Channel_9 }, + { .pin = DEFIO_TAG_E__PA0, .channel = ADC_Channel_0 }, + { .pin = DEFIO_TAG_E__PA1, .channel = ADC_Channel_1 }, + { .pin = DEFIO_TAG_E__PA2, .channel = ADC_Channel_2 }, + { .pin = DEFIO_TAG_E__PA3, .channel = ADC_Channel_3 }, + { .pin = DEFIO_TAG_E__PA4, .channel = ADC_Channel_4 }, + { .pin = DEFIO_TAG_E__PA5, .channel = ADC_Channel_5 }, + { .pin = DEFIO_TAG_E__PA6, .channel = ADC_Channel_6 }, + { .pin = DEFIO_TAG_E__PA7, .channel = ADC_Channel_7 }, }; ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) @@ -47,11 +78,12 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) if (instance == ADC1) return ADCDEV_1; /* - if (instance == ADC2) + if (instance == ADC2) // TODO add ADC2 and 3 return ADCDEV_2; */ return ADCINVALID; } + void adcInit(drv_adc_config_t *init) { ADC_InitTypeDef ADC_InitStructure; @@ -70,7 +102,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableVBat) { IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; + adcConfig[ADC_BATTERY].adcChannel = adcChannelByPin(IO_TAG(VBAT_ADC_PIN)); //VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_480Cycles; @@ -81,7 +113,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; + adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByPin(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_480Cycles; @@ -92,7 +124,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; + adcConfig[ADC_RSSI].adcChannel = adcChannelByPin(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_480Cycles; @@ -103,7 +135,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; + adcConfig[ADC_CURRENT].adcChannel = adcChannelByPin(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_480Cycles; @@ -125,7 +157,7 @@ void adcInit(drv_adc_config_t *init) DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR; - DMA_InitStructure.DMA_Channel = DMA_Channel_0; + DMA_InitStructure.DMA_Channel = adc.channel; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; DMA_InitStructure.DMA_BufferSize = configuredAdcChannels; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index dc1d71f3d5..a81739bf12 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -23,7 +23,9 @@ typedef enum { OWNER_FLASH, OWNER_USB, OWNER_BEEPER, - OWNER_OSD + OWNER_OSD, + OWNER_BARO, + OWNER_TOTAL_COUNT } resourceOwner_t; // Currently TIMER should be shared resource (softserial dualtimer and timerqueue needs to allocate timer channel, but pin can be used for other function) diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index ce09e14746..0cd5f684ec 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -47,9 +47,10 @@ typedef uint32_t timCCER_t; typedef uint32_t timSR_t; typedef uint32_t timCNT_t; #else -# error "Unknown CPU defined" +#error "Unknown CPU defined" #endif + // use different types from capture and overflow - multiple overflow handlers are implemented as linked list struct timerCCHandlerRec_s; struct timerOvrHandlerRec_s; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c33e6838db..36782a2d67 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3038,7 +3038,7 @@ void cliProcess(void) } } -const char * const ownerNames[] = { +const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM IN", "PPM IN", @@ -3060,6 +3060,8 @@ const char * const ownerNames[] = { "FLASH", "USB", "BEEPER", + "OSD", + "BARO", }; static void cliResource(char *cmdline) diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index a391414cfd..9a8522b83d 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -118,13 +118,8 @@ #define USE_ADC #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - //#define BOARD_HAS_VOLTAGE_DIVIDER - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 // alternative defaults for AlienFlight F3 target #define ALIENFLIGHT diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 88ce6a5791..f52c192fa2 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -152,18 +152,10 @@ #define USE_ADC //#define BOARD_HAS_VOLTAGE_DIVIDER - #define VBAT_ADC_PIN PC0 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PC1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_0 - #define RSSI_ADC_PIN PC4 -#define RSSI_ADC_CHANNEL ADC_Channel_4 - #define EXTERNAL1_ADC_GPIO_PIN PC5 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 // LED strip configuration using RC5 pin. //#define LED_STRIP diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index a7d3ed0128..0c7613b624 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -138,7 +138,6 @@ #define USE_ADC #define VBAT_ADC_PIN PC3 -#define VBAT_ADC_CHANNEL ADC_Channel_13 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 8e0839b515..a0415670d0 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -92,15 +92,9 @@ #define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 #define USE_ADC - #define CURRENT_METER_ADC_PIN PB1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 - #define VBAT_ADC_PIN PA0 -#define VBAT_ADC_CHANNEL ADC_Channel_0 - #define RSSI_ADC_PIN PB0 -#define RSSI_ADC_CHANNEL ADC_Channel_8 #define LED_STRIP #define LED_STRIP_TIMER TIM3 diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 4e5faa607f..5e7f79ce05 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -101,22 +101,11 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC - #define ADC_INSTANCE ADC1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define ADC_DMA_CHANNEL DMA1_Channel1 - #define VBAT_ADC_PIN PC0 -#define VBAT_ADC_CHANNEL ADC_Channel_6 - #define CURRENT_METER_ADC_PIN PC1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 - #define RSSI_ADC_PIN PC2 -#define RSSI_ADC_CHANNEL ADC_Channel_8 - #define EXTERNAL1_ADC_PIN PC3 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 // IO - assuming 303 in 64pin package, TODO #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 385a11535e..68bf284483 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -103,16 +103,8 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) -#define I2C2_SCL_GPIO GPIOA -#define I2C2_SCL_GPIO_AF GPIO_AF_4 -#define I2C2_SCL_PIN GPIO_Pin_9 -#define I2C2_SCL_PIN_SOURCE GPIO_PinSource9 -#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOA -#define I2C2_SDA_GPIO GPIOA -#define I2C2_SDA_GPIO_AF GPIO_AF_4 -#define I2C2_SDA_PIN GPIO_Pin_10 -#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 -#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA +#define I2C2_SCL_PIN PA9 +#define I2C2_SDA_PIN PA10 #define USE_BST #define BST_DEVICE (BSTDEV_1) @@ -120,28 +112,17 @@ #define BST_CRC_POLYNOM 0xD5 #define USE_ADC - #define ADC_INSTANCE ADC1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define ADC_DMA_CHANNEL DMA1_Channel1 - #define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PC0 -#define VBAT_ADC_CHANNEL ADC_Channel_6 - #define CURRENT_METER_ADC_PIN PC1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 - #define RSSI_ADC_PIN PC2 -#define RSSI_ADC_CHANNEL ADC_Channel_8 - #define EXTERNAL1_ADC_PIN PC3 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 #define LED_STRIP #define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG -#define LED_STRIP_TIMER TIM16 +#define LED_STRIP_TIMER TIM16 #define WS2811_GPIO GPIOA #define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index b0ed7d9e10..44a16782b1 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -34,44 +34,29 @@ #define BEEPER_INVERTED // tqfp48 pin 3 -#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOC -#define MPU6500_CS_GPIO GPIOC #define MPU6500_CS_PIN PC14 #define MPU6500_SPI_INSTANCE SPI1 // tqfp48 pin 25 -#define BMP280_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOB -#define BMP280_CS_GPIO GPIOB -#define BMP280_CS_PIN GPIO_Pin_12 +#define BMP280_CS_PIN PB12 #define BMP280_SPI_INSTANCE SPI2 #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 -#define SPI1_GPIO GPIOB -#define SPI1_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB // tqfp48 pin 39 #define SPI1_SCK_PIN PB3 -#define SPI1_SCK_PIN_SOURCE GPIO_PinSource3 // tqfp48 pin 40 #define SPI1_MISO_PIN PB4 -#define SPI1_MISO_PIN_SOURCE GPIO_PinSource4 // tqfp48 pin 41 #define SPI1_MOSI_PIN PB5 -#define SPI1_MOSI_PIN_SOURCE GPIO_PinSource5 - -#define SPI2_GPIO GPIOB -#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB // tqfp48 pin 26 #define SPI2_SCK_PIN PB13 -#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13 // tqfp48 pin 27 #define SPI2_MISO_PIN PB14 -#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14 // tqfp48 pin 28 #define SPI2_MOSI_PIN PB15 -#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15 #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -139,18 +124,9 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC2 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 -#define ADC_DMA_CHANNEL DMA2_Channel1 - -// tqfp48 pin 14 #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - -// tqfp48 pin 15 #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 // mpu_int definition in sensors/initialisation.c #define USE_EXTI diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 05b31ec8c3..d6d916bb28 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -97,18 +97,10 @@ // #define SOFT_I2C_PB67 #define USE_ADC - #define CURRENT_METER_ADC_PIN PB1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_4 - #define RSSI_ADC_PIN PA1 -#define RSSI_ADC_CHANNEL ADC_Channel_1 - #define EXTERNAL1_ADC_PIN PA5 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 //#define LED_STRIP #define LED_STRIP_TIMER TIM3 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 74e54c1f10..6201b8b3f0 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -150,22 +150,13 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC1 -#define ADC_DMA_CHANNEL DMA1_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 - #define VBAT_ADC_PIN PA0 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define RSSI_ADC_PIN PA1 -#define RSSI_ADC_CHANNEL ADC_Channel_2 - #define CURRENT_METER_ADC_PIN PA2 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_3 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 +#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define WS2811_GPIO GPIOA diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 0c524c3e6e..a6586c95cd 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -144,15 +144,9 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - -#define VBAT_ADC_PIN PC1 -#define VBAT_ADC_CHANNEL ADC_Channel_11 - -#define RSSI_ADC_GPIO_PIN PC2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 - +#define VBAT_ADC_PIN PC1 +#define RSSI_ADC_GPIO_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_13 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index dd60a320a0..9588159722 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -85,20 +85,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 #define SPEKTRUM_BIND // USART3, diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index a0ef949db1..03c3cf31f6 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -72,20 +72,11 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA - +//#define USE_ADC #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 - #define SPEKTRUM_BIND diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index d67e64b030..6292d2435f 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -88,23 +88,11 @@ #define I2C_DEVICE (I2CDEV_2) #define USE_ADC - #define ADC_INSTANCE ADC1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define ADC_DMA_CHANNEL DMA1_Channel1 - #define VBAT_ADC_PIN PC0 -#define VBAT_ADC_CHANNEL ADC_Channel_6 - #define CURRENT_METER_ADC_PIN PC1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 - #define RSSI_ADC_PIN PC2 -#define RSSI_ADC_CHANNEL ADC_Channel_8 - #define EXTERNAL1_ADC_PIN PC3 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 - #define LED_STRIP #define LED_STRIP_TIMER TIM16 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 81fb0b8ec8..27bf98691b 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -119,19 +119,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA5 -#define VBAT_ADC_CHANNEL ADC_Channel_2 - //#define CURRENT_METER_ADC_PIN PA5 -//#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #if 1 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 3a59bbb307..9f6e3d2b22 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -147,18 +147,10 @@ // #define SOFT_I2C_PB67 #define USE_ADC - #define CURRENT_METER_ADC_PIN PB1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_4 - #define RSSI_ADC_PIN PA1 -#define RSSI_ADC_CHANNEL ADC_Channel_1 - #define EXTERNAL1_ADC_PIN PA5 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 #define LED_STRIP diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index d9ff0b9bd5..b7972ad777 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -83,18 +83,10 @@ // #define SOFT_I2C_PB67 #define USE_ADC - #define CURRENT_METER_ADC_PIN PB1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_4 - #define RSSI_ADC_PIN PA1 -#define RSSI_ADC_CHANNEL ADC_Channel_1 - #define EXTERNAL1_ADC_PIN PA5 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 //#define LED_STRIP //#define LED_STRIP_TIMER TIM3 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index b4523502d9..3384558a93 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -159,19 +159,11 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 + #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index b190deadc1..8b14570c91 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -87,19 +87,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define CURRENT_METER_ADC_PIN PA2 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_3 - #define VBAT_ADC_PIN PA5 -#define VBAT_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #if 1 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 4d73c24e86..64e0b6455c 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -38,22 +38,17 @@ #define USE_SPI_DEVICE_2 #define PORT103R_SPI_INSTANCE SPI2 -#define PORT103R_SPI_CS_GPIO GPIOB #define PORT103R_SPI_CS_PIN PB12 // We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: -#define M25P16_CS_GPIO PORT103R_SPI_CS_GPIO #define M25P16_CS_PIN PORT103R_SPI_CS_PIN #define M25P16_SPI_INSTANCE PORT103R_SPI_INSTANCE -#define MPU6000_CS_GPIO PORT103R_SPI_CS_GPIO #define MPU6000_CS_PIN PORT103R_SPI_CS_PIN #define MPU6000_SPI_INSTANCE PORT103R_SPI_INSTANCE -#define MPU6500_CS_GPIO PORT103R_SPI_CS_GPIO #define MPU6500_CS_PIN PORT103R_SPI_CS_PIN #define MPU6500_SPI_INSTANCE PORT103R_SPI_INSTANCE -#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB #define GYRO #define USE_FAKE_GYRO @@ -115,18 +110,10 @@ // #define SOFT_I2C_PB67 #define USE_ADC - #define CURRENT_METER_ADC_PIN PB1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_4 - #define RSSI_ADC_PIN PA1 -#define RSSI_ADC_CHANNEL ADC_Channel_1 - #define EXTERNAL1_ADC_PIN PA5 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5 //#define LED_STRIP //#define LED_STRIP_TIMER TIM3 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 49734248be..50f5790a3e 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -32,11 +32,11 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -#define LED0 PB5 -#define LED1 PB4 -#define BEEPER PB4 -#define INVERTER PC0 // PC0 used as inverter select GPIO -#define INVERTER_USART USART1 +#define LED0 PB5 +#define LED1 PB4 +#define BEEPER PB4 +#define INVERTER PC0 // PC0 used as inverter select GPIO +#define INVERTER_USART USART1 #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -110,14 +110,10 @@ #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_11 - #define VBAT_ADC_PIN PC2 -#define VBAT_ADC_CHANNEL ADC_Channel_12 - #define RSSI_ADC_GPIO_PIN PA0 -#define RSSI_ADC_CHANNEL ADC_Channel_0 + #define SENSORS_SET (SENSOR_ACC) //#define LED_STRIP diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 3769f0f701..b3d1a4941f 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -87,18 +87,9 @@ #define I2C_DEVICE (I2CDEV_3) #define USE_ADC - -//FLEXI-IO 6 #define CURRENT_METER_ADC_PIN PA7 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 - -//FLEXI-IO 7 #define VBAT_ADC_PIN PA6 -#define VBAT_ADC_CHANNEL ADC_Channel_6 - -//FLEXI-IO 8 #define RSSI_ADC_PIN PA5 -#define RSSI_ADC_CHANNEL ADC_Channel_5 //#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 4cb76f81ac..779b8ab6ce 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -106,20 +106,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 4ea4fbf1a7..22d0c20c07 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -94,11 +94,7 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PB2 -#define VBAT_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 9ebf4d9a7c..3417c7e09b 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -140,11 +140,7 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define ADC_INSTANCE ADC1 -#define ADC_DMA_CHANNEL DMA1_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 - #define VBAT_ADC_PIN PA0 -#define VBAT_ADC_CHANNEL ADC_Channel_1 //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index c8dfb4a37b..58101f0b85 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -88,16 +88,9 @@ #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) #define USE_ADC - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA7 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_4 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 1614539b4b..b78d16dec9 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -114,20 +114,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 88ad9d70b2..5e0d3ef6fd 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -136,19 +136,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 2cc9becfa5..3c7bea7140 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -145,17 +145,9 @@ #define ADC_INSTANCE ADC2 -#define ADC_DMA_CHANNEL DMA2_Channel1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 - #define VBAT_ADC_PIN PA4 -#define VBAT_ADC_CHANNEL ADC_Channel_1 - #define CURRENT_METER_ADC_PIN PA5 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 - #define RSSI_ADC_PIN PB2 -#define RSSI_ADC_CHANNEL ADC_Channel_12 #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 9039292a83..947ad4913a 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -139,22 +139,11 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC - #define ADC_INSTANCE ADC1 -#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 -#define ADC_DMA_CHANNEL DMA1_Channel1 - #define VBAT_ADC_PIN PC0 -#define VBAT_ADC_CHANNEL ADC_Channel_6 - #define CURRENT_METER_ADC_PIN PC1 -#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 - #define RSSI_ADC_PIN PC2 -#define RSSI_ADC_CHANNEL ADC_Channel_8 - #define EXTERNAL1_ADC_PIN PC3 -#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 #define LED_STRIP #define LED_STRIP_TIMER TIM16 From 258e9c26b11fc46f77a068af32a96f4926769b9c Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 19 Jun 2016 21:45:30 +1000 Subject: [PATCH 09/41] Fix up BMP280 SPI to use new IO --- src/main/drivers/barometer_spi_bmp280.c | 33 ++++++------------------- 1 file changed, 8 insertions(+), 25 deletions(-) diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c index 0e047383a1..2edc3c6001 100644 --- a/src/main/drivers/barometer_spi_bmp280.c +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -28,12 +28,14 @@ #include "barometer.h" #include "barometer_bmp280.h" -#define DISABLE_BMP280 GPIO_SetBits(BMP280_CS_GPIO, BMP280_CS_PIN) -#define ENABLE_BMP280 GPIO_ResetBits(BMP280_CS_GPIO, BMP280_CS_PIN) +#define DISABLE_BMP280 IOHi(bmp280CsPin) +#define ENABLE_BMP280 IOLo(bmp280CsPin) extern int32_t bmp280_up; extern int32_t bmp280_ut; +static IO_t bmp280CsPin = IO_NONE; + bool bmp280WriteRegister(uint8_t reg, uint8_t data) { ENABLE_BMP280; @@ -62,30 +64,11 @@ void bmp280SpiInit(void) return; } -#ifdef STM32F303 - RCC_AHBPeriphClockCmd(BMP280_CS_GPIO_CLK_PERIPHERAL, ENABLE); + bmp280CsPin = IOGetByTag(IO_TAG(BMP280_CS_PIN)); + IOInit(bmp280CsPin, OWNER_BARO, RESOURCE_SPI); + IOConfigGPIO(bmp280CsPin, IOCFG_OUT_PP); - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = BMP280_CS_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - - GPIO_Init(BMP280_CS_GPIO, &GPIO_InitStructure); -#endif - -#ifdef STM32F10X - RCC_APB2PeriphClockCmd(BMP280_CS_GPIO_CLK_PERIPHERAL, ENABLE); - - gpio_config_t gpio; - gpio.mode = Mode_Out_PP; - gpio.pin = BMP280_CS_PIN; - gpio.speed = Speed_50MHz; - gpioInit(BMP280_CS_GPIO, &gpio); -#endif - - GPIO_SetBits(BMP280_CS_GPIO, BMP280_CS_PIN); + DISABLE_BMP280; spiSetDivisor(BMP280_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); From c764b0a919a8723623f7c3fb8ebf7ecd05365846 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 12:15:18 +1000 Subject: [PATCH 10/41] Consistent naming of tag versus pin. Updates based on feedback. --- src/main/drivers/adc.c | 8 +-- src/main/drivers/adc_impl.h | 16 +++--- src/main/drivers/adc_stm32f10x.c | 30 +++++------ src/main/drivers/adc_stm32f30x.c | 88 ++++++++++++++++---------------- src/main/drivers/adc_stm32f4xx.c | 58 ++++++++++----------- 5 files changed, 100 insertions(+), 100 deletions(-) diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 672b4c469d..46f987dfc7 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -33,11 +33,11 @@ adc_config_t adcConfig[ADC_CHANNEL_COUNT]; volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; -uint8_t adcChannelByPin(ioTag_t pin) +uint8_t adcChannelByTag(ioTag_t ioTag) { - for (uint8_t i = 0; i < ARRAYLEN(adcPinMap); i++) { - if (pin == adcPinMap[i].pin) - return adcPinMap[i].channel; + for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { + if (ioTag == adcTagMap[i].tag) + return adcTagMap[i].channel; } return 0; } diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 1d72b4b357..ee2e92d40f 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -21,11 +21,11 @@ #include "rcc.h" #if defined(STM32F4) -#define ADC_PIN_MAP_COUNT 16 +#define ADC_TAG_MAP_COUNT 16 #elif defined(STM32F3) -#define ADC_PIN_MAP_COUNT 39 +#define ADC_TAG_MAP_COUNT 39 #else -#define ADC_PIN_MAP_COUNT 10 +#define ADC_TAG_MAP_COUNT 10 #endif typedef enum ADCDevice { @@ -43,10 +43,10 @@ typedef enum ADCDevice { #endif } ADCDevice; -typedef struct adcPinMap_s { - ioTag_t pin; +typedef struct adcTagMap_s { + ioTag_t tag; uint8_t channel; -} adcPinMap_t; +} adcTagMap_t; typedef struct adcDevice_s { ADC_TypeDef* ADCx; @@ -61,8 +61,8 @@ typedef struct adcDevice_s { } adcDevice_t; extern const adcDevice_t adcHardware[]; -extern const adcPinMap_t adcPinMap[ADC_PIN_MAP_COUNT]; +extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; -uint8_t adcChannelByPin(ioTag_t pin); \ No newline at end of file +uint8_t adcChannelByTag(ioTag_t ioTag); \ No newline at end of file diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 2532bf4ad6..b082388fe2 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -54,17 +54,17 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) return ADCINVALID; } -const adcPinMap_t adcPinMap[] = { - { .pin = DEFIO_TAG_E__PA0, .channel = ADC_Channel_0 }, // ADC12 - { .pin = DEFIO_TAG_E__PA1, .channel = ADC_Channel_1 }, // ADC12 - { .pin = DEFIO_TAG_E__PA2, .channel = ADC_Channel_2 }, // ADC12 - { .pin = DEFIO_TAG_E__PA3, .channel = ADC_Channel_3 }, // ADC12 - { .pin = DEFIO_TAG_E__PA4, .channel = ADC_Channel_4 }, // ADC12 - { .pin = DEFIO_TAG_E__PA5, .channel = ADC_Channel_5 }, // ADC12 - { .pin = DEFIO_TAG_E__PA6, .channel = ADC_Channel_6 }, // ADC12 - { .pin = DEFIO_TAG_E__PA7, .channel = ADC_Channel_7 }, // ADC12 - { .pin = DEFIO_TAG_E__PB0, .channel = ADC_Channel_8 }, // ADC12 - { .pin = DEFIO_TAG_E__PB1, .channel = ADC_Channel_9 }, // ADC12 +const adcTagMap_t adcTagMap[] = { + { DEFIO_TAG_E__PA0, ADC_Channel_0 }, // ADC12 + { DEFIO_TAG_E__PA1, ADC_Channel_1 }, // ADC12 + { DEFIO_TAG_E__PA2, ADC_Channel_2 }, // ADC12 + { DEFIO_TAG_E__PA3, ADC_Channel_3 }, // ADC12 + { DEFIO_TAG_E__PA4, ADC_Channel_4 }, // ADC12 + { DEFIO_TAG_E__PA5, ADC_Channel_5 }, // ADC12 + { DEFIO_TAG_E__PA6, ADC_Channel_6 }, // ADC12 + { DEFIO_TAG_E__PA7, ADC_Channel_7 }, // ADC12 + { DEFIO_TAG_E__PB0, ADC_Channel_8 }, // ADC12 + { DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12 }; // Driver for STM32F103CB onboard ADC @@ -93,7 +93,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableVBat) { IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_BATTERY].adcChannel = adcChannelByPin(IO_TAG(VBAT_ADC_PIN)); + adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5; @@ -104,7 +104,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_RSSI].adcChannel = adcChannelByPin(IO_TAG(RSSI_ADC_PIN)); + adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5; @@ -115,7 +115,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByPin(IO_TAG(EXTERNAL1_ADC_PIN)); + adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5; @@ -126,7 +126,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); - adcConfig[ADC_CURRENT].adcChannel = adcChannelByPin(IO_TAG(CURRENT_METER_ADC_PIN)); + adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index b9b4f6f8e1..dc9c98ee51 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -41,46 +41,46 @@ const adcDevice_t adcHardware[] = { { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } }; -const adcPinMap_t adcPinMap[] = { - { .pin = DEFIO_TAG_E__PA0, .channel = ADC_Channel_1 }, // ADC1 - { .pin = DEFIO_TAG_E__PA1, .channel = ADC_Channel_2 }, // ADC1 - { .pin = DEFIO_TAG_E__PA2, .channel = ADC_Channel_3 }, // ADC1 - { .pin = DEFIO_TAG_E__PA3, .channel = ADC_Channel_4 }, // ADC1 - { .pin = DEFIO_TAG_E__PA4, .channel = ADC_Channel_1 }, // ADC2 - { .pin = DEFIO_TAG_E__PA5, .channel = ADC_Channel_2 }, // ADC2 - { .pin = DEFIO_TAG_E__PA6, .channel = ADC_Channel_3 }, // ADC2 - { .pin = DEFIO_TAG_E__PA7, .channel = ADC_Channel_4 }, // ADC2 - { .pin = DEFIO_TAG_E__PB0, .channel = ADC_Channel_12 }, // ADC3 - { .pin = DEFIO_TAG_E__PB1, .channel = ADC_Channel_1 }, // ADC3 - { .pin = DEFIO_TAG_E__PB2, .channel = ADC_Channel_12 }, // ADC2 - { .pin = DEFIO_TAG_E__PB12, .channel = ADC_Channel_3 }, // ADC4 - { .pin = DEFIO_TAG_E__PB13, .channel = ADC_Channel_5 }, // ADC3 - { .pin = DEFIO_TAG_E__PB14, .channel = ADC_Channel_4 }, // ADC4 - { .pin = DEFIO_TAG_E__PB15, .channel = ADC_Channel_5 }, // ADC4 - { .pin = DEFIO_TAG_E__PC0, .channel = ADC_Channel_6 }, // ADC12 - { .pin = DEFIO_TAG_E__PC1, .channel = ADC_Channel_7 }, // ADC12 - { .pin = DEFIO_TAG_E__PC2, .channel = ADC_Channel_8 }, // ADC12 - { .pin = DEFIO_TAG_E__PC3, .channel = ADC_Channel_9 }, // ADC12 - { .pin = DEFIO_TAG_E__PC4, .channel = ADC_Channel_5 }, // ADC2 - { .pin = DEFIO_TAG_E__PC5, .channel = ADC_Channel_11 }, // ADC2 - { .pin = DEFIO_TAG_E__PD8, .channel = ADC_Channel_12 }, // ADC4 - { .pin = DEFIO_TAG_E__PD9, .channel = ADC_Channel_13 }, // ADC4 - { .pin = DEFIO_TAG_E__PD10, .channel = ADC_Channel_7 }, // ADC34 - { .pin = DEFIO_TAG_E__PD11, .channel = ADC_Channel_8 }, // ADC34 - { .pin = DEFIO_TAG_E__PD12, .channel = ADC_Channel_9 }, // ADC34 - { .pin = DEFIO_TAG_E__PD13, .channel = ADC_Channel_10 }, // ADC34 - { .pin = DEFIO_TAG_E__PD14, .channel = ADC_Channel_11 }, // ADC34 - { .pin = DEFIO_TAG_E__PE7, .channel = ADC_Channel_13 }, // ADC3 - { .pin = DEFIO_TAG_E__PE8, .channel = ADC_Channel_6 }, // ADC34 - { .pin = DEFIO_TAG_E__PE9, .channel = ADC_Channel_2 }, // ADC3 - { .pin = DEFIO_TAG_E__PE10, .channel = ADC_Channel_14 }, // ADC3 - { .pin = DEFIO_TAG_E__PE11, .channel = ADC_Channel_15 }, // ADC3 - { .pin = DEFIO_TAG_E__PE12, .channel = ADC_Channel_16 }, // ADC3 - { .pin = DEFIO_TAG_E__PE13, .channel = ADC_Channel_3 }, // ADC3 - { .pin = DEFIO_TAG_E__PE14, .channel = ADC_Channel_1 }, // ADC4 - { .pin = DEFIO_TAG_E__PE15, .channel = ADC_Channel_2 }, // ADC4 - { .pin = DEFIO_TAG_E__PF2, .channel = ADC_Channel_10 }, // ADC12 - { .pin = DEFIO_TAG_E__PF4, .channel = ADC_Channel_5 }, // ADC1 +const adcTagMap_t adcTagMap[] = { + { DEFIO_TAG_E__PA0, ADC_Channel_1 }, // ADC1 + { DEFIO_TAG_E__PA1, ADC_Channel_2 }, // ADC1 + { DEFIO_TAG_E__PA2, ADC_Channel_3 }, // ADC1 + { DEFIO_TAG_E__PA3, ADC_Channel_4 }, // ADC1 + { DEFIO_TAG_E__PA4, ADC_Channel_1 }, // ADC2 + { DEFIO_TAG_E__PA5, ADC_Channel_2 }, // ADC2 + { DEFIO_TAG_E__PA6, ADC_Channel_3 }, // ADC2 + { DEFIO_TAG_E__PA7, ADC_Channel_4 }, // ADC2 + { DEFIO_TAG_E__PB0, ADC_Channel_12 }, // ADC3 + { DEFIO_TAG_E__PB1, ADC_Channel_1 }, // ADC3 + { DEFIO_TAG_E__PB2, ADC_Channel_12 }, // ADC2 + { DEFIO_TAG_E__PB12, ADC_Channel_3 }, // ADC4 + { DEFIO_TAG_E__PB13, ADC_Channel_5 }, // ADC3 + { DEFIO_TAG_E__PB14, ADC_Channel_4 }, // ADC4 + { DEFIO_TAG_E__PB15, ADC_Channel_5 }, // ADC4 + { DEFIO_TAG_E__PC0, ADC_Channel_6 }, // ADC12 + { DEFIO_TAG_E__PC1, ADC_Channel_7 }, // ADC12 + { DEFIO_TAG_E__PC2, ADC_Channel_8 }, // ADC12 + { DEFIO_TAG_E__PC3, ADC_Channel_9 }, // ADC12 + { DEFIO_TAG_E__PC4, ADC_Channel_5 }, // ADC2 + { DEFIO_TAG_E__PC5, ADC_Channel_11 }, // ADC2 + { DEFIO_TAG_E__PD8, ADC_Channel_12 }, // ADC4 + { DEFIO_TAG_E__PD9, ADC_Channel_13 }, // ADC4 + { DEFIO_TAG_E__PD10, ADC_Channel_7 }, // ADC34 + { DEFIO_TAG_E__PD11, ADC_Channel_8 }, // ADC34 + { DEFIO_TAG_E__PD12, ADC_Channel_9 }, // ADC34 + { DEFIO_TAG_E__PD13, ADC_Channel_10 }, // ADC34 + { DEFIO_TAG_E__PD14, ADC_Channel_11 }, // ADC34 + { DEFIO_TAG_E__PE7, ADC_Channel_13 }, // ADC3 + { DEFIO_TAG_E__PE8, ADC_Channel_6 }, // ADC34 + { DEFIO_TAG_E__PE9, ADC_Channel_2 }, // ADC3 + { DEFIO_TAG_E__PE10, ADC_Channel_14 }, // ADC3 + { DEFIO_TAG_E__PE11, ADC_Channel_15 }, // ADC3 + { DEFIO_TAG_E__PE12, ADC_Channel_16 }, // ADC3 + { DEFIO_TAG_E__PE13, ADC_Channel_3 }, // ADC3 + { DEFIO_TAG_E__PE14, ADC_Channel_1 }, // ADC4 + { DEFIO_TAG_E__PE15, ADC_Channel_2 }, // ADC4 + { DEFIO_TAG_E__PF2, ADC_Channel_10 }, // ADC12 + { DEFIO_TAG_E__PF4, ADC_Channel_5 }, // ADC1 }; ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) @@ -110,7 +110,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_BATTERY].adcChannel = adcChannelByPin(IO_TAG(VBAT_ADC_PIN)); + adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_BATTERY].enabled = true; @@ -123,7 +123,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_RSSI].adcChannel = adcChannelByPin(IO_TAG(RSSI_ADC_PIN)); + adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_RSSI].enabled = true; @@ -136,7 +136,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_CURRENT].adcChannel = adcChannelByPin(IO_TAG(CURRENT_METER_ADC_PIN)); + adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_CURRENT].enabled = true; @@ -149,7 +149,7 @@ void adcInit(drv_adc_config_t *init) IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByPin(IO_TAG(EXTERNAL1_ADC_PIN)); + adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_EXTERNAL1].enabled = true; diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 44eb3b7c96..90ca22d12a 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -44,33 +44,33 @@ const adcDevice_t adcHardware[] = { }; /* note these could be packed up for saving space */ -const adcPinMap_t adcPinMap[] = { +const adcTagMap_t adcTagMap[] = { /* - { .pin = DEFIO_TAG_E__PF3, .channel = ADC_Channel_9 }, - { .pin = DEFIO_TAG_E__PF4, .channel = ADC_Channel_14 }, - { .pin = DEFIO_TAG_E__PF5, .channel = ADC_Channel_15 }, - { .pin = DEFIO_TAG_E__PF6, .channel = ADC_Channel_4 }, - { .pin = DEFIO_TAG_E__PF7, .channel = ADC_Channel_5 }, - { .pin = DEFIO_TAG_E__PF8, .channel = ADC_Channel_6 }, - { .pin = DEFIO_TAG_E__PF9, .channel = ADC_Channel_7 }, - { .pin = DEFIO_TAG_E__PF10, .channel = ADC_Channel_8 }, + { DEFIO_TAG_E__PF3, ADC_Channel_9 }, + { DEFIO_TAG_E__PF4, ADC_Channel_14 }, + { DEFIO_TAG_E__PF5, ADC_Channel_15 }, + { DEFIO_TAG_E__PF6, ADC_Channel_4 }, + { DEFIO_TAG_E__PF7, ADC_Channel_5 }, + { DEFIO_TAG_E__PF8, ADC_Channel_6 }, + { DEFIO_TAG_E__PF9, ADC_Channel_7 }, + { DEFIO_TAG_E__PF10, ADC_Channel_8 }, */ - { .pin = DEFIO_TAG_E__PC0, .channel = ADC_Channel_10 }, - { .pin = DEFIO_TAG_E__PC1, .channel = ADC_Channel_11 }, - { .pin = DEFIO_TAG_E__PC2, .channel = ADC_Channel_12 }, - { .pin = DEFIO_TAG_E__PC3, .channel = ADC_Channel_13 }, - { .pin = DEFIO_TAG_E__PC4, .channel = ADC_Channel_14 }, - { .pin = DEFIO_TAG_E__PC5, .channel = ADC_Channel_15 }, - { .pin = DEFIO_TAG_E__PB0, .channel = ADC_Channel_8 }, - { .pin = DEFIO_TAG_E__PB1, .channel = ADC_Channel_9 }, - { .pin = DEFIO_TAG_E__PA0, .channel = ADC_Channel_0 }, - { .pin = DEFIO_TAG_E__PA1, .channel = ADC_Channel_1 }, - { .pin = DEFIO_TAG_E__PA2, .channel = ADC_Channel_2 }, - { .pin = DEFIO_TAG_E__PA3, .channel = ADC_Channel_3 }, - { .pin = DEFIO_TAG_E__PA4, .channel = ADC_Channel_4 }, - { .pin = DEFIO_TAG_E__PA5, .channel = ADC_Channel_5 }, - { .pin = DEFIO_TAG_E__PA6, .channel = ADC_Channel_6 }, - { .pin = DEFIO_TAG_E__PA7, .channel = ADC_Channel_7 }, + { DEFIO_TAG_E__PC0, ADC_Channel_10 }, + { DEFIO_TAG_E__PC1, ADC_Channel_11 }, + { DEFIO_TAG_E__PC2, ADC_Channel_12 }, + { DEFIO_TAG_E__PC3, ADC_Channel_13 }, + { DEFIO_TAG_E__PC4, ADC_Channel_14 }, + { DEFIO_TAG_E__PC5, ADC_Channel_15 }, + { DEFIO_TAG_E__PB0, ADC_Channel_8 }, + { DEFIO_TAG_E__PB1, ADC_Channel_9 }, + { DEFIO_TAG_E__PA0, ADC_Channel_0 }, + { DEFIO_TAG_E__PA1, ADC_Channel_1 }, + { DEFIO_TAG_E__PA2, ADC_Channel_2 }, + { DEFIO_TAG_E__PA3, ADC_Channel_3 }, + { DEFIO_TAG_E__PA4, ADC_Channel_4 }, + { DEFIO_TAG_E__PA5, ADC_Channel_5 }, + { DEFIO_TAG_E__PA6, ADC_Channel_6 }, + { DEFIO_TAG_E__PA7, ADC_Channel_7 }, }; ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) @@ -102,7 +102,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableVBat) { IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_BATTERY].adcChannel = adcChannelByPin(IO_TAG(VBAT_ADC_PIN)); //VBAT_ADC_CHANNEL; + adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); //VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_480Cycles; @@ -113,7 +113,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByPin(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL; + adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_480Cycles; @@ -124,7 +124,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_RSSI].adcChannel = adcChannelByPin(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL; + adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_480Cycles; @@ -135,7 +135,7 @@ void adcInit(drv_adc_config_t *init) if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); - adcConfig[ADC_CURRENT].adcChannel = adcChannelByPin(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL; + adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_480Cycles; From 134bb6f466ce0fa2253fca9e55691f65e8e9b87f Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 17:28:59 +1000 Subject: [PATCH 11/41] Move standard clock to 10.5mhz --- src/main/drivers/bus_spi.c | 336 ++++++++++++++++++------------------- src/main/drivers/bus_spi.h | 2 +- 2 files changed, 168 insertions(+), 170 deletions(-) diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 5af40d0750..919b68d75b 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -36,7 +36,7 @@ #define GPIO_AF_SPI2 GPIO_AF_5 #endif #ifndef GPIO_AF_SPI3 -#define GPIO_AF_SPI3 GPIO_AF_6 +#define GPIO_AF_SPI3 GPIO_AF_6 #endif #endif @@ -73,168 +73,168 @@ static spiDevice_t spiHardwareMap[] = { #if defined(STM32F10X) - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false }, + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false }, + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false }, #else - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false }, + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false }, + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false }, #endif #if defined(STM32F303xC) || defined(STM32F40_41xxx) || defined(STM32F411xE) - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false } + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false } #endif }; SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { - if (instance == SPI1) - return SPIDEV_1; + if (instance == SPI1) + return SPIDEV_1; - if (instance == SPI2) - return SPIDEV_2; + if (instance == SPI2) + return SPIDEV_2; - if (instance == SPI3) - return SPIDEV_3; + if (instance == SPI3) + return SPIDEV_3; - return SPIINVALID; + return SPIINVALID; } void spiInitDevice(SPIDevice device) { - SPI_InitTypeDef spiInit; + SPI_InitTypeDef spiInit; - spiDevice_t *spi = &(spiHardwareMap[device]); + spiDevice_t *spi = &(spiHardwareMap[device]); #ifdef SDCARD_SPI_INSTANCE - if (spi->dev == SDCARD_SPI_INSTANCE) - spi->sdcard = true; + if (spi->dev == SDCARD_SPI_INSTANCE) + spi->sdcard = true; #endif - // Enable SPI clock - RCC_ClockCmd(spi->rcc, ENABLE); - RCC_ResetCmd(spi->rcc, ENABLE); + // Enable SPI clock + RCC_ClockCmd(spi->rcc, ENABLE); + RCC_ResetCmd(spi->rcc, ENABLE); - IOInit(IOGetByTag(spi->sck), OWNER_SYSTEM, RESOURCE_SPI); - IOInit(IOGetByTag(spi->miso), OWNER_SYSTEM, RESOURCE_SPI); - IOInit(IOGetByTag(spi->mosi), OWNER_SYSTEM, RESOURCE_SPI); + IOInit(IOGetByTag(spi->sck), OWNER_SYSTEM, RESOURCE_SPI); + IOInit(IOGetByTag(spi->miso), OWNER_SYSTEM, RESOURCE_SPI); + IOInit(IOGetByTag(spi->mosi), OWNER_SYSTEM, RESOURCE_SPI); #if defined(STM32F303xC) || defined(STM32F4) - if (spi->sdcard) { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); - } - else { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); - } - IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); + if (spi->sdcard) { + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); + } + else { + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); + } + IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); - if (spi->nss) - IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af); + if (spi->nss) + IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af); #endif #if defined(STM32F10X) - IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_CFG); - IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_CFG); - IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_CFG); + IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_CFG); + IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_CFG); + IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_CFG); - if (spi->nss) - IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); + if (spi->nss) + IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); #endif - // Init SPI hardware - SPI_I2S_DeInit(spi->dev); + // Init SPI hardware + SPI_I2S_DeInit(spi->dev); - spiInit.SPI_Mode = SPI_Mode_Master; - spiInit.SPI_Direction = SPI_Direction_2Lines_FullDuplex; - spiInit.SPI_DataSize = SPI_DataSize_8b; - spiInit.SPI_NSS = SPI_NSS_Soft; - spiInit.SPI_FirstBit = SPI_FirstBit_MSB; - spiInit.SPI_CRCPolynomial = 7; - spiInit.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; + spiInit.SPI_Mode = SPI_Mode_Master; + spiInit.SPI_Direction = SPI_Direction_2Lines_FullDuplex; + spiInit.SPI_DataSize = SPI_DataSize_8b; + spiInit.SPI_NSS = SPI_NSS_Soft; + spiInit.SPI_FirstBit = SPI_FirstBit_MSB; + spiInit.SPI_CRCPolynomial = 7; + spiInit.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; - if (spi->sdcard) { - spiInit.SPI_CPOL = SPI_CPOL_Low; - spiInit.SPI_CPHA = SPI_CPHA_1Edge; - } - else { - spiInit.SPI_CPOL = SPI_CPOL_High; - spiInit.SPI_CPHA = SPI_CPHA_2Edge; - } + if (spi->sdcard) { + spiInit.SPI_CPOL = SPI_CPOL_Low; + spiInit.SPI_CPHA = SPI_CPHA_1Edge; + } + else { + spiInit.SPI_CPOL = SPI_CPOL_High; + spiInit.SPI_CPHA = SPI_CPHA_2Edge; + } #ifdef STM32F303xC - // Configure for 8-bit reads. - SPI_RxFIFOThresholdConfig(spi->dev, SPI_RxFIFOThreshold_QF); + // Configure for 8-bit reads. + SPI_RxFIFOThresholdConfig(spi->dev, SPI_RxFIFOThreshold_QF); #endif - SPI_Init(spi->dev, &spiInit); - SPI_Cmd(spi->dev, ENABLE); + SPI_Init(spi->dev, &spiInit); + SPI_Cmd(spi->dev, ENABLE); - if (spi->nss) - IOHi(IOGetByTag(spi->nss)); + if (spi->nss) + IOHi(IOGetByTag(spi->nss)); } bool spiInit(SPIDevice device) { - switch (device) - { - case SPIINVALID: - return false; - case SPIDEV_1: + switch (device) + { + case SPIINVALID: + return false; + case SPIDEV_1: #ifdef USE_SPI_DEVICE_1 - spiInitDevice(device); - return true; + spiInitDevice(device); + return true; #else - break; + break; #endif - case SPIDEV_2: + case SPIDEV_2: #ifdef USE_SPI_DEVICE_2 - spiInitDevice(device); - return true; + spiInitDevice(device); + return true; #else - break; + break; #endif - case SPIDEV_3: + case SPIDEV_3: #if defined(USE_SPI_DEVICE_3) && (defined(STM32F303xC) || defined(STM32F4)) - spiInitDevice(device); - return true; + spiInitDevice(device); + return true; #else - break; + break; #endif - } - return false; + } + return false; } uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) { - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) - return -1; - spiHardwareMap[device].errorCount++; - return spiHardwareMap[device].errorCount; + SPIDevice device = spiDeviceByInstance(instance); + if (device == SPIINVALID) + return -1; + spiHardwareMap[device].errorCount++; + return spiHardwareMap[device].errorCount; } // return uint8_t value or -1 when failure uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data) { - uint16_t spiTimeout = 1000; + uint16_t spiTimeout = 1000; - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) - if ((spiTimeout--) == 0) - return spiTimeoutUserCallback(instance); + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); #ifdef STM32F303xC - SPI_SendData8(instance, data); + SPI_SendData8(instance, data); #else - SPI_I2S_SendData(instance, data); + SPI_I2S_SendData(instance, data); #endif - spiTimeout = 1000; - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) - if ((spiTimeout--) == 0) - return spiTimeoutUserCallback(instance); + spiTimeout = 1000; + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); #ifdef STM32F303xC - return ((uint8_t)SPI_ReceiveData8(instance)); + return ((uint8_t)SPI_ReceiveData8(instance)); #else - return ((uint8_t)SPI_I2S_ReceiveData(instance)); + return ((uint8_t)SPI_I2S_ReceiveData(instance)); #endif } @@ -244,47 +244,45 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data) bool spiIsBusBusy(SPI_TypeDef *instance) { #ifdef STM32F303xC - return SPI_GetTransmissionFIFOStatus(instance) != SPI_TransmissionFIFOStatus_Empty || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; + return SPI_GetTransmissionFIFOStatus(instance) != SPI_TransmissionFIFOStatus_Empty || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; #else - return SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; + return SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET; #endif } bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len) { - uint16_t spiTimeout = 1000; + uint16_t spiTimeout = 1000; - uint8_t b; - instance->DR; - while (len--) { - b = in ? *(in++) : 0xFF; - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) { - if ((spiTimeout--) == 0) - return spiTimeoutUserCallback(instance); - } + uint8_t b; + instance->DR; + while (len--) { + b = in ? *(in++) : 0xFF; + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) { + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + } #ifdef STM32F303xC - SPI_SendData8(instance, b); - //SPI_I2S_SendData16(instance, b); + SPI_SendData8(instance, b); #else - SPI_I2S_SendData(instance, b); + SPI_I2S_SendData(instance, b); #endif - spiTimeout = 1000; - while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) { - if ((spiTimeout--) == 0) - return spiTimeoutUserCallback(instance); - } + spiTimeout = 1000; + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) { + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + } #ifdef STM32F303xC - b = SPI_ReceiveData8(instance); - //b = SPI_I2S_ReceiveData16(instance); + b = SPI_ReceiveData8(instance); #else - b = SPI_I2S_ReceiveData(instance); + b = SPI_I2S_ReceiveData(instance); #endif - if (out) - *(out++) = b; - } + if (out) + *(out++) = b; + } - return true; + return true; } @@ -292,70 +290,70 @@ void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { #define BR_CLEAR_MASK 0xFFC7 - uint16_t tempRegister; + uint16_t tempRegister; - SPI_Cmd(instance, DISABLE); + SPI_Cmd(instance, DISABLE); - tempRegister = instance->CR1; + tempRegister = instance->CR1; - switch (divisor) { - case 2: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_2; - break; + switch (divisor) { + case 2: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_2; + break; - case 4: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_4; - break; + case 4: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_4; + break; - case 8: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_8; - break; + case 8: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_8; + break; - case 16: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_16; - break; + case 16: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_16; + break; - case 32: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_32; - break; + case 32: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_32; + break; - case 64: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_64; - break; + case 64: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_64; + break; - case 128: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_128; - break; + case 128: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_128; + break; - case 256: - tempRegister &= BR_CLEAR_MASK; - tempRegister |= SPI_BaudRatePrescaler_256; - break; - } + case 256: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_256; + break; + } - instance->CR1 = tempRegister; + instance->CR1 = tempRegister; - SPI_Cmd(instance, ENABLE); + SPI_Cmd(instance, ENABLE); } uint16_t spiGetErrorCounter(SPI_TypeDef *instance) { - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) - return 0; - return spiHardwareMap[device].errorCount; + SPIDevice device = spiDeviceByInstance(instance); + if (device == SPIINVALID) + return 0; + return spiHardwareMap[device].errorCount; } void spiResetErrorCounter(SPI_TypeDef *instance) { - SPIDevice device = spiDeviceByInstance(instance); - if (device != SPIINVALID) - spiHardwareMap[device].errorCount = 0; + SPIDevice device = spiDeviceByInstance(instance); + if (device != SPIINVALID) + spiHardwareMap[device].errorCount = 0; } \ No newline at end of file diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 39074e418d..aea3e133e9 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -40,7 +40,7 @@ #if defined(STM32F4) #define SPI_SLOW_CLOCK 128 //00.65625 MHz -#define SPI_STANDARD_CLOCK 16 //05.25000 MHz +#define SPI_STANDARD_CLOCK 8 //10.50000 MHz #define SPI_FAST_CLOCK 4 //21.00000 MHz #define SPI_ULTRAFAST_CLOCK 2 //42.00000 MHz #else From 95c844b17867d5c90a2bce52eac0a7d578f61c96 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 19:43:34 +1000 Subject: [PATCH 12/41] Updated CLOCK params to enum following review --- src/main/drivers/accgyro_l3gd20.c | 2 +- src/main/drivers/accgyro_spi_mpu6000.c | 10 +++---- src/main/drivers/accgyro_spi_mpu6500.c | 2 +- src/main/drivers/accgyro_spi_mpu9250.c | 18 ++++++++----- src/main/drivers/barometer_spi_bmp280.c | 2 +- src/main/drivers/bus_spi.c | 5 ++-- src/main/drivers/bus_spi.h | 36 +++++++++++++++---------- src/main/drivers/flash_m25p16.c | 6 ++--- 8 files changed, 46 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index 7b14634b04..dd28aceb11 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -81,7 +81,7 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) DISABLE_L3GD20; - spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER); + spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD); } void l3gd20GyroInit(uint8_t lpf) diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 56143f0e4e..3d58913d0a 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -128,13 +128,13 @@ void mpu6000SpiGyroInit(uint8_t lpf) mpu6000AccAndGyroInit(); - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); // Accel and Gyro DLPF Setting mpu6000WriteRegister(MPU6000_CONFIG, lpf); delayMicroseconds(1); - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock int16_t data[3]; mpuGyroRead(data); @@ -162,7 +162,7 @@ bool mpu6000SpiDetect(void) IOInit(mpuSpi6000CsPin, OWNER_SYSTEM, RESOURCE_SPI); IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); @@ -209,7 +209,7 @@ static void mpu6000AccAndGyroInit(void) { return; } - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); // Device Reset mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); @@ -251,7 +251,7 @@ static void mpu6000AccAndGyroInit(void) { delayMicroseconds(15); #endif - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); delayMicroseconds(1); mpuSpi6000InitDone = true; diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 6f9a25fe79..4df65c19cb 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -72,7 +72,7 @@ static void mpu6500SpiInit(void) IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI); IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_STANDARD_CLOCK); + spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); hardwareInitialised = true; } diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 2721d05594..2bad53bf35 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -55,7 +55,8 @@ static IO_t mpuSpi9250CsPin = IO_NONE; #define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin) #define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin) -void mpu9250ResetGyro(void) { +void mpu9250ResetGyro(void) +{ // Device Reset mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(150); @@ -105,7 +106,7 @@ void mpu9250SpiGyroInit(uint8_t lpf) spiResetErrorCounter(MPU9250_SPI_INSTANCE); - spiSetDivisor(MPU9250_SPI_INSTANCE, 5); //high speed now that we don't need to write to the slow registers + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers int16_t data[3]; mpuGyroRead(data); @@ -123,9 +124,8 @@ void mpu9250SpiAccInit(acc_t *acc) acc->acc_1G = 512 * 8; } - -bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) { - +bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) +{ uint8_t in; uint8_t attemptsRemaining = 20; @@ -151,7 +151,7 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) { return; } - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_SLOW_CLOCK); //low speed for writing to slow registers + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); @@ -177,6 +177,8 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) { verifympu9250WriteRegister(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); + mpuSpi9250InitDone = true; //init done } @@ -192,7 +194,7 @@ bool mpu9250SpiDetect(void) IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI); IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_SLOW_CLOCK); //low speed + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); do { @@ -207,6 +209,8 @@ bool mpu9250SpiDetect(void) } } while (attemptsRemaining--); + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); + return true; } diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c index 0e047383a1..f3747ef02f 100644 --- a/src/main/drivers/barometer_spi_bmp280.c +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -87,7 +87,7 @@ void bmp280SpiInit(void) GPIO_SetBits(BMP280_CS_GPIO, BMP280_CS_PIN); - spiSetDivisor(BMP280_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); + spiSetDivisor(BMP280_SPI_INSTANCE, SPI_CLOCK_STANDARD); hardwareInitialised = true; } diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 919b68d75b..c78c2024a9 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -72,14 +72,14 @@ #endif static spiDevice_t spiHardwareMap[] = { -#if defined(STM32F10X) +#if defined(STM32F1) { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false }, { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false }, #else { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false }, { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false }, #endif -#if defined(STM32F303xC) || defined(STM32F40_41xxx) || defined(STM32F411xE) +#if defined(STM32F3) || defined(STM32F4) { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false } #endif }; @@ -285,7 +285,6 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len return true; } - void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { #define BR_CLEAR_MASK 0xFFC7 diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index aea3e133e9..5acf9150e1 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,38 +17,46 @@ #pragma once +/* #define SPI_0_28125MHZ_CLOCK_DIVIDER 256 #define SPI_0_5625MHZ_CLOCK_DIVIDER 128 #define SPI_18MHZ_CLOCK_DIVIDER 2 #define SPI_9MHZ_CLOCK_DIVIDER 4 +*/ #include #include "io.h" #include "rcc.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) -#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN) -#define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP) -#define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) +#define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) +#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN) +#define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP) +#define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #elif defined(STM32F1) -#define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz) -#define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_Out_OD, GPIO_Speed_50MHz) +#define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz) +#define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_Out_OD, GPIO_Speed_50MHz) #else #error "Unknown processor" #endif +/* + Flash M25p16 tolerates 20mhz, SPI_CLOCK_FAST should sit around 20 or less. +*/ +typedef enum { + SPI_CLOCK_INITIALIZATON = 256, #if defined(STM32F4) -#define SPI_SLOW_CLOCK 128 //00.65625 MHz -#define SPI_STANDARD_CLOCK 8 //10.50000 MHz -#define SPI_FAST_CLOCK 4 //21.00000 MHz -#define SPI_ULTRAFAST_CLOCK 2 //42.00000 MHz + SPI_CLOCK_SLOW = 128, //00.65625 MHz + SPI_CLOCK_STANDARD = 8, //10.50000 MHz + SPI_CLOCK_FAST = 4, //21.00000 MHz + SPI_CLOCK_ULTRAFAST = 2, //42.00000 MHz #else -#define SPI_SLOW_CLOCK 128 //00.56250 MHz -#define SPI_STANDARD_CLOCK 4 //09.00000 MHz -#define SPI_FAST_CLOCK 2 //18.00000 MHz -#define SPI_ULTRAFAST_CLOCK 2 //18.00000 MHz + SPI_CLOCK_SLOW = 128, //00.56250 MHz + SPI_CLOCK_STANDARD = 4, //09.00000 MHz + SPI_CLOCK_FAST = 2, //18.00000 MHz + SPI_CLOCK_ULTRAFAST = 2, //18.00000 MHz #endif +} SPIClockDivider_e; typedef enum SPIDevice { SPIINVALID = -1, diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 7640ab768f..1063debae6 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -95,7 +95,7 @@ static void m25p16_writeEnable() static uint8_t m25p16_readStatus() { - uint8_t command[2] = {M25P16_INSTRUCTION_READ_STATUS_REG, 0}; + uint8_t command[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 }; uint8_t in[2]; ENABLE_M25P16; @@ -134,7 +134,7 @@ bool m25p16_waitForReady(uint32_t timeoutMillis) */ static bool m25p16_readIdentification() { - uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0}; + uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 }; uint8_t in[4]; uint32_t chipID; @@ -210,7 +210,7 @@ bool m25p16_init() #ifndef M25P16_SPI_SHARED //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz - spiSetDivisor(M25P16_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); + spiSetDivisor(M25P16_SPI_INSTANCE, SPI_CLOCK_FAST); #endif return m25p16_readIdentification(); From 869d639a00d0172686f6aa5c6a4927422275726c Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 19:51:51 +1000 Subject: [PATCH 13/41] Missed 7456 enabled targets --- src/main/drivers/max7456.c | 10 +++++----- src/main/drivers/vtx_rtc6705.c | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index b8a59bcfc9..d3efe7e7d2 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -55,11 +55,11 @@ uint8_t max7456_send(uint8_t add, uint8_t data) { } -void max7456_init(uint8_t video_system) { +void max7456_init(uint8_t video_system) +{ uint8_t max_screen_rows; uint8_t srdata = 0; uint16_t x; - char buf[LINE]; #ifdef MAX7456_SPI_CS_PIN max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN)); @@ -68,7 +68,7 @@ void max7456_init(uint8_t video_system) { IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); //Minimum spi clock period for max7456 is 100ns (10Mhz) - spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER); + spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD); delay(1000); // force soft reset on Max7456 @@ -77,10 +77,10 @@ void max7456_init(uint8_t video_system) { delay(100); srdata = max7456_send(0xA0, 0xFF); - if ((0x01 & srdata) == 0x01){ //PAL + if ((0x01 & srdata) == 0x01) { //PAL video_signal_type = VIDEO_MODE_PAL; } - else if((0x02 & srdata) == 0x02){ //NTSC + else if ((0x02 & srdata) == 0x02) { //NTSC video_signal_type = VIDEO_MODE_NTSC; } diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 423bd94ed6..35e78fe0dc 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -133,7 +133,7 @@ static uint32_t reverse32(uint32_t in) bool rtc6705Init(void) { DISABLE_RTC6705; - spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); return rtc6705IsReady(); } From 339bd0b0d33da60ccf1fecbd3c2199fc9c32b2c2 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 20:19:03 +1000 Subject: [PATCH 14/41] Minor cleanup and rename of fastpwm lookup table --- src/main/config/config.c | 8 ++++---- src/main/drivers/bus_spi.h | 7 ------- src/main/io/serial_cli.c | 4 ++-- src/main/main.c | 1 + 4 files changed, 7 insertions(+), 13 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 9489760cf1..b4baeb24df 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -399,7 +399,7 @@ uint8_t getCurrentControlRateProfile(void) static void setControlRateProfile(uint8_t profileIndex) { currentControlRateProfileIndex = profileIndex; - masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex; + masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex; currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex]; } @@ -538,10 +538,10 @@ static void resetConf(void) #ifdef GPS // gps/nav stuff - masterConfig.gpsConfig.provider = GPS_NMEA; - masterConfig.gpsConfig.sbasMode = SBAS_AUTO; + masterConfig.gpsConfig.provider = GPS_NMEA; + masterConfig.gpsConfig.sbasMode = SBAS_AUTO; masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; - masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; + masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif resetSerialConfig(&masterConfig.serialConfig); diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 5acf9150e1..d408eac2f7 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,13 +17,6 @@ #pragma once -/* -#define SPI_0_28125MHZ_CLOCK_DIVIDER 256 -#define SPI_0_5625MHZ_CLOCK_DIVIDER 128 -#define SPI_18MHZ_CLOCK_DIVIDER 2 -#define SPI_9MHZ_CLOCK_DIVIDER 4 -*/ - #include #include "io.h" #include "rcc.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 36782a2d67..eaabecb370 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -463,7 +463,7 @@ static const char * const lookupTableSuperExpoYaw[] = { "OFF", "ON", "ALWAYS" }; -static const char * const lookupTableFastPwm[] = { +static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED" }; @@ -525,7 +525,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, - { lookupTableFastPwm, sizeof(lookupTableFastPwm) / sizeof(char *) }, + { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, diff --git a/src/main/main.c b/src/main/main.c index 59dc9e8295..16c581c598 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -322,6 +322,7 @@ void init(void) pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; + if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) { pwm_params.idlePulse = 0; // brushed motors use_unsyncedPwm = false; From 99e1411e712b160ef6eebacace00248b86ff9e66 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 24 Jun 2016 13:07:44 +0200 Subject: [PATCH 15/41] Inconsitant conditional compile directives in config.c. Added BLACKBOX global condirtion. --- src/main/config/config.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index b4baeb24df..1fa036c084 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -623,6 +623,8 @@ static void resetConf(void) memcpy(masterConfig.transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); #endif +#ifdef BLACKBOX + #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = BLACKBOX_DEVICE_FLASH; @@ -636,6 +638,8 @@ static void resetConf(void) masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; +#endif // BLACKBOX + // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) masterConfig.escAndServoConfig.minthrottle = 1025; From 0a0bd20d78dc2ef20c0165f817791087d6782ec3 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 24 Jun 2016 13:29:41 +0200 Subject: [PATCH 16/41] /EUSTM32F103RC buil failed. Target.mk fixed. --- src/main/target/EUSTM32F103RC/target.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/EUSTM32F103RC/target.mk b/src/main/target/EUSTM32F103RC/target.mk index c12a9fa269..b315c0aacb 100644 --- a/src/main/target/EUSTM32F103RC/target.mk +++ b/src/main/target/EUSTM32F103RC/target.mk @@ -11,6 +11,7 @@ TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6000.c \ drivers/accgyro_spi_mpu6500.c \ drivers/barometer_bmp085.c \ From 389d74bd8d34ebddf88314544c630312931dfc50 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 24 Jun 2016 13:41:55 +0200 Subject: [PATCH 17/41] /PORT103R build failed. Target.mk fixed. --- src/main/target/PORT103R/target.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/PORT103R/target.mk b/src/main/target/PORT103R/target.mk index 507b303bff..411a04e02b 100644 --- a/src/main/target/PORT103R/target.mk +++ b/src/main/target/PORT103R/target.mk @@ -11,6 +11,7 @@ TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6000.c \ drivers/accgyro_spi_mpu6500.c \ drivers/barometer_bmp085.c \ From e666f10e84dc9b3ce1b9b352f9dedbfca8e3446c Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 24 Jun 2016 23:07:51 +1000 Subject: [PATCH 18/41] Clean of superfluous F4 defines and add ZCORE (SPRF3 clone with SPI 6500) --- src/main/drivers/pwm_mapping.c | 4 +- src/main/drivers/pwm_mapping.h | 9 +- src/main/drivers/pwm_output.c | 9 +- src/main/drivers/timer_stm32f30x.c | 7 +- src/main/main.c | 6 +- src/main/target/ALIENFLIGHTF4/target.h | 7 -- src/main/target/BLUEJAYF4/target.h | 9 +- src/main/target/REVO/target.h | 6 -- src/main/target/REVONANO/target.h | 6 -- src/main/target/SPRACINGF3/target.mk | 2 +- src/main/target/ZCOREF3/target.c | 104 ++++++++++++++++++++ src/main/target/ZCOREF3/target.h | 126 +++++++++++++++++++++++++ src/main/target/ZCOREF3/target.mk | 11 +++ 13 files changed, 260 insertions(+), 46 deletions(-) create mode 100644 src/main/target/ZCOREF3/target.c create mode 100644 src/main/target/ZCOREF3/target.h create mode 100644 src/main/target/ZCOREF3/target.mk diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2b9fb09e2f..50061fb790 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -31,7 +31,7 @@ #include "pwm_rx.h" #include "pwm_mapping.h" -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse); void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); @@ -327,7 +327,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->pwmProtocolType, init->motorPwmRate, init->idlePulse); pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { - pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); + pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate); pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; } else { pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index a56e0eaa2e..2212153cde 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -30,17 +30,14 @@ #error Invalid motor/servo/port configuration #endif - #define PULSE_1MS (1000) // 1ms pulse width - #define MAX_INPUTS 8 - #define PWM_TIMER_MHZ 1 #define PWM_BRUSHED_TIMER_MHZ 24 -#define MULTISHOT_TIMER_MHZ 72 -#define ONESHOT42_TIMER_MHZ 24 -#define ONESHOT125_TIMER_MHZ 8 +#define MULTISHOT_TIMER_MHZ 72 +#define ONESHOT42_TIMER_MHZ 24 +#define ONESHOT125_TIMER_MHZ 8 typedef struct sonarIOConfig_s { ioTag_t triggerTag; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 3aba0c097f..b445949381 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -170,12 +170,11 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) uint8_t index; TIM_TypeDef *lastTimerPtr = NULL; - for(index = 0; index < motorCount; index++){ + for (index = 0; index < motorCount; index++) { // Force the timer to overflow if it's the first motor to output, or if we change timers - if(motors[index]->tim != lastTimerPtr){ + if (motors[index]->tim != lastTimerPtr) { lastTimerPtr = motors[index]->tim; - timerForceOverflow(motors[index]->tim); } @@ -185,10 +184,10 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) } } -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate) { uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse); + motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0); motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; } diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index ca80287522..4bce8a70f3 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -68,8 +68,7 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t tmp = (uint32_t) TIMx; tmp += CCMR_OFFSET; - if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) - { + if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) { tmp += (TIM_Channel>>1); /* Reset the OCxM bits in the CCMRx register */ @@ -77,9 +76,7 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t /* Configure the OCxM bits in the CCMRx register */ *(__IO uint32_t *) tmp |= TIM_OCMode; - } - else - { + } else { tmp += (uint32_t)(TIM_Channel - (uint32_t)4)>> (uint32_t)1; /* Reset the OCxM bits in the CCMRx register */ diff --git a/src/main/main.c b/src/main/main.c index 16c581c598..dba36d3f43 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -314,7 +314,7 @@ void init(void) } bool use_unsyncedPwm = masterConfig.use_unsyncedPwm; - + // Configurator feature abused for enabling Fast PWM pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol; @@ -324,6 +324,7 @@ void init(void) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) { + featureClear(FEATURE_3D); pwm_params.idlePulse = 0; // brushed motors use_unsyncedPwm = false; } @@ -336,9 +337,12 @@ void init(void) mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm); +/* + // TODO is this needed here? enables at the end if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; +*/ systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 496f2e3657..3bc3e20d29 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -19,11 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "AFF4" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 -#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD -#define CONFIG_FEATURE_RX_SERIAL -#define CONFIG_MSP_PORT 1 -#define CONFIG_RX_SERIAL_PORT 2 #define USBD_PRODUCT_STRING "AlienFlight F4" @@ -48,12 +43,10 @@ #define ACC #define USE_ACC_SPI_MPU6500 - #define ACC_MPU6500_ALIGN CW270_DEG #define GYRO #define USE_GYRO_SPI_MPU6500 - #define GYRO_MPU6500_ALIGN CW270_DEG #define MAG diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 222cd71cee..7160da98f7 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -19,19 +19,14 @@ #define TARGET_BOARD_IDENTIFIER "BJF4" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS -#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD -#define CONFIG_FEATURE_RX_SERIAL -#define CONFIG_FEATURE_ONESHOT125 -#define CONFIG_RX_SERIAL_PORT 3 #define USBD_PRODUCT_STRING "BlueJayF4" #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_EXTI -#define INVERTER PB15 -#define INVERTER_USART USART6 +#define INVERTER PB15 +#define INVERTER_USART USART6 #define BEEPER PB7 #define BEEPER_INVERTED diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 50f5790a3e..cda67742e6 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -20,12 +20,6 @@ #define TARGET_BOARD_IDENTIFIER "REVO" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define CONFIG_SERIALRX_PROVIDER SERIALRX_SBUS -#define CONFIG_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH -#define CONFIG_FEATURE_RX_SERIAL -#define CONFIG_FEATURE_ONESHOT125 -#define CONFIG_MSP_PORT 2 -#define CONFIG_RX_SERIAL_PORT 1 #define USBD_PRODUCT_STRING "Revolution" #ifdef OPBL diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index b3d1a4941f..3c0287f2f7 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -19,12 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "REVN" #define CONFIG_START_FLASH_ADDRESS (0x08060000) //0x08060000 to 0x08080000 (FLASH_Sector_7) -#define CONFIG_SERIALRX_PROVIDER 2 -#define CONFIG_BLACKBOX_DEVICE 1 -#define CONFIG_FEATURE_RX_SERIAL -#define CONFIG_FEATURE_ONESHOT125 -#define CONFIG_MSP_PORT 1 -#define CONFIG_RX_SERIAL_PORT 2 #define USBD_PRODUCT_STRING "Revo Nano" #ifdef OPBL diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index 00e15bd25f..ed7391c7a8 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP ONBOARDFLASH +FEATURES = ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c new file mode 100644 index 0000000000..e3b40ee630 --- /dev/null +++ b/src/main/target/ZCOREF3/target.c @@ -0,0 +1,104 @@ + +#include +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // server #6 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h new file mode 100644 index 0000000000..c02b4d3ecb --- /dev/null +++ b/src/main/target/ZCOREF3/target.h @@ -0,0 +1,126 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "ZCF3" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT + +#define LED0 PB8 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + + +#define GYRO +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 + +#define ACC +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 + +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG + +#define BARO +#define USE_BARO_BMP280 + +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define SERIAL_PORT_COUNT 3 + +#ifndef UART1_GPIO +#define UART1_TX_PIN GPIO_Pin_9 // PA9 +#define UART1_RX_PIN GPIO_Pin_10 // PA10 +#define UART1_GPIO GPIOA +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource9 +#define UART1_RX_PINSOURCE GPIO_PinSource10 +#endif + +#define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK +#define UART2_RX_PIN GPIO_Pin_15 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource14 +#define UART2_RX_PINSOURCE GPIO_PinSource15 + +#ifndef UART3_GPIO +#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) +#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) +#define UART3_GPIO_AF GPIO_AF_7 +#define UART3_GPIO GPIOB +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 +#endif + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 SPI2 (SDCard) + +#define SPI1_NSS_PIN PB9 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define MPU6500_CS_PIN PB9 +#define MPU6500_SPI_INSTANCE SPI1 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) + diff --git a/src/main/target/ZCOREF3/target.mk b/src/main/target/ZCOREF3/target.mk new file mode 100644 index 0000000000..4b232a7870 --- /dev/null +++ b/src/main/target/ZCOREF3/target.mk @@ -0,0 +1,11 @@ +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c + From df5e79d74eacf3008b02bff5ddd1eb17184269a0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 25 Jun 2016 00:34:09 +1000 Subject: [PATCH 19/41] Small fix to dumping of values from CLI on F4 --- src/main/io/serial_cli.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eaabecb370..d7279327f7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1904,7 +1904,7 @@ static void dumpValues(uint16_t valueSection) cliPrint("\r\n"); #ifdef STM32F4 - delayMicroseconds(1000); + delay(2); #endif } } @@ -2717,6 +2717,10 @@ static void cliSet(char *cmdline) cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui cliPrint("\r\n"); + +#ifdef STM32F4 + delay(2); +#endif } } else if ((eqptr = strstr(cmdline, "=")) != NULL) { // has equals @@ -2891,14 +2895,14 @@ static void cliTasks(char *cmdline) subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f)); if (masterConfig.pid_process_denom > 1) { taskFrequency = subTaskFrequency / masterConfig.pid_process_denom; - cliPrintf("%d - (%s) ", taskId, taskInfo.taskName); + cliPrintf("%02d - (%s) ", taskId, taskInfo.taskName); } else { taskFrequency = subTaskFrequency; - cliPrintf("%d - (%s/%s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); + cliPrintf("%02d - (%s/%s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); } } else { taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f)); - cliPrintf("%d - (%s) ", taskId, taskInfo.taskName); + cliPrintf("%02d - (%s) ", taskId, taskInfo.taskName); } cliPrintf("max: %dus, avg: %dus, rate: %dhz, total: ", taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency); @@ -2909,7 +2913,7 @@ static void cliTasks(char *cmdline) cliPrintf("%dms", taskTotalTime); } - if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) cliPrintf("\r\n- - (%s) rate: %dhz", taskInfo.subTaskName, subTaskFrequency); + if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) cliPrintf("\r\n - - (%s) rate: %dhz", taskInfo.subTaskName, subTaskFrequency); cliPrintf("\r\n", taskTotalTime); } } From d384fae9f472256b2b345e4809f25cd35a073e62 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 25 Jun 2016 10:06:29 +1000 Subject: [PATCH 20/41] Fix for needing to slow down serial in the CLI for F4 targets. Problem is with the CLI in configurator dropping packets. It is understood that too much processing occurs following each packet, and the receive buffer starts to drop packets in the configurator. Once the BF config is updated, this fix can be removed. --- src/main/drivers/serial_usb_vcp.c | 1 + src/main/io/serial_cli.c | 32 ++++++++++++++++++++++++++----- src/main/target/common.h | 5 +++++ src/main/vcpf4/usbd_cdc_vcp.c | 14 ++++++-------- 4 files changed, 39 insertions(+), 13 deletions(-) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 5dc3ddb872..d4a25e6932 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -117,6 +117,7 @@ static bool usbVcpFlush(vcpPort_t *port) if (count == 0) { return true; } + if (!usbIsConnected() || !usbIsConfigured()) { return false; } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d7279327f7..04a0526dea 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1902,10 +1902,11 @@ static void dumpValues(uint16_t valueSection) cliPrintf("set %s = ", valueTable[i].name); cliPrintVar(value, 0); cliPrint("\r\n"); - -#ifdef STM32F4 + +#ifdef USE_SLOW_SERIAL_CLI delay(2); #endif + } } @@ -1979,6 +1980,9 @@ static void cliDump(char *cmdline) if (yaw < 0) cliWrite(' '); cliPrintf("%s\r\n", ftoa(yaw, buf)); +#ifdef USE_SLOW_SERIAL_CLI + delay(2); +#endif } #ifdef USE_SERVOS @@ -2000,6 +2004,10 @@ static void cliDump(char *cmdline) masterConfig.customServoMixer[i].max, masterConfig.customServoMixer[i].box ); + +#ifdef USE_SLOW_SERIAL_CLI + delay(2); +#endif } #endif @@ -2012,12 +2020,18 @@ static void cliDump(char *cmdline) if (featureNames[i] == NULL) break; cliPrintf("feature -%s\r\n", featureNames[i]); +#ifdef USE_SLOW_SERIAL_CLI + delay(2); +#endif } for (i = 0; ; i++) { // reenable what we want. if (featureNames[i] == NULL) break; if (mask & (1 << i)) cliPrintf("feature %s\r\n", featureNames[i]); +#ifdef USE_SLOW_SERIAL_CLI + delay(2); +#endif } @@ -2077,6 +2091,9 @@ static void cliDump(char *cmdline) for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { if (servoDirection(i, channel) < 0) { cliPrintf("smix reverse %d %d r\r\n", i , channel); +#ifdef USE_SLOW_SERIAL_CLI + delay(2); +#endif } } } @@ -2109,6 +2126,9 @@ static void cliDump(char *cmdline) changeControlRateProfile(currentRateIndex); cliRateProfile(""); +#ifdef USE_SLOW_SERIAL_CLI + delay(2); +#endif } cliPrint("\r\n# restore original profile selection\r\n"); @@ -2133,7 +2153,8 @@ static void cliDump(char *cmdline) } } -void cliDumpProfile(uint8_t profileIndex) { +void cliDumpProfile(uint8_t profileIndex) +{ if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; @@ -2148,7 +2169,8 @@ void cliDumpProfile(uint8_t profileIndex) { cliRateProfile(""); } -void cliDumpRateProfile(uint8_t rateProfileIndex) { +void cliDumpRateProfile(uint8_t rateProfileIndex) +{ if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values return; @@ -2718,7 +2740,7 @@ static void cliSet(char *cmdline) cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui cliPrint("\r\n"); -#ifdef STM32F4 +#ifdef USE_SLOW_SERIAL_CLI delay(2); #endif } diff --git a/src/main/target/common.h b/src/main/target/common.h index a9d961e4da..9a8d6aede7 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -18,11 +18,16 @@ #pragma once #ifdef STM32F4 + #define TASK_GYROPID_DESIRED_PERIOD 125 #define SCHEDULER_DELAY_LIMIT 10 +#define USE_SLOW_SERIAL_CLI + #else + #define TASK_GYROPID_DESIRED_PERIOD 1000 #define SCHEDULER_DELAY_LIMIT 100 + #endif #define SERIAL_RX diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index b42757adf1..93ac1aaa5c 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -153,12 +153,11 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) *******************************************************************************/ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) { - if(USB_Tx_State!=1) - { - VCP_DataTx(ptrBuffer,sendLength); - return sendLength; - } - return 0; + if (USB_Tx_State) + return 0; + + VCP_DataTx(ptrBuffer, sendLength); + return sendLength; } /** @@ -171,7 +170,6 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) */ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) { - uint16_t ptr = APP_Rx_ptr_in; uint32_t i; @@ -179,7 +177,7 @@ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) APP_Rx_Buffer[ptr++ & (APP_RX_DATA_SIZE-1)] = Buf[i]; APP_Rx_ptr_in = ptr % APP_RX_DATA_SIZE; - + return USBD_OK; } From b953e1c0b4b161a026b6e209e4eb60fc2f31c0af Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 25 Jun 2016 10:37:53 +1000 Subject: [PATCH 21/41] Additional delay never eventuates with new pwm_protocol and disabling of feature for OS42 and MS - why needed for 125? --- src/main/config/config.c | 14 ++------------ src/main/config/config.h | 1 - src/main/io/serial_cli.c | 4 ++-- src/main/io/serial_msp.c | 1 - 4 files changed, 4 insertions(+), 16 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 1fa036c084..72091a643e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -1046,7 +1046,8 @@ void changeProfile(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } -void changeControlRateProfile(uint8_t profileIndex) { +void changeControlRateProfile(uint8_t profileIndex) +{ if (profileIndex > MAX_RATEPROFILES) { profileIndex = MAX_RATEPROFILES - 1; } @@ -1054,17 +1055,6 @@ void changeControlRateProfile(uint8_t profileIndex) { activateControlRateConfig(); } -void handleOneshotFeatureChangeOnRestart(void) -{ - // Shutdown PWM on all motors prior to soft restart - StopPwmAllMotors(); - delay(50); - // Apply additional delay when OneShot125 feature changed from on to off state - if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) { - delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); - } -} - void latchActiveFeatures() { activeFeaturesLatch = masterConfig.enabledFeatures; diff --git a/src/main/config/config.h b/src/main/config/config.h index 9150540cfd..c26bbe6455 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -50,7 +50,6 @@ typedef enum { FEATURE_VTX = 1 << 25, } features_e; -void handleOneshotFeatureChangeOnRestart(void); void latchActiveFeatures(void); bool featureConfigured(uint32_t mask); bool feature(uint32_t mask); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 04a0526dea..237a5310f1 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2563,12 +2563,12 @@ static void cliRateProfile(char *cmdline) { } } -static void cliReboot(void) { +static void cliReboot(void) +{ cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); waitForSerialPortToFinishTransmitting(cliPort); stopMotors(); - handleOneshotFeatureChangeOnRestart(); systemReset(); } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 4e31f76cef..d2b6cc0c8d 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1969,7 +1969,6 @@ void mspProcess(void) if (isRebootScheduled) { waitForSerialPortToFinishTransmitting(candidatePort->port); stopMotors(); - handleOneshotFeatureChangeOnRestart(); // On real flight controllers, systemReset() will do a soft reset of the device, // reloading the program. But to support offline testing this flag needs to be // cleared so that the software doesn't continuously attempt to reboot itself. From 499d7522c276c834e0123a3f03f19cdbb7f0892a Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 25 Jun 2016 21:22:10 +1000 Subject: [PATCH 22/41] Re-enabling i2c overclocking (lower CPU usage) --- src/main/drivers/bus_i2c_stm32f10x.c | 11 +- src/main/drivers/bus_i2c_stm32f30x.c | 273 +++++++++++++-------------- src/main/target/common.h | 8 +- 3 files changed, 148 insertions(+), 144 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index a444332aba..c7e972b2d7 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -82,10 +82,10 @@ static void i2cUnstick(IO_t scl, IO_t sda); #endif static i2cDevice_t i2cHardwareMap[] = { - { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = false, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn }, - { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = false, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn }, + { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn }, + { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn }, #ifdef STM32F4 - { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = false, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn } + { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn } #endif }; @@ -396,7 +396,7 @@ void i2cInit(I2CDevice device) i2cUnstick(scl, sda); // Init pins -#if defined(STM32F40_41xxx) || defined(STM32F411xE) +#ifdef STM32F4 IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C); IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_I2C); #else @@ -416,8 +416,7 @@ void i2cInit(I2CDevice device) if (i2c->overClock) { i2cInit.I2C_ClockSpeed = 800000; // 800khz Maximum speed tested on various boards without issues - } - else { + } else { i2cInit.I2C_ClockSpeed = 400000; // 400khz Operation according specs } diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 8f9225ef74..c324d03e7a 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -54,8 +54,8 @@ static volatile uint16_t i2cErrorCount = 0; //static volatile uint16_t i2c2ErrorCount = 0; static i2cDevice_t i2cHardwareMap[] = { - { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = false }, - { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = false } + { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK }, + { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK } }; /////////////////////////////////////////////////////////////////////////////// @@ -64,190 +64,189 @@ static i2cDevice_t i2cHardwareMap[] = { uint32_t i2cTimeoutUserCallback(void) { - i2cErrorCount++; - return false; + i2cErrorCount++; + return false; } void i2cInit(I2CDevice device) { - i2cDevice_t *i2c; - i2c = &(i2cHardwareMap[device]); + i2cDevice_t *i2c; + i2c = &(i2cHardwareMap[device]); - I2C_TypeDef *I2Cx; - I2Cx = i2c->dev; + I2C_TypeDef *I2Cx; + I2Cx = i2c->dev; - IO_t scl = IOGetByTag(i2c->scl); - IO_t sda = IOGetByTag(i2c->sda); + IO_t scl = IOGetByTag(i2c->scl); + IO_t sda = IOGetByTag(i2c->sda); - RCC_ClockCmd(i2c->rcc, ENABLE); - RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); + RCC_ClockCmd(i2c->rcc, ENABLE); + RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); - IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4); - IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4); + IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4); + IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4); - I2C_InitTypeDef i2cInit = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_AnalogFilter = I2C_AnalogFilter_Enable, - .I2C_DigitalFilter = 0x00, - .I2C_OwnAddress1 = 0x00, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_Timing = 0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. - //.I2C_Timing = 0x8000050B; - }; + I2C_InitTypeDef i2cInit = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_AnalogFilter = I2C_AnalogFilter_Enable, + .I2C_DigitalFilter = 0x00, + .I2C_OwnAddress1 = 0x00, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_Timing = i2c->overClock ? + 0x00500E30 : // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. + 0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + //.I2C_Timing = 0x8000050B; + }; - if (i2c->overClock) { - i2cInit.I2C_Timing = 0x00500E30; // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. - } - I2C_Init(I2Cx, &i2cInit); + I2C_Init(I2Cx, &i2cInit); - I2C_Cmd(I2Cx, ENABLE); + I2C_Cmd(I2Cx, ENABLE); } uint16_t i2cGetErrorCounter(void) { - return i2cErrorCount; + return i2cErrorCount; } bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) { - addr_ <<= 1; + addr_ <<= 1; - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2C_TypeDef *I2Cx; + I2Cx = i2cHardwareMap[device].dev; - /* Test on BUSY Flag */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Test on BUSY Flag */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ - I2C_TransferHandling(I2Cx, addr_, 1, I2C_Reload_Mode, I2C_Generate_Start_Write); + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, 1, I2C_Reload_Mode, I2C_Generate_Start_Write); - /* Wait until TXIS flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until TXIS flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Send Register address */ - I2C_SendData(I2Cx, (uint8_t) reg); + /* Send Register address */ + I2C_SendData(I2Cx, (uint8_t) reg); - /* Wait until TCR flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET) - { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until TCR flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET) + { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ - I2C_TransferHandling(I2Cx, addr_, 1, I2C_AutoEnd_Mode, I2C_No_StartStop); + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, 1, I2C_AutoEnd_Mode, I2C_No_StartStop); - /* Wait until TXIS flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until TXIS flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Write data to TXDR */ - I2C_SendData(I2Cx, data); + /* Write data to TXDR */ + I2C_SendData(I2Cx, data); - /* Wait until STOPF flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until STOPF flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Clear STOPF flag */ - I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); + /* Clear STOPF flag */ + I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); - return true; + return true; } bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) { - addr_ <<= 1; + addr_ <<= 1; - I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2C_TypeDef *I2Cx; + I2Cx = i2cHardwareMap[device].dev; - /* Test on BUSY Flag */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Test on BUSY Flag */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ - I2C_TransferHandling(I2Cx, addr_, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write); + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write); - /* Wait until TXIS flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until TXIS flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Send Register address */ - I2C_SendData(I2Cx, (uint8_t) reg); + /* Send Register address */ + I2C_SendData(I2Cx, (uint8_t) reg); - /* Wait until TC flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TC) == RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until TC flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TC) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ - I2C_TransferHandling(I2Cx, addr_, len, I2C_AutoEnd_Mode, I2C_Generate_Start_Read); + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, len, I2C_AutoEnd_Mode, I2C_Generate_Start_Read); - /* Wait until all data are received */ - while (len) { - /* Wait until RXNE flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_RXNE) == RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until all data are received */ + while (len) { + /* Wait until RXNE flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_RXNE) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Read data from RXDR */ - *buf = I2C_ReceiveData(I2Cx); - /* Point to the next location where the byte read will be saved */ - buf++; + /* Read data from RXDR */ + *buf = I2C_ReceiveData(I2Cx); + /* Point to the next location where the byte read will be saved */ + buf++; - /* Decrement the read bytes counter */ - len--; - } + /* Decrement the read bytes counter */ + len--; + } - /* Wait until STOPF flag is set */ - i2cTimeout = I2C_LONG_TIMEOUT; - while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { - if ((i2cTimeout--) == 0) { - return i2cTimeoutUserCallback(); - } - } + /* Wait until STOPF flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(); + } + } - /* Clear STOPF flag */ - I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); + /* Clear STOPF flag */ + I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); - /* If all operations OK */ - return true; + /* If all operations OK */ + return true; } #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index 9a8d6aede7..dbe2c3a374 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,13 +17,19 @@ #pragma once +#define I2C1_OVERCLOCK true +#define I2C2_OVERCLOCK true + + +/* STM32F4 specific settings that apply to all F4 targets */ #ifdef STM32F4 #define TASK_GYROPID_DESIRED_PERIOD 125 #define SCHEDULER_DELAY_LIMIT 10 #define USE_SLOW_SERIAL_CLI +#define I2C3_OVERCLOCK true -#else +#else /* when not an F4 */ #define TASK_GYROPID_DESIRED_PERIOD 1000 #define SCHEDULER_DELAY_LIMIT 100 From a28a2d28817757372607793c3cf7ded245e9c48b Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 25 Jun 2016 21:29:18 +1000 Subject: [PATCH 23/41] Removed OS125 reconfig call from i2c_bst --- src/main/target/COLIBRI_RACE/i2c_bst.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 304db985ec..10c99ccc01 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1545,7 +1545,6 @@ void taskBstMasterProcess(void) bstMasterWriteLoop(); if (isRebootScheduled) { stopMotors(); - handleOneshotFeatureChangeOnRestart(); systemReset(); } resetBstChecker(); @@ -1555,12 +1554,14 @@ void taskBstMasterProcess(void) static uint8_t masterWriteBufferPointer; static uint8_t masterWriteData[DATA_BUFFER_SIZE]; -static void bstMasterStartBuffer(uint8_t address) { +static void bstMasterStartBuffer(uint8_t address) +{ masterWriteData[0] = address; masterWriteBufferPointer = 2; } -static void bstMasterWrite8(uint8_t data) { +static void bstMasterWrite8(uint8_t data) +{ masterWriteData[masterWriteBufferPointer++] = data; masterWriteData[1] = masterWriteBufferPointer; } From 7317f9138a9b5ba3b694d354d4afce21b936e7df Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 25 Jun 2016 21:41:24 +1000 Subject: [PATCH 24/41] FIXED: SIRINFPV default_features defined twice in target file. --- src/main/target/SIRINFPV/target.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 07f7343574..1700d13cf9 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -135,7 +135,7 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) + #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER @@ -150,7 +150,7 @@ #define USE_SERVOS #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE From a4e5e30152abe1a7920608c3db271de2c15a9987 Mon Sep 17 00:00:00 2001 From: Scott Shawcroft Date: Sat, 25 Jun 2016 10:48:24 -0700 Subject: [PATCH 25/41] Stop using uint8_t to determine if bytes are available from USB. If we buffer more than that we'll never be able to read it all. --- src/main/drivers/serial.h | 2 +- src/main/drivers/serial_uart.c | 8 ++++---- src/main/drivers/serial_uart.h | 4 ++-- src/main/drivers/serial_usb_vcp.c | 8 ++++---- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 288721ad6b..d8f868ae79 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -62,7 +62,7 @@ typedef struct serialPort_s { struct serialPortVTable { void (*serialWrite)(serialPort_t *instance, uint8_t ch); - uint8_t (*serialTotalRxWaiting)(serialPort_t *instance); + uint32_t (*serialTotalRxWaiting)(serialPort_t *instance); uint8_t (*serialTotalTxFree)(serialPort_t *instance); uint8_t (*serialRead)(serialPort_t *instance); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 4b51024216..8a7999bea5 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -165,7 +165,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; #endif DMA_InitStructure.DMA_BufferSize = s->port.rxBufferSize; - + #ifdef STM32F4 DMA_InitStructure.DMA_Channel = s->rxDMAChannel; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; @@ -176,7 +176,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, DMA_Cmd(s->rxDMAStream, ENABLE); USART_DMACmd(s->USARTx, USART_DMAReq_Rx, ENABLE); s->rxDMAPos = DMA_GetCurrDataCounter(s->rxDMAStream); -#else +#else DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->port.rxBuffer; @@ -228,7 +228,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, DMA_Init(s->txDMAStream, &DMA_InitStructure); DMA_ITConfig(s->txDMAStream, DMA_IT_TC | DMA_IT_FE | DMA_IT_TE | DMA_IT_DME, ENABLE); DMA_SetCurrDataCounter(s->txDMAStream, 0); -#else +#else DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_DeInit(s->txDMAChannel); @@ -292,7 +292,7 @@ void uartStartTxDMA(uartPort_t *s) #endif } -uint8_t uartTotalRxBytesWaiting(serialPort_t *instance) +uint32_t uartTotalRxBytesWaiting(serialPort_t *instance) { uartPort_t *s = (uartPort_t*)instance; #ifdef STM32F4 diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 78b6d7042f..a332edebf7 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -38,7 +38,7 @@ typedef struct { serialPort_t port; - + #ifdef STM32F4 DMA_Stream_TypeDef *rxDMAStream; DMA_Stream_TypeDef *txDMAStream; @@ -65,7 +65,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, // serialPort API void uartWrite(serialPort_t *instance, uint8_t ch); -uint8_t uartTotalRxBytesWaiting(serialPort_t *instance); +uint32_t uartTotalRxBytesWaiting(serialPort_t *instance); uint8_t uartTotalTxBytesFree(serialPort_t *instance); uint8_t uartRead(serialPort_t *instance); void uartSetBaudRate(serialPort_t *s, uint32_t baudRate); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 5dc3ddb872..01fdf2f057 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -66,11 +66,11 @@ static bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance) return true; } -static uint8_t usbVcpAvailable(serialPort_t *instance) +static uint32_t usbVcpAvailable(serialPort_t *instance) { UNUSED(instance); - return receiveLength & 0xFF; // FIXME use uint32_t return type everywhere + return receiveLength; } static uint8_t usbVcpRead(serialPort_t *instance) @@ -120,7 +120,7 @@ static bool usbVcpFlush(vcpPort_t *port) if (!usbIsConnected() || !usbIsConfigured()) { return false; } - + uint32_t txed; uint32_t start = millis(); @@ -147,7 +147,7 @@ static void usbVcpBeginWrite(serialPort_t *instance) port->buffering = true; } -uint8_t usbTxBytesFree() +uint8_t usbTxBytesFree() { // Because we block upon transmit and don't buffer bytes, our "buffer" capacity is effectively unlimited. return 255; From 33f8c76a968d776cba3889ce3b943bb0cd12976c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 25 Jun 2016 21:47:57 +0200 Subject: [PATCH 26/41] VCP fix from cleanflight PR#2120 by ledvinap --- lib/main/STM32_USB-FS-Device_Driver/inc/usb_regs.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/main/STM32_USB-FS-Device_Driver/inc/usb_regs.h b/lib/main/STM32_USB-FS-Device_Driver/inc/usb_regs.h index 88c6e6735d..1f24863052 100644 --- a/lib/main/STM32_USB-FS-Device_Driver/inc/usb_regs.h +++ b/lib/main/STM32_USB-FS-Device_Driver/inc/usb_regs.h @@ -228,8 +228,8 @@ enum EP_BUF_NUM /* GetDADDR */ #define _GetDADDR() ((__IO uint16_t) *DADDR) -/* GetBTABLE */ -#define _GetBTABLE() ((__IO uint16_t) *BTABLE) +/* GetBTABLE ; clear low-order bits explicitly to avoid problems in gcc 5.x */ +#define _GetBTABLE() (((__IO uint16_t) *BTABLE) & ~0x07) /* SetENDPOINT */ #define _SetENDPOINT(bEpNum,wRegValue) (*(EP0REG + bEpNum)= \ From 3d9b180b332f30a9d55e5fe7b8fd8d9327402bbc Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 26 Jun 2016 00:29:32 +0200 Subject: [PATCH 27/41] Fix Configurator Pid Controller Reset // Fix luxfloat no filter bug // Defaults for Version 2.9.0 --- src/main/config/config.c | 10 +++++----- src/main/io/serial_msp.c | 2 +- src/main/sensors/gyro.c | 2 ++ src/main/version.h | 4 ++-- src/main/version.h.save | 37 ------------------------------------- 5 files changed, 10 insertions(+), 45 deletions(-) delete mode 100644 src/main/version.h.save diff --git a/src/main/config/config.c b/src/main/config/config.c index b66c8838c0..330561d7e8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -154,7 +154,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) accelerometerTrims->values.yaw = 0; } -static void resetPidProfile(pidProfile_t *pidProfile) +void resetPidProfile(pidProfile_t *pidProfile) { #if (defined(STM32F10X)) @@ -169,7 +169,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PITCH] = 50; pidProfile->I8[PITCH] = 40; pidProfile->D8[PITCH] = 18; - pidProfile->P8[YAW] = 90; + pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; pidProfile->P8[PIDALT] = 50; @@ -194,10 +194,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 200; + pidProfile->rollPitchItermIgnoreRate = 180; pidProfile->yawItermIgnoreRate = 35; - pidProfile->dterm_lpf_hz = 50; // filtering ON by default - pidProfile->deltaMethod = DELTA_FROM_ERROR; + pidProfile->dterm_lpf_hz = 100; // filtering ON by default + pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->dynamic_pid = 1; #ifdef GTUNE diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index d50a8caeda..ab195fa8fd 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1500,7 +1500,7 @@ static bool processInCommand(void) break; case MSP_SET_RESET_CURR_PID: - //resetPidProfile(¤tProfile->pidProfile); + resetPidProfile(¤tProfile->pidProfile); break; case MSP_SET_SENSOR_ALIGNMENT: diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b22955cead..8fe83d2445 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -166,6 +166,8 @@ void gyroUpdate(void) gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } + } else { + gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled } } } diff --git a/src/main/version.h b/src/main/version.h index c973dcd678..5ec30cdd32 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,8 +16,8 @@ */ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 8 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed +#define FC_VERSION_MINOR 9 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) diff --git a/src/main/version.h.save b/src/main/version.h.save deleted file mode 100644 index 91390c7c6c..0000000000 --- a/src/main/version.h.save +++ /dev/null @@ -1,37 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 3 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 5 // increment when a bug is fixed - -#define STR_HELPER(x) #x -#define STR(x) STR_HELPER(x) -#define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL) - -#define MW_VERSION 231 - -extern const char* const targetName; - -#define GIT_SHORT_REVISION_LENGTH 7 // lower case hexadecimal digits. -extern const char* const shortGitRevision; - -#define BUILD_DATE_LENGTH 11 -extern const char* const buildDate; // "MMM DD YYYY" MMM = Jan/Feb/... - -#define BUILD_TIME_LENGTH 8 -extern const char* const buildTime; // "HH:MM:SS" From d779daad2d86b8ac7545e4675b5178aa6cbbe4f5 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 26 Jun 2016 08:59:32 +1000 Subject: [PATCH 28/41] Fixed array out of bounds issue - not all set. --- src/main/sensors/gyro.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8fe83d2445..bce60a1fca 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -56,7 +56,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) } void initGyroFilterCoefficients(void) { - int axis; + int axis; if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime); gyroFilterStateIsSet = true; @@ -137,8 +137,8 @@ static void applyGyroZero(void) void gyroUpdate(void) { - int16_t gyroADCRaw[XYZ_AXIS_COUNT]; - int axis; + int16_t gyroADCRaw[XYZ_AXIS_COUNT]; + int axis; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { @@ -161,13 +161,13 @@ void gyroUpdate(void) if (gyroLpfCutFreq) { if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */ - if (gyroFilterStateIsSet) { - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++){ + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + if (gyroFilterStateIsSet) { gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); + } else { + gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled } - } else { - gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled } } } From ee860b122ba794dd01d5e5cae3645b9cd26f489e Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Sun, 26 Jun 2016 16:40:32 +1000 Subject: [PATCH 29/41] X_RACERSPI support for betaflight V3 --- fake_travis_build.sh | 3 +- src/main/target/X_RACERSPI/target.c | 106 ++++++++++++++++++ src/main/target/X_RACERSPI/target.h | 161 +++++++++++++++++++++++++++ src/main/target/X_RACERSPI/target.mk | 15 +++ 4 files changed, 284 insertions(+), 1 deletion(-) create mode 100644 src/main/target/X_RACERSPI/target.c create mode 100644 src/main/target/X_RACERSPI/target.h create mode 100644 src/main/target/X_RACERSPI/target.mk diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 5bae319ada..e9b25f9b26 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -20,7 +20,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=ALIENFLIGHTF3" \ "TARGET=DOGE" \ "TARGET=SINGULARITY" \ - "TARGET=SIRINFPV") + "TARGET=SIRINFPV" \ + "TARGET=X_RACERSPI") #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c new file mode 100644 index 0000000000..ff9c8044f8 --- /dev/null +++ b/src/main/target/X_RACERSPI/target.c @@ -0,0 +1,106 @@ + +#include +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // server #6 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h new file mode 100644 index 0000000000..a655c93e25 --- /dev/null +++ b/src/main/target/X_RACERSPI/target.h @@ -0,0 +1,161 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "X_RACERSPI" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + + +#define LED0 PC14 +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 17 + + + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_INSTANCE SPI1 + + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG + +// MPU6000 interrupts +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PC13 +#define USE_EXTI + + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_USART1 +#define USE_USART2 +#define USE_USART3 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#ifndef UART1_GPIO +#define UART1_TX_PIN GPIO_Pin_9 // PA9 +#define UART1_RX_PIN GPIO_Pin_10 // PA10 +#define UART1_GPIO GPIOA +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource9 +#define UART1_RX_PINSOURCE GPIO_PinSource10 +#endif + +#define UART2_TX_PIN GPIO_Pin_2 // PA14 / SWCLK +#define UART2_RX_PIN GPIO_Pin_3 // PA15 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource2 +#define UART2_RX_PINSOURCE GPIO_PinSource3 + +#ifndef UART3_GPIO +#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) +#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) +#define UART3_GPIO_AF GPIO_AF_7 +#define UART3_GPIO GPIOB +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 +#endif + +#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 + + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +//GPIO_AF_1 + +#define SPI1_NSS_PIN PA15 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define LED_STRIP +#define LED_STRIP_TIMER TIM1 + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_GPIO GPIOA +#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define WS2811_GPIO_AF GPIO_AF_6 +#define WS2811_PIN GPIO_Pin_8 +#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_TIMER TIM1 +#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) + + diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk new file mode 100644 index 0000000000..2b09dc76d6 --- /dev/null +++ b/src/main/target/X_RACERSPI/target.mk @@ -0,0 +1,15 @@ +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c + From 016886fb3d04fc955bc29ece841ee58cc40096ba Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Sun, 26 Jun 2016 17:21:20 +1000 Subject: [PATCH 30/41] VARIANT of SPRACINGF3 --- src/main/target/X_RACERSPI/target.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index 2b09dc76d6..71e749cc2f 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -1,5 +1,6 @@ F3_TARGETS += $(TARGET) FEATURES = ONBOARDFLASH +TARGET_FLAGS = -DSPRACINGF3 TARGET_SRC = \ drivers/accgyro_mpu.c \ From 27e6034bb5c162c3f73874e2efb6b211d081b15c Mon Sep 17 00:00:00 2001 From: 4712betaflight <4712betaflight@outlook.de> Date: Sun, 26 Jun 2016 13:47:34 +0200 Subject: [PATCH 31/41] Add missing uint32_t tp #585 --- src/main/drivers/serial.c | 2 +- src/main/drivers/serial.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index bb81d3dd89..8a17ce6793 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -56,7 +56,7 @@ void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count) } } -uint8_t serialRxBytesWaiting(serialPort_t *instance) +uint32_t serialRxBytesWaiting(serialPort_t *instance) { return instance->vTable->serialTotalRxWaiting(instance); } diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index d8f868ae79..1f75e931d3 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -81,7 +81,7 @@ struct serialPortVTable { }; void serialWrite(serialPort_t *instance, uint8_t ch); -uint8_t serialRxBytesWaiting(serialPort_t *instance); +uint32_t serialRxBytesWaiting(serialPort_t *instance); uint8_t serialTxBytesFree(serialPort_t *instance); void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count); uint8_t serialRead(serialPort_t *instance); From c14af2506c5b7e85a71d9ffb7977e6ad7440e06c Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Sun, 26 Jun 2016 23:43:44 +1000 Subject: [PATCH 32/41] Fix whitespace to tab --- fake_travis_build.sh | 48 +++++++++++++++------------- src/main/target/X_RACERSPI/target.mk | 22 ++++++------- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index e9b25f9b26..6c1077209d 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -1,28 +1,30 @@ #!/bin/bash targets=("PUBLISHMETA=True" \ - "TARGET=CC3D" \ - "TARGET=CC3D_OPBL" \ - "TARGET=COLIBRI_RACE" \ - "TARGET=LUX_RACE" \ - "TARGET=SPRACINGF3" \ - "TARGET=SPRACINGF3EVO" \ - "TARGET=SPRACINGF3MINI" \ - "TARGET=OMNIBUS" \ - "TARGET=NAZE" \ - "TARGET=AFROMINI" \ - "TARGET=RMDO" \ - "TARGET=SPARKY" \ - "TARGET=MOTOLAB" \ - "TARGET=PIKOBLX" \ - "TARGET=IRCFUSIONF3" \ - "TARGET=ALIENFLIGHTF1" \ - "TARGET=ALIENFLIGHTF3" \ - "TARGET=DOGE" \ - "TARGET=SINGULARITY" \ - "TARGET=SIRINFPV" \ + "TARGET=CC3D" \ + "TARGET=CC3D_OPBL" \ + "TARGET=COLIBRI_RACE" \ + "TARGET=LUX_RACE" \ + "TARGET=SPRACINGF3" \ + "TARGET=SPRACINGF3EVO" \ + "TARGET=SPRACINGF3MINI" \ + "TARGET=OMNIBUS" \ + "TARGET=NAZE" \ + "TARGET=AFROMINI" \ + "TARGET=RMDO" \ + "TARGET=SPARKY" \ + "TARGET=MOTOLAB" \ + "TARGET=PIKOBLX" \ + "TARGET=IRCFUSIONF3" \ + "TARGET=ALIENFLIGHTF1" \ + "TARGET=ALIENFLIGHTF3" \ + "TARGET=DOGE" \ + "TARGET=SINGULARITY" \ + "TARGET=SIRINFPV" \ "TARGET=X_RACERSPI") + + #fake a travis build environment export TRAVIS_BUILD_NUMBER=$(date +%s) export BUILDNAME=${BUILDNAME:=fake_travis} @@ -31,9 +33,9 @@ export TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/simulated} for target in "${targets[@]}" do unset RUNTESTS PUBLISHMETA TARGET - echo - echo - echo "BUILDING '$target'" + echo + echo + echo "BUILDING '$target'" eval "export $target" make -f Makefile clean ./.travis.sh diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index 71e749cc2f..97b4407297 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -1,16 +1,16 @@ -F3_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH TARGET_FLAGS = -DSPRACINGF3 TARGET_SRC = \ drivers/accgyro_mpu.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/display_ug2864hsweg01.h \ - drivers/flash_m25p16.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c From 2f85dfc3650db8bee7e502bf6d55b4adcd7cd73a Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Sun, 26 Jun 2016 19:14:14 +0200 Subject: [PATCH 33/41] Fix newline issue in CLI for allowed range --- src/main/io/serial_cli.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 237a5310f1..36d254556a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2675,7 +2675,7 @@ static void cliPrintVarRange(const clivalue_t *var) { switch (var->type & VALUE_MODE_MASK) { case (MODE_DIRECT): { - cliPrintf("Allowed range: %d - %d\n", var->config.minmax.min, var->config.minmax.max); + cliPrintf("Allowed range: %d - %d\r\n", var->config.minmax.min, var->config.minmax.max); } break; case (MODE_LOOKUP): { @@ -2687,7 +2687,7 @@ static void cliPrintVarRange(const clivalue_t *var) cliPrint(","); cliPrintf(" %s", tableEntry->values[i]); } - cliPrint("\n"); + cliPrint("\r\n"); } break; } From f3eb19430149ba58c74e1e06dd6dcfc22c334b76 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 27 Jun 2016 06:16:23 +1000 Subject: [PATCH 34/41] Update README.md Logo change --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0239f59611..28d9c66a13 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Betaflight -![Betaflight](https://dl.dropboxusercontent.com/u/31537757/betaflight%20logo.jpg) +![Betaflight](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088900-228-bf_logo.jpg) Clean-code version of baseflight flight-controller - flight controllers are used to fly multi-rotor craft and fixed wing craft. From 67f7ae7206ad8cb1a9aed196216dfdc4bcee136e Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 27 Jun 2016 06:34:25 +1000 Subject: [PATCH 35/41] Using githubusercontent for logo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 28d9c66a13..0253334eff 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Betaflight -![Betaflight](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088900-228-bf_logo.jpg) +![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067) Clean-code version of baseflight flight-controller - flight controllers are used to fly multi-rotor craft and fixed wing craft. From deaeb23c84cd65d27afc12e134a8f0da8e2d3b76 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 27 Jun 2016 06:42:21 +1000 Subject: [PATCH 36/41] fixed incompatible pointer types in softserial --- src/main/drivers/serial_softserial.c | 2 +- src/main/drivers/serial_softserial.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index f5ed4d5931..a2ad403b26 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -408,7 +408,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) } } -uint8_t softSerialRxBytesWaiting(serialPort_t *instance) +uint32_t softSerialRxBytesWaiting(serialPort_t *instance) { if ((instance->mode & MODE_RX) == 0) { return 0; diff --git a/src/main/drivers/serial_softserial.h b/src/main/drivers/serial_softserial.h index daa9c5c64e..af80423b07 100644 --- a/src/main/drivers/serial_softserial.h +++ b/src/main/drivers/serial_softserial.h @@ -28,7 +28,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb // serialPort API void softSerialWriteByte(serialPort_t *instance, uint8_t ch); -uint8_t softSerialRxBytesWaiting(serialPort_t *instance); +uint32_t softSerialRxBytesWaiting(serialPort_t *instance); uint8_t softSerialTxBytesFree(serialPort_t *instance); uint8_t softSerialReadByte(serialPort_t *instance); void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate); From 6b5ffd14505a65e50561b61bd7f83490c375728b Mon Sep 17 00:00:00 2001 From: rotcehdnih Date: Mon, 27 Jun 2016 08:02:57 +1000 Subject: [PATCH 37/41] tab to space --- fake_travis_build.sh | 42 ++++++++++++++-------------- src/main/target/X_RACERSPI/target.mk | 24 ++++++++-------- 2 files changed, 33 insertions(+), 33 deletions(-) diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 6c1077209d..1a2c9af909 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -1,27 +1,27 @@ #!/bin/bash targets=("PUBLISHMETA=True" \ - "TARGET=CC3D" \ - "TARGET=CC3D_OPBL" \ - "TARGET=COLIBRI_RACE" \ - "TARGET=LUX_RACE" \ - "TARGET=SPRACINGF3" \ - "TARGET=SPRACINGF3EVO" \ - "TARGET=SPRACINGF3MINI" \ - "TARGET=OMNIBUS" \ - "TARGET=NAZE" \ - "TARGET=AFROMINI" \ - "TARGET=RMDO" \ - "TARGET=SPARKY" \ - "TARGET=MOTOLAB" \ - "TARGET=PIKOBLX" \ - "TARGET=IRCFUSIONF3" \ - "TARGET=ALIENFLIGHTF1" \ - "TARGET=ALIENFLIGHTF3" \ - "TARGET=DOGE" \ - "TARGET=SINGULARITY" \ - "TARGET=SIRINFPV" \ - "TARGET=X_RACERSPI") + "TARGET=CC3D" \ + "TARGET=CC3D_OPBL" \ + "TARGET=COLIBRI_RACE" \ + "TARGET=LUX_RACE" \ + "TARGET=SPRACINGF3" \ + "TARGET=SPRACINGF3EVO" \ + "TARGET=SPRACINGF3MINI" \ + "TARGET=OMNIBUS" \ + "TARGET=NAZE" \ + "TARGET=AFROMINI" \ + "TARGET=RMDO" \ + "TARGET=SPARKY" \ + "TARGET=MOTOLAB" \ + "TARGET=PIKOBLX" \ + "TARGET=IRCFUSIONF3" \ + "TARGET=ALIENFLIGHTF1" \ + "TARGET=ALIENFLIGHTF3" \ + "TARGET=DOGE" \ + "TARGET=SINGULARITY" \ + "TARGET=SIRINFPV" \ + "TARGET=X_RACERSPI") diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index 97b4407297..c0c219bd30 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -1,16 +1,16 @@ -F3_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH TARGET_FLAGS = -DSPRACINGF3 TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/display_ug2864hsweg01.h \ - drivers/flash_m25p16.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c From afda764e45c4af82be7d242133da743616317500 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 27 Jun 2016 18:38:43 +0200 Subject: [PATCH 38/41] Tidy up the Makefile clean rules, same way main build/all rules was made. Single target clean now available as make goals. --- Makefile | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/Makefile b/Makefile index c083a61906..d97613b3e4 100644 --- a/Makefile +++ b/Makefile @@ -667,24 +667,36 @@ $(VALID_TARGETS): $(MAKE) -j binary hex TARGET=$@ && \ echo "Building $@ succeeded." -## clean : clean up all temporary / machine-generated files + + +CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) ) +TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) ) + +## clean : clean up temporary / machine-generated files clean: + echo "Cleaning $(TARGET)" rm -f $(CLEAN_ARTIFACTS) rm -rf $(OBJECT_DIR)/$(TARGET) + echo "Cleaning $(TARGET) succeeded." -## clean_test : clean up all temporary / machine-generated files (tests) +## clean_test : clean up temporary / machine-generated files (tests) clean_test: cd src/test && $(MAKE) clean || true -## clean_all_targets : clean all valid target platforms -clean_all: - for clean_target in $(VALID_TARGETS); do \ - echo "" && \ - echo "Cleaning $$clean_target" && \ - $(MAKE) -j TARGET=$$clean_target clean || \ - break; \ - echo "Cleaning $$clean_target succeeded."; \ - done +## clean_ : clean up one specific target +$(CLEAN_TARGETS) : + $(MAKE) -j TARGET=$(subst clean_,,$@) clean + +## _clean : clean up one specific target (alias for above) +$(TARGETS_CLEAN) : + $(MAKE) -j TARGET=$(subst _clean,,$@) clean + +## clean_all : clean all valid targets +clean_all:$(CLEAN_TARGETS) + +## all_clean : clean all valid targets (alias for above) +all_clean:$(TARGETS_CLEAN) + flash_$(TARGET): $(TARGET_HEX) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon From 97fe5afd6c1e7f0c066c71ba3c3da0fb4f4ab2ec Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 19:26:02 +0100 Subject: [PATCH 39/41] Converted tabs to spaces. --- src/main/drivers/accgyro_bma280.c | 8 +- src/main/drivers/accgyro_l3g4200d.c | 8 +- src/main/drivers/accgyro_lsm303dlhc.c | 6 +- src/main/drivers/accgyro_mma845x.c | 8 +- src/main/drivers/accgyro_mpu.c | 36 +- src/main/drivers/accgyro_mpu.h | 12 +- src/main/drivers/accgyro_spi_mpu6000.c | 16 +- src/main/drivers/accgyro_spi_mpu6500.c | 2 +- src/main/drivers/accgyro_spi_mpu9250.c | 64 ++-- src/main/drivers/accgyro_spi_mpu9250.h | 2 +- src/main/drivers/adc.c | 10 +- src/main/drivers/adc_stm32f10x.c | 6 +- src/main/drivers/adc_stm32f30x.c | 16 +- src/main/drivers/adc_stm32f4xx.c | 26 +- src/main/drivers/barometer_bmp085.c | 18 +- src/main/drivers/barometer_bmp085.h | 4 +- src/main/drivers/barometer_bmp280.c | 10 +- src/main/drivers/barometer_ms5611.c | 12 +- src/main/drivers/bus_i2c_soft.c | 310 +++++++++--------- src/main/drivers/bus_spi.h | 28 +- src/main/drivers/compass_ak8975.c | 24 +- src/main/drivers/compass_hmc5883l.c | 20 +- src/main/drivers/dma_stm32f4xx.c | 2 +- src/main/drivers/inverter.c | 6 +- src/main/drivers/pwm_mapping.c | 2 +- src/main/drivers/pwm_mapping.h | 14 +- src/main/drivers/pwm_rx.c | 4 +- src/main/drivers/serial_softserial.c | 2 +- src/main/drivers/serial_uart.h | 12 +- src/main/drivers/serial_uart_stm32f4xx.c | 188 +++++------ src/main/drivers/serial_usb_vcp.c | 10 +- src/main/drivers/sonar_hcsr04.c | 6 +- src/main/drivers/sound_beeper.c | 22 +- src/main/drivers/sound_beeper.h | 6 +- src/main/drivers/system.h | 14 +- src/main/drivers/system_stm32f4xx.c | 34 +- src/main/drivers/timer.c | 6 +- src/main/main.c | 14 +- src/main/mw.c | 4 +- .../target/ALIENFLIGHTF3/hardware_revision.c | 4 +- src/main/target/CC3D/target.c | 52 +-- src/main/target/CJMCU/hardware_revision.c | 4 +- src/main/target/COLIBRI_RACE/target.h | 2 +- src/main/target/FURYF3/target.h | 6 +- src/main/target/KISSFC/target.h | 2 +- src/main/target/LUX_RACE/target.c | 12 +- src/main/target/REVO/target.h | 2 +- src/main/target/REVONANO/target.h | 2 +- src/main/target/SPRACINGF3/target.c | 14 +- src/main/target/X_RACERSPI/target.c | 14 +- src/main/target/X_RACERSPI/target.mk | 2 +- src/main/target/ZCOREF3/target.h | 6 +- src/main/telemetry/hott.c | 12 +- src/main/vcpf4/usb_bsp.c | 10 +- src/main/vcpf4/usb_conf.h | 6 +- src/main/vcpf4/usbd_cdc_vcp.h | 2 +- src/main/vcpf4/usbd_desc.c | 6 +- 57 files changed, 575 insertions(+), 575 deletions(-) diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c index 866d944e3d..9d0ab92060 100644 --- a/src/main/drivers/accgyro_bma280.c +++ b/src/main/drivers/accgyro_bma280.c @@ -40,7 +40,7 @@ bool bma280Detect(acc_t *acc) bool ack = false; uint8_t sig = 0; - ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig); + ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig); if (!ack || sig != 0xFB) return false; @@ -51,8 +51,8 @@ bool bma280Detect(acc_t *acc) static void bma280Init(acc_t *acc) { - i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range - i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW + i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range + i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW acc->acc_1G = 512 * 8; } @@ -61,7 +61,7 @@ static bool bma280Read(int16_t *accelData) { uint8_t buf[6]; - if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) { + if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) { return false; } diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c index 27beb9d63d..33bd2f1f80 100644 --- a/src/main/drivers/accgyro_l3g4200d.c +++ b/src/main/drivers/accgyro_l3g4200d.c @@ -63,7 +63,7 @@ bool l3g4200dDetect(gyro_t *gyro) delay(25); - i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid); + i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid); if (deviceid != L3G4200D_ID) return false; @@ -100,12 +100,12 @@ static void l3g4200dInit(uint8_t lpf) delay(100); - ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); + ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); if (!ack) failureMode(FAILURE_ACC_INIT); delay(5); - i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter); + i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter); } // Read 3 gyro values into user-provided buffer. No overrun checking is done. @@ -113,7 +113,7 @@ static bool l3g4200dRead(int16_t *gyroADC) { uint8_t buf[6]; - if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) { + if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) { return false; } diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 505764b29a..7a7222d4d6 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -115,15 +115,15 @@ int32_t accelSummedSamples500Hz[3]; void lsm303dlhcAccInit(acc_t *acc) { - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); + i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); delay(100); - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE); + i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE); delay(10); - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G); + i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G); delay(100); diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 2687c34b1a..4adbe59ab8 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -89,7 +89,7 @@ bool mma8452Detect(acc_t *acc) bool ack = false; uint8_t sig = 0; - ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig); + ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig); if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE)) return false; @@ -109,9 +109,9 @@ static inline void mma8451ConfigureInterrupt(void) IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? #endif - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 } static void mma8452Init(acc_t *acc) diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 0882581d34..63cb88ecfc 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -228,43 +228,43 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb) void mpuIntExtiInit(void) { - static bool mpuExtiInitDone = false; + static bool mpuExtiInitDone = false; - if (mpuExtiInitDone || !mpuIntExtiConfig) { - return; - } + if (mpuExtiInitDone || !mpuIntExtiConfig) { + return; + } #if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI) - IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag); - + IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag); + #ifdef ENSURE_MPU_DATA_READY_IS_LOW - uint8_t status = IORead(mpuIntIO); - if (status) { - return; - } + uint8_t status = IORead(mpuIntIO); + if (status) { + return; + } #endif - IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI); - IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? + IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI); + IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? - EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler); - EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); - EXTIEnable(mpuIntIO, true); + EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler); + EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); + EXTIEnable(mpuIntIO, true); #endif - mpuExtiInitDone = true; + mpuExtiInitDone = true; } static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) { - bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); + bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); return ack; } static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data) { - bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); + bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); return ack; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 6dbfcebf6c..15b6ee9429 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -120,12 +120,12 @@ typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); typedef void(*mpuResetFuncPtr)(void); typedef struct mpuConfiguration_s { - uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each - mpuReadRegisterFunc read; - mpuWriteRegisterFunc write; - mpuReadRegisterFunc slowread; - mpuWriteRegisterFunc verifywrite; - mpuResetFuncPtr reset; + uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each + mpuReadRegisterFunc read; + mpuWriteRegisterFunc write; + mpuReadRegisterFunc slowread; + mpuWriteRegisterFunc verifywrite; + mpuResetFuncPtr reset; } mpuConfiguration_t; extern mpuConfiguration_t mpuConfiguration; diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 3d58913d0a..cb78ffe102 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -49,12 +49,12 @@ static bool mpuSpi6000InitDone = false; // Bits -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define BITS_CLKSEL 0x07 -#define MPU_CLK_SEL_PLLGYROX 0x01 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_EXT_SYNC_GYROX 0x02 +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 @@ -74,9 +74,9 @@ static bool mpuSpi6000InitDone = false; #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_MASK 0x07 #define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_RAW_RDY_EN 0x01 +#define BIT_RAW_RDY_EN 0x01 #define BIT_I2C_IF_DIS 0x10 -#define BIT_INT_STATUS_DATA 0x01 +#define BIT_INT_STATUS_DATA 0x01 #define BIT_GYRO 3 #define BIT_ACC 2 #define BIT_TEMP 1 diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 4df65c19cb..2731797258 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -72,7 +72,7 @@ static void mpu6500SpiInit(void) IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI); IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); hardwareInitialised = true; } diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 2bad53bf35..95c0b3f50f 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -64,7 +64,7 @@ void mpu9250ResetGyro(void) bool mpu9250WriteRegister(uint8_t reg, uint8_t data) { - ENABLE_MPU9250; + ENABLE_MPU9250; delayMicroseconds(1); spiTransferByte(MPU9250_SPI_INSTANCE, reg); spiTransferByte(MPU9250_SPI_INSTANCE, data); @@ -76,7 +76,7 @@ bool mpu9250WriteRegister(uint8_t reg, uint8_t data) bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU9250; + ENABLE_MPU9250; spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); DISABLE_MPU9250; @@ -86,7 +86,7 @@ bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU9250; + ENABLE_MPU9250; delayMicroseconds(1); spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); @@ -98,7 +98,7 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data) void mpu9250SpiGyroInit(uint8_t lpf) { - (void)(lpf); + (void)(lpf); mpuIntExtiInit(); @@ -126,55 +126,55 @@ void mpu9250SpiAccInit(acc_t *acc) bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) { - uint8_t in; - uint8_t attemptsRemaining = 20; + uint8_t in; + uint8_t attemptsRemaining = 20; - mpu9250WriteRegister(reg, data); - delayMicroseconds(100); + mpu9250WriteRegister(reg, data); + delayMicroseconds(100); do { - mpu9250SlowReadRegister(reg, 1, &in); - if (in == data) { - return true; - } else { - debug[3]++; - mpu9250WriteRegister(reg, data); - delayMicroseconds(100); - } + mpu9250SlowReadRegister(reg, 1, &in); + if (in == data) { + return true; + } else { + debug[3]++; + mpu9250WriteRegister(reg, data); + delayMicroseconds(100); + } } while (attemptsRemaining--); return false; } static void mpu9250AccAndGyroInit(uint8_t lpf) { - if (mpuSpi9250InitDone) { - return; - } + if (mpuSpi9250InitDone) { + return; + } spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); - delay(50); + delay(50); - verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); - verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11 + verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11 if (lpf == 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF + verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF } else if (lpf < 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF + verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF } else if (lpf > 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF + verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops + verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops - verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - verifympu9250WriteRegister(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 + verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + verifympu9250WriteRegister(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) - verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. + verifympu9250WriteRegister(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); @@ -191,10 +191,10 @@ bool mpu9250SpiDetect(void) #ifdef MPU9250_CS_PIN mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)); #endif - IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI); - IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG); + IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI); + IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); do { diff --git a/src/main/drivers/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro_spi_mpu9250.h index 521cefdf28..1af293288b 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro_spi_mpu9250.h @@ -1,7 +1,7 @@ #pragma once -#define mpu9250_CONFIG 0x1A +#define mpu9250_CONFIG 0x1A /* We should probably use these. :) #define BITS_DLPF_CFG_256HZ 0x00 diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 46f987dfc7..d214b49d35 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -35,11 +35,11 @@ volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; uint8_t adcChannelByTag(ioTag_t ioTag) { - for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { - if (ioTag == adcTagMap[i].tag) - return adcTagMap[i].channel; - } - return 0; + for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { + if (ioTag == adcTagMap[i].tag) + return adcTagMap[i].channel; + } + return 0; } uint16_t adcGetChannel(uint8_t channel) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index b082388fe2..a810ac731b 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -81,7 +81,7 @@ void adcInit(drv_adc_config_t *init) { #if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN) - UNUSED(init); + UNUSED(init); #endif uint8_t i; @@ -114,7 +114,7 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; @@ -125,7 +125,7 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index dc9c98ee51..8c2ccba5ec 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -107,8 +107,8 @@ void adcInit(drv_adc_config_t *init) #ifdef VBAT_ADC_PIN if (init->enableVBat) { - IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; @@ -120,8 +120,8 @@ void adcInit(drv_adc_config_t *init) #ifdef RSSI_ADC_PIN if (init->enableRSSI) { - IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; @@ -133,8 +133,8 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; @@ -146,8 +146,8 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_GPIO if (init->enableExternal1) { - IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 90ca22d12a..e4c086a609 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -39,8 +39,8 @@ #endif const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, - //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } }; /* note these could be packed up for saving space */ @@ -75,13 +75,13 @@ const adcTagMap_t adcTagMap[] = { ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) - return ADCDEV_1; + if (instance == ADC1) + return ADCDEV_1; /* - if (instance == ADC2) // TODO add ADC2 and 3 - return ADCDEV_2; + if (instance == ADC2) // TODO add ADC2 and 3 + return ADCDEV_2; */ - return ADCINVALID; + return ADCINVALID; } void adcInit(drv_adc_config_t *init) @@ -112,7 +112,7 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; @@ -123,7 +123,7 @@ void adcInit(drv_adc_config_t *init) #ifdef RSSI_ADC_PIN if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; @@ -134,7 +134,7 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); - IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); + IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; @@ -184,11 +184,11 @@ void adcInit(drv_adc_config_t *init) ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; - ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; - ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group + ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group ADC_Init(adc.ADCx, &ADC_InitStructure); diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index a746500b56..34a01cd38e 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -45,8 +45,8 @@ static bool isEOCConnected = true; // EXTI14 for BMP085 End of Conversion Interrupt void bmp085_extiHandler(extiCallbackRec_t* cb) { - UNUSED(cb); - isConversionComplete = true; + UNUSED(cb); + isConversionComplete = true; } bool bmp085TestEOCConnected(const bmp085Config_t *config); @@ -184,13 +184,13 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro) delay(20); // datasheet says 10ms, we'll be careful and do 20. - ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ + ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ if (ack) { bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); bmp085.oversampling_setting = 3; if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */ - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */ + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */ bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */ bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */ bmp085_get_cal_param(); /* readout bmp085 calibparam structure */ @@ -277,7 +277,7 @@ static void bmp085_start_ut(void) #if defined(BARO_EOC_GPIO) isConversionComplete = false; #endif - i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); + i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); } static void bmp085_get_ut(void) @@ -291,7 +291,7 @@ static void bmp085_get_ut(void) } #endif - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data); + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data); bmp085_ut = (data[0] << 8) | data[1]; } @@ -305,7 +305,7 @@ static void bmp085_start_up(void) isConversionComplete = false; #endif - i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data); + i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data); } /** read out up for pressure conversion @@ -323,7 +323,7 @@ static void bmp085_get_up(void) } #endif - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data); + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data); bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting); } @@ -343,7 +343,7 @@ STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature static void bmp085_get_cal_param(void) { uint8_t data[22]; - i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data); + i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data); /*parameters AC1-AC6*/ bmp085.cal_param.ac1 = (data[0] << 8) | data[1]; diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index dafb03a819..328bffb134 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -18,8 +18,8 @@ #pragma once typedef struct bmp085Config_s { - ioTag_t xclrIO; - ioTag_t eocIO; + ioTag_t xclrIO; + ioTag_t eocIO; } bmp085Config_t; bool bmp085Detect(const bmp085Config_t *config, baro_t *baro); diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c index cf7e60a817..6d42458486 100644 --- a/src/main/drivers/barometer_bmp280.c +++ b/src/main/drivers/barometer_bmp280.c @@ -83,14 +83,14 @@ bool bmp280Detect(baro_t *baro) // set oversampling + power mode (forced), and start sampling bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE); #else - i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */ + i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */ if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) return false; // read calibration - i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal); + i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal); // set oversampling + power mode (forced), and start sampling - i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); + i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); #endif bmp280InitDone = true; @@ -129,7 +129,7 @@ static void bmp280_start_up(void) { // start measurement // set oversampling + power mode (forced), and start sampling - i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); + i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE); } static void bmp280_get_up(void) @@ -137,7 +137,7 @@ static void bmp280_get_up(void) uint8_t data[BMP280_DATA_FRAME_SIZE]; // read data from sensor - i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data); + i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data); bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); } diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c index 577146e2dc..468f86046f 100644 --- a/src/main/drivers/barometer_ms5611.c +++ b/src/main/drivers/barometer_ms5611.c @@ -67,7 +67,7 @@ bool ms5611Detect(baro_t *baro) delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms - ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig); + ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig); if (!ack) return false; @@ -93,14 +93,14 @@ bool ms5611Detect(baro_t *baro) static void ms5611_reset(void) { - i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1); + i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1); delayMicroseconds(2800); } static uint16_t ms5611_prom(int8_t coef_num) { uint8_t rxbuf[2] = { 0, 0 }; - i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command + i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command return rxbuf[0] << 8 | rxbuf[1]; } @@ -137,13 +137,13 @@ STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom) static uint32_t ms5611_read_adc(void) { uint8_t rxbuf[3]; - i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC + i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2]; } static void ms5611_start_ut(void) { - i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! + i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! } static void ms5611_get_ut(void) @@ -153,7 +153,7 @@ static void ms5611_get_ut(void) static void ms5611_start_up(void) { - i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! + i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! } static void ms5611_get_up(void) diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index edf9e6d726..cac5c62211 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -48,218 +48,218 @@ static volatile uint16_t i2cErrorCount = 0; static void I2C_delay(void) { - volatile int i = 7; - while (i) { - i--; - } + volatile int i = 7; + while (i) { + i--; + } } static bool I2C_Start(void) { - SDA_H; - SCL_H; - I2C_delay(); - if (!SDA_read) { - return false; - } - SDA_L; - I2C_delay(); - if (SDA_read) { - return false; - } - SDA_L; - I2C_delay(); - return true; + SDA_H; + SCL_H; + I2C_delay(); + if (!SDA_read) { + return false; + } + SDA_L; + I2C_delay(); + if (SDA_read) { + return false; + } + SDA_L; + I2C_delay(); + return true; } static void I2C_Stop(void) { - SCL_L; - I2C_delay(); - SDA_L; - I2C_delay(); - SCL_H; - I2C_delay(); - SDA_H; - I2C_delay(); + SCL_L; + I2C_delay(); + SDA_L; + I2C_delay(); + SCL_H; + I2C_delay(); + SDA_H; + I2C_delay(); } static void I2C_Ack(void) { - SCL_L; - I2C_delay(); - SDA_L; - I2C_delay(); - SCL_H; - I2C_delay(); - SCL_L; - I2C_delay(); + SCL_L; + I2C_delay(); + SDA_L; + I2C_delay(); + SCL_H; + I2C_delay(); + SCL_L; + I2C_delay(); } static void I2C_NoAck(void) { - SCL_L; - I2C_delay(); - SDA_H; - I2C_delay(); - SCL_H; - I2C_delay(); - SCL_L; - I2C_delay(); + SCL_L; + I2C_delay(); + SDA_H; + I2C_delay(); + SCL_H; + I2C_delay(); + SCL_L; + I2C_delay(); } static bool I2C_WaitAck(void) { - SCL_L; - I2C_delay(); - SDA_H; - I2C_delay(); - SCL_H; - I2C_delay(); - if (SDA_read) { - SCL_L; - return false; - } - SCL_L; - return true; + SCL_L; + I2C_delay(); + SDA_H; + I2C_delay(); + SCL_H; + I2C_delay(); + if (SDA_read) { + SCL_L; + return false; + } + SCL_L; + return true; } static void I2C_SendByte(uint8_t byte) { - uint8_t i = 8; - while (i--) { - SCL_L; - I2C_delay(); - if (byte & 0x80) { - SDA_H; - } - else { - SDA_L; - } - byte <<= 1; - I2C_delay(); - SCL_H; - I2C_delay(); - } - SCL_L; + uint8_t i = 8; + while (i--) { + SCL_L; + I2C_delay(); + if (byte & 0x80) { + SDA_H; + } + else { + SDA_L; + } + byte <<= 1; + I2C_delay(); + SCL_H; + I2C_delay(); + } + SCL_L; } static uint8_t I2C_ReceiveByte(void) { - uint8_t i = 8; - uint8_t byte = 0; + uint8_t i = 8; + uint8_t byte = 0; - SDA_H; - while (i--) { - byte <<= 1; - SCL_L; - I2C_delay(); - SCL_H; - I2C_delay(); - if (SDA_read) { - byte |= 0x01; - } - } - SCL_L; - return byte; + SDA_H; + while (i--) { + byte <<= 1; + SCL_L; + I2C_delay(); + SCL_H; + I2C_delay(); + if (SDA_read) { + byte |= 0x01; + } + } + SCL_L; + return byte; } void i2cInit(I2CDevice device) { - UNUSED(device); + UNUSED(device); - scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL)); - sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA)); + scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL)); + sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA)); - IOConfigGPIO(scl, IOCFG_OUT_OD); - IOConfigGPIO(sda, IOCFG_OUT_OD); + IOConfigGPIO(scl, IOCFG_OUT_OD); + IOConfigGPIO(sda, IOCFG_OUT_OD); } bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) { UNUSED(device); - int i; - if (!I2C_Start()) { - i2cErrorCount++; - return false; - } - I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); - if (!I2C_WaitAck()) { - I2C_Stop(); - return false; - } - I2C_SendByte(reg); - I2C_WaitAck(); - for (i = 0; i < len; i++) { - I2C_SendByte(data[i]); - if (!I2C_WaitAck()) { - I2C_Stop(); - i2cErrorCount++; - return false; - } - } - I2C_Stop(); - return true; + int i; + if (!I2C_Start()) { + i2cErrorCount++; + return false; + } + I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); + if (!I2C_WaitAck()) { + I2C_Stop(); + return false; + } + I2C_SendByte(reg); + I2C_WaitAck(); + for (i = 0; i < len; i++) { + I2C_SendByte(data[i]); + if (!I2C_WaitAck()) { + I2C_Stop(); + i2cErrorCount++; + return false; + } + } + I2C_Stop(); + return true; } bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data) { UNUSED(device); - if (!I2C_Start()) { - return false; - } - I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); - if (!I2C_WaitAck()) { - I2C_Stop(); - i2cErrorCount++; - return false; - } - I2C_SendByte(reg); - I2C_WaitAck(); - I2C_SendByte(data); - I2C_WaitAck(); - I2C_Stop(); - return true; + if (!I2C_Start()) { + return false; + } + I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); + if (!I2C_WaitAck()) { + I2C_Stop(); + i2cErrorCount++; + return false; + } + I2C_SendByte(reg); + I2C_WaitAck(); + I2C_SendByte(data); + I2C_WaitAck(); + I2C_Stop(); + return true; } bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { UNUSED(device); - if (!I2C_Start()) { - return false; - } - I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); - if (!I2C_WaitAck()) { - I2C_Stop(); - i2cErrorCount++; - return false; - } - I2C_SendByte(reg); - I2C_WaitAck(); - I2C_Start(); - I2C_SendByte(addr << 1 | I2C_Direction_Receiver); - I2C_WaitAck(); - while (len) { - *buf = I2C_ReceiveByte(); - if (len == 1) { - I2C_NoAck(); - } - else { - I2C_Ack(); - } - buf++; - len--; - } - I2C_Stop(); - return true; + if (!I2C_Start()) { + return false; + } + I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); + if (!I2C_WaitAck()) { + I2C_Stop(); + i2cErrorCount++; + return false; + } + I2C_SendByte(reg); + I2C_WaitAck(); + I2C_Start(); + I2C_SendByte(addr << 1 | I2C_Direction_Receiver); + I2C_WaitAck(); + while (len) { + *buf = I2C_ReceiveByte(); + if (len == 1) { + I2C_NoAck(); + } + else { + I2C_Ack(); + } + buf++; + len--; + } + I2C_Stop(); + return true; } uint16_t i2cGetErrorCounter(void) { - return i2cErrorCount; + return i2cErrorCount; } #endif diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index d408eac2f7..019080d963 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -52,23 +52,23 @@ typedef enum { } SPIClockDivider_e; typedef enum SPIDevice { - SPIINVALID = -1, - SPIDEV_1 = 0, - SPIDEV_2, - SPIDEV_3, - SPIDEV_MAX = SPIDEV_3, + SPIINVALID = -1, + SPIDEV_1 = 0, + SPIDEV_2, + SPIDEV_3, + SPIDEV_MAX = SPIDEV_3, } SPIDevice; typedef struct SPIDevice_s { - SPI_TypeDef *dev; - ioTag_t nss; - ioTag_t sck; - ioTag_t mosi; - ioTag_t miso; - rccPeriphTag_t rcc; - uint8_t af; - volatile uint16_t errorCount; - bool sdcard; + SPI_TypeDef *dev; + ioTag_t nss; + ioTag_t sck; + ioTag_t mosi; + ioTag_t miso; + rccPeriphTag_t rcc; + uint8_t af; + volatile uint16_t errorCount; + bool sdcard; } spiDevice_t; bool spiInit(SPIDevice device); diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 386a06e7d8..c35db1d1f6 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -64,7 +64,7 @@ bool ak8975detect(mag_t *mag) bool ack = false; uint8_t sig = 0; - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig); if (!ack || sig != 'H') // 0x48 / 01001000 / 'H' return false; @@ -86,24 +86,24 @@ void ak8975Init() UNUSED(ack); - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode delay(20); - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode delay(10); - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values delay(10); - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading. + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading. delay(10); // Clear status registers - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); // Trigger first measurement - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); } #define BIT_STATUS1_REG_DATA_READY (1 << 0) @@ -118,13 +118,13 @@ bool ak8975Read(int16_t *magData) uint8_t status; uint8_t buf[6]; - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) { return false; } #if 1 // USE_I2C_SINGLE_BYTE_READS - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH #else for (uint8_t i = 0; i < 6; i++) { ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH @@ -134,7 +134,7 @@ bool ak8975Read(int16_t *magData) } #endif - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); + ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); if (!ack) { return false; } @@ -152,6 +152,6 @@ bool ak8975Read(int16_t *magData) magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; - ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again + ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again return true; } diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 4e59c375b8..a018ab0f52 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -204,7 +204,7 @@ bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse) hmc5883Config = hmc5883ConfigToUse; - ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig); + ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig); if (!ack || sig != 'H') return false; @@ -241,15 +241,15 @@ void hmc5883lInit(void) } delay(50); - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias // Note that the very first measurement after a gain change maintains the same gain as the previous setting. // The new gain setting is effective from the second measurement and on. - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) delay(100); hmc5883lRead(magADC); for (i = 0; i < 10; i++) { // Collect 10 samples - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. @@ -267,9 +267,9 @@ void hmc5883lInit(void) } // Apply the negative bias. (Same gain) - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. for (i = 0; i < 10; i++) { - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. @@ -291,9 +291,9 @@ void hmc5883lInit(void) magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); // leave test mode - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga + i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode delay(100); if (!bret) { // Something went wrong so get a best guess @@ -309,7 +309,7 @@ bool hmc5883lRead(int16_t *magData) { uint8_t buf[6]; - bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); + bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); if (!ack) { return false; } diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index 6b49fe6299..fefbae2801 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -32,7 +32,7 @@ static dmaHandlers_t dmaHandlers; void dmaNoOpHandler(DMA_Stream_TypeDef *stream) { - UNUSED(stream); + UNUSED(stream); } void DMA1_Stream2_IRQHandler(void) diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index 82ce8b0994..e0231ddb78 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -31,15 +31,15 @@ static const IO_t pin = DEFIO_IO(INVERTER); void initInverter(void) { - IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT); - IOConfigGPIO(pin, IOCFG_OUT_PP); + IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(pin, IOCFG_OUT_PP); inverterSet(false); } void inverterSet(bool on) { - IOWrite(pin, on); + IOWrite(pin, on); } #endif diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 50061fb790..8d332ba56e 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -306,7 +306,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); } #endif diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 2212153cde..5d577af8e0 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -78,19 +78,19 @@ typedef struct drv_pwm_config_s { } drv_pwm_config_t; enum { - MAP_TO_PPM_INPUT = 1, + MAP_TO_PPM_INPUT = 1, MAP_TO_PWM_INPUT, MAP_TO_MOTOR_OUTPUT, MAP_TO_SERVO_OUTPUT, }; typedef enum { - PWM_PF_NONE = 0, - PWM_PF_MOTOR = (1 << 0), - PWM_PF_SERVO = (1 << 1), - PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), - PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), - PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) + PWM_PF_NONE = 0, + PWM_PF_MOTOR = (1 << 0), + PWM_PF_SERVO = (1 << 1), + PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), + PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), + PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; enum {PWM_INVERTED = 1}; diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 9452a89eae..856613d2b3 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -339,8 +339,8 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture static void pwmGPIOConfig(ioTag_t pin, ioConfig_t mode) { - IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT); - IOConfigGPIO(IOGetByTag(pin), mode); + IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT); + IOConfigGPIO(IOGetByTag(pin), mode); } void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index a2ad403b26..da2c032681 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -96,7 +96,7 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state) if (state) { IOHi(softSerial->txIO); } else { - IOLo(softSerial->txIO); + IOLo(softSerial->txIO); } } diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index a332edebf7..33dde32167 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -40,13 +40,13 @@ typedef struct { serialPort_t port; #ifdef STM32F4 - DMA_Stream_TypeDef *rxDMAStream; - DMA_Stream_TypeDef *txDMAStream; - uint32_t rxDMAChannel; - uint32_t txDMAChannel; + DMA_Stream_TypeDef *rxDMAStream; + DMA_Stream_TypeDef *txDMAStream; + uint32_t rxDMAChannel; + uint32_t txDMAChannel; #else - DMA_Channel_TypeDef *rxDMAChannel; - DMA_Channel_TypeDef *txDMAChannel; + DMA_Channel_TypeDef *rxDMAChannel; + DMA_Channel_TypeDef *txDMAChannel; #endif uint32_t rxDMAIrq; diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 71b112e368..f5bba84699 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -365,22 +365,22 @@ void DMA2_Stream7_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); - } } // USART1 Rx/Tx IRQ Handler @@ -402,30 +402,30 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t optio void DMA1_Stream6_IRQHandler(void) { uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port); - if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6)) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); - } - handleUsartTxDma(s); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); - } + if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6)) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); + } } void USART2_IRQHandler(void) { uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port); - usartIrqHandler(s); + usartIrqHandler(s); } #endif @@ -442,22 +442,22 @@ void DMA1_Stream3_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_3]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF3)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3); - } } void USART3_IRQHandler(void) @@ -480,22 +480,22 @@ void DMA1_Stream4_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_4]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF4)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4); - } } void UART4_IRQHandler(void) @@ -518,22 +518,22 @@ void DMA1_Stream7_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_5]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7); - } } void UART5_IRQHandler(void) @@ -556,22 +556,22 @@ void DMA2_Stream6_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_6]->port); if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6)) { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); - } - handleUsartTxDma(s); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6); + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6); + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6); + } + handleUsartTxDma(s); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); + } + if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) + { + DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6); - } - if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET) - { - DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6); - } } void USART6_IRQHandler(void) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index e9ccef82fb..bb74b36b3d 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -183,12 +183,12 @@ serialPort_t *usbVcpOpen(void) #ifdef STM32F4 IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, RESOURCE_IO); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, RESOURCE_IO); - USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); + USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); #else - Set_System(); - Set_USBClock(); - USB_Interrupts_Config(); - USB_Init(); + Set_System(); + Set_USBClock(); + USB_Interrupts_Config(); + USB_Init(); #endif s = &vcpPort; diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 6374f81d3f..3b8ec91f7e 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -92,9 +92,9 @@ void hcsr04_init(sonarRange_t *sonarRange) IOConfigGPIO(echoIO, IOCFG_IN_FLOATING); #ifdef USE_EXTI - EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); - EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! - EXTIEnable(echoIO, true); + EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); + EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! + EXTIEnable(echoIO, true); #endif lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance() diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 1e71845d20..cba9a2a5c4 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -39,31 +39,31 @@ static bool beeperInverted = false; void systemBeep(bool onoff) { #ifndef BEEPER - UNUSED(onoff); + UNUSED(onoff); #else - IOWrite(beeperIO, beeperInverted ? onoff : !onoff); + IOWrite(beeperIO, beeperInverted ? onoff : !onoff); #endif } void systemBeepToggle(void) { #ifdef BEEPER - IOToggle(beeperIO); + IOToggle(beeperIO); #endif } void beeperInit(const beeperConfig_t *config) { #ifndef BEEPER - UNUSED(config); + UNUSED(config); #else - beeperIO = IOGetByTag(config->ioTag); - beeperInverted = config->isInverted; + beeperIO = IOGetByTag(config->ioTag); + beeperInverted = config->isInverted; - if (beeperIO) { - IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT); - IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); - } - systemBeep(false); + if (beeperIO) { + IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT); + IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); + } + systemBeep(false); #endif } diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index bdd17b7454..ab7a7c3dfc 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -30,9 +30,9 @@ #endif typedef struct beeperConfig_s { - ioTag_t ioTag; - unsigned isInverted : 1; - unsigned isOD : 1; + ioTag_t ioTag; + unsigned isInverted : 1; + unsigned isOD : 1; } beeperConfig_t; void systemBeep(bool on); diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 6f8bf23d6e..92e17f8a82 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -25,13 +25,13 @@ uint32_t micros(void); uint32_t millis(void); typedef enum { - FAILURE_DEVELOPER = 0, - FAILURE_MISSING_ACC, - FAILURE_ACC_INIT, - FAILURE_ACC_INCOMPATIBLE, - FAILURE_INVALID_EEPROM_CONTENTS, - FAILURE_FLASH_WRITE_FAILED, - FAILURE_GYRO_INIT_FAILED + FAILURE_DEVELOPER = 0, + FAILURE_MISSING_ACC, + FAILURE_ACC_INIT, + FAILURE_ACC_INCOMPATIBLE, + FAILURE_INVALID_EEPROM_CONTENTS, + FAILURE_FLASH_WRITE_FAILED, + FAILURE_GYRO_INIT_FAILED } failureMode_e; // failure diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index bcaf71c9d0..f0042026f9 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -45,8 +45,8 @@ void systemReset(void) if (mpuConfiguration.reset) mpuConfiguration.reset(); - __disable_irq(); - NVIC_SystemReset(); + __disable_irq(); + NVIC_SystemReset(); } void systemResetToBootloader(void) @@ -54,10 +54,10 @@ void systemResetToBootloader(void) if (mpuConfiguration.reset) mpuConfiguration.reset(); - *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX + *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); + __disable_irq(); + NVIC_SystemReset(); } void enableGPIOPowerUsageAndNoiseReductions(void) @@ -82,7 +82,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void) RCC_AHB1Periph_BKPSRAM | RCC_AHB1Periph_DMA1 | RCC_AHB1Periph_DMA2 | - 0, ENABLE + 0, ENABLE ); RCC_AHB2PeriphClockCmd(0, ENABLE); @@ -172,25 +172,25 @@ void systemInit(void) SetSysClock(); // Configure NVIC preempt/priority groups - NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); + NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); // cache RCC->CSR value to use it in isMPUSoftreset() and others - cachedRccCsrValue = RCC->CSR; + cachedRccCsrValue = RCC->CSR; /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ - extern void *isr_vector_table_base; - NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0); - RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE); + extern void *isr_vector_table_base; + NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0); + RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE); - RCC_ClearFlag(); + RCC_ClearFlag(); - enableGPIOPowerUsageAndNoiseReductions(); + enableGPIOPowerUsageAndNoiseReductions(); // Init cycle counter - cycleCounterInit(); + cycleCounterInit(); - memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); - // SysTick - SysTick_Config(SystemCoreClock / 1000); + memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); + // SysTick + SysTick_Config(SystemCoreClock / 1000); } diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 673265f545..732fa5f4ae 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -148,7 +148,7 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; - } + } } return 0; } @@ -190,7 +190,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) #else TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; #endif - + TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure); @@ -660,7 +660,7 @@ void timerInit(void) IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); } #endif - + // initialize timer channel structures for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { timerChannelInfo[i].type = TYPE_FREE; diff --git a/src/main/main.c b/src/main/main.c index dba36d3f43..b1a157c4d5 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -488,12 +488,12 @@ void init(void) #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, - masterConfig.acc_hardware, - masterConfig.mag_hardware, - masterConfig.baro_hardware, - masterConfig.mag_declination, - masterConfig.gyro_lpf, - masterConfig.gyro_sync_denom)) { + masterConfig.acc_hardware, + masterConfig.mag_hardware, + masterConfig.baro_hardware, + masterConfig.mag_declination, + masterConfig.gyro_lpf, + masterConfig.gyro_sync_denom)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } @@ -688,7 +688,7 @@ void processLoopback(void) { #define processLoopback() #endif -void main_init(void) +void main_init(void) { init(); diff --git a/src/main/mw.c b/src/main/mw.c index e2819b722c..f77c426733 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -185,7 +185,7 @@ float calculateRate(int axis, int16_t rc) { } - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection } void processRcCommand(void) @@ -778,7 +778,7 @@ void subTaskMotorUpdate(void) uint8_t setPidUpdateCountDown(void) { if (masterConfig.gyro_soft_lpf_hz) { - return masterConfig.pid_process_denom - 1; + return masterConfig.pid_process_denom - 1; } else { return 1; } diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c index 2108369958..b51d38658f 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c @@ -39,7 +39,7 @@ static IO_t HWDetectPin = IO_NONE; void detectHardwareRevision(void) { - HWDetectPin = IOGetByTag(IO_TAG(HW_PIN)); + HWDetectPin = IOGetByTag(IO_TAG(HW_PIN)); IOInit(HWDetectPin, OWNER_SYSTEM, RESOURCE_INPUT); IOConfigGPIO(HWDetectPin, IOCFG_IPU); @@ -74,4 +74,4 @@ const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) else { return &alienFlightF3V2MPUIntExtiConfig; } -} \ No newline at end of file +} diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index b146e6a72b..abfe089aa9 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -28,12 +28,12 @@ const uint16_t multiPWM[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 0xFFFF }; @@ -59,26 +59,26 @@ const uint16_t airPWM[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 0xFFFF }; const uint16_t multiPPM_BP6[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -89,9 +89,9 @@ const uint16_t multiPWM_BP6[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -118,11 +118,11 @@ const uint16_t airPWM_BP6[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 0xFFFF }; diff --git a/src/main/target/CJMCU/hardware_revision.c b/src/main/target/CJMCU/hardware_revision.c index 29071314b8..558e19a760 100755 --- a/src/main/target/CJMCU/hardware_revision.c +++ b/src/main/target/CJMCU/hardware_revision.c @@ -55,5 +55,5 @@ void updateHardwareRevision(void) const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) { - return NULL; -} \ No newline at end of file + return NULL; +} diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 68bf284483..976b287a05 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -34,7 +34,7 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 #define USE_SPI diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 6201b8b3f0..f849070b7d 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -25,9 +25,9 @@ #define USE_EXTI #define CONFIG_PREFER_ACC_ON -#define LED0 PC14 +#define LED0 PC14 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect @@ -188,7 +188,7 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTF (BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 03c3cf31f6..6d79046a74 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -86,6 +86,6 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTF (BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 30804c5754..b665072acf 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -26,12 +26,12 @@ const uint16_t multiPWM[] = { PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index cda67742e6..d2124e5b28 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -23,7 +23,7 @@ #define USBD_PRODUCT_STRING "Revolution" #ifdef OPBL - #define USBD_SERIALNUMBER_STRING "0x8020000" +#define USBD_SERIALNUMBER_STRING "0x8020000" #endif #define LED0 PB5 diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 3c0287f2f7..6f4a2c4315 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -22,7 +22,7 @@ #define USBD_PRODUCT_STRING "Revo Nano" #ifdef OPBL - #define USBD_SERIALNUMBER_STRING "0x8010000" +#define USBD_SERIALNUMBER_STRING "0x8010000" #endif #define LED0 PC14 diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ff9c8044f8..ddebee117f 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -92,15 +92,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index ff9c8044f8..ddebee117f 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -92,15 +92,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index c0c219bd30..3d831edc43 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -12,5 +12,5 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c + drivers/sonar_hcsr04.c diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c02b4d3ecb..d8ac143e88 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0 PB8 #define BEEPER PC15 #define BEEPER_INVERTED @@ -45,8 +45,8 @@ #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 1eec8b9c06..6fb0bc1a92 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -424,8 +424,8 @@ static void hottCheckSerialData(uint32_t currentMicros) } static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) { - closeSerialPort(hottPort); - hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED); + closeSerialPort(hottPort); + hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED); } static void hottSendTelemetryData(void) { @@ -433,9 +433,9 @@ static void hottSendTelemetryData(void) { hottIsSending = true; // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3)) - workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); + workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); else - serialSetMode(hottPort, MODE_TX); + serialSetMode(hottPort, MODE_TX); hottMsgCrc = 0; return; } @@ -445,9 +445,9 @@ static void hottSendTelemetryData(void) { hottIsSending = false; // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3)) - workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); + workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); else - serialSetMode(hottPort, MODE_RX); + serialSetMode(hottPort, MODE_RX); flushHottRxBuffer(); return; } diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/vcpf4/usb_bsp.c index ba3fee7677..1160c47d8a 100644 --- a/src/main/vcpf4/usb_bsp.c +++ b/src/main/vcpf4/usb_bsp.c @@ -28,12 +28,12 @@ #include "../drivers/io.h" void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; } void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) { - (void)pdev; - (void)state; + (void)pdev; + (void)state; } @@ -46,7 +46,7 @@ void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) { void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; GPIO_InitTypeDef GPIO_InitStructure; #ifndef USE_ULPI_PHY @@ -100,7 +100,7 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev) */ void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; NVIC_InitTypeDef NVIC_InitStructure; NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); diff --git a/src/main/vcpf4/usb_conf.h b/src/main/vcpf4/usb_conf.h index 6caf298889..60c8036202 100644 --- a/src/main/vcpf4/usb_conf.h +++ b/src/main/vcpf4/usb_conf.h @@ -233,9 +233,9 @@ #elif defined (__ICCARM__) /* IAR Compiler */ #define __packed __packed #elif defined ( __GNUC__ ) /* GNU Compiler */ - #ifndef __packed - #define __packed __attribute__ ((__packed__)) - #endif + #ifndef __packed + #define __packed __attribute__ ((__packed__)) + #endif #elif defined (__TASKING__) /* TASKING Compiler */ #define __packed __unaligned #endif /* __CC_ARM */ diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 3c89b5b9e8..3ca22aa4ef 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -#define USB_RX_BUFSIZE 1024 +#define USB_RX_BUFSIZE 1024 __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index 41282dab25..f9478327e6 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -197,7 +197,7 @@ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = */ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; *length = sizeof(USBD_DeviceDesc); return USBD_DeviceDesc; } @@ -211,7 +211,7 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc; } @@ -245,7 +245,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; USBD_GetString ((uint8_t*)USBD_MANUFACTURER_STRING, USBD_StrDesc, length); return USBD_StrDesc; } From 7d5c8de5520b5d177f683a415be42318e4600878 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 19:32:41 +0100 Subject: [PATCH 40/41] Changed tabs to spaces --- src/main/drivers/accgyro_l3gd20.c | 2 +- src/main/drivers/accgyro_spi_mpu6000.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index dd28aceb11..aade7445b2 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -62,7 +62,7 @@ #define BLOCK_DATA_UPDATE_CONTINUOUS ((uint8_t)0x00) -#define BLE_MSB ((uint8_t)0x40) +#define BLE_MSB ((uint8_t)0x40) #define BOOT ((uint8_t)0x80) diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h index c717a7bb16..0081bddebb 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -1,7 +1,7 @@ #pragma once -#define MPU6000_CONFIG 0x1A +#define MPU6000_CONFIG 0x1A #define BITS_DLPF_CFG_256HZ 0x00 #define BITS_DLPF_CFG_188HZ 0x01 From a2d1af04aafa5c23b105aad894deb3e6cc787da4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 20:04:21 +0100 Subject: [PATCH 41/41] Minor cosmetic tidying --- src/main/drivers/timer.c | 2 -- src/main/main.c | 9 ++++----- src/main/sensors/acceleration.c | 2 +- src/main/sensors/barometer.c | 7 ++++--- src/main/sensors/compass.c | 9 +++++---- src/main/target/X_RACERSPI/target.c | 1 - src/main/target/X_RACERSPI/target.h | 2 +- 7 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 732fa5f4ae..6999a9798e 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -17,7 +17,6 @@ #include #include -#include #include #include "platform.h" @@ -26,7 +25,6 @@ #include "nvic.h" -#include "gpio.h" #include "gpio.h" #include "rcc.h" #include "system.h" diff --git a/src/main/main.c b/src/main/main.c index b1a157c4d5..e31c6933df 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -17,11 +17,11 @@ #include #include -#include #include #include #include "platform.h" + #include "common/axis.h" #include "common/color.h" #include "common/maths.h" @@ -153,9 +153,6 @@ static uint8_t systemState = SYSTEM_STATE_INITIALISING; void init(void) { - uint8_t i; - drv_pwm_config_t pwm_params; - printfSupportInit(); initEEPROM(); @@ -260,6 +257,7 @@ void init(void) mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif + drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR @@ -333,6 +331,7 @@ void init(void) #endif pwmRxInit(masterConfig.inputFilteringMode); + // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm); @@ -504,7 +503,7 @@ void init(void) LED0_OFF; LED2_OFF; - for (i = 0; i < 10; i++) { + for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 320d00ddde..1a4f6ef219 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,7 +45,7 @@ acc_t acc; // acc access functions sensor_align_e accAlign = 0; uint32_t accTargetLooptime; -uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. +static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; extern bool AccInflightCalibrationArmed; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 2a011d8e2b..1793e37948 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -20,6 +20,10 @@ #include #include "platform.h" + +int32_t BaroAlt = 0; + +#ifdef BARO #include "common/maths.h" #include "drivers/barometer.h" @@ -32,9 +36,6 @@ baro_t baro; // barometer access functions uint16_t calibratingB = 0; // baro calibration = get new ground pressure value int32_t baroPressure = 0; int32_t baroTemperature = 0; -int32_t BaroAlt = 0; - -#ifdef BARO static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 7ee14da4c7..de075bfe23 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,13 +40,14 @@ #endif mag_t mag; // mag access functions +int32_t magADC[XYZ_AXIS_COUNT]; +sensor_align_e magAlign = 0; + +#ifdef MAG extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead. -int16_t magADCRaw[XYZ_AXIS_COUNT]; -int32_t magADC[XYZ_AXIS_COUNT]; -sensor_align_e magAlign = 0; -#ifdef MAG +static int16_t magADCRaw[XYZ_AXIS_COUNT]; static uint8_t magInit = 0; void compassInit(void) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index ddebee117f..9c0bd04034 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -84,7 +84,6 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index a655c93e25..39c2e6c209 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "X_RACERSPI" +#define TARGET_BOARD_IDENTIFIER "XRC3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE