use gyro to determine desired looptime

src/core/looptime.c

@@ -1,17 +1,22 @@
 #include "core/looptime.h"
 
 #include "core/project.h"
+#include "driver/spi_gyro.h"
 #include "driver/time.h"
 #include "flight/control.h"
+#include "util/util.h"
 
 uint8_t looptime_warning = 0;
 
 static uint32_t last_loop_cycles;
 
 void looptime_init() {
-  // attempt 8k looptime for f405 or 4k looptime for f411
-  state.looptime = LOOPTIME * 1e-6;
-  state.looptime_autodetect = LOOPTIME;
+  float target = gyro_update_period();
+  while (target < LOOPTIME_MAX)
+    target *= 2.0f;
+  state.looptime = target * 1e-6f;
+  state.looptime_us = target;
+  state.looptime_autodetect = target;
 
   looptime_reset();
 }
@@ -37,15 +42,11 @@ static void looptime_auto_detect() {
 
   if (loop_counter == 200) {
     loop_avg /= 200;
-
-    if (loop_avg < 130.f) {
-      state.looptime_autodetect = LOOPTIME_8K;
-    } else if (loop_avg < 255.f) {
-      state.looptime_autodetect = LOOPTIME_4K;
-    } else {
-      state.looptime_autodetect = LOOPTIME_2K;
+    if (loop_avg > (state.looptime_autodetect + 5.0f)) {
+      state.looptime_autodetect = min(500, state.looptime_autodetect * 2.0f);
+    } else if (loop_avg < (state.looptime_autodetect * 0.5f)) {
+      state.looptime_autodetect = max(LOOPTIME_MAX, state.looptime_autodetect * 0.5f);
     }
-
     loop_counter++;
   }
 

src/core/looptime.h

@@ -2,12 +2,6 @@
 
 #include <stdint.h>
 
-typedef enum {
-  LOOPTIME_2K = 500,
-  LOOPTIME_4K = 250,
-  LOOPTIME_8K = 125,
-} looptime_t;
-
 void looptime_init();
 void looptime_reset();
 void looptime_update();

src/core/main.c

@@ -6,7 +6,6 @@
 #include "core/debug.h"
 #include "core/failloop.h"
 #include "core/flash.h"
-#include "core/looptime.h"
 #include "core/profile.h"
 #include "core/project.h"
 #include "core/scheduler.h"
@@ -66,11 +65,8 @@ memory_section_init() {
 }
 
 __attribute__((__used__)) int main() {
-  scheduler_init();
-
   // init timer so we can use delays etc
   time_init();
-  looptime_init();
 
   // load settings from flash
   flash_load();
@@ -104,11 +100,14 @@ __attribute__((__used__)) int main() {
   // wait for devices to wake up
   time_delay_ms(300);
 
-  if (!sixaxis_init()) {
+  if (!sixaxis_detect()) {
     // gyro not found
     failloop(FAILLOOP_GYRO);
   }
 
+  scheduler_init();
+  sixaxis_init();
+
   // give the gyro some time to settle
   time_delay_ms(100);
 

src/core/scheduler.c

@@ -95,11 +95,20 @@ static inline void task_run(task_t *task) {
   }
 }
 
+static 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) {
-    return;
+  if (active_task != NULL) {
+    active_task->flags |= TASK_FLAG_SKIP_STATS;
   }
-  active_task->flags |= TASK_FLAG_SKIP_STATS;
   looptime_reset();
 }
 
@@ -116,14 +125,7 @@ void scheduler_run() {
 
   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;
-    }
-
+    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)) {

src/core/tasks.c

@@ -19,13 +19,7 @@
 #include "project.h"
 #include "rx/rx.h"
 
-void task_main() {
-  // all flight calculations and motors
-  control();
-
-  // attitude calculations for level mode
-  imu_calc();
-
+void util_task() {
   // battery low logic
   vbat_calc();
 
@@ -48,8 +42,10 @@ void task_main() {
 
 FAST_RAM task_t tasks[TASK_MAX] = {
     [TASK_GYRO] = CREATE_TASK("GYRO", TASK_MASK_ALWAYS, TASK_PRIORITY_REALTIME, sixaxis_read),
-    [TASK_MAIN] = CREATE_TASK("MAIN", TASK_MASK_ALWAYS, TASK_PRIORITY_REALTIME, task_main),
+    [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_HIGH, 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),

src/core/tasks.h

@@ -7,8 +7,10 @@
 
 typedef enum {
   TASK_GYRO,
-  TASK_MAIN,
+  TASK_IMU,
+  TASK_PID,
   TASK_RX,
+  TASK_UTIL,
   TASK_BLACKBOX,
   TASK_OSD,
   TASK_VTX,

src/driver/at32/system.h

@@ -10,7 +10,7 @@
 #define PWM_CLOCK_FREQ_HZ 288000000
 #define SPI_CLOCK_FREQ_HZ (SYS_CLOCK_FREQ_HZ / 2)
 
-#define LOOPTIME LOOPTIME_8K
+#define LOOPTIME_MAX 125
 
 #define UID_BASE 0x1FFFF7E8
 #endif

src/driver/spi_gyro.c

@@ -111,6 +111,28 @@ uint8_t gyro_spi_init() {
   return gyro_type;
 }
 
+float gyro_update_period() {
+  switch (gyro_type) {
+  case GYRO_TYPE_MPU6000:
+  case GYRO_TYPE_MPU6500:
+  case GYRO_TYPE_ICM20601:
+  case GYRO_TYPE_ICM20602:
+  case GYRO_TYPE_ICM20608:
+  case GYRO_TYPE_ICM20689:
+    return 125.0f;
+
+  case GYRO_TYPE_ICM42605:
+  case GYRO_TYPE_ICM42688P:
+    return 125.0f;
+
+  case GYRO_TYPE_BMI270:
+    return 312.5f;
+
+  default:
+    return 250.0f;
+  }
+}
+
 gyro_data_t gyro_spi_read() {
   static gyro_data_t data;
 

src/driver/spi_gyro.h

@@ -29,5 +29,6 @@ typedef struct {
 extern gyro_types_t gyro_type;
 
 uint8_t gyro_spi_init();
+float gyro_update_period();
 gyro_data_t gyro_spi_read();
 void gyro_spi_calibrate();

src/driver/stm32/system.h

@@ -53,23 +53,23 @@
 #define PWM_CLOCK_FREQ_HZ 108000000
 #define SPI_CLOCK_FREQ_HZ (SYS_CLOCK_FREQ_HZ / 2)
 
-#define LOOPTIME LOOPTIME_4K
+#define LOOPTIME_MAX 250
 #endif
 
 #ifdef STM32F405
 #define SYS_CLOCK_FREQ_HZ 168000000
 #define PWM_CLOCK_FREQ_HZ 84000000
 #define SPI_CLOCK_FREQ_HZ (SYS_CLOCK_FREQ_HZ / 4)
 
-#define LOOPTIME LOOPTIME_8K
+#define LOOPTIME_MAX 125
 #endif
 
 #ifdef STM32F7
 #define SYS_CLOCK_FREQ_HZ 216000000
 #define PWM_CLOCK_FREQ_HZ 216000000
 #define SPI_CLOCK_FREQ_HZ (SYS_CLOCK_FREQ_HZ / 4)
 
-#define LOOPTIME LOOPTIME_8K
+#define LOOPTIME_MAX 125
 
 #define WITHIN_DTCM_RAM(p) (((uint32_t)p & 0xffff0000) == 0x20000000)
 #define WITHIN_DMA_RAM(p) (false)
@@ -80,7 +80,7 @@
 #define PWM_CLOCK_FREQ_HZ (SYS_CLOCK_FREQ_HZ / 2)
 #define SPI_CLOCK_FREQ_HZ (SYS_CLOCK_FREQ_HZ / 4)
 
-#define LOOPTIME LOOPTIME_8K
+#define LOOPTIME_MAX 125
 
 #define WITHIN_DTCM_RAM(p) (((uint32_t)p & 0xfffe0000) == 0x20000000)
 #define WITHIN_DMA_RAM(p) (((uint32_t)p & 0xfffe0000) == 0x30000000)

src/flight/control.h

@@ -3,8 +3,8 @@
 #include <stdint.h>
 
 #include "core/failloop.h"
-#include "core/looptime.h"
 #include "core/project.h"
+#include "core/scheduler.h"
 #include "rx/rx.h"
 #include "util/vector.h"
 
@@ -43,14 +43,15 @@ extern control_flags_t flags;
 typedef struct {
   failloop_t failloop;
 
-  looptime_t looptime_autodetect;
-  float looptime;        // looptime in seconds
-  float timefactor;      // timefactor for pid calc
-  uint32_t looptime_us;  // looptime in us
-  uint32_t loop_counter; // number of loops ran
-  float uptime;          // running sum of looptimes
-  float armtime;         // running sum of looptimes (while armed)
-  uint32_t cpu_load;     // micros we have had left last loop
+  float looptime;            // looptime in seconds
+  float looptime_us;         // looptime in us
+  float looptime_autodetect; // desired looptime in us
+  float timefactor;          // timefactor for pid calc
+  uint32_t loop_counter;     // number of loops ran
+
+  float uptime;      // running sum of looptimes
+  float armtime;     // running sum of looptimes (while armed)
+  uint32_t cpu_load; // micros we have had left last loop
 
   uint32_t failsafe_time_ms; // time the last failsafe occured in ms
 
@@ -109,10 +110,10 @@ typedef struct {
 
 #define STATE_MEMBERS                         \
   MEMBER(failloop, uint8_t)                   \
-  MEMBER(looptime_autodetect, uint16_t)       \
   MEMBER(looptime, float)                     \
+  MEMBER(looptime_us, float)                  \
+  MEMBER(looptime_autodetect, float)          \
   MEMBER(timefactor, float)                   \
-  MEMBER(looptime_us, uint32_t)               \
   MEMBER(loop_counter, uint32_t)              \
   MEMBER(uptime, float)                       \
   MEMBER(armtime, float)                      \

src/flight/filter.h

@@ -26,21 +26,21 @@ typedef struct {
 
 typedef struct {
   float hz;
-  uint32_t sample_period_us;
+  float sample_period_us;
 
   float alpha;
 } filter_lp_pt1;
 
 typedef struct {
   float hz;
-  uint32_t sample_period_us;
+  float sample_period_us;
 
   float alpha;
 } filter_lp_pt2;
 
 typedef struct {
   float hz;
-  uint32_t sample_period_us;
+  float sample_period_us;
 
   float alpha;
 } filter_lp_pt3;

src/flight/sixaxis.c

@@ -44,11 +44,12 @@ static sdft_t gyro_sdft[SDFT_AXES];
 static filter_biquad_notch_t notch_filter[SDFT_AXES][SDFT_PEAKS];
 static filter_biquad_state_t notch_filter_state[SDFT_AXES][SDFT_PEAKS];
 
-bool sixaxis_init() {
-  const gyro_types_t id = gyro_spi_init();
-
-  target_info.gyro_id = id;
+bool sixaxis_detect() {
+  target_info.gyro_id = gyro_spi_init();
+  return target_info.gyro_id != GYRO_TYPE_INVALID;
+}
 
+void sixaxis_init() {
   for (uint8_t i = 0; i < FILTER_MAX_SLOTS; i++) {
     filter_init(profile.filter.gyro[i].type, &filter[i], filter_state[i], 3, profile.filter.gyro[i].cutoff_freq);
   }
@@ -59,8 +60,6 @@ bool sixaxis_init() {
       filter_biquad_notch_init(&notch_filter[i][j], &notch_filter_state[i][j], 1, 0);
     }
   }
-
-  return id != GYRO_TYPE_INVALID;
 }
 
 void sixaxis_read() {

src/flight/sixaxis.h

@@ -3,7 +3,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-bool sixaxis_init();
+bool sixaxis_detect();
+void sixaxis_init();
 void sixaxis_read();
 
 void sixaxis_gyro_cal();

src/io/blackbox_device.c

@@ -118,7 +118,7 @@ void blackbox_device_reset() {
   task_reset_runtime();
 }
 
-bool blackbox_device_restart(uint32_t field_flags, uint32_t blackbox_rate, uint32_t looptime) {
+bool blackbox_device_restart(uint32_t field_flags, uint32_t blackbox_rate, float looptime) {
   if (dev == NULL) {
     return false;
   }

src/io/blackbox_device.h

@@ -43,15 +43,15 @@ typedef struct {
 
 typedef struct {
   uint32_t field_flags;
-  uint32_t looptime;
+  float looptime;
   uint8_t blackbox_rate;
   uint32_t start;
   uint32_t size;
 } blackbox_device_file_t;
 
 #define BLACKBOX_DEVICE_FILE_MEMBERS \
   MEMBER(field_flags, uint32_t)      \
-  MEMBER(looptime, uint32_t)         \
+  MEMBER(looptime, float)            \
   MEMBER(blackbox_rate, uint8_t)     \
   MEMBER(start, uint32_t)            \
   MEMBER(size, uint32_t)
@@ -86,7 +86,7 @@ uint32_t blackbox_device_usage();
 blackbox_device_file_t *blackbox_current_file();
 
 void blackbox_device_reset();
-bool blackbox_device_restart(uint32_t field_flags, uint32_t blackbox_rate, uint32_t looptime);
+bool blackbox_device_restart(uint32_t field_flags, uint32_t blackbox_rate, float looptime);
 void blackbox_device_finish();
 
 void blackbox_device_read(const uint32_t file_index, const uint32_t offset, uint8_t *buffer, const uint32_t size);