Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00
Code Issues Wiki Activity

Fix AK8963 SPI mag support

Browse source 
Fix and simplify AK8963/MPU9250 mag gain calculation and switch to 16bit
mode
Fix AK8963 overflow issue (mag will stop updating if an overflow
happens)
AlienFlightNG target updates
This commit is contained in:
MJ666 2017-11-28 21:21:55 +01:00
parent 291cf0eb5a
commit 3d024c6f35
11 changed files with 73 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -65,8 +65,7 @@
#define STATUS1_DATA_READY 0x01
#define STATUS1_DATA_OVERRUN 0x02
#define STATUS2_DATA_ERROR 0x02
#define STATUS2_MAG_SENSOR_OVERFLOW 0x03
#define STATUS2_MAG_SENSOR_OVERFLOW 0x02
#define CNTL_MODE_POWER_DOWN 0x00
#define CNTL_MODE_ONCE 0x01
@ -74,8 +73,10 @@
#define CNTL_MODE_CONT2 0x06
#define CNTL_MODE_SELF_TEST 0x08
#define CNTL_MODE_FUSE_ROM 0x0F
#define CNTL_BIT_14_BIT 0x00
#define CNTL_BIT_16_BIT 0x10
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
static int16_t magGain[3];
static bool ak8963Init(magDev_t * mag)
{
@ -93,9 +94,9 @@ static bool ak8963Init(magDev_t * mag)
ack = busReadBuf(mag->busDev, 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);
magGain[X] = calibration[X] + 128;
magGain[Y] = calibration[Y] + 128;
magGain[Z] = calibration[Z] + 128;
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
delay(10);
@ -104,15 +105,19 @@ static bool ak8963Init(magDev_t * mag)
ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS1, &status);
ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS2, &status);
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
return true;
}
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)
{
bool ack = false;
bool readResult = false;
uint8_t buf[7];
ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS1, &buf[0]);
@ -123,17 +128,21 @@ static bool ak8963Read(magDev_t * mag)
ack = busReadBuf(mag->busDev, AK8963_MAG_REG_HXL, &buf[0], 7);
if (!ack || (buf[6] & STATUS2_DATA_ERROR) || (buf[6] & STATUS2_MAG_SENSOR_OVERFLOW)) {
if (!ack) {
return false;
}
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * magGain[X];
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
if (buf[6] & STATUS2_MAG_SENSOR_OVERFLOW) {
return false;
}
return readResult;
mag->magADCRaw[X] = -parseMag(buf + 0, magGain[X]);
mag->magADCRaw[Y] = -parseMag(buf + 2, magGain[Y]);
mag->magADCRaw[Z] = -parseMag(buf + 4, magGain[Z]);
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
@ -155,7 +164,7 @@ static bool deviceDetect(magDev_t * mag)
bool ak8963Detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_AK8963, 0, OWNER_COMPASS);
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_AK8963, 0, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}

28
src/main/drivers/compass/compass_mpu9250.c
View file

@ -70,8 +70,7 @@
#define STATUS1_DATA_READY 0x01
#define STATUS1_DATA_OVERRUN 0x02
#define STATUS2_DATA_ERROR 0x02
#define STATUS2_MAG_SENSOR_OVERFLOW 0x03
#define STATUS2_MAG_SENSOR_OVERFLOW 0x02
#define CNTL_MODE_POWER_DOWN 0x00
#define CNTL_MODE_ONCE 0x01
@ -79,6 +78,8 @@
#define CNTL_MODE_CONT2 0x06
#define CNTL_MODE_SELF_TEST 0x08
#define CNTL_MODE_FUSE_ROM 0x0F
#define CNTL_BIT_14_BIT 0x00
#define CNTL_BIT_16_BIT 0x10
#define DETECTION_MAX_RETRY_COUNT 5
@ -95,7 +96,7 @@ typedef struct {
timeUs_t readStartedAt; // time read was queued in micros.
} mpu9250CompassReadContext_s;
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
static int16_t magGain[3];
static mpu9250CompassReadContext_s ctx;
static bool lastReadResult = false;
static int16_t cachedMagData[3];
@ -184,9 +185,9 @@ static bool mpu9250CompassInit(magDev_t * mag)
mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, 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);
magGain[X] = calibration[X] + 128;
magGain[Y] = calibration[Y] + 128;
magGain[Z] = calibration[Z] + 128;
mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
delay(10);
@ -196,12 +197,17 @@ static bool mpu9250CompassInit(magDev_t * mag)
mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, &status, 1);
// Trigger first measurement
mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1);
mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_CONT1);
busSetSpeed(mag->busDev, BUS_SPEED_FAST);
return true;
}
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 mpu9250CompassRead(magDev_t * mag)
{
bool ack = false;
@ -257,16 +263,16 @@ static bool mpu9250CompassRead(magDev_t * mag)
} while(reprocess);
uint8_t status2 = buf[6];
if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
if (!ack || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
ctx.state = CHECK_STATUS;
debug[1]++;
lastReadResult = false;
return lastReadResult;
}
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * magGain[X];
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
mag->magADCRaw[X] = -parseMag(buf + 0, magGain[X]);
mag->magADCRaw[Y] = -parseMag(buf + 2, magGain[Y]);
mag->magADCRaw[Z] = -parseMag(buf + 4, magGain[Z]);
memcpy(cachedMagData, &mag->magADCRaw, sizeof(cachedMagData));
ctx.state = CHECK_STATUS;

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

@ -21,7 +21,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER",
"RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
"BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
"SOFTSPI", "NRF24", "VTX", "SPI_PREINIT"
"SOFTSPI", "NRF24", "VTX", "SPI_PREINIT", "COMPASS"
};
const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {

2
src/main/target/ALIENFLIGHTF3/hardware_revision.c
View file

@ -39,7 +39,7 @@ void detectHardwareRevision(void)
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
// Check hardware revision
delayMicroseconds(10); // allow configuration to settle
delayMicroseconds(40); // allow configuration to settle
if (IORead(HWDetectPin)) {
hardwareRevision = AFF3_REV_1;

5
src/main/target/ALIENFLIGHTF3/target.h
View file

@ -46,6 +46,9 @@
#define GYRO_MPU6050_ALIGN CW270_DEG
#define USE_GYRO_MPU9250
#define GYRO_MPU6500_ALIGN CW270_DEG
#define MPU6500_CS_PIN PA15
#define MPU6500_SPI_BUS BUS_SPI3
#define GYRO_MPU9250_ALIGN CW270_DEG
#define MPU9250_CS_PIN PA15
#define MPU9250_SPI_BUS BUS_SPI3
@ -53,6 +56,8 @@
#define ACC
#define USE_ACC_MPU6050
#define ACC_MPU6050_ALIGN CW270_DEG
#define USE_ACC_MPU6500
#define ACC_MPU6500_ALIGN CW270_DEG
#define USE_ACC_MPU9250
#define ACC_MPU9250_ALIGN CW270_DEG

2
src/main/target/ALIENFLIGHTF4/hardware_revision.c
View file

@ -38,7 +38,7 @@ void detectHardwareRevision(void)
IOInit(HWDetectPin, OWNER_SYSTEM, RESOURCE_INPUT, 0);
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
delayMicroseconds(10); // allow configuration to settle
delayMicroseconds(40); // allow configuration to settle
// Check hardware revision
if (IORead(HWDetectPin)) {

5
src/main/target/ALIENFLIGHTF4/target.h
View file

@ -64,8 +64,7 @@
#define USE_MAG_MPU9250
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW180_DEG
#define MAG_AK8963_ALIGN CW270_DEG
#define MAG_MPU9250_ALIGN CW0_DEG_FLIP
#define BARO
#define BARO_I2C_BUS BUS_I2C1
@ -74,7 +73,7 @@
#define USE_SDCARD
//#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_PIN PB11
#define SDCARD_DETECT_EXTI_LINE EXTI_Line10

2
src/main/target/ALIENFLIGHTNGF7/hardware_revision.c
View file

@ -38,7 +38,7 @@ void detectHardwareRevision(void)
IOInit(HWDetectPin, OWNER_SYSTEM, RESOURCE_INPUT, 0);
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
delayMicroseconds(10); // allow configuration to settle
delayMicroseconds(40); // allow configuration to settle
// Check hardware revision
if (IORead(HWDetectPin)) {

10
src/main/target/ALIENFLIGHTNGF7/target.h
View file

@ -57,12 +57,16 @@
#define MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_AK8963
#define USE_MAG_MPU9250
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW180_DEG
#define MAG_MPU9250_ALIGN CW270_DEG
#define MAG_AK9863_ALIGN CW0_DEG_FLIP
#define MAG_MPU9250_ALIGN CW0_DEG_FLIP
#define AK8963_CS_PIN PC15
#define AK8963_SPI_BUS BUS_SPI3
#define BARO
#define BARO_I2C_BUS BUS_I2C1
@ -77,7 +81,7 @@
#define USE_SDCARD
//#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_PIN PB11
#define SDCARD_DETECT_EXTI_LINE EXTI_Line10

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

@ -6,6 +6,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu9250.c \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms56xx.c \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_mpu9250.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \

11
src/main/target/common_hardware.c
View file

@ -111,6 +111,17 @@
BUSDEV_REGISTER_I2C(busdev_qmc5883, DEVHW_QMC5883, QMC5883_I2C_BUS, 0x0D, NONE, DEVFLAGS_NONE);
#endif
#if defined(USE_MAG_AK8963)
#if defined(AK8963_SPI_BUS)
BUSDEV_REGISTER_SPI(busdev_ak8963, DEVHW_AK8963, AK8963_SPI_BUS, AK8963_CS_PIN, NONE, DEVFLAGS_NONE);
#elif defined(AK8963_I2C_BUS) || defined(MAG_I2C_BUS)
#if !defined(AK8963_I2C_BUS)
#define AK8963_I2C_BUS MAG_I2C_BUS
#endif
BUSDEV_REGISTER_I2C(busdev_ak8963, DEVHW_AK8963, AK8963_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE);
#endif
#endif
#if defined(USE_MAG_MAG3110)
#if !defined(MAG3110_I2C_BUS)
#define MAG3110_I2C_BUS MAG_I2C_BUS