add simple cooperative scheduler

src/core/looptime.c

@@ -89,4 +89,6 @@ void looptime_update() {
   if (flags.arm_state) {
     state.armtime += state.looptime;
   }
+
+  state.loop_counter++;
 }

src/core/main.c

@@ -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;
@@ -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();
@@ -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();
+}

src/core/scheduler.c

@@ -0,0 +1,181 @@
+#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->last_run_time = start;
+
+  task->runtime_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->runtime_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;
+  }
+}
+
+void task_reset_runtime() {
+  if (active_task == NULL) {
+    return;
+  }
+  active_task->runtime_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();
+
+    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;
+    }
+
+    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_str(enc, "last"));
+    ENCODE_CYCLES(time_cycles() - tasks[i].last_run_time)
+
+    CBOR_CHECK_ERROR(res = cbor_encode_end_indefinite(enc));
+  }
+
+  CBOR_CHECK_ERROR(res = cbor_encode_end_indefinite(enc));
+
+  return res;
+}
+
+#endif

src/core/scheduler.h

@@ -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);