Add compass QMC5883L driver (#5309)

This commit is contained in:
Míguel Ángel Mulero Martínez 2018-03-03 23:45:54 +01:00 committed by Michael Keller
parent 6560e96e98
commit 73eb5d396e
56 changed files with 248 additions and 14 deletions

View File

@ -284,6 +284,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_fake.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/display_ug2864hsweg01.c \
drivers/inverter.c \
drivers/light_ws2811strip.c \

View File

@ -0,0 +1,142 @@
/*
* 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 "platform.h"
#ifdef USE_MAG_QMC5883
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
#include "compass.h"
#include "compass_qmc5883l.h"
#define QMC5883L_MAG_I2C_ADDRESS 0x0D
// Registers
#define QMC5883L_REG_CONF1 0x09
#define QMC5883L_REG_CONF2 0x0A
// data output rates for 5883L
#define QMC5883L_ODR_10HZ (0x00 << 2)
#define QMC5883L_ODR_50HZ (0x01 << 2)
#define QMC5883L_ODR_100HZ (0x02 << 2)
#define QMC5883L_ODR_200HZ (0x03 << 2)
// Sensor operation modes
#define QMC5883L_MODE_STANDBY 0x00
#define QMC5883L_MODE_CONTINUOUS 0x01
#define QMC5883L_RNG_2G (0x00 << 4)
#define QMC5883L_RNG_8G (0x01 << 4)
#define QMC5883L_OSR_512 (0x00 << 6)
#define QMC5883L_OSR_256 (0x01 << 6)
#define QMC5883L_OSR_128 (0x10 << 6)
#define QMC5883L_OSR_64 (0x11 << 6)
#define QMC5883L_RST 0x80
#define QMC5883L_REG_DATA_OUTPUT_X 0x00
#define QMC5883L_REG_STATUS 0x06
#define QMC5883L_REG_ID 0x0D
#define QMC5883_ID_VAL 0xFF
static bool qmc5883lInit(magDev_t *magDev)
{
UNUSED(magDev);
bool ack = true;
busDevice_t *busdev = &magDev->busdev;
ack = ack && busWriteRegister(busdev, 0x0B, 0x01);
ack = ack && busWriteRegister(busdev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
if (!ack) {
return false;
}
return true;
}
static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
{
uint8_t status;
uint8_t buf[6];
// set magData to zero for case of failed read
magData[X] = 0;
magData[Y] = 0;
magData[Z] = 0;
busDevice_t *busdev = &magDev->busdev;
bool ack = busReadRegisterBuffer(busdev, QMC5883L_REG_STATUS, &status, 1);
if (!ack || (status & 0x04) == 0) {
return false;
}
ack = busReadRegisterBuffer(busdev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6);
if (!ack) {
return false;
}
magData[X] = (int16_t)(buf[1] << 8 | buf[0]);
magData[Y] = (int16_t)(buf[3] << 8 | buf[2]);
magData[Z] = (int16_t)(buf[5] << 8 | buf[4]);
return true;
}
bool qmc5883lDetect(magDev_t *magDev)
{
busDevice_t *busdev = &magDev->busdev;
if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) {
busdev->busdev_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS;
}
// Must write reset first - don't care about the result
busWriteRegister(busdev, QMC5883L_REG_CONF2, QMC5883L_RST);
delay(20);
uint8_t sig = 0;
bool ack = busReadRegisterBuffer(busdev, QMC5883L_REG_ID, &sig, 1);
if (ack && sig == QMC5883_ID_VAL) {
magDev->init = qmc5883lInit;
magDev->read = qmc5883lRead;
return true;
}
return false;
}
#endif

View File

@ -0,0 +1,23 @@
/*
* 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
#include "drivers/io_types.h"
bool qmc5883lDetect(magDev_t *magDev);

View File

@ -116,7 +116,7 @@ const char * const lookupTableBaroHardware[] = {
#if defined(USE_SENSOR_NAMES) || defined(USE_MAG)
// sync with magSensor_e
const char * const lookupTableMagHardware[] = {
"AUTO", "NONE", "HMC5883", "AK8975", "AK8963"
"AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883"
};
#endif
#if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)

View File

@ -34,6 +34,7 @@
#include "drivers/compass/compass_ak8963.h"
#include "drivers/compass/compass_fake.h"
#include "drivers/compass/compass_hmc5883l.h"
#include "drivers/compass/compass_qmc5883l.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
@ -83,7 +84,7 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
#endif
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
compassConfig->mag_i2c_address = 0;
#elif defined(USE_MAG_HMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)))
#elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)))
compassConfig->mag_bustype = BUSTYPE_I2C;
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE);
compassConfig->mag_i2c_address = 0;
@ -180,6 +181,22 @@ bool compassDetect(magDev_t *dev)
#endif
FALLTHROUGH;
case MAG_QMC5883:
#ifdef USE_MAG_QMC5883
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (qmc5883lDetect(dev)) {
#ifdef MAG_QMC5883L_ALIGN
dev->magAlign = MAG_QMC5883L_ALIGN;
#endif
magHardware = MAG_QMC5883;
break;
}
#endif
FALLTHROUGH;
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (busdev->bustype == BUSTYPE_I2C) {

View File

@ -30,7 +30,8 @@ typedef enum {
MAG_NONE = 1,
MAG_HMC5883 = 2,
MAG_AK8975 = 3,
MAG_AK8963 = 4
MAG_AK8963 = 4,
MAG_QMC5883 = 5
} magSensor_e;
typedef struct mag_s {

View File

@ -53,6 +53,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_AK8963
#define MAG_AK8963_ALIGN CW180_DEG_FLIP

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_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -52,6 +52,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_SPI_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_AK8963
#define USE_MAG_SPI_AK8963

View File

@ -7,4 +7,5 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -45,6 +45,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_I2C_INSTANCE (I2CDEV_2)
//#define MAG_HMC5883_ALIGN CW270_DEG_FLIP

View File

@ -6,6 +6,7 @@ TARGET_SRC = \
drivers/barometer/barometer_ms5611.c \
drivers/barometer/barometer_bmp280.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_hal.c \
drivers/max7456.c

View File

@ -54,6 +54,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAX7456
#define MAX7456_SPI_INSTANCE SPI2

View File

@ -8,4 +8,5 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c

View File

@ -62,6 +62,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
//#define USE_MAG_AK8963
#define HMC5883_I2C_INSTANCE I2CDEV_1

View File

@ -5,5 +5,6 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_mpu6500.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -51,6 +51,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW270_DEG_FLIP
#define MAG_INT_EXTI PC1

View File

@ -5,4 +5,5 @@ HSE_VALUE = 16000000
TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -70,6 +70,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_AK8963
#define USE_MAG_AK8975

View File

@ -12,5 +12,6 @@ TARGET_SRC = \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -48,6 +48,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW90_DEG
#define USE_BARO

View File

@ -4,4 +4,5 @@ FEATURES += SDCARD VCP
TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -63,6 +63,7 @@
#define USE_MAG
#define USE_MAG_HMC5883 //External, connect to I2C1
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW180_DEG
#define USE_BARO

View File

@ -7,4 +7,5 @@ TARGET_SRC = \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c

View File

@ -156,6 +156,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_I2C_INSTANCE I2C_DEVICE
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO)

View File

@ -7,6 +7,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/barometer/barometer_bmp280.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_hal.c \
drivers/max7456.c

View File

@ -53,6 +53,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW180_DEG
#define MAG_I2C_INSTANCE I2CDEV_1

View File

@ -6,4 +6,5 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c

View File

@ -62,6 +62,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
// *************** Baro **************************
#define USE_I2C

View File

@ -9,4 +9,5 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c

View File

@ -76,6 +76,7 @@
//*********** Magnetometer / Compass *************
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
// *************** SD Card **************************
#define USE_SDCARD

View File

@ -9,4 +9,5 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c

View File

@ -60,6 +60,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW180_DEG
#define USE_UART1

View File

@ -6,4 +6,5 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -48,6 +48,7 @@
#define USE_MAG
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW270_DEG
#define USE_MAG_DATA_READY_SIGNAL

View File

@ -8,4 +8,5 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -45,6 +45,7 @@
#define USE_MAG
#define USE_FAKE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW270_DEG_FLIP
#define USE_BARO

View File

@ -7,4 +7,5 @@ TARGET_SRC = \
drivers/barometer/barometer_fake.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_fake.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -47,6 +47,7 @@
#define USE_MAG
#define USE_FAKE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW270_DEG_FLIP
#define USE_BARO

View File

@ -8,5 +8,6 @@ TARGET_SRC = \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_fake.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_hal.c

View File

@ -107,6 +107,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW90_DEG
//#define USE_MAG_NAZA // Delete this on next major release

View File

@ -8,5 +8,6 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c

View File

@ -210,6 +210,7 @@
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO)
//ADC---------------------------------------

View File

@ -13,6 +13,7 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp280.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_hal.c \
drivers/max7456.c

View File

@ -147,6 +147,7 @@
// Configure MAG and BARO unconditionally.
#define USE_MAG
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW90_DEG
#define USE_BARO

View File

@ -12,4 +12,5 @@ TARGET_SRC = \
drivers/barometer/barometer_ms5611.c \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c

View File

@ -111,6 +111,7 @@
#define USE_MAG
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define MAG_HMC5883_ALIGN CW270_DEG
#define USE_MAG_DATA_READY_SIGNAL

View File

@ -7,7 +7,8 @@ TARGET_SRC = \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_hmc5883l.c
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c
ifeq ($(TARGET), FLIP32F3OSD)
TARGET_SRC += \

View File

@ -72,6 +72,7 @@
#define USE_MPU9250_MAG // Enables bypass configuration
#define USE_MAG_AK8975
#define USE_MAG_HMC5883 // External
#define USE_MAG_QMC5883
#define MAG_AK8975_ALIGN CW90_DEG_FLIP
#endif

View File

@ -8,6 +8,7 @@ TARGET_SRC = \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/flash_m25p16.c
ifeq ($(TARGET), TINYBEEF3)

View File

@ -81,6 +81,7 @@
#define USE_MAG
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_VCP
#define USE_UART1

View File

@ -8,6 +8,7 @@ TARGET_SRC = \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c \
drivers/vtx_rtc6705.c

View File

@ -73,6 +73,7 @@
#define USE_MAG
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_VCP
#define USE_UART1

View File

@ -8,6 +8,7 @@ TARGET_SRC = \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c \
drivers/transponder_ir.c \
drivers/vtx_rtc6705.c \

View File

@ -152,6 +152,7 @@
#define USE_MAG_AK8963
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_VCP
#define USE_UART1

View File

@ -27,5 +27,6 @@ TARGET_SRC = \
drivers/compass/compass_ak8975.c \
drivers/compass/compass_fake.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/flash_m25p16.c \
drivers/max7456.c