Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Fix compass loop time (#13125)

Browse source
This commit is contained in:
Steve Evans 2023-11-07 05:37:27 +00:00 • committed by GitHub
parent f223b2376e
commit 96e6b79430
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 18 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -31,6 +31,7 @@
#include "cms/cms.h" #include "cms/cms.h"
#include "common/color.h" #include "common/color.h"
#include "common/time.h"
#include "common/utils.h" #include "common/utils.h"
#include "config/feature.h" #include "config/feature.h"

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

@ -71,6 +71,8 @@ mag_t mag;
PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3); PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
#define COMPASS_READ_US 500 // Allow 500us for compass data read
void pgResetFn_compassConfig(compassConfig_t *compassConfig) void pgResetFn_compassConfig(compassConfig_t *compassConfig)
{ {
compassConfig->mag_alignment = ALIGN_DEFAULT; compassConfig->mag_alignment = ALIGN_DEFAULT;
@ -373,10 +375,16 @@ bool compassIsCalibrationComplete(void)
uint32_t compassUpdate(timeUs_t currentTimeUs) uint32_t compassUpdate(timeUs_t currentTimeUs)
{ {
static uint8_t busyCount = 0;
uint32_t nextPeriod = COMPASS_READ_US;
if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) { if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
// No action was taken as the read has not completed // No action was taken as the read has not completed
schedulerIgnoreTaskExecRate(); schedulerIgnoreTaskStateTime();
return 500; // Wait 500us between states
busyCount++;
return nextPeriod; // Wait COMPASS_READ_US between states
} }
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -419,6 +427,11 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
} }
} }
return TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ); nextPeriod = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ) - COMPASS_READ_US * busyCount;
// Reset the busy count
busyCount = 0;
return nextPeriod;
} }
#endif // USE_MAG #endif // USE_MAG

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

@ -27,7 +27,7 @@
#include "pg/pg.h" #include "pg/pg.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#define TASK_COMPASS_RATE_HZ 200 #define TASK_COMPASS_RATE_HZ 300
// Type of magnetometer used/detected // Type of magnetometer used/detected
typedef enum { typedef enum {