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/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 a20beacc4..b10745e02 100644 --- a/src/driver/spi_gyro.c +++ b/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; diff --git a/src/driver/spi_gyro.h b/src/driver/spi_gyro.h index d372ed7b4..0a6ced028 100644 --- 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(); \ 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 a7e642711..3f57a305b 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/status.c b/src/osd/status.c index bec76f33c..1f5b56257 100644 --- 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); }