MAG: Support for Freescale MAG3110 compass chip

Makefile

@@ -318,6 +318,7 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
 		   drivers/bus_spi.c \
 		   drivers/bus_i2c_stm32f10x.c \
 		   drivers/compass_hmc5883l.c \
+           drivers/compass_mag3110.c \
            drivers/compass_ak8975.c \
 		   drivers/display_ug2864hsweg01.h \
 		   drivers/flash_m25p16.c \
@@ -361,6 +362,7 @@ EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \
 		   drivers/bus_i2c_stm32f10x.c \
 		   drivers/bus_spi.c \
 		   drivers/compass_ak8975.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_hmc5883l.c \
 		   drivers/display_ug2864hsweg01.c \
 		   drivers/flash_m25p16.c \
@@ -395,6 +397,7 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \
 		   drivers/bus_i2c_stm32f10x.c \
 		   drivers/bus_spi.c \
 		   drivers/compass_hmc5883l.c \
+           drivers/compass_mag3110.c \
            drivers/compass_ak8975.c \
 		   drivers/gpio_stm32f10x.c \
 		   drivers/light_led_stm32f10x.c \
@@ -421,6 +424,7 @@ CJMCU_SRC = \
 		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/bus_i2c_stm32f10x.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_hmc5883l.c \
            drivers/compass_ak8975.c \
 		   drivers/gpio_stm32f10x.c \
@@ -451,6 +455,7 @@ CC3D_SRC = \
 		   drivers/barometer_bmp280.c \
 		   drivers/bus_spi.c \
 		   drivers/bus_i2c_stm32f10x.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_hmc5883l.c \
            drivers/compass_ak8975.c \
 		   drivers/display_ug2864hsweg01.c \
@@ -507,6 +512,7 @@ STM32F3DISCOVERY_COMMON_SRC = \
 		   drivers/accgyro_l3gd20.c \
 		   drivers/accgyro_l3gd20.c \
 		   drivers/accgyro_lsm303dlhc.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_hmc5883l.c \
            drivers/compass_ak8975.c \
 		   $(VCP_SRC)
@@ -521,6 +527,7 @@ STM32F3DISCOVERY_SRC = \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/accgyro_l3g4200d.c \
 		   drivers/barometer_ms5611.c \
+           drivers/compass_mag3110.c \
            drivers/compass_hmc5883l.c \
 		   drivers/compass_ak8975.c \
 		   $(HIGHEND_SRC) \
@@ -539,6 +546,7 @@ COLIBRI_RACE_SRC = \
 		   drivers/accgyro_spi_mpu6500.c \
 		   drivers/accgyro_mpu6500.c \
 		   drivers/barometer_ms5611.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_ak8975.c \
 		   drivers/compass_hmc5883l.c \
 		   drivers/serial_usb_vcp.c \
@@ -554,6 +562,7 @@ SPARKY_SRC = \
 		   drivers/barometer_bmp085.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/barometer_bmp280.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_ak8975.c \
 		   drivers/compass_hmc5883l.c \
 		   drivers/serial_usb_vcp.c \
@@ -571,6 +580,7 @@ RMDO_SRC = \
 		   drivers/barometer_bmp085.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/barometer_bmp280.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_ak8975.c \
 		   drivers/compass_hmc5883l.c \
 		   drivers/display_ug2864hsweg01.h \
@@ -588,6 +598,7 @@ SPRACINGF3_SRC = \
 		   drivers/barometer_bmp085.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/barometer_bmp280.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_ak8975.c \
 		   drivers/compass_hmc5883l.c \
 		   drivers/display_ug2864hsweg01.h \
@@ -603,6 +614,7 @@ MOTOLAB_SRC = \
 		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/barometer_ms5611.c \
+           drivers/compass_mag3110.c \
 		   drivers/compass_ak8975.c \
 		   drivers/compass_hmc5883l.c \
 		   drivers/display_ug2864hsweg01.c \

src/main/drivers/compass_mag3110.c

@@ -0,0 +1,110 @@
+/*
+ * 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 <math.h>
+
+#include "build_config.h"
+
+#include "platform.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "system.h"
+#include "gpio.h"
+#include "bus_i2c.h"
+
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+
+#include "sensor.h"
+#include "compass.h"
+
+#include "compass_mag3110.h"
+
+#ifdef USE_MAG_MAG3110
+
+#define MAG3110_MAG_I2C_ADDRESS     0x0E
+
+
+// Registers
+#define MAG3110_MAG_REG_STATUS       0x00
+#define MAG3110_MAG_REG_HXL          0x01
+#define MAG3110_MAG_REG_HXH          0x02
+#define MAG3110_MAG_REG_HYL          0x03
+#define MAG3110_MAG_REG_HYH          0x04
+#define MAG3110_MAG_REG_HZL          0x05
+#define MAG3110_MAG_REG_HZH          0x06
+#define MAG3110_MAG_REG_WHO_AM_I     0x07
+#define MAG3110_MAG_REG_SYSMODE      0x08
+#define MAG3110_MAG_REG_CTRL_REG1    0x10
+#define MAG3110_MAG_REG_CTRL_REG2    0x11
+
+bool mag3110detect(mag_t *mag)
+{
+    bool ack = false;
+    uint8_t sig = 0;
+
+    ack = i2cRead(MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_WHO_AM_I, 1, &sig);
+    if (!ack || sig != 0xC4)
+        return false;
+
+    mag->init = mag3110Init;
+    mag->read = mag3110Read;
+
+    return true;
+}
+
+void mag3110Init()
+{
+    bool ack;
+    UNUSED(ack);
+
+    ack = i2cWrite(MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_CTRL_REG1, 0x01); //  active mode 80 Hz ODR with OSR = 1
+    delay(20);
+
+    ack = i2cWrite(MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
+    delay(10);
+}
+
+#define BIT_STATUS_REG_DATA_READY               (1 << 3)
+
+bool mag3110Read(int16_t *magData)
+{
+    bool ack;
+    UNUSED(ack);
+    uint8_t status;
+    uint8_t buf[6];
+
+    ack = i2cRead(MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_STATUS, 1, &status);
+    if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) {
+        return false;
+    }
+
+    ack = i2cRead(MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_HXL, 6, buf);
+
+    magData[X] = (int16_t)(buf[0] << 8 | buf[1]);
+    magData[Y] = (int16_t)(buf[2] << 8 | buf[3]);
+    magData[Z] = (int16_t)(buf[4] << 8 | buf[5]);
+
+    return true;
+}
+
+#endif

src/main/drivers/compass_mag3110.h

@@ -0,0 +1,22 @@
+/*
+ * 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 mag3110detect(mag_t *mag);
+void mag3110Init(void);
+bool mag3110Read(int16_t *magData);

src/main/sensors/compass.h

@@ -24,7 +24,8 @@ typedef enum {
     MAG_HMC5883 = 2,
     MAG_AK8975 = 3,
     MAG_GPS = 4,
-    MAG_FAKE = 5
+    MAG_MAG3110 = 5,
+    MAG_FAKE = 6,
 } magSensor_e;
 
 #define MAG_MAX  MAG_FAKE

src/main/sensors/initialisation.c

@@ -55,6 +55,7 @@
 #include "drivers/compass.h"
 #include "drivers/compass_hmc5883l.h"
 #include "drivers/compass_ak8975.h"
+#include "drivers/compass_mag3110.h"
 
 #include "drivers/sonar_hcsr04.h"
 
@@ -690,6 +691,18 @@ retry:
 #endif
             ; // fallthrough
 
+        case MAG_MAG3110:
+#ifdef USE_MAG_MAG3110
+            if (mag3110detect(&mag)) {
+#ifdef MAG_MAG3110_ALIGN
+                magAlign = MAG_MAG3110_ALIGN;
+#endif
+                magHardware = MAG_MAG3110;
+                break;
+            }
+#endif
+            ; // fallthrough
+
         case MAG_FAKE:
 #ifdef USE_FAKE_MAG
             if (fakeMagDetect(&mag)) {

src/main/target/CC3D/target.h

@@ -66,6 +66,7 @@
 #define MAG
 #define USE_MAG_HMC5883
 #define USE_MAG_AK8975
+#define USE_MAG_MAG3110
 
 #define INVERTER
 #define BEEPER

src/main/target/NAZE/target.h

@@ -114,6 +114,7 @@
 #define MAG
 #define USE_MAG_HMC5883
 #define USE_MAG_AK8975
+#define USE_MAG_MAG3110
 
 #define MAG_HMC5883_ALIGN CW180_DEG
 

src/main/target/RMDO/target.h

@@ -53,6 +53,7 @@
 #define MAG
 #define USE_MAG_AK8975
 #define USE_MAG_HMC5883
+#define USE_MAG_MAG3110
 
 #define USE_FLASHFS
 #define USE_FLASH_M25P16

src/main/target/SPARKY/target.h

@@ -45,6 +45,7 @@
 #define MAG
 #define USE_MAG_HMC5883
 #define USE_MAG_AK8975
+#define USE_MAG_MAG3110
 
 #define MAG_AK8975_ALIGN CW180_DEG_FLIP
 

src/main/target/SPRACINGF3/target.h

@@ -55,6 +55,8 @@
 #define MAG
 #define USE_MAG_AK8975
 #define USE_MAG_HMC5883
+#define USE_MAG_MAG3110
+
 #define MAG_HMC5883_ALIGN CW270_DEG
 
 #define USE_FLASHFS