Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +03:00
Code Issues Wiki Activity

Merge pull request #11439 from SteveCEvans/compass_timing

Browse source 
Perform compass reads in the background
This commit is contained in:
haslinghuis 2022-03-01 21:37:07 +01:00 • committed by GitHub
commit 5b5df65934
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 125 additions and 58 deletions
Download patch file Download diff file Expand all files Collapse all files

80
src/main/drivers/compass/compass_ak8975.c
View file

@ -118,39 +118,63 @@ static int16_t parseMag(uint8_t *raw, int16_t gain) {
static bool ak8975Read(magDev_t *mag, int16_t *magData)
{
bool ack;
uint8_t status;
uint8_t buf[6];
static uint8_t buf[6];
static uint8_t status;
static enum {
STATE_READ_STATUS1,
STATE_WAIT_STATUS1,
STATE_READ_STATUS2,
STATE_WAIT_STATUS2,
STATE_WAIT_START,
} state = STATE_READ_STATUS1;
extDevice_t *dev = &mag->dev;
ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1);
if (!ack || (status & ST1_REG_DATA_READY) == 0) {
return false;
switch (state) {
default:
case STATE_READ_STATUS1:
busReadRegisterBufferStart(dev, AK8975_MAG_REG_ST1, &status, sizeof(status));
state = STATE_WAIT_STATUS1;
return false;
case STATE_WAIT_STATUS1:
if ((status & ST1_REG_DATA_READY) == 0) {
state = STATE_READ_STATUS1;
return false;
}
busReadRegisterBufferStart(dev, AK8975_MAG_REG_HXL, buf, sizeof(buf));
state = STATE_READ_STATUS2;
return false;
case STATE_READ_STATUS2:
busReadRegisterBufferStart(dev, AK8975_MAG_REG_ST2, &status, sizeof(status));
state = STATE_WAIT_STATUS2;
return false;
case STATE_WAIT_STATUS2:
busWriteRegisterStart(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again
if ((status & ST2_REG_DATA_ERROR) || (status & ST2_REG_MAG_SENSOR_OVERFLOW)) {
state = STATE_READ_STATUS1;
return false;
}
state = STATE_WAIT_START;
return false;
case STATE_WAIT_START:
magData[X] = -parseMag(buf + 0, mag->magGain[X]);
magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
state = STATE_READ_STATUS1;
return true;
}
busReadRegisterBuffer(dev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1);
if (!ack) {
return false;
}
busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again uint8_t status2 = buf[6];
if (status & ST2_REG_DATA_ERROR) {
return false;
}
if (status & ST2_REG_MAG_SENSOR_OVERFLOW) {
return false;
}
magData[X] = -parseMag(buf + 0, mag->magGain[X]);
magData[Y] = -parseMag(buf + 2, mag->magGain[Y]);
magData[Z] = -parseMag(buf + 4, mag->magGain[Z]);
return true;
return false;
}
bool ak8975Detect(magDev_t *mag)

10
src/main/drivers/compass/compass_hmc5883l.c
View file

@ -200,13 +200,15 @@ static void hmc5883SpiInit(const extDevice_t *dev)
static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
{
uint8_t buf[6];
static uint8_t buf[6];
static bool pendingRead = true;
extDevice_t *dev = &mag->dev;
bool ack = busReadRegisterBuffer(dev, HMC58X3_REG_DATA, buf, 6);
if (pendingRead) {
busReadRegisterBufferStart(dev, HMC58X3_REG_DATA, buf, sizeof(buf));
if (!ack) {
pendingRead = false;
return false;
}
@ -214,6 +216,8 @@ static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
magData[Z] = (int16_t)(buf[2] << 8 | buf[3]);
magData[Y] = (int16_t)(buf[4] << 8 | buf[5]);
pendingRead = true;
return true;
}

10
src/main/drivers/compass/compass_lis3mdl.c
View file

@ -103,13 +103,15 @@
static bool lis3mdlRead(magDev_t * mag, int16_t *magData)
{
uint8_t buf[6];
static uint8_t buf[6];
static bool pendingRead = true;
extDevice_t *dev = &mag->dev;
bool ack = busReadRegisterBuffer(dev, LIS3MDL_REG_OUT_X_L, buf, 6);
if (pendingRead) {
busReadRegisterBufferStart(dev, LIS3MDL_REG_OUT_X_L, buf, sizeof(buf));
if (!ack) {
pendingRead = false;
return false;
}
@ -117,6 +119,8 @@ static bool lis3mdlRead(magDev_t * mag, int16_t *magData)
magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) / 4;
magData[Z] = (int16_t)(buf[5] << 8 | buf[4]) / 4;
pendingRead = true;
return true;
}

53
src/main/drivers/compass/compass_qmc5883l.c
View file

@ -91,32 +91,45 @@ static bool qmc5883lInit(magDev_t *magDev)
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;
static uint8_t buf[6];
static uint8_t status;
static enum {
STATE_READ_STATUS,
STATE_WAIT_STATUS,
STATE_WAIT_READ,
} state = STATE_READ_STATUS;
extDevice_t *dev = &magDev->dev;
bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_STATUS, &status, 1);
switch (state) {
default:
case STATE_READ_STATUS:
busReadRegisterBufferStart(dev, QMC5883L_REG_STATUS, &status, sizeof(status));
state = STATE_WAIT_STATUS;
return false;
if (!ack || (status & 0x04) == 0) {
return false;
case STATE_WAIT_STATUS:
if ((status & 0x04) == 0) {
state = STATE_READ_STATUS;
return false;
}
busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, sizeof(buf));
state = STATE_WAIT_READ;
return false;
case STATE_WAIT_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_READ_STATUS;
return true;
}
ack = busReadRegisterBuffer(dev, 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;
return false;
}
bool qmc5883lDetect(magDev_t *magDev)

16
src/main/fc/tasks.c
View file

@ -259,6 +259,20 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
}
#endif
#ifdef USE_MAG
static void taskUpdateMag(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (sensors(SENSOR_MAG)) {
const uint32_t newDeadline = compassUpdate(currentTimeUs);
if (newDeadline != 0) {
rescheduleTask(TASK_SELF, newDeadline);
}
}
}
#endif
#if defined(USE_RANGEFINDER)
void taskUpdateRangefinder(timeUs_t currentTimeUs)
{
@ -351,7 +365,7 @@ task_attribute_t task_attributes[TASK_COUNT] = {
#endif
#ifdef USE_MAG
[TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, compassUpdate,TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
[TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
#endif
#ifdef USE_BARO

12
src/main/sensors/compass.c
View file

@ -52,6 +52,8 @@
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "scheduler/scheduler.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/gyro_init.h"
@ -347,10 +349,14 @@ bool compassIsCalibrationComplete(void)
return tCal == 0;
}
void compassUpdate(timeUs_t currentTimeUs)
uint32_t compassUpdate(timeUs_t currentTimeUs)
{
if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
// No action was taken as the read has not completed
schedulerIgnoreTaskExecRate();
return 1000; // Wait 1ms between states
}
magDev.read(&magDev, magADCRaw);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] = magADCRaw[axis];
}
@ -385,5 +391,7 @@ void compassUpdate(timeUs_t currentTimeUs)
saveConfigAndNotify();
}
}
return TASK_PERIOD_HZ(10);
}
#endif // USE_MAG

2
src/main/sensors/compass.h
View file

@ -62,7 +62,7 @@ typedef struct compassConfig_s {
PG_DECLARE(compassConfig_t, compassConfig);
bool compassIsHealthy(void);
void compassUpdate(timeUs_t currentTime);
uint32_t compassUpdate(timeUs_t currentTime);
bool compassInit(void);
void compassPreInit(void);
void compassStartCalibration(void);