Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Merge pull request #4317 from AlienWiiBF/AK8963_SPI

Browse source 
Configurable compass drivers based on jflyper's PR #3613
This commit is contained in:
Michael Keller 2017-10-18 11:18:05 +13:00 • committed by GitHub
commit 3038eb79ea
28 changed files with 744 additions and 760 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/drivers/barometer/barometer_bmp085.c
View file

@ -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];

42
src/main/drivers/barometer/barometer_bmp280.c
View file

@ -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));
}

44
src/main/drivers/barometer/barometer_ms5611.c
View file

@ -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)

37
src/main/drivers/bus.c
View file

@ -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
}

23
src/main/drivers/bus.h
View file

@ -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

10
src/main/drivers/compass/compass.h
View file

@ -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

408
src/main/drivers/compass/compass_ak8963.c
View file

@ -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

46
src/main/drivers/compass/compass_ak8963.h
View file

@ -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);

118
src/main/drivers/compass/compass_ak8975.c
View file

@ -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;

10
src/main/drivers/compass/compass_fake.c
View file

@ -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

280
src/main/drivers/compass/compass_hmc5883l.c
View file

@ -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;

2
src/main/drivers/compass/compass_hmc5883l.h
View file

@ -19,4 +19,4 @@
#include "drivers/io_types.h"
bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag);
bool hmc5883lDetect(magDev_t* mag);

122
src/main/drivers/compass/compass_spi_ak8963.c
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#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

20
src/main/drivers/compass/compass_spi_ak8963.h
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
bool ak8963SpiDetect(magDev_t *mag);

86
src/main/drivers/compass/compass_spi_hmc5883l.c
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <platform.h>
#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

22
src/main/drivers/compass/compass_spi_hmc5883l.h
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
void hmc5883SpiInit(void);
bool hmc5883SpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data);
bool hmc5883SpiWriteCommand(uint8_t reg, uint8_t data);

1
src/main/drivers/resource.c
View file

@ -52,6 +52,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"SPI_CS",
"MPU_EXTI",
"BARO_EXTI",
"COMPASS_EXTI",
"USB",
"USB_DETECT",
"BEEPER",

1
src/main/drivers/resource.h
View file

@ -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,

5
src/main/drivers/sensor.h
View file

@ -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);

3
src/main/fc/cli.c
View file

@ -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)

4
src/main/fc/fc_tasks.c
View file

@ -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));

9
src/main/fc/settings.c
View file

@ -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) },

157
src/main/sensors/compass.c
View file

@ -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];
}

5
src/main/sensors/compass.h
View file

@ -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;

4
src/main/target/ALIENFLIGHTNGF7/target.mk
View file

@ -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

8
src/test/unit/baro_bmp085_unittest.cc.txt
View file

@ -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;}
}

7
src/test/unit/baro_bmp280_unittest.cc
View file

@ -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() {
}

6
src/test/unit/baro_ms5611_unittest.cc
View file

@ -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() {
}