diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f1966c45f60..3216bbdb8ee 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -152,6 +152,8 @@ main_sources(COMMON_SRC drivers/compass/compass_mpu9250.h drivers/compass/compass_qmc5883l.c drivers/compass/compass_qmc5883l.h + drivers/compass/compass_qmc5883p.c + drivers/compass/compass_qmc5883p.h drivers/compass/compass_rm3100.c drivers/compass/compass_rm3100.h drivers/compass/compass_vcm5883.c diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 6c2cedaff02..65a21f8fce1 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -107,6 +107,7 @@ typedef enum { DEVHW_IST8310_1, DEVHW_IST8308, DEVHW_QMC5883, + DEVHW_QMC5883P, DEVHW_MAG3110, DEVHW_LIS3MDL, DEVHW_RM3100, diff --git a/src/main/drivers/compass/compass_qmc5883p.c b/src/main/drivers/compass/compass_qmc5883p.c new file mode 100644 index 00000000000..e5c21afb74a --- /dev/null +++ b/src/main/drivers/compass/compass_qmc5883p.c @@ -0,0 +1,203 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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. + * + * This file 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 this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include + +#include "platform.h" + +#ifdef USE_MAG_QMC5883P + +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" + +#include "drivers/sensor.h" +#include "drivers/compass/compass.h" + +#include "drivers/compass/compass_qmc5883p.h" + +#define QMC5883P_MAG_I2C_ADDRESS 0x2C + +#define QMC5883P_REG_ID 0x00 +#define QMC5883P_ID_VAL 0x80 + +#define QMC5883P_REG_DATA_OUTPUT_X 0x01 +#define QMC5883P_DATA_BYTES 6 + +#define QMC5883P_REG_CONF1 0x0A + +#define QMC5883P_CONF1_MODE_SUSPEND 0x00 +#define QMC5883P_CONF1_MODE_NORMAL 0x01 +#define QMC5883P_CONF1_MODE_SINGLE 0x02 +#define QMC5883P_CONF1_MODE_CONTINUOUS 0x03 + +#define QMC5883P_CONF1_ODR_10HZ (0x00 << 2) +#define QMC5883P_CONF1_ODR_50HZ (0x01 << 2) +#define QMC5883P_CONF1_ODR_100HZ (0x02 << 2) +#define QMC5883P_CONF1_ODR_200HZ (0x03 << 2) + +#define QMC5883P_CONF1_OSR1_8 (0x00 << 4) +#define QMC5883P_CONF1_OSR1_4 (0x01 << 4) +#define QMC5883P_CONF1_OSR1_2 (0x02 << 4) +#define QMC5883P_CONF1_OSR1_1 (0x03 << 4) + +#define QMC5883P_CONF1_OSR2_1 (0x00 << 6) +#define QMC5883P_CONF1_OSR2_2 (0x01 << 6) +#define QMC5883P_CONF1_OSR2_4 (0x02 << 6) +#define QMC5883P_CONF1_OSR2_8 (0x03 << 6) + + +#define QMC5883P_REG_CONF2 0x0B + +#define QMC5883P_CONF2_SET_RESET_ON 0x00 +#define QMC5883P_CONF2_SET_ON_RESET_OFF 0x01 +#define QMC5883P_CONF2_SET_RESET_OFF 0x02 + +#define QMC5883P_CONF2_RNG_30G (0x00 << 2) +#define QMC5883P_CONF2_RNG_12G (0x01 << 2) +#define QMC5883P_CONF2_RNG_8G (0x02 << 2) +#define QMC5883P_CONF2_RNG_2G (0x03 << 2) + +#define QMC5883P_CONF2_SELF_TEST 0x40 + +#define QMC5883P_CONF2_RESET 0x80 + + +#define QMC5883P_REG_STATUS 0x09 +#define QMC5883P_STATUS_DRDY_MASK 0x01 +#define QMC5883P_STATUS_OVFL_MASK 0x02 + +// This register has no definition in the datasheet and only mentioned in an example +#define QMC5883P_REG_DATA_SIGN 0x29 +#define QMC5883P_DATA_SIGN_MAGIC_VALUE 0x06 + +static bool qmc5883pInit(magDev_t * mag) +{ + bool ack = true; + + ack = ack && busWrite(mag->busDev, QMC5883P_REG_CONF2, QMC5883P_CONF2_RESET); + + delay(30); + + ack = ack && busWrite(mag->busDev, QMC5883P_REG_DATA_SIGN, QMC5883P_DATA_SIGN_MAGIC_VALUE); + ack = ack && busWrite(mag->busDev, QMC5883P_REG_CONF2, QMC5883P_CONF2_RNG_8G); + ack = ack && busWrite(mag->busDev, + QMC5883P_REG_CONF1, + QMC5883P_CONF1_MODE_CONTINUOUS | + QMC5883P_CONF1_ODR_200HZ | + QMC5883P_CONF1_OSR1_8 | + QMC5883P_CONF1_OSR2_8); + + return ack; +} + +static bool qmc5883pRead(magDev_t * mag) +{ + uint8_t status; + uint8_t buf[QMC5883P_DATA_BYTES]; + + // set magData to zero for case of failed read + mag->magADCRaw[X] = 0; + mag->magADCRaw[Y] = 0; + mag->magADCRaw[Z] = 0; + + bool ack = busRead(mag->busDev, QMC5883P_REG_STATUS, &status); + if (!ack || (status & QMC5883P_STATUS_DRDY_MASK) == 0) { + return false; + } + + ack = busReadBuf(mag->busDev, QMC5883P_REG_DATA_OUTPUT_X, buf, QMC5883P_DATA_BYTES); + if (!ack) { + return false; + } + + /* + Initially, this sensor provided the data like this: + mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]); + mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]); + mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]); + + But QMC5883P has a different reference point and axis directions, compared to QMC5883L. + As QMC5883P is designed to be a drop-in replacement for QMC5883L, apply alignment at + readout to obtain the same readings in the same position. In particular, it does + the same transformation to the data, as the CW270_DEG_FLIP option: + dest[X] = -y; + dest[Y] = -x; + dest[Z] = -z; + */ + mag->magADCRaw[X] = -(int16_t)(buf[3] << 8 | buf[2]); + mag->magADCRaw[Y] = -(int16_t)(buf[1] << 8 | buf[0]); + mag->magADCRaw[Z] = -(int16_t)(buf[5] << 8 | buf[4]); + + return true; +} + +#define DETECTION_MAX_RETRY_COUNT 5 +static bool deviceDetect(magDev_t * mag) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + // Must write reset first - don't care about the result + busWrite(mag->busDev, QMC5883P_REG_CONF2, QMC5883P_CONF2_RESET); + delay(30); + + uint8_t sig = 0; + bool ack = busRead(mag->busDev, QMC5883P_REG_ID, &sig); + + if (ack && sig == QMC5883P_ID_VAL) { + return true; + } + } + + return false; +} + +bool qmc5883pDetect(magDev_t * mag) +{ + mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_QMC5883P, mag->magSensorToUse, OWNER_COMPASS); + if (mag->busDev == NULL) { + return false; + } + + if (!deviceDetect(mag)) { + busDeviceDeInit(mag->busDev); + return false; + } + + mag->init = qmc5883pInit; + mag->read = qmc5883pRead; + + return true; +} +#endif diff --git a/src/main/drivers/compass/compass_qmc5883p.h b/src/main/drivers/compass/compass_qmc5883p.h new file mode 100644 index 00000000000..9f20c183c89 --- /dev/null +++ b/src/main/drivers/compass/compass_qmc5883p.h @@ -0,0 +1,27 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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. + * + * This file 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 this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +bool qmc5883pDetect(magDev_t *mag); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 811269afa92..86194985d7c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -8,7 +8,7 @@ tables: values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"] enum: rangefinderType_e - name: mag_hardware - values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] + values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "QMC5883P", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] enum: magSensor_e - name: opflow_hardware values: ["NONE", "CXOF", "MSP", "FAKE"] diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 02a9df69c44..4e87b944168 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -38,6 +38,7 @@ #include "drivers/compass/compass_ist8310.h" #include "drivers/compass/compass_ist8308.h" #include "drivers/compass/compass_qmc5883l.h" +#include "drivers/compass/compass_qmc5883p.h" #include "drivers/compass/compass_mpu9250.h" #include "drivers/compass/compass_lis3mdl.h" #include "drivers/compass/compass_rm3100.h" @@ -107,6 +108,19 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) } FALLTHROUGH; + case MAG_QMC5883P: +#ifdef USE_MAG_QMC5883P + if (qmc5883pDetect(dev)) { + magHardware = MAG_QMC5883P; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(dev)) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index c6dd78f7db5..17930678f58 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -36,6 +36,7 @@ typedef enum { MAG_AK8963, MAG_IST8310, MAG_QMC5883, + MAG_QMC5883P, MAG_MPU9250, MAG_IST8308, MAG_LIS3MDL, diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 0169e8099bf..69a2146fc86 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -213,6 +213,13 @@ BUSDEV_REGISTER_I2C(busdev_qmc5883, DEVHW_QMC5883, QMC5883_I2C_BUS, 0x0D, NONE, DEVFLAGS_NONE, 0); #endif +#if defined(USE_MAG_QMC5883P) + #if !defined(QMC5883P_I2C_BUS) + #define QMC5883P_I2C_BUS MAG_I2C_BUS + #endif + BUSDEV_REGISTER_I2C(busdev_qmc5883p, DEVHW_QMC5883P, QMC5883P_I2C_BUS, 0x2C, NONE, DEVFLAGS_NONE, 0); +#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, 0); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 104898c92f5..ccd14d35a4a 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -85,6 +85,7 @@ extern uint8_t __config_end; #define USE_MAG_LIS3MDL #define USE_MAG_MAG3110 #define USE_MAG_QMC5883 +#define USE_MAG_QMC5883P //#if (MCU_FLASH_SIZE > 512) #define USE_MAG_AK8963