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

Merge pull request #4997 from martinbudden/bfa_compass_update

Browse source 
Avoided compass task wrapper function
This commit is contained in:
Michael Keller 2018-01-21 10:31:53 +13:00 • committed by GitHub
commit ee4edb3da9
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 11 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/fc/fc_tasks.c
View file

@ -157,15 +157,6 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
} }
#endif #endif
#ifdef USE_MAG
static void taskUpdateCompass(timeUs_t currentTimeUs)
{
if (sensors(SENSOR_MAG)) {
compassUpdate(currentTimeUs, &compassConfigMutable()->magZero);
}
}
#endif
#ifdef USE_BARO #ifdef USE_BARO
static void taskUpdateBaro(timeUs_t currentTimeUs) static void taskUpdateBaro(timeUs_t currentTimeUs)
{ {
@ -463,7 +454,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
#ifdef USE_MAG #ifdef USE_MAG
[TASK_COMPASS] = { [TASK_COMPASS] = {
.taskName = "COMPASS", .taskName = "COMPASS",
.taskFunc = taskUpdateCompass, .taskFunc = compassUpdate,
.desiredPeriod = TASK_PERIOD_HZ(10), // Compass is updated at 10 Hz .desiredPeriod = TASK_PERIOD_HZ(10), // Compass is updated at 10 Hz
.staticPriority = TASK_PRIORITY_LOW, .staticPriority = TASK_PRIORITY_LOW,
}, },

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

@ -262,9 +262,9 @@ bool compassInit(void)
return true; return true;
} }
void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) void compassUpdate(timeUs_t currentTimeUs)
{ {
static uint32_t tCal = 0; static timeUs_t tCal = 0;
static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax; static flightDynamicsTrims_t magZeroTempMax;
@ -274,8 +274,9 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
} }
alignSensors(mag.magADC, magDev.magAlign); alignSensors(mag.magADC, magDev.magAlign);
flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
if (STATE(CALIBRATE_MAG)) { if (STATE(CALIBRATE_MAG)) {
tCal = currentTime; tCal = currentTimeUs;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0; magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = mag.magADC[axis]; magZeroTempMin.raw[axis] = mag.magADC[axis];
@ -291,7 +292,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
} }
if (tCal != 0) { if (tCal != 0) {
if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions if ((currentTimeUs - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE; LED0_TOGGLE;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
if (mag.magADC[axis] < magZeroTempMin.raw[axis]) if (mag.magADC[axis] < magZeroTempMin.raw[axis])

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

@ -17,12 +17,13 @@
#pragma once #pragma once
#include "pg/pg.h" #include "drivers/io_types.h"
#include "drivers/io.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "common/time.h"
#include "pg/pg.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
// Type of magnetometer used/detected // Type of magnetometer used/detected
typedef enum { typedef enum {
MAG_DEFAULT = 0, MAG_DEFAULT = 0,
@ -56,6 +57,5 @@ typedef struct compassConfig_s {
PG_DECLARE(compassConfig_t, compassConfig); PG_DECLARE(compassConfig_t, compassConfig);
bool compassInit(void); bool compassInit(void);
union flightDynamicsTrims_u; void compassUpdate(timeUs_t currentTime);
void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);