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

Updates according to ledvinap's recomendations

Browse source
This commit is contained in:
Michael Jakob 2017-10-17 21:04:06 +02:00
parent 60b8e0f05e
commit ae8256f142
8 changed files with 130 additions and 142 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -69,9 +69,9 @@ void bmp280BusInit(busDevice_t *busdev)
{
#ifdef USE_BARO_SPI_BMP280
if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX
}
#else

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

@ -65,9 +65,9 @@ void ms5611BusInit(busDevice_t *busdev)
{
#ifdef USE_BARO_SPI_MS5611
if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX
}
#else

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

@ -68,18 +68,12 @@ bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data
uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(busdev);
UNUSED(reg);
return false;
#else
uint8_t data;
busReadRegisterBuffer(busdev, reg, &data, 1);
return data;
#endif
switch (busdev->bustype) {
#ifdef USE_SPI
case BUSTYPE_SPI:
return spiBusReadRegister(busdev, reg & 0x7f);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C:
return i2cBusReadRegister(busdev, reg);
#endif
default:
return false;
}
}

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

@ -26,7 +26,7 @@ typedef enum {
BUSTYPE_NONE = 0,
BUSTYPE_I2C,
BUSTYPE_SPI,
BUSTYPE_SLAVE // Slave I2C on SPI master
BUSTYPE_MPU_SLAVE // Slave I2C on SPI master
} busType_e;
typedef struct busDevice_s {
@ -40,10 +40,13 @@ typedef struct busDevice_s {
IO_t csnPin;
} spi;
struct deviceI2C_s {
const struct busDevice_s *master;
I2CDevice device;
uint8_t address;
I2CDevice device;
uint8_t address;
} i2c;
struct deviceMpuSlave_s {
const struct busDevice_s *master;
uint8_t address;
} mpuSlave;
} busdev_u;
} busDevice_t;

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

@ -106,9 +106,9 @@ static bool ak8963SpiWriteRegisterDelay(const busDevice_t *bus, uint8_t reg, uin
static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t reg, uint8_t *buf, uint8_t len)
{
const busDevice_t *bus = slavedev->busdev_u.i2c.master;
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
delay(4);
@ -120,9 +120,9 @@ static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t r
static bool ak8963SlaveWriteRegister(const busDevice_t *slavedev, uint8_t reg, uint8_t data)
{
const busDevice_t *bus = slavedev->busdev_u.i2c.master;
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address); // set I2C slave address for write
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address); // set I2C slave address for write
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C sLave value
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN); // write 1 byte
@ -143,11 +143,11 @@ static bool ak8963SlaveStartRead(const busDevice_t *slavedev, uint8_t reg, uint8
return false;
}
const busDevice_t *bus = slavedev->busdev_u.i2c.master;
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
queuedRead.len = len;
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.i2c.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
@ -178,7 +178,7 @@ static bool ak8963SlaveCompleteRead(const busDevice_t *slavedev, uint8_t *buf)
{
uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining();
const busDevice_t *bus = slavedev->busdev_u.i2c.master;
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
if (timeRemaining > 0) {
delayMicroseconds(timeRemaining);
@ -260,7 +260,7 @@ restart:
static bool ak8963ReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *buf, uint8_t len)
{
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
if (busdev->bustype == BUSTYPE_SLAVE) {
if (busdev->bustype == BUSTYPE_MPU_SLAVE) {
return ak8963SlaveReadRegisterBuffer(busdev, reg, buf, len);
}
#endif
@ -270,14 +270,14 @@ static bool ak8963ReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uin
static bool ak8963WriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
{
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
if (busdev->bustype == BUSTYPE_SLAVE) {
if (busdev->bustype == BUSTYPE_MPU_SLAVE) {
return ak8963SlaveWriteRegister(busdev, reg, data);
}
#endif
return busWriteRegister(busdev, reg, data);
}
static bool ak8963ReadData(const busDevice_t *busdev, uint8_t *buf)
static bool ak8963DirectReadData(const busDevice_t *busdev, uint8_t *buf)
{
uint8_t status;
@ -290,23 +290,28 @@ static bool ak8963ReadData(const busDevice_t *busdev, uint8_t *buf)
return ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_HXL, buf, 7);
}
static bool ak8963Read(magDev_t *magdev, int16_t *magData)
static int16_t parseMag(uint8_t *raw, int16_t gain) {
int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
return constrain(ret, INT16_MIN, INT16_MAX);
}
static bool ak8963Read(magDev_t *mag, int16_t *magData)
{
bool ack = false;
uint8_t buf[7];
const busDevice_t *busdev = &magdev->busdev;
const busDevice_t *busdev = &mag->busdev;
switch (busdev->bustype) {
#if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963)
case BUSTYPE_I2C:
case BUSTYPE_SPI:
ack = ak8963ReadData(busdev, buf);
ack = ak8963DirectReadData(busdev, buf);
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_SLAVE:
case BUSTYPE_MPU_SLAVE:
ack = ak8963SlaveReadData(busdev, buf);
break;
#endif
@ -325,27 +330,27 @@ static bool ak8963Read(magDev_t *magdev, int16_t *magData)
return false;
}
magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256;
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256;
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256;
magData[X] = -parseMag(buf + 0, mag->magGain[X]);
magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
return true;
}
static bool ak8963Init(magDev_t *magdev)
static bool ak8963Init(magDev_t *mag)
{
uint8_t asa[3];
uint8_t status;
const busDevice_t *busdev = &magdev->busdev;
const busDevice_t *busdev = &mag->busdev;
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode
ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values
magdev->magGain[X] = asa[X] + 128;
magdev->magGain[Y] = asa[Y] + 128;
magdev->magGain[Z] = asa[Z] + 128;
mag->magGain[X] = asa[X] + 128;
mag->magGain[Y] = asa[Y] + 128;
mag->magGain[Z] = asa[Z] + 128;
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading.
@ -369,22 +374,22 @@ void ak8963BusInit(const busDevice_t *busdev)
#ifdef USE_MAG_SPI_AK8963
case BUSTYPE_SPI:
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
case BUSTYPE_SLAVE:
case BUSTYPE_MPU_SLAVE:
rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
// initialze I2C master via SPI bus
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
break;
#endif
default:
@ -410,8 +415,8 @@ void ak8963BusDeInit(const busDevice_t *busdev)
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_SLAVE:
ak8963SpiWriteRegisterDelay(busdev->busdev_u.i2c.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);
case BUSTYPE_MPU_SLAVE:
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);
break;
#endif
default:
@ -425,8 +430,8 @@ bool ak8963Detect(magDev_t *mag)
busDevice_t *busdev = &mag->busdev;
if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_SLAVE) && busdev->busdev_u.i2c.address == 0) {
busdev->busdev_u.i2c.address = AK8963_MAG_I2C_ADDRESS;
if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_MPU_SLAVE) && busdev->busdev_u.mpuSlave.address == 0) {
busdev->busdev_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS;
}
ak8963BusInit(busdev);

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

@ -74,12 +74,12 @@
#define CNTL_BIT_14_BIT 0x00
#define CNTL_BIT_16_BIT 0x10
static bool ak8975Init(magDev_t *magdev)
static bool ak8975Init(magDev_t *mag)
{
uint8_t asa[3];
uint8_t status;
busDevice_t *busdev = &magdev->busdev;
busDevice_t *busdev = &mag->busdev;
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
delay(20);
@ -90,9 +90,9 @@ static bool ak8975Init(magDev_t *magdev)
busReadRegisterBuffer(busdev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values
delay(10);
magdev->magGain[X] = asa[X] + 128;
magdev->magGain[Y] = asa[Y] + 128;
magdev->magGain[Z] = asa[Z] + 128;
mag->magGain[X] = asa[X] + 128;
mag->magGain[Y] = asa[Y] + 128;
mag->magGain[Z] = asa[Z] + 128;
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
delay(10);
@ -106,13 +106,18 @@ static bool ak8975Init(magDev_t *magdev)
return true;
}
static bool ak8975Read(magDev_t *magdev, int16_t *magData)
static int16_t parseMag(uint8_t *raw, int16_t gain) {
int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
return constrain(ret, INT16_MIN, INT16_MAX);
}
static bool ak8975Read(magDev_t *mag, int16_t *magData)
{
bool ack;
uint8_t status;
uint8_t buf[6];
busDevice_t *busdev = &magdev->busdev;
busDevice_t *busdev = &mag->busdev;
ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1);
if (!ack || (status & ST1_REG_DATA_READY) == 0) {
@ -136,9 +141,9 @@ static bool ak8975Read(magDev_t *magdev, int16_t *magData)
return false;
}
magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magdev->magGain[X] / 256;
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magdev->magGain[Y] / 256;
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magdev->magGain[Z] / 256;
magData[X] = -parseMag(buf + 0, mag->magGain[X]);
magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
return true;
}

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

@ -173,6 +173,7 @@ static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag)
IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
EXTIHandlerInit(&mag->exti, hmc5883_extiHandler);
EXTIConfig(magIntIO, &gmag->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL));
EXTIEnable(magIntIO, true);
#else
IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
IOConfigGPIO(magIntIO, IOCFG_IN_FLOATING);
@ -186,39 +187,27 @@ static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag)
}
#ifdef USE_MAG_SPI_HMC5883
/*
* XXX Fixme
* HMC5983 datasheet states SPI mode is
* SCK is high when CS is high
* Data is sampled at the rising edge of SCK
* However, it seems that SCK condition is ignored (works with SCK low when CS is high).
* Nevertheless, it is nice to conform to the datasheet specification when per device SPI mode
* is implemented.
*/
static void hmc5883SpiInit(busDevice_t *busdev)
{
static bool hardwareInitialised = false;
if (hardwareInitialised) {
return;
}
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
hardwareInitialised = true;
}
#endif
static bool hmc5883lRead(magDev_t *magdev, int16_t *magData)
static int16_t parseMag(uint8_t *raw, int16_t gain) {
int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
return constrain(ret, INT16_MIN, INT16_MAX);
}
static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
{
uint8_t buf[6];
busDevice_t *busdev = &magdev->busdev;
busDevice_t *busdev = &mag->busdev;
bool ack = busReadRegisterBuffer(busdev, HMC58X3_REG_DATA, buf, 6);
@ -228,25 +217,30 @@ static bool hmc5883lRead(magDev_t *magdev, int16_t *magData)
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magdev->magGain[X] / 256;
magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magdev->magGain[Z] / 256;
magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magdev->magGain[Y] / 256;
magData[X] = parseMag(buf + 0, mag->magGain[X]);
magData[Z] = parseMag(buf + 2, mag->magGain[Z]);
magData[Y] = parseMag(buf + 4, mag->magGain[Y]);
return true;
}
static bool hmc5883lInit(magDev_t *magdev)
static bool hmc5883lInit(magDev_t *mag)
{
busDevice_t *busdev = &magdev->busdev;
enum {
polPos,
polNeg
};
busDevice_t *busdev = &mag->busdev;
int16_t magADC[3];
int i;
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
bool bret = true; // Error indicator
magdev->magGain[X] = 256;
magdev->magGain[Y] = 256;
magdev->magGain[Z] = 256;
mag->magGain[X] = 256;
mag->magGain[Y] = 256;
mag->magGain[Z] = 256;
delay(50);
@ -259,53 +253,40 @@ static bool hmc5883lInit(magDev_t *magdev)
delay(100);
hmc5883lRead(magdev, magADC);
hmc5883lRead(mag, magADC);
for (i = 0; i < 10; i++) { // Collect 10 samples
busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE);
delay(20);
hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
for (int polarity = polPos; polarity <= polNeg; polarity++) {
switch(polarity) {
case polPos:
busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
break;
case polNeg:
busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
break;
}
for (i = 0; i < 10; i++) { // Collect 10 samples
busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE);
delay(20);
hmc5883lRead(mag, magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += ((polarity == polPos) ? 1 : -1) * magADC[X];
xyz_total[Y] += ((polarity == polPos) ? 1 : -1) * magADC[Y];
xyz_total[Z] += ((polarity == polPos) ? 1 : -1) * magADC[Z];
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
LED1_TOGGLE;
}
// Apply the negative bias. (Same gain)
busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE);
delay(20);
hmc5883lRead(magdev, magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged.
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
magdev->magGain[X] = ABS((int32_t)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X]);
magdev->magGain[Y] = ABS((int32_t)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y]);
magdev->magGain[Z] = ABS((int32_t)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z]);
mag->magGain[X] = (int)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X];
mag->magGain[Y] = (int)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y];
mag->magGain[Z] = (int)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z];
// leave test mode
@ -316,12 +297,12 @@ static bool hmc5883lInit(magDev_t *magdev)
delay(100);
if (!bret) { // Something went wrong so get a best guess
magdev->magGain[X] = 256;
magdev->magGain[Y] = 256;
magdev->magGain[Z] = 256;
mag->magGain[X] = 256;
mag->magGain[Y] = 256;
mag->magGain[Z] = 256;
}
hmc5883lConfigureDataReadyInterruptHandling(magdev);
hmc5883lConfigureDataReadyInterruptHandling(mag);
return true;
}

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

@ -90,7 +90,7 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
compassConfig->mag_spi_csn = IO_TAG_NONE;
#elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
compassConfig->mag_bustype = BUSTYPE_SLAVE;
compassConfig->mag_bustype = BUSTYPE_MPU_SLAVE;
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
compassConfig->mag_i2c_address = 0;
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
@ -141,12 +141,12 @@ bool compassDetect(magDev_t *dev)
break;
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_SLAVE:
case BUSTYPE_MPU_SLAVE:
{
if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
busdev->bustype = BUSTYPE_SLAVE;
busdev->busdev_u.i2c.master = gyroSensorBus();
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
busdev->bustype = BUSTYPE_MPU_SLAVE;
busdev->busdev_u.mpuSlave.master = gyroSensorBus();
busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
} else {
return false;
}
@ -202,9 +202,9 @@ bool compassDetect(magDev_t *dev)
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
dev->busdev.bustype = BUSTYPE_SLAVE;
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
dev->busdev.busdev_u.i2c.master = gyroSensorBus();
dev->busdev.bustype = BUSTYPE_MPU_SLAVE;
busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
dev->busdev.busdev_u.mpuSlave.master = gyroSensorBus();
}
if (ak8963Detect(dev)) {