rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3215 from AlienWiiBF/AK8963_SPI 

AK8963 SPI compass support
This commit is contained in:
Martin Budden 2017-07-24 07:06:10 +01:00 committed by GitHub
parent 6a8678aceb 04d3331ed5
commit bc9ef72663
6 changed files with 213 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
src/main/drivers/compass/compass_ak8963.c
View File

@ -43,47 +43,7 @@
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/compass/compass_ak8963.h"
// This sensor is available in MPU-9250.
// AK8963, mag sensor address
#define AK8963_MAG_I2C_ADDRESS 0x0C
#define AK8963_Device_ID 0x48
// Registers
#define AK8963_MAG_REG_WHO_AM_I 0x00
#define AK8963_MAG_REG_INFO 0x01
#define AK8963_MAG_REG_STATUS1 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_STATUS2 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_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 STATUS1_DATA_READY 0x01
#define STATUS1_DATA_OVERRUN 0x02
#define STATUS2_DATA_ERROR 0x02
#define STATUS2_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 CNTL2_SOFT_RESET 0x01
#include "drivers/compass/compass_spi_ak8963.h"
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
@ -201,15 +161,15 @@ static bool ak8963Init()
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);
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_STATUS1, 1, &status);
ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status);
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);
@ -233,7 +193,7 @@ static bool ak8963Read(int16_t *magData)
restart:
switch (state) {
case CHECK_STATUS:
ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1);
ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ST1, 1);
state++;
return false;
@ -247,7 +207,7 @@ restart:
uint8_t status = buf[0];
if (!ack || (status & STATUS1_DATA_READY) == 0) {
if (!ack || (status & ST1_DATA_READY) == 0) {
// too early. queue the status read again
state = CHECK_STATUS;
if (retry) {
@ -276,18 +236,18 @@ restart:
}
}
#else
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &buf[0]);
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ST1, 1, &buf[0]);
uint8_t status = buf[0];
if (!ack || (status & STATUS1_DATA_READY) == 0) {
if (!ack || (status & ST1_DATA_READY) == 0) {
return false;
}
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]);
#endif
uint8_t status2 = buf[6];
if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
if (!ack || (status2 & ST2_DATA_ERROR) || (status2 & ST2_MAG_SENSOR_OVERFLOW)) {
return false;
}
@ -305,6 +265,14 @@ bool ak8963Detect(magDev_t *mag)
{
uint8_t sig = 0;
#if defined(USE_SPI) && defined(AK8963_SPI_INSTANCE)
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 (ak8963SpiDetect(mag)) return true;
#endif
#if defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE)
bus = &mag->bus;
#if defined(MPU6500_SPI_INSTANCE)
@ -322,7 +290,7 @@ bool ak8963Detect(magDev_t *mag)
ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG
delay(4);
bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig); // check for AK8963
bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WIA, 1, &sig); // check for AK8963
if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H'
{
mag->init = ak8963Init;

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

@ -17,4 +17,50 @@
#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);

122
src/main/drivers/compass/compass_spi_ak8963.c Normal file
View File

@ -0,0 +1,122 @@
/*
* 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 Normal file
View File

@ -0,0 +1,20 @@
/*
* 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);

4
src/main/target/ALIENFLIGHTNGF7/target.h
View File

@ -53,10 +53,14 @@
#define USE_MAG_HMC5883
#define USE_MAG_SPI_HMC5883
#define USE_MAG_AK8963
#define USE_MAG_SPI_AK8963
#define HMC5883_CS_PIN PC15
#define HMC5883_SPI_INSTANCE SPI3
#define AK8963_CS_PIN PC15
#define AK8963_SPI_INSTANCE SPI3
#define MAG_HMC5883_ALIGN CW180_DEG
#define MAG_AK8963_ALIGN CW270_DEG

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

@ -7,5 +7,6 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_spi_ak8963.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_spi_hmc5883l.c