From 157c37280107f4e1beabf9f486a18bc3175af60a 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 | 23 ++++++++++++----------- src/core/looptime.h | 6 ------ src/core/main.c | 9 ++++----- src/core/scheduler.c | 24 +++++++++++++----------- 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/flight/filter.h | 6 +++--- src/flight/sixaxis.c | 11 +++++------ src/flight/sixaxis.h | 3 ++- src/io/blackbox_device.c | 2 +- src/io/blackbox_device.h | 6 +++--- src/osd/render.c | 2 +- 17 files changed, 91 insertions(+), 73 deletions(-) diff --git a/src/core/looptime.c b/src/core/looptime.c index 56ec55d81..5ba12bbc4 100644 --- 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 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..4446c3b98 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,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/core/scheduler.c b/src/core/scheduler.c index d2d2887ff..d9cc38800 100644 --- a/src/core/scheduler.c +++ b/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)) { diff --git a/src/core/tasks.c b/src/core/tasks.c index e8254ae07..c12dd5b91 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), [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), diff --git a/src/core/tasks.h b/src/core/tasks.h index 19be65741..955f9e2cc 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, TASK_BLACKBOX, TASK_OSD, TASK_VTX, 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 7d2532558..cc05ff477 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() { static 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/flight/filter.h b/src/flight/filter.h index 652a47357..e1344520b 100644 --- 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 index c809a6043..4228c762e 100644 --- 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(¬ch_filter[i][j], ¬ch_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 index 2c5420b49..24989c195 100644 --- a/src/flight/sixaxis.h +++ b/src/flight/sixaxis.h @@ -3,7 +3,8 @@ #include #include -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 index ca292ec59..b00dd2257 100644 --- 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 index 7a844314c..c1819a864 100644 --- a/src/io/blackbox_device.h +++ b/src/io/blackbox_device.h @@ -43,7 +43,7 @@ typedef struct { typedef struct { uint32_t field_flags; - uint32_t looptime; + float looptime; uint8_t blackbox_rate; uint32_t start; uint32_t size; @@ -51,7 +51,7 @@ typedef struct { #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/render.c b/src/osd/render.c index 24ecca42b..3039b6989 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 > 125.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);