From 37e7b5ee4aa6044d58c4ee949ce1cb107de25ff5 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 3 Jun 2016 21:20:11 +1000 Subject: [PATCH] Adjust interface for i2c to add requirement for device to be specified. --- src/main/drivers/accgyro.h | 4 ++++ src/main/drivers/accgyro_adxl345.c | 20 ++++++++-------- src/main/drivers/accgyro_bma280.c | 8 +++---- src/main/drivers/accgyro_l3g4200d.c | 8 +++---- src/main/drivers/accgyro_mma845x.c | 30 +++++++++++++----------- src/main/drivers/accgyro_mpu.c | 8 +++++-- src/main/drivers/barometer.h | 4 ++++ src/main/drivers/barometer_bmp085.c | 14 +++++------ src/main/drivers/barometer_bmp280.c | 10 ++++---- src/main/drivers/barometer_ms5611.c | 12 +++++----- src/main/drivers/bus_i2c.h | 9 +++---- src/main/drivers/bus_i2c_soft.c | 28 +++++++--------------- src/main/drivers/bus_i2c_stm32f10x.c | 28 ++++------------------ src/main/drivers/bus_i2c_stm32f30x.c | 22 ++--------------- src/main/drivers/compass.h | 4 ++++ src/main/drivers/compass_hmc5883l.c | 20 ++++++++-------- src/main/drivers/display_ug2864hsweg01.c | 8 +++++-- 17 files changed, 104 insertions(+), 133 deletions(-) diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 5e5e528cf..1a99d9838 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -17,6 +17,10 @@ #pragma once +#ifndef MPU_I2C_INSTANCE +#define MPU_I2C_INSTANCE I2C_DEVICE +#endif + extern uint16_t acc_1G; // FIXME move into acc_t typedef struct gyro_s { diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c index feada6597..57b5ea339 100644 --- a/src/main/drivers/accgyro_adxl345.c +++ b/src/main/drivers/accgyro_adxl345.c @@ -66,7 +66,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc) bool ack = false; uint8_t sig = 0; - ack = i2cRead(ADXL345_ADDRESS, 0x00, 1, &sig); + ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig); if (!ack || sig != 0xE5) return false; @@ -82,14 +82,14 @@ static void adxl345Init(void) { if (useFifo) { uint8_t fifoDepth = 16; - i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); - i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); - i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_400); - i2cWrite(ADXL345_ADDRESS, ADXL345_FIFO_CTL, (fifoDepth & 0x1F) | ADXL345_FIFO_STREAM); + i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); + i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); + i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_400); + i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_FIFO_CTL, (fifoDepth & 0x1F) | ADXL345_FIFO_STREAM); } else { - i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); - i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); - i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); + i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); + i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); + i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); } acc_1G = 265; // 3.3V operation // FIXME verify this is supposed to be 265, not 256. Typo? } @@ -110,7 +110,7 @@ static bool adxl345Read(int16_t *accelData) do { i++; - if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) { + if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) { return false; } @@ -125,7 +125,7 @@ static bool adxl345Read(int16_t *accelData) acc_samples = i; } else { - if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) { + if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) { return false; } diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c index 193ae09ce..b93fda695 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(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(void) { - i2cWrite(BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range - i2cWrite(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_1G = 512 * 8; } @@ -61,7 +61,7 @@ static bool bma280Read(int16_t *accelData) { uint8_t buf[6]; - if (!i2cRead(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 2cc663c49..27beb9d63 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(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(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(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(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_mma845x.c b/src/main/drivers/accgyro_mma845x.c index bf39f41b0..dc2a28619 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -28,6 +28,10 @@ #include "accgyro.h" #include "accgyro_mma845x.h" +#ifndef MMA8452_I2C_INSTANCE +#define MMA8452_I2C_INSTANCE I2CDEV_1 +#endif + // MMA8452QT, Standard address 0x1C // ACC_INT2 routed to PA5 @@ -85,7 +89,7 @@ bool mma8452Detect(acc_t *acc) bool ack = false; uint8_t sig = 0; - ack = i2cRead(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; @@ -106,28 +110,26 @@ static inline void mma8451ConfigureInterrupt(void) RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - gpio.pin = Pin_5; - gpio.speed = Speed_2MHz; - gpio.mode = Mode_IN_FLOATING; - gpioInit(GPIOA, &gpio); + IOInit(IOGetByTag(IO_TAG(PA5)), OWNER_SYSTEM, RESOURCE_I2C); + IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? #endif - i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) - i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) - i2cWrite(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(void) { - i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff - i2cWrite(MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G); - i2cWrite(MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4); - i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G); + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4); + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes mma8451ConfigureInterrupt(); - i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G. + i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G. acc_1G = 256; } @@ -136,7 +138,7 @@ static bool mma8452Read(int16_t *accelData) { uint8_t buf[6]; - if (!i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf)) { + if (!i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf)) { return false; } diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 83757c653..cea7f4612 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -56,6 +56,10 @@ static volatile bool mpuDataReady; static bool detectSPISensorsAndUpdateDetectionResult(void); #endif +#ifndef MPU_I2C_INSTANCE +#define MPU_I2C_INSTANCE I2C_DEVICE +#endif + mpuDetectionResult_t mpuDetectionResult; mpuConfiguration_t mpuConfiguration; @@ -239,13 +243,13 @@ void mpuIntExtiInit(void) static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) { - bool ack = i2cRead(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_ADDRESS, reg, data); + bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); return ack; } diff --git a/src/main/drivers/barometer.h b/src/main/drivers/barometer.h index 6d0e1c034..1d9d8f891 100644 --- a/src/main/drivers/barometer.h +++ b/src/main/drivers/barometer.h @@ -29,3 +29,7 @@ typedef struct baro_s { baroOpFuncPtr get_up; baroCalculateFuncPtr calculate; } baro_t; + +#ifndef BARO_I2C_INSTANCE +#define BARO_I2C_INSTANCE I2C_DEVICE +#endif \ No newline at end of file diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index 79e65ad85..f6dcbfa2c 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -186,13 +186,13 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro) delay(20); // datasheet says 10ms, we'll be careful and do 20. - ack = i2cRead(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(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 */ @@ -279,7 +279,7 @@ static void bmp085_start_ut(void) #if defined(BARO_EOC_GPIO) isConversionComplete = false; #endif - i2cWrite(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) @@ -293,7 +293,7 @@ static void bmp085_get_ut(void) } #endif - i2cRead(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]; } @@ -307,7 +307,7 @@ static void bmp085_start_up(void) isConversionComplete = false; #endif - i2cWrite(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 @@ -325,7 +325,7 @@ static void bmp085_get_up(void) } #endif - i2cRead(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); } @@ -345,7 +345,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(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_bmp280.c b/src/main/drivers/barometer_bmp280.c index 257f2dd7e..cf7e60a81 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(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(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(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(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(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 a8a48057a..577146e2d 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(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(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(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(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(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(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.h b/src/main/drivers/bus_i2c.h index db935a6e0..bdfc75dfa 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -61,11 +61,8 @@ typedef struct i2cState_s { } i2cState_t; void i2cInit(I2CDevice device); -bool i2cWriteBufferByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); -bool i2cWriteByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data); -bool i2cReadByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data); +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); -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); -bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); uint16_t i2cGetErrorCounter(void); diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 2a2c2ec71..edf9e6d72 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -175,8 +175,10 @@ void i2cInit(I2CDevice device) IOConfigGPIO(sda, IOCFG_OUT_OD); } -bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) { + UNUSED(device); + int i; if (!I2C_Start()) { i2cErrorCount++; @@ -201,8 +203,10 @@ bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) return true; } -bool i2cWrite(uint8_t addr, uint8_t reg, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data) { + UNUSED(device); + if (!I2C_Start()) { return false; } @@ -220,8 +224,10 @@ bool i2cWrite(uint8_t addr, uint8_t reg, uint8_t data) return true; } -bool i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) +bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { + UNUSED(device); + if (!I2C_Start()) { return false; } @@ -256,21 +262,5 @@ uint16_t i2cGetErrorCounter(void) return i2cErrorCount; } -bool i2cWriteBufferByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) -{ - return i2cWriteBuffer(addr_, reg_, len_, data); -} - -bool i2cWriteByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) -{ - return i2cWrite(addr_, reg, data); -} - -bool i2cReadByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) -{ - return i2cRead(addr_, reg, len, buf); -} - - #endif diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index e083f2d62..d0dbcd8ae 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -68,9 +68,6 @@ static i2cDevice_t i2cHardwareMap[] = { #endif }; -// Copy of device index for reinit, etc purposes -static I2CDevice I2Cx_device; - static volatile uint16_t i2cErrorCount = 0; static i2cState_t i2cState[] = { @@ -113,7 +110,7 @@ static bool i2cHandleHardwareFailure(I2CDevice device) return false; } -bool i2cWriteBufferByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) { if (device == I2CINVALID) @@ -155,12 +152,12 @@ bool i2cWriteBufferByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8 return !(state->error); } -bool i2cWriteByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) { - return i2cWriteBufferByDevice(device, addr_, reg_, 1, &data); + return i2cWriteBuffer(device, addr_, reg_, 1, &data); } -bool i2cReadByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { if (device == I2CINVALID) return false; @@ -355,8 +352,6 @@ void i2c_ev_handler(I2CDevice device) { void i2cInit(I2CDevice device) { - I2Cx_device = device; - if (device == I2CINVALID) return; @@ -462,19 +457,4 @@ static void i2cUnstick(IO_t scl, IO_t sda) IOHi(sda); // Set bus sda high } -bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) -{ - return i2cWriteBufferByDevice(I2Cx_device, addr_, reg_, len_, data); -} - -bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) -{ - return i2cWriteByDevice(I2Cx_device, addr_, reg, data); -} - -bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) -{ - return i2cReadByDevice(I2Cx_device, addr_, reg, len, buf); -} - #endif diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 82df59509..8f9225ef7 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -68,11 +68,8 @@ uint32_t i2cTimeoutUserCallback(void) return false; } -static I2CDevice I2Cx_device; - void i2cInit(I2CDevice device) { - I2Cx_device = device; i2cDevice_t *i2c; i2c = &(i2cHardwareMap[device]); @@ -113,7 +110,7 @@ uint16_t i2cGetErrorCounter(void) return i2cErrorCount; } -bool i2cWriteByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) { addr_ <<= 1; @@ -179,7 +176,7 @@ bool i2cWriteByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data return true; } -bool i2cReadByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) { addr_ <<= 1; @@ -253,19 +250,4 @@ bool i2cReadByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, return true; } -bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) -{ - return i2cWriteBufferByDevice(I2Cx_device, addr_, reg_, len_, data); -} - -bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) -{ - return i2cWriteByDevice(I2Cx_device, addr_, reg, data); -} - -bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) -{ - return i2cReadByDevice(I2Cx_device, addr_, reg, len, buf); -} - #endif diff --git a/src/main/drivers/compass.h b/src/main/drivers/compass.h index c5babb893..68d44442f 100644 --- a/src/main/drivers/compass.h +++ b/src/main/drivers/compass.h @@ -21,3 +21,7 @@ typedef struct mag_s { sensorInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function } mag_t; + +#ifndef MAG_I2C_INSTANCE +#define MAG_I2C_INSTANCE I2C_DEVICE +#endif \ No newline at end of file diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 0f34be0a7..4e59c375b 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_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_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_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_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_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_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_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode - i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga - i2cWrite(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_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/display_ug2864hsweg01.c b/src/main/drivers/display_ug2864hsweg01.c index 2f863a9d2..d16798e04 100644 --- a/src/main/drivers/display_ug2864hsweg01.c +++ b/src/main/drivers/display_ug2864hsweg01.c @@ -26,6 +26,10 @@ #include "display_ug2864hsweg01.h" +#ifndef OLED_I2C_INSTANCE +#define OLED_I2C_INSTANCE I2CDEV_1 +#endif + #define INVERSE_CHAR_FORMAT 0x7f // 0b01111111 #define NORMAL_CHAR_FORMAT 0x00 // 0b00000000 @@ -172,12 +176,12 @@ static const uint8_t multiWiiFont[][5] = { // Refer to "Times New Roman" Font Da static bool i2c_OLED_send_cmd(uint8_t command) { - return i2cWrite(OLED_address, 0x80, command); + return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x80, command); } static bool i2c_OLED_send_byte(uint8_t val) { - return i2cWrite(OLED_address, 0x40, val); + return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x40, val); } void i2c_OLED_clear_display(void)