Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix compass loop time #13125

Merged
merged 1 commit into from
Nov 7, 2023
Merged

Fix compass loop time #13125

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 1 addition & 0 deletions src/main/fc/tasks.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include "cms/cms.h"

#include "common/color.h"
#include "common/time.h"
#include "common/utils.h"

#include "config/feature.h"
Expand Down
19 changes: 16 additions & 3 deletions src/main/sensors/compass.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ mag_t mag;

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)
{
compassConfig->mag_alignment = ALIGN_DEFAULT;
Expand Down Expand Up @@ -373,10 +375,16 @@ bool compassIsCalibrationComplete(void)

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)) {
// No action was taken as the read has not completed
schedulerIgnoreTaskExecRate();
return 500; // Wait 500us between states
schedulerIgnoreTaskStateTime();

busyCount++;

return nextPeriod; // Wait COMPASS_READ_US between states
}

for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
Expand Down Expand Up @@ -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
2 changes: 1 addition & 1 deletion src/main/sensors/compass.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
#include "pg/pg.h"
#include "sensors/sensors.h"

#define TASK_COMPASS_RATE_HZ 200
#define TASK_COMPASS_RATE_HZ 300

// Type of magnetometer used/detected
typedef enum {
Expand Down