use gyro to determine desired looptime

BossHobby · Dec 13, 2023 · 69b54d1 · 69b54d1
1 parent edc307b
commit 69b54d1
Show file tree

Hide file tree

Showing 14 changed files with 72 additions and 53 deletions.
diff --git a/src/core/looptime.c b/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++;
   }
 

diff --git a/src/core/looptime.h b/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();
diff --git a/src/core/main.c b/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);
 

diff --git a/src/driver/at32/system.h b/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

diff --git a/src/driver/spi_gyro.c b/src/driver/spi_gyro.c
@@ -111,6 +111,29 @@ 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:
+  case GYRO_TYPE_BMI323:
+    return 312.5f;
+
+  default:
+    return 250.0f;
+  }
+}
+
 gyro_data_t gyro_spi_read() {
   static gyro_data_t data;
 

diff --git a/src/driver/spi_gyro.h b/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();
diff --git a/src/driver/stm32/system.h b/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)

diff --git a/src/flight/control.h b/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)                      \

diff --git a/src/flight/filter.h b/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;

diff --git a/src/flight/sixaxis.c b/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() {

diff --git a/src/flight/sixaxis.h b/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();

diff --git a/src/io/blackbox_device.c b/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;
   }

diff --git a/src/io/blackbox_device.h b/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);

diff --git a/src/osd/status.c b/src/osd/status.c
@@ -146,7 +146,7 @@ bool osd_status_update(osd_element_t *el) {
 
   {
     extern uint8_t looptime_warning;
-    if (looptime_warning && (state.looptime_autodetect != LOOPTIME_8K)) {
+    if (looptime_warning && (state.looptime_autodetect > 125.0f)) {
       osd_status_show(MODE_HOLD, STATUS_LOOPTIME);
       return osd_status_print(el);
     }