AK8963 SPI compass support

General AK8963 driver cleanup

src/main/drivers/compass/compass_ak8963.c

@@ -43,47 +43,7 @@
 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
 #include "drivers/compass/compass_ak8963.h"
-
+#include "drivers/compass/compass_spi_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
 
 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[X] = ((((float)(int8_t)calibration[X] - 128) / 256) + 1) * 30;
-    magGain[Y] = (((((float)(int8_t)calibration[Y] - 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[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_ST1, 1, &status);
-    ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 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;

src/main/drivers/compass/compass_ak8963.h

@@ -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);

src/main/drivers/compass/compass_spi_ak8963.c

@@ -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

src/main/drivers/compass/compass_spi_ak8963.h

@@ -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);

src/main/target/ALIENFLIGHTNGF7/target.h

@@ -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
 

src/main/target/ALIENFLIGHTNGF7/target.mk

@@ -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