Skip to content

Commit

Permalink
add simple cooperative scheduler
Browse files Browse the repository at this point in the history
  • Loading branch information
bkleiner committed Dec 13, 2023
1 parent fb0e13d commit 433df91
Show file tree
Hide file tree
Showing 18 changed files with 377 additions and 117 deletions.
2 changes: 2 additions & 0 deletions src/core/looptime.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,4 +91,6 @@ void looptime_update() {
if (flags.arm_state) {
state.armtime += state.looptime;
}

state.loop_counter++;
}
99 changes: 8 additions & 91 deletions src/core/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,34 +9,27 @@
#include "core/looptime.h"
#include "core/profile.h"
#include "core/project.h"
#include "core/scheduler.h"
#include "driver/adc.h"
#include "driver/fmc.h"
#include "driver/gpio.h"
#include "driver/motor.h"
#include "driver/reset.h"
#include "driver/rgb_led.h"
#include "driver/serial.h"
#include "driver/spi.h"
#include "driver/time.h"
#include "driver/timer.h"
#include "driver/usb.h"
#include "flight/control.h"
#include "flight/filter.h"
#include "flight/gestures.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/sixaxis.h"
#include "io/blackbox.h"
#include "io/buzzer.h"
#include "io/led.h"
#include "io/rgb_led.h"
#include "io/usb_configurator.h"
#include "io/vbat.h"
#include "io/vtx.h"
#include "osd/render.h"
#include "rx/rx.h"
#include "util/util.h"

__attribute__((__used__)) void memory_section_init() {
__attribute__((__used__)) void
memory_section_init() {
#ifdef USE_FAST_RAM
extern uint8_t _fast_ram_start;
extern uint8_t _fast_ram_end;
Expand Down Expand Up @@ -73,6 +66,8 @@ __attribute__((__used__)) void memory_section_init() {
}

__attribute__((__used__)) int main() {
scheduler_init();

// init timer so we can use delays etc
time_init();
looptime_init();
Expand Down Expand Up @@ -137,85 +132,7 @@ __attribute__((__used__)) int main() {

blackbox_init();
imu_init();

osd_clear();
perf_counter_init();

looptime_reset();

while (1) {
// updates looptime counters & runs auto detect
looptime_update();

perf_counter_start(PERF_COUNTER_TOTAL);

// read gyro and accelerometer data
perf_counter_start(PERF_COUNTER_GYRO);
sixaxis_read();
perf_counter_end(PERF_COUNTER_GYRO);

// all flight calculations and motors
perf_counter_start(PERF_COUNTER_CONTROL);
control();
perf_counter_end(PERF_COUNTER_CONTROL);

perf_counter_start(PERF_COUNTER_MISC);

// attitude calculations for level mode
imu_calc();

// battery low logic
vbat_calc();

// check gestures
if (flags.on_ground && !flags.gestures_disabled) {
gestures();
}

// handle led commands
led_update();

#if (RGB_LED_NUMBER > 0)
// RGB led control
rgb_led_lvc();
#ifdef RGB_LED_DMA
rgb_dma_start();
#endif
#endif

buzzer_update();
vtx_update();

perf_counter_end(PERF_COUNTER_MISC);

// receiver function
perf_counter_start(PERF_COUNTER_RX);
rx_update();
perf_counter_end(PERF_COUNTER_RX);

perf_counter_start(PERF_COUNTER_BLACKBOX);
const uint8_t blackbox_active = blackbox_update();
perf_counter_end(PERF_COUNTER_BLACKBOX);

if (!blackbox_active) {
perf_counter_start(PERF_COUNTER_OSD);
osd_display();
perf_counter_end(PERF_COUNTER_OSD);
}

if (usb_detect()) {
flags.usb_active = 1;
#ifndef ALLOW_USB_ARMING
if (flags.arm_switch)
flags.arm_safety = 1; // final safety check to disallow arming during USB operation
#endif
usb_configurator();
} else {
flags.usb_active = 0;
motor_test.active = 0;
}

perf_counter_end(PERF_COUNTER_TOTAL);
perf_counter_update();
} // end loop
}
scheduler_run();
}
179 changes: 179 additions & 0 deletions src/core/scheduler.c
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,179 @@
#include "core/scheduler.h"

#include <stdbool.h>
#include <string.h>

#include "core/debug.h"
#include "core/looptime.h"
#include "driver/time.h"
#include "flight/control.h"
#include "io/usb_configurator.h"
#include "tasks.h"
#include "util/cbor_helper.h"

#define TASK_AVERAGE_SAMPLES 32
#define TASK_RUNTIME_REDUCTION 0.75
#define TASK_RUNTIME_MARGIN 1.25
#define TASK_RUNTIME_BUFFER 10

static FAST_RAM uint32_t task_queue_size = 0;
static FAST_RAM task_t *task_queue[TASK_MAX];
static FAST_RAM task_t *active_task = NULL;

static bool task_queue_contains(task_t *task) {
for (uint32_t i = 0; i < task_queue_size; i++) {
if (task_queue[i] == task) {
return true;
}
}
return false;
}

static bool task_queue_push(task_t *task) {
if (task_queue_size >= TASK_MAX || task_queue_contains(task)) {
return false;
}
for (uint32_t i = 0; i < TASK_MAX; i++) {
if (task_queue[i] != NULL && task_queue[i]->priority <= task->priority) {
continue;
}

memcpy(task_queue + i + 1, task_queue + i, (task_queue_size - i) * sizeof(task_t *));
task_queue[i] = task;
task_queue_size++;
return true;
}
return false;
}

static inline bool task_should_run(const uint32_t start_cycles, uint8_t task_mask, task_t *task) {
if ((task_mask & task->mask) == 0) {
// task shall not run in this firmware state
return false;
}

const int32_t time_left = US_TO_CYCLES(state.looptime_autodetect - TASK_RUNTIME_BUFFER) - (time_cycles() - start_cycles);
if (task->priority != TASK_PRIORITY_REALTIME && task->runtime_worst > time_left) {
// we dont have any time left this loop and task is not realtime
task->runtime_worst *= TASK_RUNTIME_REDUCTION;
return false;
}

return true;
}

static inline void task_run(task_t *task) {
const volatile uint32_t start = time_cycles();

task->flags = 0;
active_task = task;
task->func();
active_task = NULL;

const volatile uint32_t time_taken = time_cycles() - start;
task->runtime_current = time_taken;

if (task->flags & TASK_FLAG_SKIP_STATS) {
return;
}

if (time_taken < task->runtime_min) {
task->runtime_min = time_taken;
}

task->runtime_avg_sum -= task->runtime_avg;
task->runtime_avg_sum += time_taken;
task->runtime_avg = task->runtime_avg_sum / TASK_AVERAGE_SAMPLES;

if (time_taken > task->runtime_max) {
task->runtime_max = time_taken;
}

if (task->runtime_worst < (task->runtime_avg * TASK_RUNTIME_MARGIN)) {
task->runtime_worst = task->runtime_avg * TASK_RUNTIME_MARGIN;
}
}

static inline uint8_t scheduler_task_mask() {
uint8_t task_mask = TASK_MASK_DEFAULT;
if (flags.in_air || flags.arm_state) {
task_mask |= TASK_MASK_IN_AIR;
} else {
task_mask |= TASK_MASK_ON_GROUND;
}
return task_mask;
}

void task_reset_runtime() {
if (active_task != NULL) {
active_task->flags |= TASK_FLAG_SKIP_STATS;
}
looptime_reset();
}

void scheduler_init() {
looptime_init();

for (uint32_t i = 0; i < TASK_MAX; i++) {
task_queue_push(&tasks[i]);
}
}

void scheduler_run() {
looptime_reset();

while (1) {
const volatile uint32_t cycles = time_cycles();
const uint8_t task_mask = scheduler_task_mask();
for (uint32_t i = 0; i < task_queue_size; i++) {
task_t *task = task_queue[i];
if (task_should_run(cycles, task_mask, task)) {
task_run(task);
}
}

looptime_update();
}
}

#ifdef DEBUG

#define ENCODE_CYCLES(val) \
{ \
const uint32_t us = CYCLES_TO_US(val); \
CBOR_CHECK_ERROR(res = cbor_encode_uint32_t(enc, &us)); \
}

cbor_result_t cbor_encode_task_stats(cbor_value_t *enc) {
CBOR_CHECK_ERROR(cbor_result_t res = cbor_encode_array_indefinite(enc));

for (uint32_t i = 0; i < TASK_MAX; i++) {
CBOR_CHECK_ERROR(res = cbor_encode_map_indefinite(enc));

CBOR_CHECK_ERROR(res = cbor_encode_str(enc, "name"));
CBOR_CHECK_ERROR(res = cbor_encode_str(enc, tasks[i].name));

CBOR_CHECK_ERROR(res = cbor_encode_str(enc, "current"));
ENCODE_CYCLES(tasks[i].runtime_current)

CBOR_CHECK_ERROR(res = cbor_encode_str(enc, "min"));
ENCODE_CYCLES(tasks[i].runtime_min)

CBOR_CHECK_ERROR(res = cbor_encode_str(enc, "avg"));
ENCODE_CYCLES(tasks[i].runtime_avg)

CBOR_CHECK_ERROR(res = cbor_encode_str(enc, "max"));
ENCODE_CYCLES(tasks[i].runtime_max)

CBOR_CHECK_ERROR(res = cbor_encode_str(enc, "worst"));
ENCODE_CYCLES(tasks[i].runtime_worst)

CBOR_CHECK_ERROR(res = cbor_encode_end_indefinite(enc));
}

CBOR_CHECK_ERROR(res = cbor_encode_end_indefinite(enc));

return res;
}

#endif
12 changes: 12 additions & 0 deletions src/core/scheduler.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
#pragma once

#include <stdint.h>

#include <cbor.h>

void scheduler_init();
void scheduler_run();

void task_reset_runtime();

cbor_result_t cbor_encode_task_stats(cbor_value_t *enc);
53 changes: 53 additions & 0 deletions src/core/tasks.c
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
#include "tasks.h"

#include <stddef.h>

#include "driver/serial.h"
#include "driver/usb.h"
#include "flight/control.h"
#include "flight/gestures.h"
#include "flight/imu.h"
#include "flight/sixaxis.h"
#include "io/blackbox.h"
#include "io/buzzer.h"
#include "io/led.h"
#include "io/usb_configurator.h"
#include "io/vbat.h"
#include "io/vtx.h"
#include "osd/render.h"
#include "profile.h"
#include "project.h"
#include "rx/rx.h"

void util_task() {
// battery low logic
vbat_calc();

// check gestures
gestures();

// handle led commands
led_update();

#if (RGB_LED_NUMBER > 0)
// RGB led control
rgb_led_lvc();
#ifdef RGB_LED_DMA
rgb_dma_start();
#endif
#endif

buzzer_update();
}

FAST_RAM task_t tasks[TASK_MAX] = {
[TASK_GYRO] = CREATE_TASK("GYRO", TASK_MASK_ALWAYS, TASK_PRIORITY_REALTIME, sixaxis_read),
[TASK_IMU] = CREATE_TASK("IMU", TASK_MASK_ALWAYS, TASK_PRIORITY_REALTIME, imu_calc),
[TASK_PID] = CREATE_TASK("PID", TASK_MASK_ALWAYS, TASK_PRIORITY_REALTIME, control),
[TASK_RX] = CREATE_TASK("RX", TASK_MASK_ALWAYS, TASK_PRIORITY_REALTIME, rx_update),
[TASK_UTIL] = CREATE_TASK("UTIL", TASK_MASK_ALWAYS, TASK_PRIORITY_HIGH, util_task),
[TASK_BLACKBOX] = CREATE_TASK("BLACKBOX", TASK_MASK_ALWAYS, TASK_PRIORITY_MEDIUM, blackbox_update),
[TASK_OSD] = CREATE_TASK("OSD", TASK_MASK_ALWAYS, TASK_PRIORITY_MEDIUM, osd_display),
[TASK_VTX] = CREATE_TASK("VTX", TASK_MASK_ON_GROUND, TASK_PRIORITY_LOW, vtx_update),
[TASK_USB] = CREATE_TASK("USB", TASK_MASK_ON_GROUND, TASK_PRIORITY_LOW, usb_configurator),
};
Loading

0 comments on commit 433df91

Please sign in to comment.