mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
a8834ad14b
* Improved QMC driver, scheduled silent interval * add ODR to drivers and use it to set the quiet period * add MAG_TASK_RATE debug for testing * don't do a read if the bus is busy - thanks, Petr * refactoring and simplification, thanks to Steve --------- Co-authored-by: Steve Evans <SteveCEvans@users.noreply.github.com>
163 lines
4.4 KiB
C
163 lines
4.4 KiB
C
/*
|
|
* This file is part of Cleanflight and Betaflight.
|
|
*
|
|
* Cleanflight and Betaflight are free software. You can redistribute
|
|
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
|
|
* 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 software.
|
|
*
|
|
* 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_STATUS_DRDY 0x01
|
|
|
|
#define QMC5883L_REG_ID 0x0D
|
|
#define QMC5883_ID_VAL 0xFF
|
|
|
|
static bool qmc5883lInit(magDev_t *magDev)
|
|
{
|
|
bool ack = true;
|
|
extDevice_t *dev = &magDev->dev;
|
|
|
|
busDeviceRegister(dev);
|
|
|
|
ack = ack && busWriteRegister(dev, 0x0B, 0x01);
|
|
ack = ack && busWriteRegister(dev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
|
|
|
|
if (!ack) {
|
|
return false;
|
|
}
|
|
|
|
magDev->magOdrHz = 200; // QMC5883L_ODR_200HZ
|
|
return true;
|
|
}
|
|
|
|
static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
|
|
{
|
|
static uint8_t buf[6];
|
|
static uint8_t status = 0;
|
|
static enum {
|
|
STATE_WAIT_DRDY,
|
|
STATE_READ,
|
|
} state = STATE_WAIT_DRDY;
|
|
|
|
extDevice_t *dev = &magDev->dev;
|
|
|
|
switch (state) {
|
|
default:
|
|
case STATE_WAIT_DRDY:
|
|
if (status & QMC5883L_REG_STATUS_DRDY) {
|
|
// New data is available
|
|
busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, sizeof(buf));
|
|
state = STATE_READ;
|
|
} else {
|
|
// Read status register to check for data ready
|
|
busReadRegisterBufferStart(dev, QMC5883L_REG_STATUS, &status, sizeof(status));
|
|
}
|
|
return false;
|
|
|
|
case STATE_READ:
|
|
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]);
|
|
|
|
state = STATE_WAIT_DRDY;
|
|
|
|
// Indicate that new data is required
|
|
status = 0;
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool qmc5883lDetect(magDev_t *magDev)
|
|
{
|
|
|
|
extDevice_t *dev = &magDev->dev;
|
|
|
|
if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
|
|
dev->busType_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS;
|
|
}
|
|
|
|
// Must write reset first - don't care about the result
|
|
busWriteRegister(dev, QMC5883L_REG_CONF2, QMC5883L_RST);
|
|
delay(20);
|
|
|
|
uint8_t sig = 0;
|
|
bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_ID, &sig, 1);
|
|
if (ack && sig == QMC5883_ID_VAL) {
|
|
// Should be in standby mode after soft reset and sensor is really present
|
|
// Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present
|
|
ack = busReadRegisterBuffer(dev, QMC5883L_REG_CONF1, &sig, 1);
|
|
if (ack && sig != QMC5883L_MODE_STANDBY) {
|
|
return false;
|
|
}
|
|
magDev->init = qmc5883lInit;
|
|
magDev->read = qmc5883lRead;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
#endif
|