From 60b8e0f05ea295056b823d7d23bcb363c9f4f8a3 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 6 Oct 2017 07:43:02 +0200 Subject: [PATCH 1/2] Configurable compass drivers based on jflyper's PR #3613 Eliminate static variables in mag drivers AK8975 and HMC5883l driver cleanup replace magic numbers with definitions Switched AK8963/Ak8975 to 16 bit mode Update Mag gain calculations to use integer varables (ledvinap recomendation) Update interrupt handling for HMC5883L Reschedule compass task only if slave mode is realy active Change bustye definitions to an enumeration set Fix dispatch functions and remove redundant dispatch functions from Baro and Mag drivers Fix unittest --- src/main/drivers/barometer/barometer_bmp085.c | 24 +- src/main/drivers/barometer/barometer_bmp280.c | 40 +- src/main/drivers/barometer/barometer_ms5611.c | 42 +- src/main/drivers/bus.c | 33 +- src/main/drivers/bus.h | 16 +- src/main/drivers/compass/compass.h | 10 +- src/main/drivers/compass/compass_ak8963.c | 403 ++++++++++++------ src/main/drivers/compass/compass_ak8963.h | 46 -- src/main/drivers/compass/compass_ak8975.c | 115 +++-- src/main/drivers/compass/compass_fake.c | 10 +- src/main/drivers/compass/compass_hmc5883l.c | 245 ++++++----- src/main/drivers/compass/compass_hmc5883l.h | 2 +- src/main/drivers/compass/compass_spi_ak8963.c | 122 ------ src/main/drivers/compass/compass_spi_ak8963.h | 20 - .../drivers/compass/compass_spi_hmc5883l.c | 86 ---- .../drivers/compass/compass_spi_hmc5883l.h | 22 - src/main/drivers/resource.c | 1 + src/main/drivers/resource.h | 1 + src/main/drivers/sensor.h | 5 +- src/main/fc/cli.c | 3 + src/main/fc/fc_tasks.c | 4 - src/main/fc/settings.c | 9 +- src/main/sensors/compass.c | 157 +++++-- src/main/sensors/compass.h | 5 + src/main/target/ALIENFLIGHTNGF7/target.mk | 4 +- src/test/unit/baro_bmp085_unittest.cc.txt | 8 +- src/test/unit/baro_bmp280_unittest.cc | 7 +- src/test/unit/baro_ms5611_unittest.cc | 6 +- 28 files changed, 721 insertions(+), 725 deletions(-) delete mode 100644 src/main/drivers/compass/compass_spi_ak8963.c delete mode 100644 src/main/drivers/compass/compass_spi_ak8963.h delete mode 100755 src/main/drivers/compass/compass_spi_hmc5883l.c delete mode 100755 src/main/drivers/compass/compass_spi_hmc5883l.h diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c index 54259cda6d..46e5d0420f 100644 --- a/src/main/drivers/barometer/barometer_bmp085.c +++ b/src/main/drivers/barometer/barometer_bmp085.c @@ -155,16 +155,6 @@ void bmp085Disable(const bmp085Config_t *config) BMP085_OFF; } -bool bmp085ReadRegister(busDevice_t *busdev, uint8_t cmd, uint8_t len, uint8_t *data) -{ - return i2cBusReadRegisterBuffer(busdev, cmd, data, len); -} - -bool bmp085WriteRegister(busDevice_t *busdev, uint8_t cmd, uint8_t byte) -{ - return i2cBusWriteRegister(busdev, cmd, byte); -} - bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro) { uint8_t data; @@ -205,13 +195,13 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro) defaultAddressApplied = true; } - ack = bmp085ReadRegister(busdev, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ + ack = busReadRegisterBuffer(busdev, BMP085_CHIP_ID__REG, &data, 1); /* 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 */ - bmp085ReadRegister(busdev, BMP085_VERSION_REG, 1, &data); /* read Version reg */ + busReadRegisterBuffer(busdev, BMP085_VERSION_REG, &data, 1); /* 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(busdev); /* readout bmp085 calibparam structure */ @@ -302,7 +292,7 @@ static void bmp085_start_ut(baroDev_t *baro) #if defined(BARO_EOC_GPIO) isConversionComplete = false; #endif - bmp085WriteRegister(&baro->busdev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); + busWriteRegister(&baro->busdev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); } static void bmp085_get_ut(baroDev_t *baro) @@ -316,7 +306,7 @@ static void bmp085_get_ut(baroDev_t *baro) } #endif - bmp085ReadRegister(&baro->busdev, BMP085_ADC_OUT_MSB_REG, 2, data); + busReadRegisterBuffer(&baro->busdev, BMP085_ADC_OUT_MSB_REG, data, 2); bmp085_ut = (data[0] << 8) | data[1]; } @@ -330,7 +320,7 @@ static void bmp085_start_up(baroDev_t *baro) isConversionComplete = false; #endif - bmp085WriteRegister(&baro->busdev, BMP085_CTRL_MEAS_REG, ctrl_reg_data); + busWriteRegister(&baro->busdev, BMP085_CTRL_MEAS_REG, ctrl_reg_data); } /** read out up for pressure conversion @@ -348,7 +338,7 @@ static void bmp085_get_up(baroDev_t *baro) } #endif - bmp085ReadRegister(&baro->busdev, BMP085_ADC_OUT_MSB_REG, 3, data); + busReadRegisterBuffer(&baro->busdev, BMP085_ADC_OUT_MSB_REG, data, 3); bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting); } @@ -368,7 +358,7 @@ STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature static void bmp085_get_cal_param(busDevice_t *busdev) { uint8_t data[22]; - bmp085ReadRegister(busdev, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data); + busReadRegisterBuffer(busdev, BMP085_PROM_START__ADDR, data, BMP085_PROM_DATA__LEN); /*parameters AC1-AC6*/ bmp085.cal_param.ac1 = (data[0] << 8) | data[1]; diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c index 7388ab7a51..2ef9846753 100644 --- a/src/main/drivers/barometer/barometer_bmp280.c +++ b/src/main/drivers/barometer/barometer_bmp280.c @@ -65,36 +65,6 @@ static void bmp280_get_up(baroDev_t *baro); STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature); -bool bmp280ReadRegister(busDevice_t *busdev, uint8_t reg, uint8_t length, uint8_t *data) -{ - switch (busdev->bustype) { -#ifdef USE_BARO_SPI_BMP280 - case BUSTYPE_SPI: - return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length); -#endif -#ifdef USE_BARO_BMP280 - case BUSTYPE_I2C: - return i2cBusReadRegisterBuffer(busdev, reg, data, length); -#endif - } - return false; -} - -bool bmp280WriteRegister(busDevice_t *busdev, uint8_t reg, uint8_t data) -{ - switch (busdev->bustype) { -#ifdef USE_BARO_SPI_BMP280 - case BUSTYPE_SPI: - return spiBusWriteRegister(busdev, reg & 0x7f, data); -#endif -#ifdef USE_BARO_BMP280 - case BUSTYPE_I2C: - return i2cBusWriteRegister(busdev, reg, data); -#endif - } - return false; -} - void bmp280BusInit(busDevice_t *busdev) { #ifdef USE_BARO_SPI_BMP280 @@ -137,7 +107,7 @@ bool bmp280Detect(baroDev_t *baro) defaultAddressApplied = true; } - bmp280ReadRegister(busdev, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */ + busReadRegisterBuffer(busdev, BMP280_CHIP_ID_REG, &bmp280_chip_id, 1); /* read Chip Id */ if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) { bmp280BusDeinit(busdev); @@ -148,10 +118,10 @@ bool bmp280Detect(baroDev_t *baro) } // read calibration - bmp280ReadRegister(busdev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal); + busReadRegisterBuffer(busdev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, 24); // set oversampling + power mode (forced), and start sampling - bmp280WriteRegister(busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE); + busWriteRegister(busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE); // these are dummy as temperature is measured as part of pressure baro->ut_delay = 0; @@ -182,7 +152,7 @@ static void bmp280_start_up(baroDev_t *baro) { // start measurement // set oversampling + power mode (forced), and start sampling - bmp280WriteRegister(&baro->busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE); + busWriteRegister(&baro->busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE); } static void bmp280_get_up(baroDev_t *baro) @@ -190,7 +160,7 @@ static void bmp280_get_up(baroDev_t *baro) uint8_t data[BMP280_DATA_FRAME_SIZE]; // read data from sensor - bmp280ReadRegister(&baro->busdev, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data); + busReadRegisterBuffer(&baro->busdev, BMP280_PRESSURE_MSB_REG, data, BMP280_DATA_FRAME_SIZE); 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/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c index 8cdec08ee5..7969c70024 100644 --- a/src/main/drivers/barometer/barometer_ms5611.c +++ b/src/main/drivers/barometer/barometer_ms5611.c @@ -61,36 +61,6 @@ STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM static uint8_t ms5611_osr = CMD_ADC_4096; -bool ms5611ReadCommand(busDevice_t *busdev, uint8_t cmd, uint8_t len, uint8_t *data) -{ - switch (busdev->bustype) { -#ifdef USE_BARO_SPI_MS5611 - case BUSTYPE_SPI: - return spiBusReadRegisterBuffer(busdev, cmd | 0x80, data, len); -#endif -#ifdef USE_BARO_MS5611 - case BUSTYPE_I2C: - return i2cBusReadRegisterBuffer(busdev, cmd, data, len); -#endif - } - return false; -} - -bool ms5611WriteCommand(busDevice_t *busdev, uint8_t cmd, uint8_t byte) -{ - switch (busdev->bustype) { -#ifdef USE_BARO_SPI_MS5611 - case BUSTYPE_SPI: - return spiBusWriteRegister(busdev, cmd & 0x7f, byte); -#endif -#ifdef USE_BARO_MS5611 - case BUSTYPE_I2C: - return i2cBusWriteRegister(busdev, cmd, byte); -#endif - } - return false; -} - void ms5611BusInit(busDevice_t *busdev) { #ifdef USE_BARO_SPI_MS5611 @@ -136,7 +106,7 @@ bool ms5611Detect(baroDev_t *baro) defaultAddressApplied = true; } - if (!ms5611ReadCommand(busdev, CMD_PROM_RD, 1, &sig) || sig == 0xFF) { + if (!busReadRegisterBuffer(busdev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) { goto fail; } @@ -174,7 +144,7 @@ fail:; static void ms5611_reset(busDevice_t *busdev) { - ms5611WriteCommand(busdev, CMD_RESET, 1); + busWriteRegister(busdev, CMD_RESET, 1); delayMicroseconds(2800); } @@ -183,7 +153,7 @@ static uint16_t ms5611_prom(busDevice_t *busdev, int8_t coef_num) { uint8_t rxbuf[2] = { 0, 0 }; - ms5611ReadCommand(busdev, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command + busReadRegisterBuffer(busdev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command return rxbuf[0] << 8 | rxbuf[1]; } @@ -222,14 +192,14 @@ static uint32_t ms5611_read_adc(busDevice_t *busdev) { uint8_t rxbuf[3]; - ms5611ReadCommand(busdev, CMD_ADC_READ, 3, rxbuf); // read ADC + busReadRegisterBuffer(busdev, CMD_ADC_READ, rxbuf, 3); // read ADC return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2]; } static void ms5611_start_ut(baroDev_t *baro) { - ms5611WriteCommand(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! + busWriteRegister(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! } static void ms5611_get_ut(baroDev_t *baro) @@ -239,7 +209,7 @@ static void ms5611_get_ut(baroDev_t *baro) static void ms5611_start_up(baroDev_t *baro) { - ms5611WriteCommand(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! + busWriteRegister(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! } static void ms5611_get_up(baroDev_t *baro) diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index 2bda17e655..f6303cb470 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -26,33 +26,60 @@ bool busWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) { +#if !defined(USE_SPI) && !defined(USE_I2C) + UNUSED(reg); + UNUSED(data); +#endif switch (busdev->bustype) { +#ifdef USE_SPI case BUSTYPE_SPI: return spiBusWriteRegister(busdev, reg & 0x7f, data); +#endif +#ifdef USE_I2C case BUSTYPE_I2C: return i2cBusWriteRegister(busdev, reg, data); +#endif + default: + return false; } - return false; } bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) { +#if !defined(USE_SPI) && !defined(USE_I2C) + UNUSED(reg); + UNUSED(data); + UNUSED(length); +#endif switch (busdev->bustype) { +#ifdef USE_SPI case BUSTYPE_SPI: return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length); +#endif +#ifdef USE_I2C case BUSTYPE_I2C: return i2cBusReadRegisterBuffer(busdev, reg, data, length); +#endif + default: + return false; } - return false; } uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg) { +#if !defined(USE_SPI) && !defined(USE_I2C) + UNUSED(reg); +#endif switch (busdev->bustype) { +#ifdef USE_SPI case BUSTYPE_SPI: return spiBusReadRegister(busdev, reg & 0x7f); +#endif +#ifdef USE_I2C case BUSTYPE_I2C: return i2cBusReadRegister(busdev, reg); +#endif + default: + return false; } - return false; } diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 2ca515dd85..de7b143e07 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -22,8 +22,15 @@ #include "drivers/bus_i2c.h" #include "drivers/io_types.h" +typedef enum { + BUSTYPE_NONE = 0, + BUSTYPE_I2C, + BUSTYPE_SPI, + BUSTYPE_SLAVE // Slave I2C on SPI master +} busType_e; + typedef struct busDevice_s { - uint8_t bustype; + busType_e bustype; union { struct deviceSpi_s { SPI_TypeDef *instance; @@ -33,16 +40,13 @@ typedef struct busDevice_s { IO_t csnPin; } spi; struct deviceI2C_s { + const struct busDevice_s *master; I2CDevice device; uint8_t address; - } i2c; + } i2c; } busdev_u; } busDevice_t; -#define BUSTYPE_NONE 0 -#define BUSTYPE_I2C 1 -#define BUSTYPE_SPI 2 - #ifdef TARGET_BUS_INIT void targetBusInit(void); #endif diff --git a/src/main/drivers/compass/compass.h b/src/main/drivers/compass/compass.h index cb70068ba9..e14f30d01b 100644 --- a/src/main/drivers/compass/compass.h +++ b/src/main/drivers/compass/compass.h @@ -19,12 +19,16 @@ #include "drivers/bus.h" #include "drivers/sensor.h" +#include "drivers/exti.h" typedef struct magDev_s { - sensorInitFuncPtr init; // initialize function - sensorReadFuncPtr read; // read 3 axis data function - busDevice_t bus; + sensorMagInitFuncPtr init; // initialize function + sensorMagReadFuncPtr read; // read 3 axis data function + extiCallbackRec_t exti; + busDevice_t busdev; sensor_align_e magAlign; + ioTag_t magIntExtiTag; + int16_t magGain[3]; } magDev_t; #ifndef MAG_I2C_INSTANCE diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 40ca786940..2716574634 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -22,6 +22,8 @@ #include "platform.h" +#if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963) + #include "build/debug.h" #include "common/axis.h" @@ -30,6 +32,7 @@ #include "drivers/bus.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_busdev.h" #include "drivers/bus_spi.h" #include "drivers/io.h" #include "drivers/sensor.h" @@ -43,66 +46,110 @@ #include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu9250.h" #include "drivers/compass/compass_ak8963.h" -#include "drivers/compass/compass_spi_ak8963.h" -static float magGain[3] = { 1.0f, 1.0f, 1.0f }; +#include "scheduler/scheduler.h" -#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) -static busDevice_t *bus = NULL; +// This sensor is also available also part of the MPU-9250 connected to the secondary I2C bus. -static bool spiWriteRegisterDelay(const busDevice_t *bus, uint8_t reg, uint8_t data) +// AK8963, mag sensor address +#define AK8963_MAG_I2C_ADDRESS 0x0C +#define AK8963_DEVICE_ID 0x48 + +// Registers +#define AK8963_MAG_REG_WIA 0x00 +#define AK8963_MAG_REG_INFO 0x01 +#define AK8963_MAG_REG_ST1 0x02 +#define AK8963_MAG_REG_HXL 0x03 +#define AK8963_MAG_REG_HXH 0x04 +#define AK8963_MAG_REG_HYL 0x05 +#define AK8963_MAG_REG_HYH 0x06 +#define AK8963_MAG_REG_HZL 0x07 +#define AK8963_MAG_REG_HZH 0x08 +#define AK8963_MAG_REG_ST2 0x09 +#define AK8963_MAG_REG_CNTL1 0x0A +#define AK8963_MAG_REG_CNTL2 0x0B +#define AK8963_MAG_REG_ASCT 0x0C // self test +#define AK8963_MAG_REG_I2CDIS 0x0F +#define AK8963_MAG_REG_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_MAG_REG_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_MAG_REG_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value + +#define READ_FLAG 0x80 +#define I2C_SLV0_EN 0x80 + +#define ST1_DATA_READY 0x01 +#define ST1_DATA_OVERRUN 0x02 + +#define ST2_MAG_SENSOR_OVERFLOW 0x08 + +#define CNTL1_MODE_POWER_DOWN 0x00 +#define CNTL1_MODE_ONCE 0x01 +#define CNTL1_MODE_CONT1 0x02 +#define CNTL1_MODE_CONT2 0x06 +#define CNTL1_MODE_SELF_TEST 0x08 +#define CNTL1_MODE_FUSE_ROM 0x0F +#define CNTL1_BIT_14_BIT 0x00 +#define CNTL1_BIT_16_BIT 0x10 + +#define CNTL2_SOFT_RESET 0x01 + +#define I2CDIS_DISABLE_MASK 0x1D + +#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) + +static bool ak8963SpiWriteRegisterDelay(const busDevice_t *bus, uint8_t reg, uint8_t data) { spiBusWriteRegister(bus, reg, data); delayMicroseconds(10); return true; } -typedef struct queuedReadState_s { - bool waiting; - uint8_t len; - 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}; - -static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) +static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t reg, uint8_t *buf, uint8_t len) { - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + const busDevice_t *bus = slavedev->busdev_u.i2c.master; + + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address | READ_FLAG); // set I2C slave address for read + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes delay(4); __disable_irq(); - bool ack = spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, len_); // read I2C + bool ack = spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, len); // read I2C __enable_irq(); return ack; } -static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) +static bool ak8963SlaveWriteRegister(const busDevice_t *slavedev, uint8_t reg, uint8_t data) { - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte + const busDevice_t *bus = slavedev->busdev_u.i2c.master; + + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address); // set I2C slave address for write + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C sLave value + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN); // write 1 byte return true; } -static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) +typedef struct queuedReadState_s { + bool waiting; + uint8_t len; + uint32_t readStartedAt; // time read was queued in micros. +} queuedReadState_t; + +static queuedReadState_t queuedRead = { false, 0, 0}; + +static bool ak8963SlaveStartRead(const busDevice_t *slavedev, uint8_t reg, uint8_t len) { if (queuedRead.waiting) { return false; } - queuedRead.len = len_; + const busDevice_t *bus = slavedev->busdev_u.i2c.master; - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + queuedRead.len = len; + + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address | READ_FLAG); // set I2C slave address for read + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes queuedRead.readStartedAt = micros(); queuedRead.waiting = true; @@ -110,7 +157,7 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) return true; } -static uint32_t ak8963SensorQueuedReadTimeRemaining(void) +static uint32_t ak8963SlaveQueuedReadTimeRemaining(void) { if (!queuedRead.waiting) { return 0; @@ -127,9 +174,11 @@ static uint32_t ak8963SensorQueuedReadTimeRemaining(void) return timeRemaining; } -static bool ak8963SensorCompleteRead(uint8_t *buf) +static bool ak8963SlaveCompleteRead(const busDevice_t *slavedev, uint8_t *buf) { - uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); + uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining(); + + const busDevice_t *bus = slavedev->busdev_u.i2c.master; if (timeRemaining > 0) { delayMicroseconds(timeRemaining); @@ -137,73 +186,42 @@ static bool ak8963SensorCompleteRead(uint8_t *buf) queuedRead.waiting = false; - spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len); // read I2C buffer - return true; -} -#else -static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) -{ - return i2cRead(MAG_I2C_INSTANCE, addr_, reg_, len, buf); -} - -static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) -{ - return i2cWrite(MAG_I2C_INSTANCE, addr_, reg_, data); -} -#endif - -static bool ak8963Init(void) -{ - uint8_t calibration[3]; - uint8_t status; - - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode - ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, sizeof(calibration), calibration); // Read the x-, y-, and z-axis calibration values - - 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; - - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading. - - // Clear status registers - ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ST1, 1, &status); - ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ST2, 1, &status); - - // Trigger first measurement - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_ONCE); + spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len); // read I2C buffer return true; } -static bool ak8963Read(int16_t *magData) +static bool ak8963SlaveReadData(const busDevice_t *busdev, uint8_t *buf) { - bool ack = false; - uint8_t buf[7]; - -#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) - - // 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. + typedef enum { + CHECK_STATUS = 0, + WAITING_FOR_STATUS, + WAITING_FOR_DATA + } ak8963ReadState_e; static ak8963ReadState_e state = CHECK_STATUS; + bool ack = false; + + // we currently need a different approach for the MPU9250 connected via SPI. + // we cannot use the ak8963SlaveReadRegisterBuffer() method for SPI, it is to slow and blocks for far too long. + bool retry = true; restart: switch (state) { - case CHECK_STATUS: - ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ST1, 1); - state++; + case CHECK_STATUS: { + ak8963SlaveStartRead(busdev, AK8963_MAG_REG_ST1, 1); + state = WAITING_FOR_STATUS; return false; + } case WAITING_FOR_STATUS: { - uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); + uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining(); if (timeRemaining) { return false; } - ack = ak8963SensorCompleteRead(&buf[0]); + ack = ak8963SlaveCompleteRead(busdev, &buf[0]); uint8_t status = buf[0]; @@ -213,90 +231,221 @@ restart: if (retry) { retry = false; goto restart; - } - return false; + } + return false; } - // read the 6 bytes of data and the status2 register - ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7); - - state++; + ak8963SlaveStartRead(busdev, AK8963_MAG_REG_HXL, 7); + state = WAITING_FOR_DATA; return false; } case WAITING_FOR_DATA: { - uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); + uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining(); if (timeRemaining) { return false; } - ack = ak8963SensorCompleteRead(&buf[0]); + ack = ak8963SlaveCompleteRead(busdev, &buf[0]); + state = CHECK_STATUS; } } -#else - ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ST1, 1, &buf[0]); - uint8_t status = buf[0]; + return ack; +} +#endif + +static bool ak8963ReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *buf, uint8_t len) +{ +#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) + if (busdev->bustype == BUSTYPE_SLAVE) { + return ak8963SlaveReadRegisterBuffer(busdev, reg, buf, len); + } +#endif + return busReadRegisterBuffer(busdev, reg, buf, len); +} + +static bool ak8963WriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) +{ +#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) + if (busdev->bustype == BUSTYPE_SLAVE) { + return ak8963SlaveWriteRegister(busdev, reg, data); + } +#endif + return busWriteRegister(busdev, reg, data); +} + +static bool ak8963ReadData(const busDevice_t *busdev, uint8_t *buf) +{ + uint8_t status; + + bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1); if (!ack || (status & ST1_DATA_READY) == 0) { return false; } - ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]); + return ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_HXL, buf, 7); +} + +static bool ak8963Read(magDev_t *magdev, int16_t *magData) +{ + bool ack = false; + uint8_t buf[7]; + + const busDevice_t *busdev = &magdev->busdev; + + switch (busdev->bustype) { +#if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963) + case BUSTYPE_I2C: + case BUSTYPE_SPI: + ack = ak8963ReadData(busdev, buf); + break; #endif + +#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) + case BUSTYPE_SLAVE: + ack = ak8963SlaveReadData(busdev, buf); + break; +#endif + default: + break; + } + uint8_t status2 = buf[6]; - if (!ack || (status2 & ST2_DATA_ERROR) || (status2 & ST2_MAG_SENSOR_OVERFLOW)) { + if (!ack) { 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]; + ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); // start reading again uint8_t status2 = buf[6]; -#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) - state = CHECK_STATUS; + if (status2 & ST2_MAG_SENSOR_OVERFLOW) { + return false; + } + + magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256; + magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256; + magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256; + + return true; +} + +static bool ak8963Init(magDev_t *magdev) +{ + uint8_t asa[3]; + uint8_t status; + + const busDevice_t *busdev = &magdev->busdev; + + ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode + ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode + ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values + + magdev->magGain[X] = asa[X] + 128; + magdev->magGain[Y] = asa[Y] + 128; + magdev->magGain[Z] = asa[Z] + 128; + + ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading. + + // Clear status registers + ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1); + ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST2, &status, 1); + + // Trigger first measurement + ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); + return true; +} + +void ak8963BusInit(const busDevice_t *busdev) +{ + switch (busdev->bustype) { +#ifdef USE_MAG_AK8963 + case BUSTYPE_I2C: + UNUSED(busdev); + break; #endif - return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_ONCE); // start reading again + +#ifdef USE_MAG_SPI_AK8963 + case BUSTYPE_SPI: + IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0); + IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); + IOHi(busdev->busdev_u.spi.csnPin); // Disable + spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + break; +#endif + +#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) +#define TASK_PERIOD_HZ(hz) (1000000 / (hz)) + case BUSTYPE_SLAVE: + rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); + + // initialze I2C master via SPI bus + ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); + ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + break; +#endif + default: + break; + } +} + +void ak8963BusDeInit(const busDevice_t *busdev) +{ + switch (busdev->bustype) { +#ifdef USE_MAG_AK8963 + case BUSTYPE_I2C: + UNUSED(busdev); + break; +#endif + +#ifdef USE_MAG_SPI_AK8963 + case BUSTYPE_SPI: + IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU); + IORelease(busdev->busdev_u.spi.csnPin); + IOInit(busdev->busdev_u.spi.csnPin, OWNER_SPI_PREINIT, 0); + break; +#endif + +#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) + case BUSTYPE_SLAVE: + ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); + break; +#endif + default: + break; + } } bool ak8963Detect(magDev_t *mag) { uint8_t sig = 0; -#if defined(USE_SPI) && defined(AK8963_SPI_INSTANCE) - spiBusSetInstance(&mag->bus, AK8963_SPI_INSTANCE); - mag->bus.busdev_u.spi.csnPin = mag->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(AK8963_CS_PIN)) : mag->bus.busdev_u.spi.csnPin; + busDevice_t *busdev = &mag->busdev; - // check for SPI AK8963 - if (ak8963SpiDetect(mag)) return true; -#endif + if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_SLAVE) && busdev->busdev_u.i2c.address == 0) { + busdev->busdev_u.i2c.address = AK8963_MAG_I2C_ADDRESS; + } -#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) - bus = &mag->bus; -#if defined(MPU6500_SPI_INSTANCE) - spiBusSetInstance(&mag->bus, MPU6500_SPI_INSTANCE); -#elif defined(MPU9250_SPI_INSTANCE) - spiBusSetInstance(&mag->bus, MPU9250_SPI_INSTANCE); -#endif + ak8963BusInit(busdev); - // initialze I2C master via SPI bus (MPU9250) - spiWriteRegisterDelay(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); - spiWriteRegisterDelay(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz - spiWriteRegisterDelay(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only -#endif - - ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG + ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG delay(4); - bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WIA, 1, &sig); // check for AK8963 - if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H' + bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_WIA, &sig, 1); // check for AK8963 + + if (ack && sig == AK8963_DEVICE_ID) // 0x48 / 01001000 / 'H' { mag->init = ak8963Init; mag->read = ak8963Read; return true; } + + ak8963BusDeInit(busdev); + return false; } +#endif diff --git a/src/main/drivers/compass/compass_ak8963.h b/src/main/drivers/compass/compass_ak8963.h index 2862968b72..9a13b17943 100644 --- a/src/main/drivers/compass/compass_ak8963.h +++ b/src/main/drivers/compass/compass_ak8963.h @@ -17,50 +17,4 @@ #pragma once -// This sensor is also available also part of the MPU-9250 connected to the secondary I2C bus. - -// AK8963, mag sensor address -#define AK8963_MAG_I2C_ADDRESS 0x0C -#define AK8963_Device_ID 0x48 - -// Registers -#define AK8963_MAG_REG_WIA 0x00 -#define AK8963_MAG_REG_INFO 0x01 -#define AK8963_MAG_REG_ST1 0x02 -#define AK8963_MAG_REG_HXL 0x03 -#define AK8963_MAG_REG_HXH 0x04 -#define AK8963_MAG_REG_HYL 0x05 -#define AK8963_MAG_REG_HYH 0x06 -#define AK8963_MAG_REG_HZL 0x07 -#define AK8963_MAG_REG_HZH 0x08 -#define AK8963_MAG_REG_ST2 0x09 -#define AK8963_MAG_REG_CNTL1 0x0a -#define AK8963_MAG_REG_CNTL2 0x0b -#define AK8963_MAG_REG_ASCT 0x0c // self test -#define AK8963_MAG_REG_I2CDIS 0x0f -#define AK8963_MAG_REG_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8963_MAG_REG_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8963_MAG_REG_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value - -#define READ_FLAG 0x80 - -#define ST1_DATA_READY 0x01 -#define ST1_DATA_OVERRUN 0x02 - -#define ST2_DATA_ERROR 0x02 -#define ST2_MAG_SENSOR_OVERFLOW 0x03 - -#define CNTL1_MODE_POWER_DOWN 0x00 -#define CNTL1_MODE_ONCE 0x01 -#define CNTL1_MODE_CONT1 0x02 -#define CNTL1_MODE_CONT2 0x06 -#define CNTL1_MODE_SELF_TEST 0x08 -#define CNTL1_MODE_FUSE_ROM 0x0F -#define CNTL1_BIT_14_Bit 0x00 -#define CNTL1_BIT_16_Bit 0x10 - -#define CNTL2_SOFT_RESET 0x01 - -#define I2CDIS_DISABLE_MASK 0x1d - bool ak8963Detect(magDev_t *mag); diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index 52528f54e0..b0f96ce6a1 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -28,7 +28,9 @@ #include "common/maths.h" #include "common/utils.h" +#include "drivers/bus.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_busdev.h" #include "drivers/sensor.h" #include "drivers/time.h" @@ -36,101 +38,124 @@ #include "compass_ak8975.h" // This sensor is available in MPU-9150. +// This driver only support I2C mode, direct and in bypass configuration. // AK8975, mag sensor address -#define AK8975_MAG_I2C_ADDRESS 0x0C - +#define AK8975_MAG_I2C_ADDRESS 0x0C +#define AK8975_DEVICE_ID 0x48 // Registers -#define AK8975_MAG_REG_WHO_AM_I 0x00 -#define AK8975_MAG_REG_INFO 0x01 -#define AK8975_MAG_REG_STATUS1 0x02 -#define AK8975_MAG_REG_HXL 0x03 -#define AK8975_MAG_REG_HXH 0x04 -#define AK8975_MAG_REG_HYL 0x05 -#define AK8975_MAG_REG_HYH 0x06 -#define AK8975_MAG_REG_HZL 0x07 -#define AK8975_MAG_REG_HZH 0x08 -#define AK8975_MAG_REG_STATUS2 0x09 -#define AK8975_MAG_REG_CNTL 0x0a -#define AK8975_MAG_REG_ASCT 0x0c // self test +#define AK8975_MAG_REG_WIA 0x00 +#define AK8975_MAG_REG_INFO 0x01 +#define AK8975_MAG_REG_ST1 0x02 +#define AK8975_MAG_REG_HXL 0x03 +#define AK8975_MAG_REG_HXH 0x04 +#define AK8975_MAG_REG_HYL 0x05 +#define AK8975_MAG_REG_HYH 0x06 +#define AK8975_MAG_REG_HZL 0x07 +#define AK8975_MAG_REG_HZH 0x08 +#define AK8975_MAG_REG_ST2 0x09 +#define AK8975_MAG_REG_CNTL 0x0A +#define AK8975_MAG_REG_ASTC 0x0C // self test +#define AK8975_MAG_REG_I2CDIS 0x0F +#define AK8975_MAG_REG_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8975_MAG_REG_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8975_MAG_REG_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value -#define AK8975A_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value -#define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value -#define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value +#define ST1_REG_DATA_READY 0x01 -static bool ak8975Init(void) +#define ST2_REG_DATA_ERROR 0x04 +#define ST2_REG_MAG_SENSOR_OVERFLOW 0x08 + +#define CNTL_MODE_POWER_DOWN 0x00 +#define CNTL_MODE_ONCE 0x01 +#define CNTL_MODE_CONT1 0x02 +#define CNTL_MODE_FUSE_ROM 0x0F +#define CNTL_BIT_14_BIT 0x00 +#define CNTL_BIT_16_BIT 0x10 + +static bool ak8975Init(magDev_t *magdev) { - uint8_t buffer[3]; + uint8_t asa[3]; uint8_t status; - i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode + busDevice_t *busdev = &magdev->busdev; + + busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode delay(20); - i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode + busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode delay(10); - i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values + busReadRegisterBuffer(busdev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values delay(10); - i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading. + magdev->magGain[X] = asa[X] + 128; + magdev->magGain[Y] = asa[Y] + 128; + magdev->magGain[Z] = asa[Z] + 128; + + busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading. delay(10); // Clear status registers - i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); - i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); + busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1); + busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST2, &status, 1); // Trigger first measurement - i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); + busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); return true; } -#define BIT_STATUS1_REG_DATA_READY (1 << 0) - -#define BIT_STATUS2_REG_DATA_ERROR (1 << 2) -#define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3) - -static bool ak8975Read(int16_t *magData) +static bool ak8975Read(magDev_t *magdev, int16_t *magData) { bool ack; uint8_t status; uint8_t buf[6]; - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); - if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) { + busDevice_t *busdev = &magdev->busdev; + + ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1); + if (!ack || (status & ST1_REG_DATA_READY) == 0) { return false; } - 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 + busReadRegisterBuffer(busdev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH - ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); + ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST2, &status, 1); if (!ack) { return false; } - if (status & BIT_STATUS2_REG_DATA_ERROR) { + busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again uint8_t status2 = buf[6]; + + if (status & ST2_REG_DATA_ERROR) { return false; } - if (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW) { + if (status & ST2_REG_MAG_SENSOR_OVERFLOW) { return false; } - magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4; - magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * 4; - magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; + magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256; + magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256; + magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256; - - i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again return true; } bool ak8975Detect(magDev_t *mag) { uint8_t sig = 0; - bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig); - if (!ack || sig != 'H') // 0x48 / 01001000 / 'H' + busDevice_t *busdev = &mag->busdev; + + if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) { + busdev->busdev_u.i2c.address = AK8975_MAG_I2C_ADDRESS; + } + + bool ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_WIA, &sig, 1); + + if (!ack || sig != AK8975_DEVICE_ID) // 0x48 / 01001000 / 'H' return false; mag->init = ak8975Init; diff --git a/src/main/drivers/compass/compass_fake.c b/src/main/drivers/compass/compass_fake.c index 67532a074b..6808aa340c 100644 --- a/src/main/drivers/compass/compass_fake.c +++ b/src/main/drivers/compass/compass_fake.c @@ -25,6 +25,7 @@ #include "build/build_config.h" #include "common/axis.h" +#include "common/utils.h" #include "compass.h" #include "compass_fake.h" @@ -32,8 +33,10 @@ static int16_t fakeMagData[XYZ_AXIS_COUNT]; -static bool fakeMagInit(void) +static bool fakeMagInit(magDev_t *mag) { + UNUSED(mag); + // initially point north fakeMagData[X] = 4096; fakeMagData[Y] = 0; @@ -48,8 +51,10 @@ void fakeMagSet(int16_t x, int16_t y, int16_t z) fakeMagData[Z] = z; } -static bool fakeMagRead(int16_t *magData) +static bool fakeMagRead(magDev_t *mag, int16_t *magData) { + UNUSED(mag); + magData[X] = fakeMagData[X]; magData[Y] = fakeMagData[Y]; magData[Z] = fakeMagData[Z]; @@ -63,3 +68,4 @@ bool fakeMagDetect(magDev_t *mag) return true; } #endif // USE_FAKE_MAG + diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index 4c6dc3ad27..48d0e07728 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -22,14 +22,17 @@ #include "platform.h" -#ifdef USE_MAG_HMC5883 +#if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883) #include "build/debug.h" #include "common/axis.h" #include "common/maths.h" +#include "drivers/bus.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_busdev.h" +#include "drivers/bus_spi.h" #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/light_led.h" @@ -40,7 +43,6 @@ #include "compass.h" #include "compass_hmc5883l.h" -#include "compass_spi_hmc5883l.h" //#define DEBUG_MAG_DATA_READY_INTERRUPT @@ -67,10 +69,10 @@ * 1:0 MS1-MS0: Measurement Configuration Bits * MS1 | MS0 | MODE * ------------------------------ - * 0 | 0 | Normal - * 0 | 1 | Positive Bias - * 1 | 0 | Negative Bias - * 1 | 1 | Not Used + * 0 | 0 | Normal + * 0 | 1 | Positive Bias + * 1 | 0 | Negative Bias + * 1 | 1 | Not Used * * CTRL_REGB: Control RegisterB * Read Write @@ -98,33 +100,39 @@ * 1:0 MD1-MD0: Mode Select Bits * MS1 | MS0 | MODE * ------------------------------ - * 0 | 0 | Continuous-Conversion Mode. - * 0 | 1 | Single-Conversion Mode - * 1 | 0 | Negative Bias - * 1 | 1 | Sleep Mode + * 0 | 0 | Continuous-Conversion Mode. + * 0 | 1 | Single-Conversion Mode + * 1 | 0 | Negative Bias + * 1 | 1 | Sleep Mode */ -#define MAG_ADDRESS 0x1E -#define MAG_DATA_REGISTER 0x03 +#define HMC5883_MAG_I2C_ADDRESS 0x1E +#define HMC5883_DEVICE_ID 0x48 -#define HMC58X3_R_CONFA 0 -#define HMC58X3_R_CONFB 1 -#define HMC58X3_R_MODE 2 -#define HMC58X3_X_SELF_TEST_GAUSS (+1.16f) // X axis level when bias current is applied. -#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f) // Y axis level when bias current is applied. -#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08f) // Z axis level when bias current is applied. -#define SELF_TEST_LOW_LIMIT (243.0f / 390.0f) // Low limit when gain is 5. -#define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f) // High limit when gain is 5. -#define HMC_POS_BIAS 1 -#define HMC_NEG_BIAS 2 +#define HMC58X3_REG_CONFA 0x00 +#define HMC58X3_REG_CONFB 0x01 +#define HMC58X3_REG_MODE 0x02 +#define HMC58X3_REG_DATA 0x03 +#define HMC58X3_REG_IDA 0x0A -static float magGain[3] = { 1.0f, 1.0f, 1.0f }; +#define HMC_CONFA_NORMAL 0x00 +#define HMC_CONFA_POS_BIAS 0x01 +#define HMC_CONFA_NEG_BIAS 0x02 +#define HMC_CONFA_DOR_15HZ 0X10 +#define HMC_CONFA_8_SAMLES 0X60 +#define HMC_CONFB_GAIN_2_5GA 0X60 +#define HMC_CONFB_GAIN_1_3GA 0X20 +#define HMC_MODE_CONTINOUS 0X00 +#define HMC_MODE_SINGLE 0X01 + +#define HMC58X3_X_SELF_TEST_GAUSS (+1.16f) // X axis level when bias current is applied. +#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f) // Y axis level when bias current is applied. +#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08f) // Z axis level when bias current is applied. +#define SELF_TEST_LOW_LIMIT (243.0f / 390.0f) // Low limit when gain is 5. +#define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f) // High limit when gain is 5. #ifdef USE_MAG_DATA_READY_SIGNAL -static IO_t hmc5883InterruptIO; -static extiCallbackRec_t hmc5883_extiCallbackRec; - static void hmc5883_extiHandler(extiCallbackRec_t* cb) { UNUSED(cb); @@ -145,80 +153,121 @@ static void hmc5883_extiHandler(extiCallbackRec_t* cb) } #endif -static void hmc5883lConfigureDataReadyInterruptHandling(void) +static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag) { #ifdef USE_MAG_DATA_READY_SIGNAL - - if (!(hmc5883InterruptIO)) { + if (mag->magIntExtiTag == IO_TAG_NONE) { return; } + + const IO_t magIntIO = IOGetByTag(mag->magIntExtiTag); + #ifdef ENSURE_MAG_DATA_READY_IS_HIGH - uint8_t status = IORead(hmc5883InterruptIO); + uint8_t status = IORead(magIntIO); if (!status) { return; } #endif - EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler); - EXTIConfig(hmc5883InterruptIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); - EXTIEnable(hmc5883InterruptIO, true); +#if defined (STM32F7) + IOInit(magIntIO, OWNER_COMPASS_EXTI, 0); + EXTIHandlerInit(&mag->exti, hmc5883_extiHandler); + EXTIConfig(magIntIO, &gmag->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); +#else + IOInit(magIntIO, OWNER_COMPASS_EXTI, 0); + IOConfigGPIO(magIntIO, IOCFG_IN_FLOATING); + EXTIHandlerInit(&mag->exti, hmc5883_extiHandler); + EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); + EXTIEnable(magIntIO, true); +#endif +#else + UNUSED(mag); #endif } -static bool hmc5883lRead(int16_t *magData) +#ifdef USE_MAG_SPI_HMC5883 +/* + * XXX Fixme + * HMC5983 datasheet states SPI mode is + * SCK is high when CS is high + * Data is sampled at the rising edge of SCK + * However, it seems that SCK condition is ignored (works with SCK low when CS is high). + * Nevertheless, it is nice to conform to the datasheet specification when per device SPI mode + * is implemented. + */ +static void hmc5883SpiInit(busDevice_t *busdev) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + + IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0); + IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); + + IOHi(busdev->busdev_u.spi.csnPin); // Disable + + spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + hardwareInitialised = true; +} +#endif + +static bool hmc5883lRead(magDev_t *magdev, int16_t *magData) { uint8_t buf[6]; -#ifdef USE_MAG_SPI_HMC5883 - bool ack = hmc5883SpiReadCommand(MAG_DATA_REGISTER, 6, buf); -#else - bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); -#endif + + busDevice_t *busdev = &magdev->busdev; + + bool ack = busReadRegisterBuffer(busdev, HMC58X3_REG_DATA, buf, 6); + if (!ack) { return false; } // During calibration, magGain is 1.0, so the read returns normal non-calibrated values. // After calibration is done, magGain is set to calculated gain values. - magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X]; - magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z]; - magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y]; + magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magdev->magGain[X] / 256; + magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magdev->magGain[Z] / 256; + magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magdev->magGain[Y] / 256; return true; } -static bool hmc5883lInit(void) +static bool hmc5883lInit(magDev_t *magdev) { + busDevice_t *busdev = &magdev->busdev; + int16_t magADC[3]; int i; int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. bool bret = true; // Error indicator + magdev->magGain[X] = 256; + magdev->magGain[Y] = 256; + magdev->magGain[Z] = 256; + delay(50); -#ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias -#else - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias -#endif + + busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_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. -#ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) -#else - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) -#endif + + busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_2_5GA); // Set the Gain to 2.5Ga (7:5->011) + delay(100); - hmc5883lRead(magADC); + + hmc5883lRead(magdev, magADC); for (i = 0; i < 10; i++) { // Collect 10 samples -#ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); -#else - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); -#endif - delay(50); - hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. + busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE); + delay(20); + hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed. // Since the measurements are noisy, they should be averaged rather than taking the max. + xyz_total[X] += magADC[X]; xyz_total[Y] += magADC[Y]; xyz_total[Z] += magADC[Z]; @@ -232,21 +281,16 @@ static bool hmc5883lInit(void) } // Apply the negative bias. (Same gain) -#ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. -#else - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. -#endif + + busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. + for (i = 0; i < 10; i++) { -#ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); -#else - i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); -#endif - delay(50); - hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. + busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE); + delay(20); + hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed. // Since the measurements are noisy, they should be averaged. + xyz_total[X] -= magADC[X]; xyz_total[Y] -= magADC[Y]; xyz_total[Z] -= magADC[Z]; @@ -259,50 +303,51 @@ static bool hmc5883lInit(void) LED1_TOGGLE; } - magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]); - magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]); - magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); + magdev->magGain[X] = ABS((int32_t)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X]); + magdev->magGain[Y] = ABS((int32_t)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y]); + magdev->magGain[Z] = ABS((int32_t)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z]); // leave test mode -#ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode - hmc5883SpiWriteCommand(HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga - hmc5883SpiWriteCommand(HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode -#else - 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 -#endif + + busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode + busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga + busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // Mode register -- 000000 00 continuous Conversion Mode + delay(100); if (!bret) { // Something went wrong so get a best guess - magGain[X] = 1.0f; - magGain[Y] = 1.0f; - magGain[Z] = 1.0f; + magdev->magGain[X] = 256; + magdev->magGain[Y] = 256; + magdev->magGain[Z] = 256; } - hmc5883lConfigureDataReadyInterruptHandling(); + hmc5883lConfigureDataReadyInterruptHandling(magdev); return true; } -bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag) +bool hmc5883lDetect(magDev_t* mag) { -#ifdef USE_MAG_DATA_READY_SIGNAL - hmc5883InterruptIO = IOGetByTag(interruptTag); -#else - UNUSED(interruptTag); -#endif + busDevice_t *busdev = &mag->busdev; uint8_t sig = 0; + #ifdef USE_MAG_SPI_HMC5883 - hmc5883SpiInit(); - bool ack = hmc5883SpiReadCommand(0x0A, 1, &sig); -#else - bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig); + if (busdev->bustype == BUSTYPE_SPI) { + hmc5883SpiInit(&mag->busdev); + } #endif - if (!ack || sig != 'H') +#ifdef USE_MAG_HMC5883 + if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) { + busdev->busdev_u.i2c.address = HMC5883_MAG_I2C_ADDRESS; + } +#endif + + bool ack = busReadRegisterBuffer(&mag->busdev, HMC58X3_REG_IDA, &sig, 1); + + if (!ack || sig != HMC5883_DEVICE_ID) { return false; + } mag->init = hmc5883lInit; mag->read = hmc5883lRead; diff --git a/src/main/drivers/compass/compass_hmc5883l.h b/src/main/drivers/compass/compass_hmc5883l.h index fc537cfb9d..57a0d3baf6 100644 --- a/src/main/drivers/compass/compass_hmc5883l.h +++ b/src/main/drivers/compass/compass_hmc5883l.h @@ -19,4 +19,4 @@ #include "drivers/io_types.h" -bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag); +bool hmc5883lDetect(magDev_t* mag); diff --git a/src/main/drivers/compass/compass_spi_ak8963.c b/src/main/drivers/compass/compass_spi_ak8963.c deleted file mode 100644 index 601c5bc8ef..0000000000 --- a/src/main/drivers/compass/compass_spi_ak8963.c +++ /dev/null @@ -1,122 +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 . - */ - -#include -#include -#include - -#include - -#include "platform.h" - -#include "build/debug.h" - -#include "common/axis.h" -#include "common/maths.h" -#include "common/utils.h" - -#include "drivers/bus_spi.h" -#include "drivers/io.h" -#include "drivers/sensor.h" -#include "drivers/time.h" - -#include "drivers/compass/compass.h" -#include "drivers/compass/compass_ak8963.h" -#include "drivers/compass/compass_spi_ak8963.h" - -#ifdef USE_MAG_SPI_AK8963 - -static float magGain[3] = { 1.0f, 1.0f, 1.0f }; -static busDevice_t *bus = NULL; - -static bool ak8963SpiInit(void) -{ - uint8_t calibration[3]; - uint8_t status; - - UNUSED(status); - - spiBusWriteRegister(bus, AK8963_MAG_REG_I2CDIS, I2CDIS_DISABLE_MASK); // disable I2C - delay(10); - - spiBusWriteRegister(bus, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode - delay(20); - - spiBusWriteRegister(bus, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode - delay(10); - - spiBusReadRegisterBuffer(bus, AK8963_MAG_REG_ASAX, calibration, sizeof(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; - - spiBusWriteRegister(bus, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading. - delay(10); - - // Clear status registers - status = spiBusReadRegister(bus, AK8963_MAG_REG_ST1); - status = spiBusReadRegister(bus, AK8963_MAG_REG_ST2); - - // Trigger first measurement - spiBusWriteRegister(bus, AK8963_MAG_REG_CNTL1, CNTL1_MODE_ONCE); - return true; -} - -static bool ak8963SpiRead(int16_t *magData) -{ - bool ack = false; - uint8_t buf[7]; - - uint8_t status = spiBusReadRegister(bus, AK8963_MAG_REG_ST1); - - if (!ack || (status & ST1_DATA_READY) == 0) { - return false; - } - - ack = spiBusReadRegisterBuffer(bus, AK8963_MAG_REG_HXL, &buf[0], 7); - uint8_t status2 = buf[6]; - if (!ack || (status2 & ST2_DATA_ERROR) || (status2 & ST2_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]; - - return spiBusWriteRegister(bus, AK8963_MAG_REG_CNTL1, CNTL1_MODE_ONCE); // start reading again -} - -bool ak8963SpiDetect(magDev_t *mag) -{ - uint8_t sig = 0; - - // check for SPI AK8963 - bool ack = spiBusReadRegisterBuffer(&mag->bus, AK8963_MAG_REG_WIA, &sig, 1); - if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H' - { - bus = &mag->bus; - - mag->init = ak8963SpiInit; - mag->read = ak8963SpiRead; - - return true; - } - return false; -} -#endif diff --git a/src/main/drivers/compass/compass_spi_ak8963.h b/src/main/drivers/compass/compass_spi_ak8963.h deleted file mode 100644 index 176a47be9a..0000000000 --- a/src/main/drivers/compass/compass_spi_ak8963.h +++ /dev/null @@ -1,20 +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 . - */ - -#pragma once - -bool ak8963SpiDetect(magDev_t *mag); diff --git a/src/main/drivers/compass/compass_spi_hmc5883l.c b/src/main/drivers/compass/compass_spi_hmc5883l.c deleted file mode 100755 index e014d4f226..0000000000 --- a/src/main/drivers/compass/compass_spi_hmc5883l.c +++ /dev/null @@ -1,86 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV 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. - * - * INAV 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 INAV. If not, see . - */ - -#include -#include -#include - -#include - -#include "drivers/io.h" -#include "drivers/bus_spi.h" - -#include "compass.h" -#include "compass_hmc5883l.h" - -#ifdef USE_MAG_SPI_HMC5883 - -#define DISABLE_HMC5883 IOHi(hmc5883CsPin) -#define ENABLE_HMC5883 IOLo(hmc5883CsPin) - -static IO_t hmc5883CsPin = IO_NONE; - -bool hmc5883SpiWriteCommand(uint8_t reg, uint8_t data) -{ - uint8_t buf[32]; - buf[0] = reg & 0x7F; - buf[1] = data; - - ENABLE_HMC5883; - - //spiTransferByte(HMC5883_SPI_INSTANCE, reg & 0x7F); - //spiTransferByte(HMC5883_SPI_INSTANCE, data); - spiTransfer(HMC5883_SPI_INSTANCE, buf, NULL, 2); - DISABLE_HMC5883; - - return true; -} - -bool hmc5883SpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data) -{ - uint8_t buf[32]; - - buf[0] = reg | 0x80 | 0x40; - - ENABLE_HMC5883; - spiTransfer(HMC5883_SPI_INSTANCE, buf, buf, length + 1); - DISABLE_HMC5883; - - memcpy(data, &buf[1], length); - - return true; -} - -void hmc5883SpiInit(void) -{ - static bool hardwareInitialised = false; - - if (hardwareInitialised) { - return; - } - - hmc5883CsPin = IOGetByTag(IO_TAG(HMC5883_CS_PIN)); - IOInit(hmc5883CsPin, OWNER_COMPASS_CS, 0); - IOConfigGPIO(hmc5883CsPin, IOCFG_OUT_PP); - - DISABLE_HMC5883; - - spiSetDivisor(HMC5883_SPI_INSTANCE, SPI_CLOCK_STANDARD); - - hardwareInitialised = true; -} -#endif diff --git a/src/main/drivers/compass/compass_spi_hmc5883l.h b/src/main/drivers/compass/compass_spi_hmc5883l.h deleted file mode 100755 index c910627839..0000000000 --- a/src/main/drivers/compass/compass_spi_hmc5883l.h +++ /dev/null @@ -1,22 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV 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. - * - * INAV 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 INAV. If not, see . - */ - -#pragma once - -void hmc5883SpiInit(void); -bool hmc5883SpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data); -bool hmc5883SpiWriteCommand(uint8_t reg, uint8_t data); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index f88150c695..a74ad7be8d 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -52,6 +52,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "SPI_CS", "MPU_EXTI", "BARO_EXTI", + "COMPASS_EXTI", "USB", "USB_DETECT", "BEEPER", diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index f9ed25c94d..66ab1ee25c 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -52,6 +52,7 @@ typedef enum { OWNER_SPI_CS, OWNER_MPU_EXTI, OWNER_BARO_EXTI, + OWNER_COMPASS_EXTI, OWNER_USB, OWNER_USB_DETECT, OWNER_BEEPER, diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 647bdd74a1..0eb5723730 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -32,9 +32,10 @@ typedef enum { CW270_DEG_FLIP = 8 } sensor_align_e; -typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype -typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorInterruptFuncPtr)(void); +struct magDev_s; +typedef bool (*sensorMagInitFuncPtr)(struct magDev_s *magdev); +typedef bool (*sensorMagReadFuncPtr)(struct magDev_s *magdev, int16_t *data); struct accDev_s; typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc); typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 39fa9a52cc..9fa72d0a98 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3171,6 +3171,9 @@ const cliResourceValue_t resourceTable[] = { #ifdef BARO { OWNER_BARO_CS, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_csn), 0 }, #endif +#ifdef MAG + { OWNER_COMPASS_CS, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_spi_csn), 0 }, +#endif }; static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 48d080a4cd..fbb2431ba3 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -287,10 +287,6 @@ void fcTasksInit(void) #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); -#if defined(USE_SPI) && defined(USE_MAG_AK8963) - // fixme temporary solution for AK6983 via slave I2C on MPU9250 - rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); -#endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 710dbf348b..7f0ed8b121 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -36,6 +36,8 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_spi.h" #include "drivers/light_led.h" #include "drivers/camera_control.h" #include "drivers/max7456.h" @@ -249,7 +251,7 @@ static const char * const lookupTableFailsafe[] = { }; static const char * const lookupTableBusType[] = { - "NONE", "I2C", "SPI" + "NONE", "I2C", "SPI", "SLAVE" }; #ifdef USE_MAX7456 @@ -346,6 +348,10 @@ const clivalue_t valueTable[] = { // PG_COMPASS_CONFIG #ifdef MAG { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) }, + { "mag_bustype", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_bustype) }, + { "mag_i2c_device", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_device) }, + { "mag_i2c_address", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2C_ADDR7_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_address) }, + { "mag_spi_device", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_spi_device) }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAG_HARDWARE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hardware) }, { "mag_declination", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_declination) }, { "magzero_x", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[X]) }, @@ -359,7 +365,6 @@ const clivalue_t valueTable[] = { { "baro_spi_device", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_device) }, { "baro_i2c_device", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_device) }, { "baro_i2c_address", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2C_ADDR7_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_address) }, - { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) }, { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_sample_count) }, { "baro_noise_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) }, diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index d710437f0c..6459c2cd93 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -25,6 +25,10 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_spi.h" +#include "drivers/bus.h" +#include "drivers/accgyro/accgyro_mpu.h" #include "drivers/compass/compass.h" #include "drivers/compass/compass_ak8975.h" #include "drivers/compass/compass_ak8963.h" @@ -54,36 +58,119 @@ mag_t mag; // mag access functions #define COMPASS_INTERRUPT_TAG IO_TAG_NONE #endif -PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 0); -PG_RESET_TEMPLATE(compassConfig_t, compassConfig, - .mag_align = ALIGN_DEFAULT, - // xxx_hardware: 0:default/autodetect, 1: disable - .mag_hardware = 1, - .mag_declination = 0, - .interruptTag = COMPASS_INTERRUPT_TAG -); +void pgResetFn_compassConfig(compassConfig_t *compassConfig) +{ + compassConfig->mag_align = ALIGN_DEFAULT; + compassConfig->mag_declination = 0; + compassConfig->mag_hardware = MAG_DEFAULT; -#ifdef MAG +// Generate a reasonable default for backward compatibility +// Strategy is +// 1. If SPI device is defined, it will take precedence, assuming it's onboard. +// 2. I2C devices are will be handled by address = 0 (per device default). +// 3. Slave I2C device on SPI gyro + +#if defined(USE_SPI) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963)) + compassConfig->mag_bustype = BUSTYPE_SPI; +#ifdef USE_MAG_SPI_HMC5883 + compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(HMC5883_SPI_INSTANCE)); + compassConfig->mag_spi_csn = IO_TAG(HMC5883_CS_PIN); +#else + compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(AK8963_SPI_INSTANCE)); + compassConfig->mag_spi_csn = IO_TAG(AK8963_CS_PIN); +#endif + compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); + compassConfig->mag_i2c_address = 0; +#elif defined(USE_MAG_HMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))) + compassConfig->mag_bustype = BUSTYPE_I2C; + compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE); + compassConfig->mag_i2c_address = 0; + compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); + compassConfig->mag_spi_csn = IO_TAG_NONE; +#elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) + compassConfig->mag_bustype = BUSTYPE_SLAVE; + compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); + compassConfig->mag_i2c_address = 0; + compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); + compassConfig->mag_spi_csn = IO_TAG_NONE; +#else + compassConfig->mag_hardware = MAG_NONE; + compassConfig->mag_bustype = BUSTYPE_NONE; + compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); + compassConfig->mag_i2c_address = 0; + compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); + compassConfig->mag_spi_csn = IO_TAG_NONE; +#endif + compassConfig->interruptTag = COMPASS_INTERRUPT_TAG; +} + +#if defined(MAG) static int16_t magADCRaw[XYZ_AXIS_COUNT]; static uint8_t magInit = 0; -bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) -{ - magSensor_e magHardware; +#if !defined(SITL) -retry: +bool compassDetect(magDev_t *dev) +{ + magSensor_e magHardware = MAG_NONE; + + busDevice_t *busdev = &dev->busdev; + +#ifdef USE_MAG_DATA_READY_SIGNAL + dev->magIntExtiTag = compassConfig()->interruptTag; +#endif + + switch (compassConfig()->mag_bustype) { +#ifdef USE_I2C + case BUSTYPE_I2C: + busdev->bustype = BUSTYPE_I2C; + busdev->busdev_u.i2c.device = I2C_CFG_TO_DEV(compassConfig()->mag_i2c_device); + busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; +#endif + break; + +#ifdef USE_SPI + case BUSTYPE_SPI: + busdev->bustype = BUSTYPE_SPI; + spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(compassConfig()->mag_spi_device))); + busdev->busdev_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn); +#endif + break; + +#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) + case BUSTYPE_SLAVE: + { + if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { + busdev->bustype = BUSTYPE_SLAVE; + busdev->busdev_u.i2c.master = gyroSensorBus(); + busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + } else { + return false; + } + } +#endif + break; + + default: + return false; + } dev->magAlign = ALIGN_DEFAULT; - switch (magHardwareToUse) { + switch (compassConfig()->mag_hardware) { case MAG_DEFAULT: ; // fallthrough case MAG_HMC5883: -#ifdef USE_MAG_HMC5883 - if (hmc5883lDetect(dev, compassConfig()->interruptTag)) { +#if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883) + if (busdev->bustype == BUSTYPE_I2C) { + busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + } + + if (hmc5883lDetect(dev)) { #ifdef MAG_HMC5883_ALIGN dev->magAlign = MAG_HMC5883_ALIGN; #endif @@ -95,6 +182,10 @@ retry: case MAG_AK8975: #ifdef USE_MAG_AK8975 + if (busdev->bustype == BUSTYPE_I2C) { + busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + } + if (ak8975Detect(dev)) { #ifdef MAG_AK8975_ALIGN dev->magAlign = MAG_AK8975_ALIGN; @@ -106,7 +197,16 @@ retry: ; // fallthrough case MAG_AK8963: -#ifdef USE_MAG_AK8963 +#if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963) + if (busdev->bustype == BUSTYPE_I2C) { + busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + } + if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { + dev->busdev.bustype = BUSTYPE_SLAVE; + busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + dev->busdev.busdev_u.i2c.master = gyroSensorBus(); + } + if (ak8963Detect(dev)) { #ifdef MAG_AK8963_ALIGN dev->magAlign = MAG_AK8963_ALIGN; @@ -122,12 +222,6 @@ retry: break; } - if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) { - // Nothing was found and we have a forced sensor that isn't present. - magHardwareToUse = MAG_DEFAULT; - goto retry; - } - if (magHardware == MAG_NONE) { return false; } @@ -136,15 +230,22 @@ retry: sensorsSet(SENSOR_MAG); return true; } +#else +bool compassDetect(magDev_t *dev) +{ + UNUSED(dev); + + return false; +} +#endif // !SITL bool compassInit(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // calculate magnetic declination mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. - // copy over SPI bus settings for AK8963 compass - magDev.bus = *gyroSensorBus(); - if (!compassDetect(&magDev, compassConfig()->mag_hardware)) { + + if (!compassDetect(&magDev)) { return false; } @@ -152,7 +253,7 @@ bool compassInit(void) const int16_t min = compassConfig()->mag_declination % 100; mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units LED1_ON; - magDev.init(); + magDev.init(&magDev); LED1_OFF; magInit = 1; if (compassConfig()->mag_align != ALIGN_DEFAULT) { @@ -167,7 +268,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; - magDev.read(magADCRaw); + magDev.read(&magDev, magADCRaw); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { mag.magADC[axis] = magADCRaw[axis]; } diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 6194743330..24a085231c 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -44,6 +44,11 @@ typedef struct compassConfig_s { // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. sensor_align_e mag_align; // mag alignment uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device + uint8_t mag_bustype; + uint8_t mag_i2c_device; + uint8_t mag_i2c_address; + uint8_t mag_spi_device; + ioTag_t mag_spi_csn; ioTag_t interruptTag; flightDynamicsTrims_t magZero; } compassConfig_t; diff --git a/src/main/target/ALIENFLIGHTNGF7/target.mk b/src/main/target/ALIENFLIGHTNGF7/target.mk index a559d8cc92..ad0ca8fef1 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.mk +++ b/src/main/target/ALIENFLIGHTNGF7/target.mk @@ -7,6 +7,4 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_ak8963.c \ - drivers/compass/compass_spi_ak8963.c \ - drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_spi_hmc5883l.c + drivers/compass/compass_hmc5883l.c diff --git a/src/test/unit/baro_bmp085_unittest.cc.txt b/src/test/unit/baro_bmp085_unittest.cc.txt index 60889ae50e..60209a2210 100644 --- a/src/test/unit/baro_bmp085_unittest.cc.txt +++ b/src/test/unit/baro_bmp085_unittest.cc.txt @@ -186,11 +186,7 @@ extern "C" { void RCC_APB2PeriphClockCmd() {} void delay(uint32_t) {} void delayMicroseconds(uint32_t) {} - bool i2cWrite(uint8_t, uint8_t, uint8_t) { - return 1; - } - bool i2cRead(uint8_t, uint8_t, uint8_t, uint8_t) { - return 1; - } + bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} + bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} } diff --git a/src/test/unit/baro_bmp280_unittest.cc b/src/test/unit/baro_bmp280_unittest.cc index 6cfdf1718e..848b7a65f9 100644 --- a/src/test/unit/baro_bmp280_unittest.cc +++ b/src/test/unit/baro_bmp280_unittest.cc @@ -144,11 +144,8 @@ TEST(baroBmp280Test, TestBmp280CalculateZeroP) extern "C" { void delay(uint32_t) {} -bool i2cBusReadRegisterBuffer(busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool i2cBusWriteRegister(busDevice_t*, uint8_t, uint8_t) {return true;} -bool spiBusReadRegisterBuffer(busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool spiBusWriteRegister(busDevice_t*, uint8_t, uint8_t) {return true;} - +bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} void spiSetDivisor() { } diff --git a/src/test/unit/baro_ms5611_unittest.cc b/src/test/unit/baro_ms5611_unittest.cc index 05a52eb4fc..da6d2410c2 100644 --- a/src/test/unit/baro_ms5611_unittest.cc +++ b/src/test/unit/baro_ms5611_unittest.cc @@ -146,10 +146,8 @@ extern "C" { void delay(uint32_t) {} void delayMicroseconds(uint32_t) {} -bool i2cBusReadRegisterBuffer(busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool i2cBusWriteRegister(busDevice_t*, uint8_t, uint8_t) {return true;} -bool spiBusReadRegisterBuffer(busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool spiBusWriteRegister(busDevice_t*, uint8_t, uint8_t) {return true;} +bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} void spiSetDivisor() { } From ae8256f142c28a158861fe80ffad30356e158d82 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 17 Oct 2017 21:04:06 +0200 Subject: [PATCH 2/2] Updates according to ledvinap's recomendations --- src/main/drivers/barometer/barometer_bmp280.c | 2 +- src/main/drivers/barometer/barometer_ms5611.c | 2 +- src/main/drivers/bus.c | 18 +-- src/main/drivers/bus.h | 11 +- src/main/drivers/compass/compass_ak8963.c | 67 ++++----- src/main/drivers/compass/compass_ak8975.c | 25 ++-- src/main/drivers/compass/compass_hmc5883l.c | 131 ++++++++---------- src/main/sensors/compass.c | 16 +-- 8 files changed, 130 insertions(+), 142 deletions(-) diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c index 2ef9846753..8e8bcd132d 100644 --- a/src/main/drivers/barometer/barometer_bmp280.c +++ b/src/main/drivers/barometer/barometer_bmp280.c @@ -69,9 +69,9 @@ void bmp280BusInit(busDevice_t *busdev) { #ifdef USE_BARO_SPI_BMP280 if (busdev->bustype == BUSTYPE_SPI) { + IOHi(busdev->busdev_u.spi.csnPin); // Disable IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - IOHi(busdev->busdev_u.spi.csnPin); // Disable spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX } #else diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c index 7969c70024..f38357bc03 100644 --- a/src/main/drivers/barometer/barometer_ms5611.c +++ b/src/main/drivers/barometer/barometer_ms5611.c @@ -65,9 +65,9 @@ void ms5611BusInit(busDevice_t *busdev) { #ifdef USE_BARO_SPI_MS5611 if (busdev->bustype == BUSTYPE_SPI) { + IOHi(busdev->busdev_u.spi.csnPin); // Disable IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - IOHi(busdev->busdev_u.spi.csnPin); // Disable spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX } #else diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index f6303cb470..6594993aa9 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -68,18 +68,12 @@ bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg) { #if !defined(USE_SPI) && !defined(USE_I2C) + UNUSED(busdev); UNUSED(reg); + return false; +#else + uint8_t data; + busReadRegisterBuffer(busdev, reg, &data, 1); + return data; #endif - switch (busdev->bustype) { -#ifdef USE_SPI - case BUSTYPE_SPI: - return spiBusReadRegister(busdev, reg & 0x7f); -#endif -#ifdef USE_I2C - case BUSTYPE_I2C: - return i2cBusReadRegister(busdev, reg); -#endif - default: - return false; - } } diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index de7b143e07..444f4b54d1 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -26,7 +26,7 @@ typedef enum { BUSTYPE_NONE = 0, BUSTYPE_I2C, BUSTYPE_SPI, - BUSTYPE_SLAVE // Slave I2C on SPI master + BUSTYPE_MPU_SLAVE // Slave I2C on SPI master } busType_e; typedef struct busDevice_s { @@ -40,10 +40,13 @@ typedef struct busDevice_s { IO_t csnPin; } spi; struct deviceI2C_s { - const struct busDevice_s *master; - I2CDevice device; - uint8_t address; + I2CDevice device; + uint8_t address; } i2c; + struct deviceMpuSlave_s { + const struct busDevice_s *master; + uint8_t address; + } mpuSlave; } busdev_u; } busDevice_t; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 2716574634..498534d867 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -106,9 +106,9 @@ static bool ak8963SpiWriteRegisterDelay(const busDevice_t *bus, uint8_t reg, uin static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t reg, uint8_t *buf, uint8_t len) { - const busDevice_t *bus = slavedev->busdev_u.i2c.master; + const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address | READ_FLAG); // set I2C slave address for read + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes delay(4); @@ -120,9 +120,9 @@ static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t r static bool ak8963SlaveWriteRegister(const busDevice_t *slavedev, uint8_t reg, uint8_t data) { - const busDevice_t *bus = slavedev->busdev_u.i2c.master; + const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address); // set I2C slave address for write + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address); // set I2C slave address for write ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C sLave value ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN); // write 1 byte @@ -143,11 +143,11 @@ static bool ak8963SlaveStartRead(const busDevice_t *slavedev, uint8_t reg, uint8 return false; } - const busDevice_t *bus = slavedev->busdev_u.i2c.master; + const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; queuedRead.len = len; - ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address | READ_FLAG); // set I2C slave address for read + ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes @@ -178,7 +178,7 @@ static bool ak8963SlaveCompleteRead(const busDevice_t *slavedev, uint8_t *buf) { uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining(); - const busDevice_t *bus = slavedev->busdev_u.i2c.master; + const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master; if (timeRemaining > 0) { delayMicroseconds(timeRemaining); @@ -260,7 +260,7 @@ restart: static bool ak8963ReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *buf, uint8_t len) { #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - if (busdev->bustype == BUSTYPE_SLAVE) { + if (busdev->bustype == BUSTYPE_MPU_SLAVE) { return ak8963SlaveReadRegisterBuffer(busdev, reg, buf, len); } #endif @@ -270,14 +270,14 @@ static bool ak8963ReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uin static bool ak8963WriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) { #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - if (busdev->bustype == BUSTYPE_SLAVE) { + if (busdev->bustype == BUSTYPE_MPU_SLAVE) { return ak8963SlaveWriteRegister(busdev, reg, data); } #endif return busWriteRegister(busdev, reg, data); } -static bool ak8963ReadData(const busDevice_t *busdev, uint8_t *buf) +static bool ak8963DirectReadData(const busDevice_t *busdev, uint8_t *buf) { uint8_t status; @@ -290,23 +290,28 @@ static bool ak8963ReadData(const busDevice_t *busdev, uint8_t *buf) return ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_HXL, buf, 7); } -static bool ak8963Read(magDev_t *magdev, int16_t *magData) +static int16_t parseMag(uint8_t *raw, int16_t gain) { + int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256; + return constrain(ret, INT16_MIN, INT16_MAX); +} + +static bool ak8963Read(magDev_t *mag, int16_t *magData) { bool ack = false; uint8_t buf[7]; - const busDevice_t *busdev = &magdev->busdev; + const busDevice_t *busdev = &mag->busdev; switch (busdev->bustype) { #if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963) case BUSTYPE_I2C: case BUSTYPE_SPI: - ack = ak8963ReadData(busdev, buf); + ack = ak8963DirectReadData(busdev, buf); break; #endif #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - case BUSTYPE_SLAVE: + case BUSTYPE_MPU_SLAVE: ack = ak8963SlaveReadData(busdev, buf); break; #endif @@ -325,27 +330,27 @@ static bool ak8963Read(magDev_t *magdev, int16_t *magData) return false; } - magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256; - magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256; - magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256; + magData[X] = -parseMag(buf + 0, mag->magGain[X]); + magData[Y] = -parseMag(buf + 2, mag->magGain[Y]); + magData[Z] = -parseMag(buf + 4, mag->magGain[Z]); return true; } -static bool ak8963Init(magDev_t *magdev) +static bool ak8963Init(magDev_t *mag) { uint8_t asa[3]; uint8_t status; - const busDevice_t *busdev = &magdev->busdev; + const busDevice_t *busdev = &mag->busdev; ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values - magdev->magGain[X] = asa[X] + 128; - magdev->magGain[Y] = asa[Y] + 128; - magdev->magGain[Z] = asa[Z] + 128; + mag->magGain[X] = asa[X] + 128; + mag->magGain[Y] = asa[Y] + 128; + mag->magGain[Z] = asa[Z] + 128; ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading. @@ -369,22 +374,22 @@ void ak8963BusInit(const busDevice_t *busdev) #ifdef USE_MAG_SPI_AK8963 case BUSTYPE_SPI: + IOHi(busdev->busdev_u.spi.csnPin); // Disable IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0); IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - IOHi(busdev->busdev_u.spi.csnPin); // Disable spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); break; #endif #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) #define TASK_PERIOD_HZ(hz) (1000000 / (hz)) - case BUSTYPE_SLAVE: + case BUSTYPE_MPU_SLAVE: rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); // initialze I2C master via SPI bus - ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); - ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz - ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); + ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only break; #endif default: @@ -410,8 +415,8 @@ void ak8963BusDeInit(const busDevice_t *busdev) #endif #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - case BUSTYPE_SLAVE: - ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); + case BUSTYPE_MPU_SLAVE: + ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); break; #endif default: @@ -425,8 +430,8 @@ bool ak8963Detect(magDev_t *mag) busDevice_t *busdev = &mag->busdev; - if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_SLAVE) && busdev->busdev_u.i2c.address == 0) { - busdev->busdev_u.i2c.address = AK8963_MAG_I2C_ADDRESS; + if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_MPU_SLAVE) && busdev->busdev_u.mpuSlave.address == 0) { + busdev->busdev_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS; } ak8963BusInit(busdev); diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index b0f96ce6a1..78d63e84be 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -74,12 +74,12 @@ #define CNTL_BIT_14_BIT 0x00 #define CNTL_BIT_16_BIT 0x10 -static bool ak8975Init(magDev_t *magdev) +static bool ak8975Init(magDev_t *mag) { uint8_t asa[3]; uint8_t status; - busDevice_t *busdev = &magdev->busdev; + busDevice_t *busdev = &mag->busdev; busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode delay(20); @@ -90,9 +90,9 @@ static bool ak8975Init(magDev_t *magdev) busReadRegisterBuffer(busdev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values delay(10); - magdev->magGain[X] = asa[X] + 128; - magdev->magGain[Y] = asa[Y] + 128; - magdev->magGain[Z] = asa[Z] + 128; + mag->magGain[X] = asa[X] + 128; + mag->magGain[Y] = asa[Y] + 128; + mag->magGain[Z] = asa[Z] + 128; busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading. delay(10); @@ -106,13 +106,18 @@ static bool ak8975Init(magDev_t *magdev) return true; } -static bool ak8975Read(magDev_t *magdev, int16_t *magData) +static int16_t parseMag(uint8_t *raw, int16_t gain) { + int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256; + return constrain(ret, INT16_MIN, INT16_MAX); +} + +static bool ak8975Read(magDev_t *mag, int16_t *magData) { bool ack; uint8_t status; uint8_t buf[6]; - busDevice_t *busdev = &magdev->busdev; + busDevice_t *busdev = &mag->busdev; ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1); if (!ack || (status & ST1_REG_DATA_READY) == 0) { @@ -136,9 +141,9 @@ static bool ak8975Read(magDev_t *magdev, int16_t *magData) return false; } - magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256; - magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256; - magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256; + magData[X] = -parseMag(buf + 0, mag->magGain[X]); + magData[Y] = -parseMag(buf + 2, mag->magGain[Y]); + magData[Z] = -parseMag(buf + 4, mag->magGain[Z]); return true; } diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index 48d0e07728..7b30b68c9b 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -173,6 +173,7 @@ static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag) IOInit(magIntIO, OWNER_COMPASS_EXTI, 0); EXTIHandlerInit(&mag->exti, hmc5883_extiHandler); EXTIConfig(magIntIO, &gmag->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); + EXTIEnable(magIntIO, true); #else IOInit(magIntIO, OWNER_COMPASS_EXTI, 0); IOConfigGPIO(magIntIO, IOCFG_IN_FLOATING); @@ -186,39 +187,27 @@ static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag) } #ifdef USE_MAG_SPI_HMC5883 -/* - * XXX Fixme - * HMC5983 datasheet states SPI mode is - * SCK is high when CS is high - * Data is sampled at the rising edge of SCK - * However, it seems that SCK condition is ignored (works with SCK low when CS is high). - * Nevertheless, it is nice to conform to the datasheet specification when per device SPI mode - * is implemented. - */ static void hmc5883SpiInit(busDevice_t *busdev) { - static bool hardwareInitialised = false; - - if (hardwareInitialised) { - return; - } + IOHi(busdev->busdev_u.spi.csnPin); // Disable IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0); IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); - IOHi(busdev->busdev_u.spi.csnPin); // Disable - spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); - - hardwareInitialised = true; } #endif -static bool hmc5883lRead(magDev_t *magdev, int16_t *magData) +static int16_t parseMag(uint8_t *raw, int16_t gain) { + int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256; + return constrain(ret, INT16_MIN, INT16_MAX); +} + +static bool hmc5883lRead(magDev_t *mag, int16_t *magData) { uint8_t buf[6]; - busDevice_t *busdev = &magdev->busdev; + busDevice_t *busdev = &mag->busdev; bool ack = busReadRegisterBuffer(busdev, HMC58X3_REG_DATA, buf, 6); @@ -228,25 +217,30 @@ static bool hmc5883lRead(magDev_t *magdev, int16_t *magData) // During calibration, magGain is 1.0, so the read returns normal non-calibrated values. // After calibration is done, magGain is set to calculated gain values. - magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magdev->magGain[X] / 256; - magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magdev->magGain[Z] / 256; - magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magdev->magGain[Y] / 256; + magData[X] = parseMag(buf + 0, mag->magGain[X]); + magData[Z] = parseMag(buf + 2, mag->magGain[Z]); + magData[Y] = parseMag(buf + 4, mag->magGain[Y]); return true; } -static bool hmc5883lInit(magDev_t *magdev) +static bool hmc5883lInit(magDev_t *mag) { - busDevice_t *busdev = &magdev->busdev; + enum { + polPos, + polNeg + }; + + busDevice_t *busdev = &mag->busdev; int16_t magADC[3]; int i; int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. bool bret = true; // Error indicator - magdev->magGain[X] = 256; - magdev->magGain[Y] = 256; - magdev->magGain[Z] = 256; + mag->magGain[X] = 256; + mag->magGain[Y] = 256; + mag->magGain[Z] = 256; delay(50); @@ -259,53 +253,40 @@ static bool hmc5883lInit(magDev_t *magdev) delay(100); - hmc5883lRead(magdev, magADC); + hmc5883lRead(mag, magADC); - for (i = 0; i < 10; i++) { // Collect 10 samples - busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE); - delay(20); - hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed. - - // Since the measurements are noisy, they should be averaged rather than taking the max. - - xyz_total[X] += magADC[X]; - xyz_total[Y] += magADC[Y]; - xyz_total[Z] += magADC[Z]; - - // Detect saturation. - if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { - bret = false; - break; // Breaks out of the for loop. No sense in continuing if we saturated. + for (int polarity = polPos; polarity <= polNeg; polarity++) { + switch(polarity) { + case polPos: + busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias + break; + case polNeg: + busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. + break; + } + for (i = 0; i < 10; i++) { // Collect 10 samples + busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE); + delay(20); + hmc5883lRead(mag, magADC); // Get the raw values in case the scales have already been changed. + + // Since the measurements are noisy, they should be averaged rather than taking the max. + + xyz_total[X] += ((polarity == polPos) ? 1 : -1) * magADC[X]; + xyz_total[Y] += ((polarity == polPos) ? 1 : -1) * magADC[Y]; + xyz_total[Z] += ((polarity == polPos) ? 1 : -1) * magADC[Z]; + + // Detect saturation. + if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { + bret = false; + break; // Breaks out of the for loop. No sense in continuing if we saturated. + } + LED1_TOGGLE; } - LED1_TOGGLE; } - // Apply the negative bias. (Same gain) - - busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. - - for (i = 0; i < 10; i++) { - busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE); - delay(20); - hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed. - - // Since the measurements are noisy, they should be averaged. - - xyz_total[X] -= magADC[X]; - xyz_total[Y] -= magADC[Y]; - xyz_total[Z] -= magADC[Z]; - - // Detect saturation. - if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { - bret = false; - break; // Breaks out of the for loop. No sense in continuing if we saturated. - } - LED1_TOGGLE; - } - - magdev->magGain[X] = ABS((int32_t)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X]); - magdev->magGain[Y] = ABS((int32_t)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y]); - magdev->magGain[Z] = ABS((int32_t)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z]); + mag->magGain[X] = (int)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X]; + mag->magGain[Y] = (int)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y]; + mag->magGain[Z] = (int)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z]; // leave test mode @@ -316,12 +297,12 @@ static bool hmc5883lInit(magDev_t *magdev) delay(100); if (!bret) { // Something went wrong so get a best guess - magdev->magGain[X] = 256; - magdev->magGain[Y] = 256; - magdev->magGain[Z] = 256; + mag->magGain[X] = 256; + mag->magGain[Y] = 256; + mag->magGain[Z] = 256; } - hmc5883lConfigureDataReadyInterruptHandling(magdev); + hmc5883lConfigureDataReadyInterruptHandling(mag); return true; } diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 6459c2cd93..88d91091fb 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -90,7 +90,7 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig) compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); compassConfig->mag_spi_csn = IO_TAG_NONE; #elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - compassConfig->mag_bustype = BUSTYPE_SLAVE; + compassConfig->mag_bustype = BUSTYPE_MPU_SLAVE; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); @@ -141,12 +141,12 @@ bool compassDetect(magDev_t *dev) break; #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - case BUSTYPE_SLAVE: + case BUSTYPE_MPU_SLAVE: { if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { - busdev->bustype = BUSTYPE_SLAVE; - busdev->busdev_u.i2c.master = gyroSensorBus(); - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; + busdev->bustype = BUSTYPE_MPU_SLAVE; + busdev->busdev_u.mpuSlave.master = gyroSensorBus(); + busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address; } else { return false; } @@ -202,9 +202,9 @@ bool compassDetect(magDev_t *dev) busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; } if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { - dev->busdev.bustype = BUSTYPE_SLAVE; - busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; - dev->busdev.busdev_u.i2c.master = gyroSensorBus(); + dev->busdev.bustype = BUSTYPE_MPU_SLAVE; + busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address; + dev->busdev.busdev_u.mpuSlave.master = gyroSensorBus(); } if (ak8963Detect(dev)) {