From 89d2c3152f1289a030b152459311a3ef2bd80fa1 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 13 May 2014 16:00:17 +0100 Subject: [PATCH] Code formatting and style fixes. --- src/common/maths.h | 4 +- src/drivers/accgyro_l3g4200d.c | 3 +- src/drivers/accgyro_l3gd20.c | 103 ++++++++++--------- src/drivers/accgyro_l3gd20.h | 22 ++--- src/drivers/accgyro_lsm303dlhc.c | 3 - src/drivers/accgyro_mpu6050.c | 47 ++++----- src/drivers/adc_common.c | 8 +- src/drivers/adc_common.h | 2 +- src/drivers/adc_fy90q.c | 6 +- src/drivers/barometer_bmp085.c | 32 +++--- src/drivers/barometer_common.h | 2 +- src/drivers/bus_i2c_soft.c | 49 +++++---- src/drivers/bus_i2c_stm32f10x.c | 165 ++++++++++++++++--------------- src/drivers/bus_i2c_stm32f30x.c | 137 ++++++++++++------------- src/drivers/pwm_rx.c | 12 +-- src/drivers/serial_common.c | 1 - src/drivers/serial_common.h | 1 - 17 files changed, 282 insertions(+), 315 deletions(-) diff --git a/src/common/maths.h b/src/common/maths.h index 4497bdec9..c7c6f6301 100644 --- a/src/common/maths.h +++ b/src/common/maths.h @@ -1,4 +1,3 @@ - #pragma once #ifndef sq @@ -9,8 +8,7 @@ #define M_PI 3.14159265358979323846f #endif /* M_PI */ -#define RADX10 (M_PI / 1800.0f) // 0.001745329252f -#define RAD (M_PI / 180.0f) +#define RADX10 (M_PI / 1800.0f) // 0.001745329252f #define RAD (M_PI / 180.0f) #define DEG2RAD(degrees) (degrees * RAD) diff --git a/src/drivers/accgyro_l3g4200d.c b/src/drivers/accgyro_l3g4200d.c index cf58cdcb7..c585b9b1c 100644 --- a/src/drivers/accgyro_l3g4200d.c +++ b/src/drivers/accgyro_l3g4200d.c @@ -12,7 +12,6 @@ #include "accgyro_common.h" #include "accgyro_l3g4200d.h" - // L3G4200D, Standard address 0x68 #define L3G4200D_ADDRESS 0x68 #define L3G4200D_ID 0xD3 @@ -61,7 +60,7 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf) // default LPF is set to 32Hz switch (lpf) { default: - case 32: + case 32: mpuLowPassFilter = L3G4200D_DLPF_32HZ; break; case 54: diff --git a/src/drivers/accgyro_l3gd20.c b/src/drivers/accgyro_l3gd20.c index 1a3997d1c..865dd9a66 100644 --- a/src/drivers/accgyro_l3gd20.c +++ b/src/drivers/accgyro_l3gd20.c @@ -1,17 +1,17 @@ /* - This program 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. + This program 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. - This program 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. + This program 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 this program. If not, see . -*/ + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ #include #include @@ -39,8 +39,6 @@ extern int16_t debug[4]; #define OUT_TEMP_ADDR 0x26 #define OUT_X_L_ADDR 0x28 - - #define MODE_ACTIVE ((uint8_t)0x08) #define OUTPUT_DATARATE_1 ((uint8_t)0x00) @@ -65,8 +63,6 @@ extern int16_t debug[4]; #define BOOT ((uint8_t)0x80) - - static volatile uint16_t spi1ErrorCount = 0; static volatile uint16_t spi2ErrorCount = 0; static volatile uint16_t spi3ErrorCount = 0; @@ -84,50 +80,46 @@ static volatile uint16_t spi3ErrorCount = 0; uint32_t spiTimeoutUserCallback(SPI_TypeDef *SPIx) { - if (SPIx == SPI1) - { + if (SPIx == SPI1) { spi1ErrorCount++; return spi1ErrorCount; - } - else if (SPIx == SPI2) - { + } else if (SPIx == SPI2) { spi2ErrorCount++; return spi2ErrorCount; - } - else - { spi3ErrorCount++; - return spi3ErrorCount; + } else { + spi3ErrorCount++; + return spi3ErrorCount; } } static void l3gd20SpiInit(SPI_TypeDef *SPIx) { GPIO_InitTypeDef GPIO_InitStructure; - SPI_InitTypeDef SPI_InitStructure; + SPI_InitTypeDef SPI_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); RCC_AHBPeriphClockCmd(SPI1_SCK_CLK | SPI1_MISO_CLK | SPI1_MOSI_CLK, ENABLE); - GPIO_PinAFConfig(SPI1_GPIO, SPI1_SCK_PIN_SOURCE, GPIO_AF_5); + GPIO_PinAFConfig(SPI1_GPIO, SPI1_SCK_PIN_SOURCE, GPIO_AF_5); GPIO_PinAFConfig(SPI1_GPIO, SPI1_MISO_PIN_SOURCE, GPIO_AF_5); GPIO_PinAFConfig(SPI1_GPIO, SPI1_MOSI_PIN_SOURCE, GPIO_AF_5); // Init pins - GPIO_InitStructure.GPIO_Pin = SPI1_SCK_PIN | SPI1_MISO_PIN | SPI1_MOSI_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Pin = SPI1_SCK_PIN | SPI1_MISO_PIN | SPI1_MOSI_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(SPI1_GPIO, &GPIO_InitStructure); RCC_AHBPeriphClockCmd(L3GD20_CS_GPIO_CLK, ENABLE); - GPIO_InitStructure.GPIO_Pin = L3GD20_CS_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_Pin = L3GD20_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_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(L3GD20_CS_GPIO, &GPIO_InitStructure); @@ -135,15 +127,15 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) SPI_I2S_DeInit(SPI1); - SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; - SPI_InitStructure.SPI_Mode = SPI_Mode_Master; - SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; - SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low; - SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge; - SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; + SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; + SPI_InitStructure.SPI_Mode = SPI_Mode_Master; + SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; + SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low; + SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge; + SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4; // 36/4 = 9 MHz SPI Clock - SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; - SPI_InitStructure.SPI_CRCPolynomial = 7; + SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; + SPI_InitStructure.SPI_CRCPolynomial = 7; SPI_Init(SPI1, &SPI_InitStructure); @@ -152,22 +144,27 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) SPI_Cmd(SPI1, ENABLE); } - static uint8_t spiTransfer(SPI_TypeDef *SPIx, uint8_t data) { uint16_t spiTimeout; spiTimeout = 0x1000; - while (SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_TXE) == RESET) - if ((spiTimeout--) == 0) return spiTimeoutUserCallback(SPIx); + while (SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_TXE) == RESET) { + if ((spiTimeout--) == 0) { + return spiTimeoutUserCallback(SPIx); + } + } SPI_SendData8(SPIx, data); spiTimeout = 0x1000; - while (SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_RXNE) == RESET) - if ((spiTimeout--) == 0) return spiTimeoutUserCallback(SPIx); + while (SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_RXNE) == RESET) { + if ((spiTimeout--) == 0) { + return spiTimeoutUserCallback(SPIx); + } + } - return((uint8_t)SPI_ReceiveData8(SPIx)); + return ((uint8_t) SPI_ReceiveData8(SPIx)); } void l3gd20GyroInit(void) @@ -175,16 +172,16 @@ void l3gd20GyroInit(void) l3gd20SpiInit(L3GD20_SPI); - GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); + GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); - spiTransfer(L3GD20_SPI, CTRL_REG5_ADDR); - spiTransfer(L3GD20_SPI, BOOT); + spiTransfer(L3GD20_SPI, CTRL_REG5_ADDR); + spiTransfer(L3GD20_SPI, BOOT); - GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); + GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); - delayMicroseconds(100); + delayMicroseconds(100); - GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); + GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); spiTransfer(L3GD20_SPI, CTRL_REG1_ADDR); @@ -210,7 +207,7 @@ static void l3gd20GyroRead(int16_t *gyroData) uint8_t buf[6]; GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); - spiTransfer(L3GD20_SPI, OUT_X_L_ADDR | READ_CMD |MULTIPLEBYTE_CMD); + spiTransfer(L3GD20_SPI, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD); uint8_t index; for (index = 0; index < sizeof(buf); index++) { diff --git a/src/drivers/accgyro_l3gd20.h b/src/drivers/accgyro_l3gd20.h index 60abbafec..8a1eb2e3c 100644 --- a/src/drivers/accgyro_l3gd20.h +++ b/src/drivers/accgyro_l3gd20.h @@ -1,17 +1,17 @@ /* - This program 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. + This program 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. - This program 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. + This program 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 this program. If not, see . -*/ + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ #pragma once diff --git a/src/drivers/accgyro_lsm303dlhc.c b/src/drivers/accgyro_lsm303dlhc.c index a2c925f11..ea4b385ff 100644 --- a/src/drivers/accgyro_lsm303dlhc.c +++ b/src/drivers/accgyro_lsm303dlhc.c @@ -58,7 +58,6 @@ extern int16_t debug[4]; #define CONTINUOUS_CONVERSION 0x00 - uint8_t accelCalibrating = false; float accelOneG = 9.8065; @@ -124,5 +123,3 @@ bool lsm303dlhcAccDetect(acc_t *acc) return true; } - - diff --git a/src/drivers/accgyro_mpu6050.c b/src/drivers/accgyro_mpu6050.c index 7199bd27f..6ac7563f7 100644 --- a/src/drivers/accgyro_mpu6050.c +++ b/src/drivers/accgyro_mpu6050.c @@ -19,25 +19,12 @@ #define DMP_MEM_START_ADDR 0x6E #define DMP_MEM_R_W 0x6F -#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN -#define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN -#define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN -#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS -#define MPU_RA_XA_OFFS_L_TC 0x07 -#define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS -#define MPU_RA_YA_OFFS_L_TC 0x09 -#define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS -#define MPU_RA_ZA_OFFS_L_TC 0x0B -#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register -#define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR -#define MPU_RA_XG_OFFS_USRL 0x14 -#define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR -#define MPU_RA_YG_OFFS_USRL 0x16 -#define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR -#define MPU_RA_ZG_OFFS_USRL 0x18 +#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD #define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD #define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD #define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN #define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN #define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN #define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS #define MPU_RA_XA_OFFS_L_TC 0x07 +#define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS #define MPU_RA_YA_OFFS_L_TC 0x09 +#define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS #define MPU_RA_ZA_OFFS_L_TC 0x0B +#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register #define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR #define MPU_RA_XG_OFFS_USRL 0x14 +#define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR #define MPU_RA_YG_OFFS_USRL 0x16 +#define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR #define MPU_RA_ZG_OFFS_USRL 0x18 #define MPU_RA_SMPLRT_DIV 0x19 #define MPU_RA_CONFIG 0x1A #define MPU_RA_GYRO_CONFIG 0x1B @@ -108,8 +95,7 @@ #define MPU_RA_FIFO_R_W 0x74 #define MPU_RA_WHO_AM_I 0x75 -#define MPU6050_SMPLRT_DIV 0 // 8000Hz - +#define MPU6050_SMPLRT_DIV 0 // 8000Hz #define MPU6050_LPF_256HZ 0 #define MPU6050_LPF_188HZ 1 #define MPU6050_LPF_98HZ 2 @@ -137,7 +123,7 @@ static bool mpu6050Detect(void) bool ack; uint8_t sig; - delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe + delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig); if (!ack) @@ -193,7 +179,7 @@ bool mpu6050AccDetect(acc_t *acc) acc->init = mpu6050AccInit; acc->read = mpu6050AccRead; - acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. + acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. return true; } @@ -222,7 +208,7 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf) mpuLowPassFilter = MPU6050_LPF_98HZ; break; default: - case 42: + case 42: mpuLowPassFilter = MPU6050_LPF_42HZ; break; case 20: @@ -241,7 +227,7 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf) static void mpu6050AccInit(void) { - switch(mpuAccelTrim) { + switch (mpuAccelTrim) { case MPU_6050_HALF_RESOLUTION: acc_1G = 256 * 8; break; @@ -276,11 +262,12 @@ static void mpu6050GyroInit(void) i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(5); - i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) - i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS - i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, + 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS + i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops. // Accel scale 8g (4096 LSB/g) diff --git a/src/drivers/adc_common.c b/src/drivers/adc_common.c index cb239ed13..eb6e6a4e6 100755 --- a/src/drivers/adc_common.c +++ b/src/drivers/adc_common.c @@ -17,16 +17,16 @@ extern int16_t debug[4]; uint16_t adcGetChannel(uint8_t channel) { #if 0 - switch(adcChannelCount) { + switch (adcChannelCount) { case 3: debug[2] = adcValues[adcConfig[2].dmaIndex]; - /* no break */ + /* no break */ case 2: debug[1] = adcValues[adcConfig[1].dmaIndex]; - /* no break */ + /* no break */ case 1: debug[0] = adcValues[adcConfig[0].dmaIndex]; - /* no break */ + /* no break */ default: break; } diff --git a/src/drivers/adc_common.h b/src/drivers/adc_common.h index a28b21078..2b4d2271f 100755 --- a/src/drivers/adc_common.h +++ b/src/drivers/adc_common.h @@ -15,7 +15,7 @@ typedef struct adc_config_t { } adc_config_t; typedef struct drv_adc_config_t { - uint8_t powerAdcChannel; // which channel used for current monitor, allowed PA1, PB1 (ADC_Channel_1, ADC_Channel_9) + uint8_t powerAdcChannel; // which channel used for current monitor, allowed PA1, PB1 (ADC_Channel_1, ADC_Channel_9) } drv_adc_config_t; void adcInit(drv_adc_config_t *init); diff --git a/src/drivers/adc_fy90q.c b/src/drivers/adc_fy90q.c index 59cad0978..0f1a35b9e 100644 --- a/src/drivers/adc_fy90q.c +++ b/src/drivers/adc_fy90q.c @@ -5,16 +5,14 @@ #include "platform.h" -#include "sensors_common.h" // FIXME dependency into the main code -#include "accgyro_common.h" - +#include "sensors_common.h" // FIXME dependency into the main code #include "accgyro_common.h" #include "system_common.h" #include "adc_fy90q.h" //#include "boardalignment.h" -volatile uint16_t adcData[ADC_CHANNELS] = { 0, }; +volatile uint16_t adcData[ADC_CHANNELS] = {0,}; void adcCalibrateADC(ADC_TypeDef *ADCx, int n) { diff --git a/src/drivers/barometer_bmp085.c b/src/drivers/barometer_bmp085.c index b1cfb9dbe..762134cc8 100755 --- a/src/drivers/barometer_bmp085.c +++ b/src/drivers/barometer_bmp085.c @@ -56,9 +56,7 @@ typedef struct { #define E_SENSOR_NOT_DETECTED (char) 0 #define BMP085_PROM_START__ADDR 0xaa #define BMP085_PROM_DATA__LEN 22 -#define BMP085_T_MEASURE 0x2E // temperature measurent -#define BMP085_P_MEASURE 0x34 // pressure measurement -#define BMP085_CTRL_MEAS_REG 0xF4 +#define BMP085_T_MEASURE 0x2E // temperature measurent #define BMP085_P_MEASURE 0x34 // pressure measurement #define BMP085_CTRL_MEAS_REG 0xF4 #define BMP085_ADC_OUT_MSB_REG 0xF6 #define BMP085_ADC_OUT_LSB_REG 0xF7 #define BMP085_CHIP_ID__POS 0 @@ -79,10 +77,7 @@ typedef struct { #define BMP085_GET_BITSLICE(regvar, bitname) (regvar & bitname##__MSK) >> bitname##__POS #define BMP085_SET_BITSLICE(regvar, bitname, val) (regvar & ~bitname##__MSK) | ((val<ut_delay = 6000; @@ -170,8 +165,8 @@ static int32_t bmp085_get_temperature(uint32_t ut) int32_t temperature; int32_t x1, x2; - x1 = (((int32_t)ut - (int32_t)bmp085.cal_param.ac6) * (int32_t)bmp085.cal_param.ac5) >> 15; - x2 = ((int32_t)bmp085.cal_param.mc << 11) / (x1 + bmp085.cal_param.md); + x1 = (((int32_t) ut - (int32_t) bmp085.cal_param.ac6) * (int32_t) bmp085.cal_param.ac5) >> 15; + x2 = ((int32_t) bmp085.cal_param.mc << 11) / (x1 + bmp085.cal_param.md); bmp085.param_b5 = x1 + x2; temperature = ((bmp085.param_b5 * 10 + 8) >> 4); // temperature in 0.01°C (make same as MS5611) @@ -194,7 +189,7 @@ static int32_t bmp085_get_pressure(uint32_t up) x3 = x1 + x2; - b3 = (((((int32_t)bmp085.cal_param.ac1) * 4 + x3) << bmp085.oversampling_setting) + 2) >> 2; + b3 = (((((int32_t) bmp085.cal_param.ac1) * 4 + x3) << bmp085.oversampling_setting) + 2) >> 2; // *****calculate B4************ x1 = (bmp085.cal_param.ac3 * b6) >> 13; @@ -246,9 +241,9 @@ static void bmp085_start_up(void) } /** read out up for pressure conversion - depending on the oversampling ratio setting up can be 16 to 19 bit - \return up parameter that represents the uncompensated pressure value -*/ + depending on the oversampling ratio setting up can be 16 to 19 bit + \return up parameter that represents the uncompensated pressure value + */ static void bmp085_get_up(void) { uint8_t data[3]; @@ -258,7 +253,8 @@ static void bmp085_get_up(void) convOverrun++; i2cRead(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); + bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) + >> (8 - bmp085.oversampling_setting); } static void bmp085_calculate(int32_t *pressure, int32_t *temperature) diff --git a/src/drivers/barometer_common.h b/src/drivers/barometer_common.h index 702d881b3..0a2f33a6f 100644 --- a/src/drivers/barometer_common.h +++ b/src/drivers/barometer_common.h @@ -1,7 +1,7 @@ #pragma once typedef void (*baroOpFuncPtr)(void); // baro start operation -typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) +typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) typedef struct baro_t { uint16_t ut_delay; diff --git a/src/drivers/bus_i2c_soft.c b/src/drivers/bus_i2c_soft.c index ac0f94cd1..bb855b06f 100644 --- a/src/drivers/bus_i2c_soft.c +++ b/src/drivers/bus_i2c_soft.c @@ -11,7 +11,6 @@ // SCL PB10 // SDA PB11 - #ifdef SOFT_I2C #define SCL_H GPIOB->BSRR = Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10) */ @@ -26,8 +25,9 @@ static void I2C_delay(void) { volatile int i = 7; - while (i) - i--; + while (i) { + i--; + } } static bool I2C_Start(void) @@ -35,12 +35,14 @@ static bool I2C_Start(void) SDA_H; SCL_H; I2C_delay(); - if (!SDA_read) - return false; + if (!SDA_read) { + return false; + } SDA_L; I2C_delay(); - if (SDA_read) - return false; + if (SDA_read) { + return false; + } SDA_L; I2C_delay(); return true; @@ -104,10 +106,11 @@ static void I2C_SendByte(uint8_t byte) while (i--) { SCL_L; I2C_delay(); - if (byte & 0x80) - SDA_H; - else - SDA_L; + if (byte & 0x80) { + SDA_H; + } else { + SDA_L; + } byte <<= 1; I2C_delay(); SCL_H; @@ -149,8 +152,9 @@ void i2cInit(I2C_TypeDef * I2C) bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) { int i; - if (!I2C_Start()) - return false; + if (!I2C_Start()) { + return false; + } I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); if (!I2C_WaitAck()) { I2C_Stop(); @@ -171,8 +175,9 @@ bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) bool i2cWrite(uint8_t addr, uint8_t reg, uint8_t data) { - if (!I2C_Start()) - return false; + if (!I2C_Start()) { + return false; + } I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); if (!I2C_WaitAck()) { I2C_Stop(); @@ -188,8 +193,9 @@ bool i2cWrite(uint8_t addr, uint8_t reg, uint8_t data) bool i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { - if (!I2C_Start()) - return false; + if (!I2C_Start()) { + return false; + } I2C_SendByte(addr << 1 | I2C_Direction_Transmitter); if (!I2C_WaitAck()) { I2C_Stop(); @@ -202,10 +208,11 @@ bool i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) I2C_WaitAck(); while (len) { *buf = I2C_ReceiveByte(); - if (len == 1) - I2C_NoAck(); - else - I2C_Ack(); + if (len == 1) { + I2C_NoAck(); + } else { + I2C_Ack(); + } buf++; len--; } diff --git a/src/drivers/bus_i2c_stm32f10x.c b/src/drivers/bus_i2c_stm32f10x.c index 7d5d463ce..4aa490a63 100755 --- a/src/drivers/bus_i2c_stm32f10x.c +++ b/src/drivers/bus_i2c_stm32f10x.c @@ -60,28 +60,29 @@ static void i2c_er_handler(void) volatile uint32_t SR1Register; // Read the I2C1 status register SR1Register = I2Cx->SR1; - if (SR1Register & 0x0F00) { //an error + if (SR1Register & 0x0F00) { error = true; - // I2C1error.error = ((SR1Register & 0x0F00) >> 8); //save error - // I2C1error.job = job; //the task + // I2C1error.error = ((SR1Register & 0x0F00) >> 8); // save error + // I2C1error.job = job; // the task } + // If AF, BERR or ARLO, abandon the current job and commence new if there are jobs if (SR1Register & 0x0700) { - (void)I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK) - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) - if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { // if we dont have an ARLO error, ensure sending of a stop - if (I2Cx->CR1 & 0x0100) { // We are currently trying to send a start, this is very bad as start,stop will hang the peripheral - while (I2Cx->CR1 & 0x0100); // wait for any start to finish sending - I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction - while (I2Cx->CR1 & 0x0200); // wait for stop to finish sending - i2cInit(I2Cx); // reset and configure the hardware + (void) I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK) + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) + if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { // if we dont have an ARLO error, ensure sending of a stop + if (I2Cx->CR1 & 0x0100) { // We are currently trying to send a start, this is very bad as start,stop will hang the peripheral + while (I2Cx->CR1 & 0x0100); // wait for any start to finish sending + I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction + while (I2Cx->CR1 & 0x0200); // wait for stop to finish sending + i2cInit(I2Cx); // reset and configure the hardware } else { - I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive + I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive } } } - I2Cx->SR1 &= ~0x0F00; // reset all the error bits to clear the interrupt + I2Cx->SR1 &= ~0x0F00; // reset all the error bits to clear the interrupt busy = 0; } @@ -99,12 +100,12 @@ bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) busy = 1; error = false; - if (!(I2Cx->CR2 & I2C_IT_EVT)) { //if we are restarting the driver - if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start - while (I2Cx->CR1 & 0x0200) { ; } //wait for any stop to finish sending - I2C_GenerateSTART(I2Cx, ENABLE); //send the start for the new job + if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver + if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start + while (I2Cx->CR1 & 0x0200); // wait for any stop to finish sending + I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job } - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); //allow the interrupts to fire off again + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again } while (busy && --timeout > 0); @@ -137,12 +138,12 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) busy = 1; error = false; - if (!(I2Cx->CR2 & I2C_IT_EVT)) { //if we are restarting the driver - if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start - while (I2Cx->CR1 & 0x0200) { ; } //wait for any stop to finish sending - I2C_GenerateSTART(I2Cx, ENABLE); //send the start for the new job + if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver + if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start + while (I2Cx->CR1 & 0x0200); // wait for any stop to finish sending + I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job } - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); //allow the interrupts to fire off again + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again } while (busy && --timeout > 0); @@ -158,82 +159,83 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) void i2c_ev_handler(void) { - static uint8_t subaddress_sent, final_stop; //flag to indicate if subaddess sent, flag to indicate final bus condition - static int8_t index; //index is signed -1==send the subaddress - uint8_t SReg_1 = I2Cx->SR1; //read the status register here + static uint8_t subaddress_sent, final_stop; // flag to indicate if subaddess sent, flag to indicate final bus condition + static int8_t index; // index is signed -1==send the subaddress + uint8_t SReg_1 = I2Cx->SR1; // read the status register here - if (SReg_1 & 0x0001) { //we just sent a start - EV5 in ref manual - I2Cx->CR1 &= ~0x0800; //reset the POS bit so ACK/NACK applied to the current byte - I2C_AcknowledgeConfig(I2Cx, ENABLE); //make sure ACK is on - index = 0; //reset the index - if (reading && (subaddress_sent || 0xFF == reg)) { //we have sent the subaddr - subaddress_sent = 1; //make sure this is set in case of no subaddress, so following code runs correctly + if (SReg_1 & 0x0001) { // we just sent a start - EV5 in ref manual + I2Cx->CR1 &= ~0x0800; // reset the POS bit so ACK/NACK applied to the current byte + I2C_AcknowledgeConfig(I2Cx, ENABLE); // make sure ACK is on + index = 0; // reset the index + if (reading && (subaddress_sent || 0xFF == reg)) { // we have sent the subaddr + subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly if (bytes == 2) - I2Cx->CR1 |= 0x0800; //set the POS bit so NACK applied to the final byte in the two byte read - I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Receiver); //send the address and set hardware mode - } else { //direction is Tx, or we havent sent the sub and rep start - I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Transmitter); //send the address and set hardware mode - if (reg != 0xFF) //0xFF as subaddress means it will be ignored, in Tx or Rx mode - index = -1; //send a subaddress + I2Cx->CR1 |= 0x0800; // set the POS bit so NACK applied to the final byte in the two byte read + I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Receiver); // send the address and set hardware mode + } else { // direction is Tx, or we havent sent the sub and rep start + I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Transmitter); // send the address and set hardware mode + if (reg != 0xFF) // 0xFF as subaddress means it will be ignored, in Tx or Rx mode + index = -1; // send a subaddress } - } else if (SReg_1 & 0x0002) { //we just sent the address - EV6 in ref manual + } else if (SReg_1 & 0x0002) { // we just sent the address - EV6 in ref manual // Read SR1,2 to clear ADDR __DMB(); // memory fence to control hardware - if (bytes == 1 && reading && subaddress_sent) { // we are receiving 1 byte - EV6_3 - I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + if (bytes == 1 && reading && subaddress_sent) { // we are receiving 1 byte - EV6_3 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK __DMB(); - (void)I2Cx->SR2; // clear ADDR after ACK is turned off - I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop + (void) I2Cx->SR2; // clear ADDR after ACK is turned off + I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop final_stop = 1; - I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7 - } else { // EV6 and EV6_1 - (void)I2Cx->SR2; // clear the ADDR here + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7 + } else { // EV6 and EV6_1 + (void) I2Cx->SR2; // clear the ADDR here __DMB(); - if (bytes == 2 && reading && subaddress_sent) { //rx 2 bytes - EV6_1 - I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to fill - } else if (bytes == 3 && reading && subaddress_sent) //rx 3 bytes - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //make sure RXNE disabled so we get a BTF in two bytes time - else //receiving greater than three bytes, sending subaddress, or transmitting + if (bytes == 2 && reading && subaddress_sent) { // rx 2 bytes - EV6_1 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to fill + } else if (bytes == 3 && reading && subaddress_sent) // rx 3 bytes + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // make sure RXNE disabled so we get a BTF in two bytes time + else + //receiving greater than three bytes, sending subaddress, or transmitting I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); } - } else if (SReg_1 & 0x004) { //Byte transfer finished - EV7_2, EV7_3 or EV8_2 + } else if (SReg_1 & 0x004) { // Byte transfer finished - EV7_2, EV7_3 or EV8_2 final_stop = 1; - if (reading && subaddress_sent) { //EV7_2, EV7_3 - if (bytes > 2) { //EV7_2 - I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-2 - I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop - final_stop = 1; //reuired to fix hardware - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-1 - I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); //enable TXE to allow the final EV7 - } else { //EV7_3 + if (reading && subaddress_sent) { // EV7_2, EV7_3 + if (bytes > 2) { // EV7_2 + I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK + read_p[index++] = I2C_ReceiveData(I2Cx); // read data N-2 + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop + final_stop = 1; // required to fix hardware + read_p[index++] = I2C_ReceiveData(I2Cx); // read data N-1 + I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // enable TXE to allow the final EV7 + } else { // EV7_3 if (final_stop) - I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop else - I2C_GenerateSTART(I2Cx, ENABLE); //program a rep start - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-1 - read_p[index++] = I2C_ReceiveData(I2Cx); //read data N - index++; //to show job completed + I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start + read_p[index++] = I2C_ReceiveData(I2Cx); // read data N-1 + read_p[index++] = I2C_ReceiveData(I2Cx); // read data N + index++; // to show job completed } - } else { //EV8_2, which may be due to a subaddress sent or a write completion + } else { // EV8_2, which may be due to a subaddress sent or a write completion if (subaddress_sent || (writing)) { if (final_stop) - I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop + I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop else - I2C_GenerateSTART(I2Cx, ENABLE); //program a rep start - index++; //to show that the job is complete - } else { //We need to send a subaddress - I2C_GenerateSTART(I2Cx, ENABLE); //program the repeated Start - subaddress_sent = 1; //this is set back to zero upon completion of the current task + I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start + index++; // to show that the job is complete + } else { // We need to send a subaddress + I2C_GenerateSTART(I2Cx, ENABLE); // program the repeated Start + subaddress_sent = 1; // this is set back to zero upon completion of the current task } } //we must wait for the start to clear, otherwise we get constant BTF - while (I2Cx->CR1 & 0x0100) { ; } + while (I2Cx->CR1 & 0x0100); } else if (SReg_1 & 0x0040) { //Byte received - EV7 read_p[index++] = I2C_ReceiveData(I2Cx); if (bytes == (index + 3)) - I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush so we can get an EV7_2 + I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush so we can get an EV7_2 if (bytes == index) //We have completed a final EV7 index++; //to show job is complete } else if (SReg_1 & 0x0080) { //Byte transmitted -EV8/EV8_1 @@ -249,12 +251,12 @@ void i2c_ev_handler(void) } } if (index == bytes + 1) { //we have completed the current job - //Completion Tasks go here - //End of completion tasks + //Completion Tasks go here + //End of completion tasks subaddress_sent = 0; //reset this here // I2Cx->CR1 &= ~0x0800; //reset the POS bit so NACK applied to the current byte if (final_stop) //If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Disable EVT and ERR interrupts while bus inactive + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Disable EVT and ERR interrupts while bus inactive busy = 0; } } @@ -280,7 +282,7 @@ void i2cInit(I2C_TypeDef *I2C) I2C_DeInit(I2Cx); I2C_StructInit(&I2C_InitStructure); - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Enable EVT and ERR interrupts - they are enabled by the first request + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Enable EVT and ERR interrupts - they are enabled by the first request I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; @@ -301,7 +303,6 @@ void i2cInit(I2C_TypeDef *I2C) NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_Init(&NVIC_InitStructure); - } uint16_t i2cGetErrorCounter(void) diff --git a/src/drivers/bus_i2c_stm32f30x.c b/src/drivers/bus_i2c_stm32f30x.c index 3469d13eb..c478b2b5c 100644 --- a/src/drivers/bus_i2c_stm32f30x.c +++ b/src/drivers/bus_i2c_stm32f30x.c @@ -15,7 +15,6 @@ #define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) - #define I2C1_SCL_GPIO GPIOB #define I2C1_SCL_PIN GPIO_Pin_6 #define I2C1_SCL_PIN_SOURCE GPIO_PinSource6 @@ -34,7 +33,7 @@ #define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 #define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA -static uint32_t i2cTimeout; +static uint32_t i2cTimeout; static volatile uint16_t i2c1ErrorCount = 0; static volatile uint16_t i2c2ErrorCount = 0; @@ -45,12 +44,9 @@ static volatile uint16_t i2c2ErrorCount = 0; uint32_t i2cTimeoutUserCallback(I2C_TypeDef *I2Cx) { - if (I2Cx == I2C1) - { + if (I2Cx == I2C1) { i2c1ErrorCount++; - } - else - { + } else { i2c2ErrorCount++; } return false; @@ -59,13 +55,10 @@ uint32_t i2cTimeoutUserCallback(I2C_TypeDef *I2Cx) void i2cInitPort(I2C_TypeDef *I2Cx) { GPIO_InitTypeDef GPIO_InitStructure; - I2C_InitTypeDef I2C_InitStructure; + I2C_InitTypeDef I2C_InitStructure; - /////////////////////////////////// - - if (I2Cx == I2C1) - { - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); + if (I2Cx == I2C1) { + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); RCC_AHBPeriphClockCmd(I2C1_SCL_CLK_SOURCE | I2C1_SDA_CLK_SOURCE, ENABLE); RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK); @@ -80,24 +73,24 @@ void i2cInitPort(I2C_TypeDef *I2Cx) // Init pins - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Pin = I2C1_SCL_PIN; + GPIO_InitStructure.GPIO_Pin = I2C1_SCL_PIN; GPIO_Init(I2C1_SCL_GPIO, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = I2C1_SDA_PIN; + GPIO_InitStructure.GPIO_Pin = I2C1_SDA_PIN; GPIO_Init(I2C1_SDA_GPIO, &GPIO_InitStructure); I2C_StructInit(&I2C_InitStructure); - I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; - I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; - I2C_InitStructure.I2C_DigitalFilter = 0x00; - I2C_InitStructure.I2C_OwnAddress1 = 0x00; - I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; + I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; + I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; + I2C_InitStructure.I2C_DigitalFilter = 0x00; + I2C_InitStructure.I2C_OwnAddress1 = 0x00; + I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. //I2C_InitStructure.I2C_Timing = 0x8000050B; @@ -107,13 +100,10 @@ void i2cInitPort(I2C_TypeDef *I2Cx) I2C_Cmd(I2C1, ENABLE); } - /////////////////////////////////// - - if (I2Cx == I2C2) - { - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); + if (I2Cx == I2C2) { + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); - RCC_AHBPeriphClockCmd(I2C2_SCL_CLK_SOURCE |I2C2_SDA_CLK_SOURCE, ENABLE); + RCC_AHBPeriphClockCmd(I2C2_SCL_CLK_SOURCE | I2C2_SDA_CLK_SOURCE, ENABLE); RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK); //i2cUnstick(I2Cx); // Clock out stuff to make sure slaves arent stuck @@ -125,24 +115,24 @@ void i2cInitPort(I2C_TypeDef *I2Cx) I2C_StructInit(&I2C_InitStructure); // Init pins - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Pin = I2C2_SCL_PIN; + GPIO_InitStructure.GPIO_Pin = I2C2_SCL_PIN; GPIO_Init(I2C2_SCL_GPIO, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = I2C2_SDA_PIN; + GPIO_InitStructure.GPIO_Pin = I2C2_SDA_PIN; GPIO_Init(I2C2_SDA_GPIO, &GPIO_InitStructure); I2C_StructInit(&I2C_InitStructure); - I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; - I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; - I2C_InitStructure.I2C_DigitalFilter = 0x00; - I2C_InitStructure.I2C_OwnAddress1 = 0x00; - I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; + I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; + I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; + I2C_InitStructure.I2C_DigitalFilter = 0x00; + I2C_InitStructure.I2C_OwnAddress1 = 0x00; + I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. //I2C_InitStructure.I2C_Timing = 0x8000050B; @@ -160,7 +150,6 @@ void i2cInit(I2C_TypeDef *I2C) uint16_t i2cGetErrorCounter(void) { - // FIXME implement return i2c1ErrorCount; } @@ -170,10 +159,10 @@ bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) - { - if((i2cTimeout--) == 0) + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) { + if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(I2Cx); + } } /* Configure slave address, nbytes, reload, end mode and start or stop generation */ @@ -181,10 +170,10 @@ bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) - { - if((i2cTimeout--) == 0) + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { + if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(I2Cx); + } } /* Send Register address */ @@ -192,10 +181,11 @@ bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) /* Wait until TCR flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET) + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET) { - if((i2cTimeout--) == 0) + if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(I2Cx); + } } /* Configure slave address, nbytes, reload, end mode and start or stop generation */ @@ -203,10 +193,10 @@ bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) - { - if((i2cTimeout--) == 0) return - i2cTimeoutUserCallback(I2Cx); + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(I2Cx); + } } /* Write data to TXDR */ @@ -214,9 +204,10 @@ bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) /* Wait until STOPF flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) - { - if((i2cTimeout--) == 0) return i2cTimeoutUserCallback(I2Cx); + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(I2Cx); + } } /* Clear STOPF flag */ @@ -230,10 +221,10 @@ bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) I2C_TypeDef* I2Cx = I2C1; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) - { - if((i2cTimeout--) == 0) + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) { + if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(I2Cx); + } } /* Configure slave address, nbytes, reload, end mode and start or stop generation */ @@ -241,38 +232,38 @@ bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) - { - if((i2cTimeout--) == 0) + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { + if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(I2Cx); + } } - if(len > 1) + if (len > 1) { reg |= 0x80; + } /* Send Register address */ - I2C_SendData(I2Cx, (uint8_t)reg); + 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) + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TC) == RESET) { + if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(I2Cx); + } } /* 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) - { + while (len) { /* Wait until RXNE flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_RXNE) == RESET) - { - if((i2cTimeout--) == 0) + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_RXNE) == RESET) { + if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(I2Cx); + } } /* Read data from RXDR */ @@ -286,10 +277,10 @@ bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) /* Wait until STOPF flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; - while(I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) - { - if((i2cTimeout--) == 0) return - i2cTimeoutUserCallback(I2Cx); + while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { + if ((i2cTimeout--) == 0) { + return i2cTimeoutUserCallback(I2Cx); + } } /* Clear STOPF flag */ diff --git a/src/drivers/pwm_rx.c b/src/drivers/pwm_rx.c index c46e32e5d..0ede9d6b7 100644 --- a/src/drivers/pwm_rx.c +++ b/src/drivers/pwm_rx.c @@ -1,4 +1,3 @@ - #include #include @@ -16,8 +15,8 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); // from pwm_output.c typedef enum { - INPUT_MODE_PPM, - INPUT_MODE_PWM, + INPUT_MODE_PPM, + INPUT_MODE_PWM, } pwmInputMode_t; typedef struct { @@ -36,7 +35,6 @@ static pwmInputPort_t pwmInputPorts[MAX_PWM_INPUT_PORTS]; static uint16_t captures[MAX_PWM_INPUT_PORTS]; - static void ppmCallback(uint8_t port, captureCompare_t capture) { int32_t diff; @@ -61,8 +59,8 @@ static void ppmCallback(uint8_t port, captureCompare_t capture) static void pwmCallback(uint8_t port, captureCompare_t capture) { - pwmInputPort_t *pwmInputPort = &pwmInputPorts[port]; - const timerHardware_t *timerHardware = pwmInputPort->timerHardware; + pwmInputPort_t *pwmInputPort = &pwmInputPorts[port]; + const timerHardware_t *timerHardware = pwmInputPort->timerHardware; if (pwmInputPort->state == 0) { pwmInputPort->rise = capture; @@ -95,7 +93,7 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { - TIM_ICInitTypeDef TIM_ICInitStructure; + TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel = channel; diff --git a/src/drivers/serial_common.c b/src/drivers/serial_common.c index 4507de451..766e15080 100644 --- a/src/drivers/serial_common.c +++ b/src/drivers/serial_common.c @@ -1,4 +1,3 @@ - #include #include diff --git a/src/drivers/serial_common.h b/src/drivers/serial_common.h index 7f4e1dde0..04ade08d4 100644 --- a/src/drivers/serial_common.h +++ b/src/drivers/serial_common.h @@ -1,6 +1,5 @@ #pragma once - typedef enum { SERIAL_NOT_INVERTED = 0, SERIAL_INVERTED