From 375c5058638f59cd9a639502e25a51be450656b0 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Sun, 29 Oct 2023 02:52:47 +0200 Subject: [PATCH] use gyro to determine desired looptime --- src/core/looptime.c | 22 +++++++++++----------- src/core/looptime.h | 6 ------ src/core/main.c | 5 +---- src/core/scheduler.c | 27 ++++++++++++++------------- src/core/tasks.c | 12 ++++-------- src/core/tasks.h | 4 +++- src/driver/at32/system.h | 2 +- src/driver/spi_gyro.c | 22 ++++++++++++++++++++++ src/driver/spi_gyro.h | 1 + src/driver/stm32/system.h | 8 ++++---- src/flight/control.h | 23 ++++++++++++----------- src/osd/render.c | 2 +- 12 files changed, 74 insertions(+), 60 deletions(-) diff --git a/src/core/looptime.c b/src/core/looptime.c index 56ec55d81..983404096 100644 --- a/src/core/looptime.c +++ b/src/core/looptime.c @@ -1,6 +1,7 @@ #include "core/looptime.h" #include "core/project.h" +#include "driver/spi_gyro.h" #include "driver/time.h" #include "flight/control.h" @@ -9,9 +10,12 @@ 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 +41,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 *= 2.0f; + } else if (loop_avg < (state.looptime_autodetect * 0.5f)) { + state.looptime_autodetect *= 0.5f; } - loop_counter++; } diff --git a/src/core/looptime.h b/src/core/looptime.h index eee2b0955..0c782cb0f 100644 --- a/src/core/looptime.h +++ b/src/core/looptime.h @@ -2,12 +2,6 @@ #include -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 index b887006fb..978d13302 100644 --- 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,9 @@ memory_section_init() { } __attribute__((__used__)) int main() { - scheduler_init(); - // init timer so we can use delays etc time_init(); - looptime_init(); + scheduler_init(); // load settings from flash flash_load(); diff --git a/src/core/scheduler.c b/src/core/scheduler.c index d2d2887ff..45fefccb7 100644 --- a/src/core/scheduler.c +++ b/src/core/scheduler.c @@ -95,16 +95,24 @@ 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(); } void scheduler_init() { - looptime_init(); for (uint32_t i = 0; i < TASK_MAX; i++) { task_queue_push(&tasks[i]); @@ -112,18 +120,11 @@ void scheduler_init() { } void scheduler_run() { - looptime_reset(); + looptime_init(); 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)) { diff --git a/src/core/tasks.c b/src/core/tasks.c index 20d4fab80..f31c832c0 100644 --- a/src/core/tasks.c +++ b/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), #ifdef ENABLE_BLACKBOX [TASK_BLACKBOX] = CREATE_TASK("BLACKBOX", TASK_MASK_ALWAYS, TASK_PRIORITY_MEDIUM, blackbox_update), #endif diff --git a/src/core/tasks.h b/src/core/tasks.h index 387fb3239..f5630b7e0 100644 --- a/src/core/tasks.h +++ b/src/core/tasks.h @@ -7,8 +7,10 @@ typedef enum { TASK_GYRO, - TASK_MAIN, + TASK_IMU, + TASK_PID, TASK_RX, + TASK_UTIL, #ifdef ENABLE_BLACKBOX TASK_BLACKBOX, #endif diff --git a/src/driver/at32/system.h b/src/driver/at32/system.h index 0c51d4373..826683572 100644 --- 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 index 6acbb6567..c76fda991 100644 --- a/src/driver/spi_gyro.c +++ b/src/driver/spi_gyro.c @@ -102,6 +102,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() { gyro_data_t data; diff --git a/src/driver/spi_gyro.h b/src/driver/spi_gyro.h index da9faf980..da5bdda5b 100644 --- a/src/driver/spi_gyro.h +++ b/src/driver/spi_gyro.h @@ -28,5 +28,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(); \ No newline at end of file diff --git a/src/driver/stm32/system.h b/src/driver/stm32/system.h index 8c135d85d..d91077ce5 100644 --- a/src/driver/stm32/system.h +++ b/src/driver/stm32/system.h @@ -53,7 +53,7 @@ #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 @@ -61,7 +61,7 @@ #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 @@ -69,7 +69,7 @@ #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 index fbffe9a89..0f7c8b12e 100644 --- a/src/flight/control.h +++ b/src/flight/control.h @@ -3,8 +3,8 @@ #include #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/osd/render.c b/src/osd/render.c index 24ecca42b..2e6928703 100644 --- a/src/osd/render.c +++ b/src/osd/render.c @@ -481,7 +481,7 @@ static uint8_t print_osd_system_status(osd_element_t *el) { printing.flags.arm_state = 0; return print_stage; } - if (((looptime_warning != last_sys_status.loop_warning) || printing.loop_warning) && (state.looptime_autodetect != LOOPTIME_8K)) { // mute warnings till we are on the edge of 4k->2k + if (((looptime_warning != last_sys_status.loop_warning) || printing.loop_warning) && (state.looptime_autodetect < 250.0f)) { // mute warnings till we are on the edge of 4k->2k last_sys_status.loop_warning = looptime_warning; printing.loop_warning = 1; print_stage = print_status(el, TEMP, LOOP);