Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +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; 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) bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
{ {
uint8_t data; uint8_t data;
@ -205,13 +195,13 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
defaultAddressApplied = true; 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) { if (ack) {
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
bmp085.oversampling_setting = 3; bmp085.oversampling_setting = 3;
if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */ 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.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.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(busdev); /* readout bmp085 calibparam structure */ 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) #if defined(BARO_EOC_GPIO)
isConversionComplete = false; isConversionComplete = false;
#endif #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) static void bmp085_get_ut(baroDev_t *baro)
@ -316,7 +306,7 @@ static void bmp085_get_ut(baroDev_t *baro)
} }
#endif #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]; bmp085_ut = (data[0] << 8) | data[1];
} }
@ -330,7 +320,7 @@ static void bmp085_start_up(baroDev_t *baro)
isConversionComplete = false; isConversionComplete = false;
#endif #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 /** read out up for pressure conversion
@ -348,7 +338,7 @@ static void bmp085_get_up(baroDev_t *baro)
} }
#endif #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]) bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2])
>> (8 - bmp085.oversampling_setting); >> (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) static void bmp085_get_cal_param(busDevice_t *busdev)
{ {
uint8_t data[22]; 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*/ /*parameters AC1-AC6*/
bmp085.cal_param.ac1 = (data[0] << 8) | data[1]; 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); 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) void bmp280BusInit(busDevice_t *busdev)
{ {
#ifdef USE_BARO_SPI_BMP280 #ifdef USE_BARO_SPI_BMP280
if (busdev->bustype == BUSTYPE_SPI) { if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); 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 spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX
} }
#else #else
@ -137,7 +107,7 @@ bool bmp280Detect(baroDev_t *baro)
defaultAddressApplied = true; 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) { if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) {
bmp280BusDeinit(busdev); bmp280BusDeinit(busdev);
@ -148,10 +118,10 @@ bool bmp280Detect(baroDev_t *baro)
} }
// read calibration // 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 // 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 // these are dummy as temperature is measured as part of pressure
baro->ut_delay = 0; baro->ut_delay = 0;
@ -182,7 +152,7 @@ static void bmp280_start_up(baroDev_t *baro)
{ {
// start measurement // start measurement
// set oversampling + power mode (forced), and start sampling // 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) 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]; uint8_t data[BMP280_DATA_FRAME_SIZE];
// read data from sensor // 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_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)); 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_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM
static uint8_t ms5611_osr = CMD_ADC_4096; 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) void ms5611BusInit(busDevice_t *busdev)
{ {
#ifdef USE_BARO_SPI_MS5611 #ifdef USE_BARO_SPI_MS5611
if (busdev->bustype == BUSTYPE_SPI) { if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0); IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP); 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 spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX
} }
#else #else
@ -136,7 +106,7 @@ bool ms5611Detect(baroDev_t *baro)
defaultAddressApplied = true; defaultAddressApplied = true;
} }
if (!ms5611ReadCommand(busdev, CMD_PROM_RD, 1, &sig) || sig == 0xFF) { if (!busReadRegisterBuffer(busdev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) {
goto fail; goto fail;
} }
@ -174,7 +144,7 @@ fail:;
static void ms5611_reset(busDevice_t *busdev) static void ms5611_reset(busDevice_t *busdev)
{ {
ms5611WriteCommand(busdev, CMD_RESET, 1); busWriteRegister(busdev, CMD_RESET, 1);
delayMicroseconds(2800); delayMicroseconds(2800);
} }
@ -183,7 +153,7 @@ static uint16_t ms5611_prom(busDevice_t *busdev, int8_t coef_num)
{ {
uint8_t rxbuf[2] = { 0, 0 }; 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]; return rxbuf[0] << 8 | rxbuf[1];
} }
@ -222,14 +192,14 @@ static uint32_t ms5611_read_adc(busDevice_t *busdev)
{ {
uint8_t rxbuf[3]; 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]; return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
} }
static void ms5611_start_ut(baroDev_t *baro) 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) 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) 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) 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) 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) { switch (busdev->bustype) {
#ifdef USE_SPI
case BUSTYPE_SPI: case BUSTYPE_SPI:
return spiBusWriteRegister(busdev, reg & 0x7f, data); return spiBusWriteRegister(busdev, reg & 0x7f, data);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C: case BUSTYPE_I2C:
return i2cBusWriteRegister(busdev, reg, data); 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) 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) { switch (busdev->bustype) {
#ifdef USE_SPI
case BUSTYPE_SPI: case BUSTYPE_SPI:
return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length); return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C: case BUSTYPE_I2C:
return i2cBusReadRegisterBuffer(busdev, reg, data, length); return i2cBusReadRegisterBuffer(busdev, reg, data, length);
#endif
default:
return false;
} }
return false;
} }
uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg) uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg)
{ {
switch (busdev->bustype) { #if !defined(USE_SPI) && !defined(USE_I2C)
case BUSTYPE_SPI: UNUSED(busdev);
return spiBusReadRegister(busdev, reg & 0x7f); UNUSED(reg);
case BUSTYPE_I2C:
return i2cBusReadRegister(busdev, reg);
}
return false; 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/bus_i2c.h"
#include "drivers/io_types.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 { typedef struct busDevice_s {
uint8_t bustype; busType_e bustype;
union { union {
struct deviceSpi_s { struct deviceSpi_s {
SPI_TypeDef *instance; SPI_TypeDef *instance;
@ -33,16 +40,16 @@ typedef struct busDevice_s {
IO_t csnPin; IO_t csnPin;
} spi; } spi;
struct deviceI2C_s { struct deviceI2C_s {
I2CDevice device; I2CDevice device;
uint8_t address; uint8_t address;
} i2c; } i2c;
struct deviceMpuSlave_s {
const struct busDevice_s *master;
uint8_t address;
} mpuSlave;
} busdev_u; } busdev_u;
} busDevice_t; } busDevice_t;
#define BUSTYPE_NONE 0
#define BUSTYPE_I2C 1
#define BUSTYPE_SPI 2
#ifdef TARGET_BUS_INIT #ifdef TARGET_BUS_INIT
void targetBusInit(void); void targetBusInit(void);
#endif #endif

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

@ -19,12 +19,16 @@
#include "drivers/bus.h" #include "drivers/bus.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/exti.h"
typedef struct magDev_s { typedef struct magDev_s {
sensorInitFuncPtr init; // initialize function sensorMagInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function sensorMagReadFuncPtr read; // read 3 axis data function
busDevice_t bus; extiCallbackRec_t exti;
busDevice_t busdev;
sensor_align_e magAlign; sensor_align_e magAlign;
ioTag_t magIntExtiTag;
int16_t magGain[3];
} magDev_t; } magDev_t;
#ifndef MAG_I2C_INSTANCE #ifndef MAG_I2C_INSTANCE

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

@ -22,6 +22,8 @@
#include "platform.h" #include "platform.h"
#if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963)
#include "build/debug.h" #include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
@ -30,6 +32,7 @@
#include "drivers/bus.h" #include "drivers/bus.h"
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h" #include "drivers/bus_spi.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
@ -43,66 +46,110 @@
#include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h" #include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/compass/compass_ak8963.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) // This sensor is also available also part of the MPU-9250 connected to the secondary I2C bus.
static busDevice_t *bus = NULL;
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); spiBusWriteRegister(bus, reg, data);
delayMicroseconds(10); delayMicroseconds(10);
return true; return true;
} }
typedef struct queuedReadState_s { static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t reg, uint8_t *buf, uint8_t len)
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)
{ {
spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
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 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); delay(4);
__disable_irq(); __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(); __enable_irq();
return ack; 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 const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address); // set I2C slave address for write
spiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte 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; 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) { if (queuedRead.waiting) {
return false; 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 queuedRead.len = len;
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 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.readStartedAt = micros();
queuedRead.waiting = true; queuedRead.waiting = true;
@ -110,7 +157,7 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
return true; return true;
} }
static uint32_t ak8963SensorQueuedReadTimeRemaining(void) static uint32_t ak8963SlaveQueuedReadTimeRemaining(void)
{ {
if (!queuedRead.waiting) { if (!queuedRead.waiting) {
return 0; return 0;
@ -127,9 +174,11 @@ static uint32_t ak8963SensorQueuedReadTimeRemaining(void)
return timeRemaining; 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) { if (timeRemaining > 0) {
delayMicroseconds(timeRemaining); delayMicroseconds(timeRemaining);
@ -137,73 +186,42 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
queuedRead.waiting = false; queuedRead.waiting = false;
spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len); // read I2C buffer 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);
return true; return true;
} }
static bool ak8963Read(int16_t *magData) static bool ak8963SlaveReadData(const busDevice_t *busdev, uint8_t *buf)
{ {
bool ack = false; typedef enum {
uint8_t buf[7]; CHECK_STATUS = 0,
WAITING_FOR_STATUS,
#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) WAITING_FOR_DATA
} ak8963ReadState_e;
// 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.
static ak8963ReadState_e state = CHECK_STATUS; 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; bool retry = true;
restart: restart:
switch (state) { switch (state) {
case CHECK_STATUS: case CHECK_STATUS: {
ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ST1, 1); ak8963SlaveStartRead(busdev, AK8963_MAG_REG_ST1, 1);
state++; state = WAITING_FOR_STATUS;
return false; return false;
}
case WAITING_FOR_STATUS: { case WAITING_FOR_STATUS: {
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining();
if (timeRemaining) { if (timeRemaining) {
return false; return false;
} }
ack = ak8963SensorCompleteRead(&buf[0]); ack = ak8963SlaveCompleteRead(busdev, &buf[0]);
uint8_t status = buf[0]; uint8_t status = buf[0];
@ -213,90 +231,226 @@ restart:
if (retry) { if (retry) {
retry = false; retry = false;
goto restart; goto restart;
} }
return false; return false;
} }
// read the 6 bytes of data and the status2 register // read the 6 bytes of data and the status2 register
ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7); ak8963SlaveStartRead(busdev, AK8963_MAG_REG_HXL, 7);
state++;
state = WAITING_FOR_DATA;
return false; return false;
} }
case WAITING_FOR_DATA: { case WAITING_FOR_DATA: {
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining(); uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining();
if (timeRemaining) { if (timeRemaining) {
return false; 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) { if (!ack || (status & ST1_DATA_READY) == 0) {
return false; 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 #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]; uint8_t status2 = buf[6];
if (!ack || (status2 & ST2_DATA_ERROR) || (status2 & ST2_MAG_SENSOR_OVERFLOW)) { if (!ack) {
return false; return false;
} }
magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X]; ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); // start reading again uint8_t status2 = buf[6];
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) if (status2 & ST2_MAG_SENSOR_OVERFLOW) {
state = CHECK_STATUS; 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 #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) bool ak8963Detect(magDev_t *mag)
{ {
uint8_t sig = 0; uint8_t sig = 0;
#if defined(USE_SPI) && defined(AK8963_SPI_INSTANCE) busDevice_t *busdev = &mag->busdev;
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;
// check for SPI AK8963 if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_MPU_SLAVE) && busdev->busdev_u.mpuSlave.address == 0) {
if (ak8963SpiDetect(mag)) return true; busdev->busdev_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS;
#endif }
#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE) ak8963BusInit(busdev);
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
// initialze I2C master via SPI bus (MPU9250) ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG
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
delay(4); delay(4);
bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WIA, 1, &sig); // check for AK8963 bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_WIA, &sig, 1); // check for AK8963
if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H'
if (ack && sig == AK8963_DEVICE_ID) // 0x48 / 01001000 / 'H'
{ {
mag->init = ak8963Init; mag->init = ak8963Init;
mag->read = ak8963Read; mag->read = ak8963Read;
return true; return true;
} }
ak8963BusDeInit(busdev);
return false; return false;
} }
#endif

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

@ -17,50 +17,4 @@
#pragma once #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); 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/maths.h"
#include "common/utils.h" #include "common/utils.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/time.h" #include "drivers/time.h"
@ -36,101 +38,129 @@
#include "compass_ak8975.h" #include "compass_ak8975.h"
// This sensor is available in MPU-9150. // This sensor is available in MPU-9150.
// This driver only support I2C mode, direct and in bypass configuration.
// AK8975, mag sensor address // AK8975, mag sensor address
#define AK8975_MAG_I2C_ADDRESS 0x0C #define AK8975_MAG_I2C_ADDRESS 0x0C
#define AK8975_DEVICE_ID 0x48
// Registers // Registers
#define AK8975_MAG_REG_WHO_AM_I 0x00 #define AK8975_MAG_REG_WIA 0x00
#define AK8975_MAG_REG_INFO 0x01 #define AK8975_MAG_REG_INFO 0x01
#define AK8975_MAG_REG_STATUS1 0x02 #define AK8975_MAG_REG_ST1 0x02
#define AK8975_MAG_REG_HXL 0x03 #define AK8975_MAG_REG_HXL 0x03
#define AK8975_MAG_REG_HXH 0x04 #define AK8975_MAG_REG_HXH 0x04
#define AK8975_MAG_REG_HYL 0x05 #define AK8975_MAG_REG_HYL 0x05
#define AK8975_MAG_REG_HYH 0x06 #define AK8975_MAG_REG_HYH 0x06
#define AK8975_MAG_REG_HZL 0x07 #define AK8975_MAG_REG_HZL 0x07
#define AK8975_MAG_REG_HZH 0x08 #define AK8975_MAG_REG_HZH 0x08
#define AK8975_MAG_REG_STATUS2 0x09 #define AK8975_MAG_REG_ST2 0x09
#define AK8975_MAG_REG_CNTL 0x0a #define AK8975_MAG_REG_CNTL 0x0A
#define AK8975_MAG_REG_ASCT 0x0c // self test #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 ST1_REG_DATA_READY 0x01
#define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
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; 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); 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); 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); 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); delay(10);
// Clear status registers // Clear status registers
i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1);
i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST2, &status, 1);
// Trigger first measurement // 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; 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) static bool ak8975Read(magDev_t *mag, int16_t *magData)
#define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3)
static bool ak8975Read(int16_t *magData)
{ {
bool ack; bool ack;
uint8_t status; uint8_t status;
uint8_t buf[6]; uint8_t buf[6];
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); busDevice_t *busdev = &mag->busdev;
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1);
if (!ack || (status & ST1_REG_DATA_READY) == 0) {
return false; 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) { if (!ack) {
return false; 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; return false;
} }
if (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW) { if (status & ST2_REG_MAG_SENSOR_OVERFLOW) {
return false; return false;
} }
magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4; magData[X] = -parseMag(buf + 0, mag->magGain[X]);
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * 4; magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; 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; return true;
} }
bool ak8975Detect(magDev_t *mag) bool ak8975Detect(magDev_t *mag)
{ {
uint8_t sig = 0; 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; return false;
mag->init = ak8975Init; mag->init = ak8975Init;

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

@ -25,6 +25,7 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/utils.h"
#include "compass.h" #include "compass.h"
#include "compass_fake.h" #include "compass_fake.h"
@ -32,8 +33,10 @@
static int16_t fakeMagData[XYZ_AXIS_COUNT]; static int16_t fakeMagData[XYZ_AXIS_COUNT];
static bool fakeMagInit(void) static bool fakeMagInit(magDev_t *mag)
{ {
UNUSED(mag);
// initially point north // initially point north
fakeMagData[X] = 4096; fakeMagData[X] = 4096;
fakeMagData[Y] = 0; fakeMagData[Y] = 0;
@ -48,8 +51,10 @@ void fakeMagSet(int16_t x, int16_t y, int16_t z)
fakeMagData[Z] = 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[X] = fakeMagData[X];
magData[Y] = fakeMagData[Y]; magData[Y] = fakeMagData[Y];
magData[Z] = fakeMagData[Z]; magData[Z] = fakeMagData[Z];
@ -63,3 +68,4 @@ bool fakeMagDetect(magDev_t *mag)
return true; return true;
} }
#endif // USE_FAKE_MAG #endif // USE_FAKE_MAG

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

@ -22,14 +22,17 @@
#include "platform.h" #include "platform.h"
#ifdef USE_MAG_HMC5883 #if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883)
#include "build/debug.h" #include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h" #include "drivers/exti.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
@ -40,7 +43,6 @@
#include "compass.h" #include "compass.h"
#include "compass_hmc5883l.h" #include "compass_hmc5883l.h"
#include "compass_spi_hmc5883l.h"
//#define DEBUG_MAG_DATA_READY_INTERRUPT //#define DEBUG_MAG_DATA_READY_INTERRUPT
@ -67,10 +69,10 @@
* 1:0 MS1-MS0: Measurement Configuration Bits * 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE * MS1 | MS0 | MODE
* ------------------------------ * ------------------------------
* 0 | 0 | Normal * 0 | 0 | Normal
* 0 | 1 | Positive Bias * 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias * 1 | 0 | Negative Bias
* 1 | 1 | Not Used * 1 | 1 | Not Used
* *
* CTRL_REGB: Control RegisterB * CTRL_REGB: Control RegisterB
* Read Write * Read Write
@ -98,33 +100,39 @@
* 1:0 MD1-MD0: Mode Select Bits * 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE * MS1 | MS0 | MODE
* ------------------------------ * ------------------------------
* 0 | 0 | Continuous-Conversion Mode. * 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode * 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias * 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode * 1 | 1 | Sleep Mode
*/ */
#define MAG_ADDRESS 0x1E #define HMC5883_MAG_I2C_ADDRESS 0x1E
#define MAG_DATA_REGISTER 0x03 #define HMC5883_DEVICE_ID 0x48
#define HMC58X3_R_CONFA 0 #define HMC58X3_REG_CONFA 0x00
#define HMC58X3_R_CONFB 1 #define HMC58X3_REG_CONFB 0x01
#define HMC58X3_R_MODE 2 #define HMC58X3_REG_MODE 0x02
#define HMC58X3_X_SELF_TEST_GAUSS (+1.16f) // X axis level when bias current is applied. #define HMC58X3_REG_DATA 0x03
#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f) // Y axis level when bias current is applied. #define HMC58X3_REG_IDA 0x0A
#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
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 #ifdef USE_MAG_DATA_READY_SIGNAL
static IO_t hmc5883InterruptIO;
static extiCallbackRec_t hmc5883_extiCallbackRec;
static void hmc5883_extiHandler(extiCallbackRec_t* cb) static void hmc5883_extiHandler(extiCallbackRec_t* cb)
{ {
UNUSED(cb); UNUSED(cb);
@ -145,164 +153,182 @@ static void hmc5883_extiHandler(extiCallbackRec_t* cb)
} }
#endif #endif
static void hmc5883lConfigureDataReadyInterruptHandling(void) static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag)
{ {
#ifdef USE_MAG_DATA_READY_SIGNAL #ifdef USE_MAG_DATA_READY_SIGNAL
if (mag->magIntExtiTag == IO_TAG_NONE) {
if (!(hmc5883InterruptIO)) {
return; return;
} }
const IO_t magIntIO = IOGetByTag(mag->magIntExtiTag);
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH #ifdef ENSURE_MAG_DATA_READY_IS_HIGH
uint8_t status = IORead(hmc5883InterruptIO); uint8_t status = IORead(magIntIO);
if (!status) { if (!status) {
return; return;
} }
#endif #endif
EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler); #if defined (STM32F7)
EXTIConfig(hmc5883InterruptIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
EXTIEnable(hmc5883InterruptIO, true); 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 #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]; uint8_t buf[6];
#ifdef USE_MAG_SPI_HMC5883
bool ack = hmc5883SpiReadCommand(MAG_DATA_REGISTER, 6, buf); busDevice_t *busdev = &mag->busdev;
#else
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); bool ack = busReadRegisterBuffer(busdev, HMC58X3_REG_DATA, buf, 6);
#endif
if (!ack) { if (!ack) {
return false; return false;
} }
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values. // 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. // After calibration is done, magGain is set to calculated gain values.
magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X]; magData[X] = parseMag(buf + 0, mag->magGain[X]);
magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z]; magData[Z] = parseMag(buf + 2, mag->magGain[Z]);
magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y]; magData[Y] = parseMag(buf + 4, mag->magGain[Y]);
return true; return true;
} }
static bool hmc5883lInit(void) static bool hmc5883lInit(magDev_t *mag)
{ {
enum {
polPos,
polNeg
};
busDevice_t *busdev = &mag->busdev;
int16_t magADC[3]; int16_t magADC[3];
int i; int i;
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
bool bret = true; // Error indicator bool bret = true; // Error indicator
mag->magGain[X] = 256;
mag->magGain[Y] = 256;
mag->magGain[Z] = 256;
delay(50); 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 busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_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
// Note that the very first measurement after a gain change maintains the same gain as the previous setting. // 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. // 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) busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_2_5GA); // 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
delay(100); delay(100);
hmc5883lRead(magADC);
for (i = 0; i < 10; i++) { // Collect 10 samples hmc5883lRead(mag, magADC);
#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 rather than taking the max. for (int polarity = polPos; polarity <= polNeg; polarity++) {
xyz_total[X] += magADC[X]; switch(polarity) {
xyz_total[Y] += magADC[Y]; case polPos:
xyz_total[Z] += magADC[Z]; busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
break;
// Detect saturation. case polNeg:
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
bret = false; break;
break; // Breaks out of the for loop. No sense in continuing if we saturated. }
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) mag->magGain[X] = (int)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X];
#ifdef USE_MAG_SPI_HMC5883 mag->magGain[Y] = (int)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y];
hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. mag->magGain[Z] = (int)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z];
#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]);
// leave test mode // 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 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
hmc5883SpiWriteCommand(HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
hmc5883SpiWriteCommand(HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // 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
delay(100); delay(100);
if (!bret) { // Something went wrong so get a best guess if (!bret) { // Something went wrong so get a best guess
magGain[X] = 1.0f; mag->magGain[X] = 256;
magGain[Y] = 1.0f; mag->magGain[Y] = 256;
magGain[Z] = 1.0f; mag->magGain[Z] = 256;
} }
hmc5883lConfigureDataReadyInterruptHandling(); hmc5883lConfigureDataReadyInterruptHandling(mag);
return true; return true;
} }
bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag) bool hmc5883lDetect(magDev_t* mag)
{ {
#ifdef USE_MAG_DATA_READY_SIGNAL busDevice_t *busdev = &mag->busdev;
hmc5883InterruptIO = IOGetByTag(interruptTag);
#else
UNUSED(interruptTag);
#endif
uint8_t sig = 0; uint8_t sig = 0;
#ifdef USE_MAG_SPI_HMC5883 #ifdef USE_MAG_SPI_HMC5883
hmc5883SpiInit(); if (busdev->bustype == BUSTYPE_SPI) {
bool ack = hmc5883SpiReadCommand(0x0A, 1, &sig); hmc5883SpiInit(&mag->busdev);
#else }
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
#endif #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; return false;
}
mag->init = hmc5883lInit; mag->init = hmc5883lInit;
mag->read = hmc5883lRead; mag->read = hmc5883lRead;

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

@ -19,4 +19,4 @@
#include "drivers/io_types.h" #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", "SPI_CS",
"MPU_EXTI", "MPU_EXTI",
"BARO_EXTI", "BARO_EXTI",
"COMPASS_EXTI",
"USB", "USB",
"USB_DETECT", "USB_DETECT",
"BEEPER", "BEEPER",

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

@ -52,6 +52,7 @@ typedef enum {
OWNER_SPI_CS, OWNER_SPI_CS,
OWNER_MPU_EXTI, OWNER_MPU_EXTI,
OWNER_BARO_EXTI, OWNER_BARO_EXTI,
OWNER_COMPASS_EXTI,
OWNER_USB, OWNER_USB,
OWNER_USB_DETECT, OWNER_USB_DETECT,
OWNER_BEEPER, OWNER_BEEPER,

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

@ -32,9 +32,10 @@ typedef enum {
CW270_DEG_FLIP = 8 CW270_DEG_FLIP = 8
} sensor_align_e; } 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); 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; struct accDev_s;
typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc); typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc);
typedef bool (*sensorAccReadFuncPtr)(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 #ifdef BARO
{ OWNER_BARO_CS, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_csn), 0 }, { OWNER_BARO_CS, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_csn), 0 },
#endif #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) 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 #endif
#ifdef MAG #ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_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 #endif
#ifdef BARO #ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_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.h"
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "drivers/camera_control.h" #include "drivers/camera_control.h"
#include "drivers/max7456.h" #include "drivers/max7456.h"
@ -249,7 +251,7 @@ static const char * const lookupTableFailsafe[] = {
}; };
static const char * const lookupTableBusType[] = { static const char * const lookupTableBusType[] = {
"NONE", "I2C", "SPI" "NONE", "I2C", "SPI", "SLAVE"
}; };
#ifdef USE_MAX7456 #ifdef USE_MAX7456
@ -346,6 +348,10 @@ const clivalue_t valueTable[] = {
// PG_COMPASS_CONFIG // PG_COMPASS_CONFIG
#ifdef MAG #ifdef MAG
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) }, { "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_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) }, { "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]) }, { "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_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_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_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_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_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) }, { "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.h"
#include "config/parameter_group_ids.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.h"
#include "drivers/compass/compass_ak8975.h" #include "drivers/compass/compass_ak8975.h"
#include "drivers/compass/compass_ak8963.h" #include "drivers/compass/compass_ak8963.h"
@ -54,36 +58,119 @@ mag_t mag; // mag access functions
#define COMPASS_INTERRUPT_TAG IO_TAG_NONE #define COMPASS_INTERRUPT_TAG IO_TAG_NONE
#endif #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, void pgResetFn_compassConfig(compassConfig_t *compassConfig)
.mag_align = ALIGN_DEFAULT, {
// xxx_hardware: 0:default/autodetect, 1: disable compassConfig->mag_align = ALIGN_DEFAULT;
.mag_hardware = 1, compassConfig->mag_declination = 0;
.mag_declination = 0, compassConfig->mag_hardware = MAG_DEFAULT;
.interruptTag = COMPASS_INTERRUPT_TAG
);
#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 int16_t magADCRaw[XYZ_AXIS_COUNT];
static uint8_t magInit = 0; static uint8_t magInit = 0;
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #if !defined(SITL)
{
magSensor_e magHardware;
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; dev->magAlign = ALIGN_DEFAULT;
switch (magHardwareToUse) { switch (compassConfig()->mag_hardware) {
case MAG_DEFAULT: case MAG_DEFAULT:
; // fallthrough ; // fallthrough
case MAG_HMC5883: case MAG_HMC5883:
#ifdef USE_MAG_HMC5883 #if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883)
if (hmc5883lDetect(dev, compassConfig()->interruptTag)) { if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (hmc5883lDetect(dev)) {
#ifdef MAG_HMC5883_ALIGN #ifdef MAG_HMC5883_ALIGN
dev->magAlign = MAG_HMC5883_ALIGN; dev->magAlign = MAG_HMC5883_ALIGN;
#endif #endif
@ -95,6 +182,10 @@ retry:
case MAG_AK8975: case MAG_AK8975:
#ifdef USE_MAG_AK8975 #ifdef USE_MAG_AK8975
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (ak8975Detect(dev)) { if (ak8975Detect(dev)) {
#ifdef MAG_AK8975_ALIGN #ifdef MAG_AK8975_ALIGN
dev->magAlign = MAG_AK8975_ALIGN; dev->magAlign = MAG_AK8975_ALIGN;
@ -106,7 +197,16 @@ retry:
; // fallthrough ; // fallthrough
case MAG_AK8963: 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)) { if (ak8963Detect(dev)) {
#ifdef MAG_AK8963_ALIGN #ifdef MAG_AK8963_ALIGN
dev->magAlign = MAG_AK8963_ALIGN; dev->magAlign = MAG_AK8963_ALIGN;
@ -122,12 +222,6 @@ retry:
break; 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) { if (magHardware == MAG_NONE) {
return false; return false;
} }
@ -136,15 +230,22 @@ retry:
sensorsSet(SENSOR_MAG); sensorsSet(SENSOR_MAG);
return true; return true;
} }
#else
bool compassDetect(magDev_t *dev)
{
UNUSED(dev);
return false;
}
#endif // !SITL
bool compassInit(void) bool compassInit(void)
{ {
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
// calculate magnetic declination // 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. 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)) {
if (!compassDetect(&magDev, compassConfig()->mag_hardware)) {
return false; return false;
} }
@ -152,7 +253,7 @@ bool compassInit(void)
const int16_t min = compassConfig()->mag_declination % 100; const int16_t min = compassConfig()->mag_declination % 100;
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
LED1_ON; LED1_ON;
magDev.init(); magDev.init(&magDev);
LED1_OFF; LED1_OFF;
magInit = 1; magInit = 1;
if (compassConfig()->mag_align != ALIGN_DEFAULT) { 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 magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax; static flightDynamicsTrims_t magZeroTempMax;
magDev.read(magADCRaw); magDev.read(&magDev, magADCRaw);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] = magADCRaw[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. // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
sensor_align_e mag_align; // mag alignment 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_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; ioTag_t interruptTag;
flightDynamicsTrims_t magZero; flightDynamicsTrims_t magZero;
} compassConfig_t; } 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_bmp280.c \
drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_ak8963.c \ drivers/compass/compass_ak8963.c \
drivers/compass/compass_spi_ak8963.c \ drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_spi_hmc5883l.c

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

@ -186,11 +186,7 @@ extern "C" {
void RCC_APB2PeriphClockCmd() {} void RCC_APB2PeriphClockCmd() {}
void delay(uint32_t) {} void delay(uint32_t) {}
void delayMicroseconds(uint32_t) {} void delayMicroseconds(uint32_t) {}
bool i2cWrite(uint8_t, uint8_t, uint8_t) { bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;}
return 1; bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;}
}
bool i2cRead(uint8_t, uint8_t, uint8_t, uint8_t) {
return 1;
}
} }

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

@ -144,11 +144,8 @@ TEST(baroBmp280Test, TestBmp280CalculateZeroP)
extern "C" { extern "C" {
void delay(uint32_t) {} void delay(uint32_t) {}
bool i2cBusReadRegisterBuffer(busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;}
bool i2cBusWriteRegister(busDevice_t*, uint8_t, uint8_t) {return true;} bool busWriteRegister(const 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;}
void spiSetDivisor() { void spiSetDivisor() {
} }

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

@ -146,10 +146,8 @@ extern "C" {
void delay(uint32_t) {} void delay(uint32_t) {}
void delayMicroseconds(uint32_t) {} void delayMicroseconds(uint32_t) {}
bool i2cBusReadRegisterBuffer(busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;}
bool i2cBusWriteRegister(busDevice_t*, uint8_t, uint8_t) {return true;} bool busWriteRegister(const 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;}
void spiSetDivisor() { void spiSetDivisor() {
} }