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..8e8bcd132d 100644
--- a/src/main/drivers/barometer/barometer_bmp280.c
+++ b/src/main/drivers/barometer/barometer_bmp280.c
@@ -65,43 +65,13 @@ 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
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
@@ -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..f38357bc03 100644
--- a/src/main/drivers/barometer/barometer_ms5611.c
+++ b/src/main/drivers/barometer/barometer_ms5611.c
@@ -61,43 +61,13 @@ 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
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
@@ -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..6594993aa9 100644
--- a/src/main/drivers/bus.c
+++ b/src/main/drivers/bus.c
@@ -26,33 +26,54 @@
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)
{
- switch (busdev->bustype) {
- case BUSTYPE_SPI:
- return spiBusReadRegister(busdev, reg & 0x7f);
- case BUSTYPE_I2C:
- return i2cBusReadRegister(busdev, 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
}
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 2ca515dd85..444f4b54d1 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_MPU_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,16 @@ typedef struct busDevice_s {
IO_t csnPin;
} spi;
struct deviceI2C_s {
- I2CDevice device;
- uint8_t address;
- } i2c;
+ I2CDevice device;
+ uint8_t address;
+ } i2c;
+ struct deviceMpuSlave_s {
+ const struct busDevice_s *master;
+ uint8_t address;
+ } mpuSlave;
} 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..498534d867 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.mpuSlave.master;
+
+ 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);
__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.mpuSlave.master;
+
+ 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
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.mpuSlave.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.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
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.mpuSlave.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,226 @@ 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_MPU_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_MPU_SLAVE) {
+ return ak8963SlaveWriteRegister(busdev, reg, data);
+ }
+#endif
+ return busWriteRegister(busdev, reg, data);
+}
+
+static bool ak8963DirectReadData(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 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 = &mag->busdev;
+
+ switch (busdev->bustype) {
+#if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963)
+ case BUSTYPE_I2C:
+ case BUSTYPE_SPI:
+ ack = ak8963DirectReadData(busdev, buf);
+ break;
#endif
+
+#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
+ case BUSTYPE_MPU_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] = -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 *mag)
+{
+ uint8_t asa[3];
+ uint8_t status;
+
+ 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
+
+ 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.
+
+ // 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:
+ 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);
+ 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_MPU_SLAVE:
+ rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
+
+ // initialze I2C master via SPI bus
+ 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:
+ 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_MPU_SLAVE:
+ ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.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_MPU_SLAVE) && busdev->busdev_u.mpuSlave.address == 0) {
+ busdev->busdev_u.mpuSlave.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..78d63e84be 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,129 @@
#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 *mag)
{
- 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 = &mag->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.
+ 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);
// 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)
+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);
+}
-#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 *mag, 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 = &mag->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] = -parseMag(buf + 0, mag->magGain[X]);
+ magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
+ magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
-
- 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..7b30b68c9b 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,164 +153,182 @@ 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));
+ EXTIEnable(magIntIO, true);
+#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
+static void hmc5883SpiInit(busDevice_t *busdev)
+{
+ 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);
+
+ spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
+}
+#endif
+
+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];
-#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 = &mag->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] = 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(void)
+static bool hmc5883lInit(magDev_t *mag)
{
+ 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
+ mag->magGain[X] = 256;
+ mag->magGain[Y] = 256;
+ mag->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);
- 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.
+ hmc5883lRead(mag, magADC);
- // 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)
-#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
- 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.
-
- // 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;
- }
-
- 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]);
+ 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
-#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;
+ mag->magGain[X] = 256;
+ mag->magGain[Y] = 256;
+ mag->magGain[Z] = 256;
}
- hmc5883lConfigureDataReadyInterruptHandling();
+ hmc5883lConfigureDataReadyInterruptHandling(mag);
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..88d91091fb 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_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);
+ 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_MPU_SLAVE:
+ {
+ if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
+ busdev->bustype = BUSTYPE_MPU_SLAVE;
+ busdev->busdev_u.mpuSlave.master = gyroSensorBus();
+ busdev->busdev_u.mpuSlave.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_MPU_SLAVE;
+ busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
+ dev->busdev.busdev_u.mpuSlave.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() {
}