diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c index 2ef9846753..8e8bcd132d 100644 --- a/src/main/drivers/barometer/barometer_bmp280.c +++ b/src/main/drivers/barometer/barometer_bmp280.c @@ -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 diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c index 7969c70024..f38357bc03 100644 --- a/src/main/drivers/barometer/barometer_ms5611.c +++ b/src/main/drivers/barometer/barometer_ms5611.c @@ -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 diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index f6303cb470..6594993aa9 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -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; - } } diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index de7b143e07..444f4b54d1 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -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; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 2716574634..498534d867 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -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); diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index b0f96ce6a1..78d63e84be 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -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; } diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index 48d0e07728..7b30b68c9b 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -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; } diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 6459c2cd93..88d91091fb 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -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)) {