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

Configurable compass drivers based on jflyper's PR #3613

Browse source 
Eliminate static variables in mag drivers
AK8975 and HMC5883l driver cleanup
replace magic numbers with definitions
Switched AK8963/Ak8975 to 16 bit mode
Update Mag gain calculations to use integer varables (ledvinap
recomendation)
Update interrupt handling for HMC5883L
Reschedule compass task only if slave mode is realy active
Change bustye definitions to an enumeration set
Fix dispatch functions and remove redundant dispatch functions from Baro
and Mag drivers 
Fix unittest
This commit is contained in:
Michael Jakob 2017-10-06 07:43:02 +02:00
parent 316845227d
commit 60b8e0f05e
28 changed files with 721 additions and 725 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];

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

@ -65,36 +65,6 @@ 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
@ -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));
} }

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

@ -61,36 +61,6 @@ STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement
STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM STATIC_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
@ -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)

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

@ -26,33 +26,60 @@
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)
{ {
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(reg);
#endif
switch (busdev->bustype) { switch (busdev->bustype) {
#ifdef USE_SPI
case BUSTYPE_SPI: case BUSTYPE_SPI:
return spiBusReadRegister(busdev, reg & 0x7f); return spiBusReadRegister(busdev, reg & 0x7f);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C: case BUSTYPE_I2C:
return i2cBusReadRegister(busdev, reg); return i2cBusReadRegister(busdev, reg);
#endif
default:
return false;
} }
return false;
} }

16
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_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,13 @@ typedef struct busDevice_s {
IO_t csnPin; IO_t csnPin;
} spi; } spi;
struct deviceI2C_s { struct deviceI2C_s {
const struct busDevice_s *master;
I2CDevice device; I2CDevice device;
uint8_t address; uint8_t address;
} i2c; } i2c;
} 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

403
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.i2c.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.i2c.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
delay(4); 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.i2c.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.i2c.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.i2c.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.i2c.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
queuedRead.readStartedAt = micros(); queuedRead.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.i2c.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,221 @@ 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_SLAVE) {
return ak8963SlaveReadRegisterBuffer(busdev, reg, buf, len);
}
#endif
return busReadRegisterBuffer(busdev, reg, buf, len);
}
static bool ak8963WriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
{
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
if (busdev->bustype == BUSTYPE_SLAVE) {
return ak8963SlaveWriteRegister(busdev, reg, data);
}
#endif
return busWriteRegister(busdev, reg, data);
}
static bool ak8963ReadData(const busDevice_t *busdev, uint8_t *buf)
{
uint8_t status;
bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1);
if (!ack || (status & ST1_DATA_READY) == 0) { 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 bool ak8963Read(magDev_t *magdev, int16_t *magData)
{
bool ack = false;
uint8_t buf[7];
const busDevice_t *busdev = &magdev->busdev;
switch (busdev->bustype) {
#if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963)
case BUSTYPE_I2C:
case BUSTYPE_SPI:
ack = ak8963ReadData(busdev, buf);
break;
#endif #endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_SLAVE:
ack = ak8963SlaveReadData(busdev, buf);
break;
#endif
default:
break;
}
uint8_t status2 = buf[6]; 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] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256;
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256;
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256;
return true;
}
static bool ak8963Init(magDev_t *magdev)
{
uint8_t asa[3];
uint8_t status;
const busDevice_t *busdev = &magdev->busdev;
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode
ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values
magdev->magGain[X] = asa[X] + 128;
magdev->magGain[Y] = asa[Y] + 128;
magdev->magGain[Z] = asa[Z] + 128;
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading.
// Clear status registers
ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1);
ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST2, &status, 1);
// Trigger first measurement
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE);
return true;
}
void ak8963BusInit(const busDevice_t *busdev)
{
switch (busdev->bustype) {
#ifdef USE_MAG_AK8963
case BUSTYPE_I2C:
UNUSED(busdev);
break;
#endif #endif
return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL1, CNTL1_MODE_ONCE); // start reading again
#ifdef USE_MAG_SPI_AK8963
case BUSTYPE_SPI:
IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
case BUSTYPE_SLAVE:
rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
// initialze I2C master via SPI bus
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
break;
#endif
default:
break;
}
}
void ak8963BusDeInit(const busDevice_t *busdev)
{
switch (busdev->bustype) {
#ifdef USE_MAG_AK8963
case BUSTYPE_I2C:
UNUSED(busdev);
break;
#endif
#ifdef USE_MAG_SPI_AK8963
case BUSTYPE_SPI:
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU);
IORelease(busdev->busdev_u.spi.csnPin);
IOInit(busdev->busdev_u.spi.csnPin, OWNER_SPI_PREINIT, 0);
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_SLAVE:
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);
break;
#endif
default:
break;
}
} }
bool ak8963Detect(magDev_t *mag) 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_SLAVE) && busdev->busdev_u.i2c.address == 0) {
if (ak8963SpiDetect(mag)) return true; busdev->busdev_u.i2c.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);

115
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,124 @@
#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 *magdev)
{ {
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 = &magdev->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. magdev->magGain[X] = asa[X] + 128;
magdev->magGain[Y] = asa[Y] + 128;
magdev->magGain[Z] = asa[Z] + 128;
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
delay(10); 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 bool ak8975Read(magDev_t *magdev, int16_t *magData)
#define BIT_STATUS2_REG_DATA_ERROR (1 << 2)
#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 = &magdev->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] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256;
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * 4; magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256;
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256;
i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
return true; 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

245
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,80 +153,121 @@ 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));
#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
/*
* XXX Fixme
* HMC5983 datasheet states SPI mode is
* SCK is high when CS is high
* Data is sampled at the rising edge of SCK
* However, it seems that SCK condition is ignored (works with SCK low when CS is high).
* Nevertheless, it is nice to conform to the datasheet specification when per device SPI mode
* is implemented.
*/
static void hmc5883SpiInit(busDevice_t *busdev)
{
static bool hardwareInitialised = false;
if (hardwareInitialised) {
return;
}
IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
hardwareInitialised = true;
}
#endif
static bool hmc5883lRead(magDev_t *magdev, int16_t *magData)
{ {
uint8_t buf[6]; uint8_t buf[6];
#ifdef USE_MAG_SPI_HMC5883
bool ack = hmc5883SpiReadCommand(MAG_DATA_REGISTER, 6, buf); busDevice_t *busdev = &magdev->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] = (int16_t)(buf[0] << 8 | buf[1]) * magdev->magGain[X] / 256;
magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z]; magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magdev->magGain[Z] / 256;
magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y]; magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magdev->magGain[Y] / 256;
return true; return true;
} }
static bool hmc5883lInit(void) static bool hmc5883lInit(magDev_t *magdev)
{ {
busDevice_t *busdev = &magdev->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
magdev->magGain[X] = 256;
magdev->magGain[Y] = 256;
magdev->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);
hmc5883lRead(magdev, magADC);
for (i = 0; i < 10; i++) { // Collect 10 samples for (i = 0; i < 10; i++) { // Collect 10 samples
#ifdef USE_MAG_SPI_HMC5883 busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE);
hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); delay(20);
#else hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed.
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. // Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += magADC[X]; xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y]; xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z]; xyz_total[Z] += magADC[Z];
@ -232,21 +281,16 @@ static bool hmc5883lInit(void)
} }
// Apply the negative bias. (Same gain) // 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. busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_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++) { for (i = 0; i < 10; i++) {
#ifdef USE_MAG_SPI_HMC5883 busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE);
hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); delay(20);
#else hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed.
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. // Since the measurements are noisy, they should be averaged.
xyz_total[X] -= magADC[X]; xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y]; xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z]; xyz_total[Z] -= magADC[Z];
@ -259,50 +303,51 @@ static bool hmc5883lInit(void)
LED1_TOGGLE; LED1_TOGGLE;
} }
magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]); magdev->magGain[X] = ABS((int32_t)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X]);
magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]); magdev->magGain[Y] = ABS((int32_t)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y]);
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); magdev->magGain[Z] = ABS((int32_t)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z]);
// leave test mode // 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; magdev->magGain[X] = 256;
magGain[Y] = 1.0f; magdev->magGain[Y] = 256;
magGain[Z] = 1.0f; magdev->magGain[Z] = 256;
} }
hmc5883lConfigureDataReadyInterruptHandling(); hmc5883lConfigureDataReadyInterruptHandling(magdev);
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_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_SLAVE:
{
if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
busdev->bustype = BUSTYPE_SLAVE;
busdev->busdev_u.i2c.master = gyroSensorBus();
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
} else {
return false;
}
}
#endif
break;
default:
return false;
}
dev->magAlign = ALIGN_DEFAULT; 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_SLAVE;
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
dev->busdev.busdev_u.i2c.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() {
} }