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

Put EKF and accelerometer on the realtime loop #5

Merged
merged 1 commit into from
Oct 31, 2024
Merged

Put EKF and accelerometer on the realtime loop #5

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.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 32 additions & 0 deletions src/main/fc/core.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1457,6 +1457,38 @@ FAST_CODE bool gyroFilterReady(void)
}
}

FAST_CODE bool accelerometerReady(void)
{
if (pidUpdateCounter % 8 == 0) {
return true;
} else {
return false;
}
}

FAST_CODE void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{
accUpdate(currentTimeUs);
#ifdef USE_THROW_TO_ARM
updateThrowFallStateMachine(currentTimeUs);
#endif
}

FAST_CODE bool EKFReady(void)
{
if (pidUpdateCounter % 16 == 0) { // todo, get correct denom
return true;
} else {
return false;
}
}

FAST_CODE void taskEKF(timeUs_t currentTimeUs)
{
updateEkf(currentTimeUs);
}


FAST_CODE bool innerLoopReady(void)
{
if ((pidUpdateCounter % activePidLoopDenom) == (activePidLoopDenom / 2)) {
Expand Down
5 changes: 5 additions & 0 deletions src/main/fc/core.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,12 @@ void updateArmingStatus(void);

void resetInnerLoopCounter(void);
void taskGyroSample(timeUs_t currentTimeUs);

void taskUpdateAccelerometer(timeUs_t currentTimeUs);
void taskEKF(timeUs_t currentTimeUs);
bool gyroFilterReady(void);
bool accelerometerReady(void);
bool EKFReady(void);
bool innerLoopReady(void);
void taskFiltering(timeUs_t currentTimeUs);
void taskMainInnerLoop(timeUs_t currentTimeUs);
Expand Down
26 changes: 6 additions & 20 deletions src/main/fc/tasks.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,15 +174,6 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
batteryUpdateAlarms();
}

#ifdef USE_ACC
static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{
accUpdate(currentTimeUs);
#ifdef USE_THROW_TO_ARM
updateThrowFallStateMachine(currentTimeUs);
#endif
}
#endif

typedef enum {
RX_STATE_CHECK,
Expand Down Expand Up @@ -370,12 +361,6 @@ static void taskPosCtl(timeUs_t currentTimeUs)
}
#endif

#ifdef USE_EKF
static void taskEkf(timeUs_t currentTimeUs)
{
updateEkf(currentTimeUs);
}
#endif

#ifdef USE_CAMERA_CONTROL
static void taskCameraControl(uint32_t currentTime)
Expand Down Expand Up @@ -422,9 +407,13 @@ task_attribute_t task_attributes[TASK_COUNT] = {
[TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
[TASK_INNER_LOOP] = DEFINE_TASK("INNER LOOP", NULL, NULL, taskMainInnerLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),

#ifdef USE_EKF
[TASK_EKF] = DEFINE_TASK("EKF", NULL, NULL, taskEKF, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
#endif

#ifdef USE_ACC
[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
[TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
[TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
#endif

[TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
Expand Down Expand Up @@ -482,9 +471,6 @@ task_attribute_t task_attributes[TASK_COUNT] = {
[TASK_POS_CTL] = DEFINE_TASK("POS_CTL", NULL, NULL, taskPosCtl, TASK_PERIOD_HZ(500), TASK_PRIORITY_MEDIUM),
#endif

#ifdef USE_EKF
[TASK_EKF] = DEFINE_TASK("EKF", NULL, NULL, taskEkf, TASK_PERIOD_HZ(500), TASK_PRIORITY_MEDIUM),
#endif

#ifdef USE_LED_STRIP
[TASK_LEDSTRIP] = DEFINE_TASK("LEDSTRIP", NULL, NULL, ledStripUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
Expand Down
11 changes: 11 additions & 0 deletions src/main/scheduler/scheduler.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -500,6 +500,17 @@ FAST_CODE void scheduler(void)
if (gyroFilterReady()) {
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs);
}

if (accelerometerReady()) {
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_ACCEL), currentTimeUs);
#ifndef USE_EKF
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_ATTITUDE), currentTimeUs);
#endif
}
if (EKFReady()) {
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_EKF), currentTimeUs);
}

if (innerLoopReady()) {
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_INNER_LOOP), currentTimeUs);
}
Expand Down