diff --git a/src/core/scheduler.c b/src/core/scheduler.c index c2f4b9254..4526680c5 100644 --- a/src/core/scheduler.c +++ b/src/core/scheduler.c @@ -5,11 +5,12 @@ #include "core/debug.h" #include "core/looptime.h" +#include "core/tasks.h" +#include "driver/gyro/gyro.h" #include "driver/time.h" #include "flight/control.h" #include "io/simulator.h" #include "io/usb_configurator.h" -#include "tasks.h" #include "util/cbor_helper.h" #define TASK_AVERAGE_SAMPLES 32 @@ -129,6 +130,8 @@ void scheduler_run() { looptime_reset(); while (1) { + gyro_wait_for_ready(); + debug_pin_toggle(1); simulator_update(); const volatile uint32_t cycles = time_cycles(); diff --git a/src/driver/gyro/gyro.c b/src/driver/gyro/gyro.c index 8cd822d74..4e6a3d768 100644 --- a/src/driver/gyro/gyro.c +++ b/src/driver/gyro/gyro.c @@ -1,6 +1,8 @@ #include "driver/gyro/gyro.h" +#include "core/debug.h" #include "core/project.h" +#include "driver/exti.h" #include "driver/spi.h" #include "driver/time.h" @@ -67,15 +69,6 @@ uint8_t gyro_int() { return GYRO_TYPE_INVALID; } - if (target.gyro.exti != PIN_NONE) { - gpio_config_t gpio_init; - gpio_init.mode = GPIO_INPUT; - gpio_init.output = GPIO_OPENDRAIN; - gpio_init.pull = GPIO_NO_PULL; - gpio_init.drive = GPIO_DRIVE_HIGH; - gpio_pin_init(target.gyro.exti, gpio_init); - } - gyro_bus.port = target.gyro.port; gyro_bus.nss = target.gyro.nss; spi_bus_device_init(&gyro_bus); @@ -108,18 +101,19 @@ uint8_t gyro_int() { break; } - gyro_read(); // dummy read to fill buffers + if (target.gyro.exti != PIN_NONE) { + gpio_config_t gpio_init; + gpio_init.mode = GPIO_INPUT; + gpio_init.output = GPIO_OPENDRAIN; + gpio_init.pull = GPIO_NO_PULL; + gpio_init.drive = GPIO_DRIVE_HIGH; + gpio_pin_init(target.gyro.exti, gpio_init); + exti_enable(target.gyro.exti, EXTI_TRIG_RISING); + } return gyro_type; } -bool gyro_exti_state() { - if (target.gyro.exti == PIN_NONE) { - return true; - } - return gpio_pin_read(target.gyro.exti); -} - float gyro_update_period() { switch (gyro_type) { case GYRO_TYPE_MPU6000: @@ -143,7 +137,72 @@ float gyro_update_period() { } } +typedef enum { + GYRO_IDLE, + GYRO_READING, + GYRO_DATA_READY, +} gyro_read_state_t; + +static volatile gyro_read_state_t gyro_read_state = GYRO_IDLE; + +static void gyro_set_ready(void *arg) { + gyro_read_state = GYRO_DATA_READY; + debug_pin_disable(0); +} + +static void gyro_start_read() { + if (gyro_read_state != GYRO_IDLE) { + return; + } + + debug_pin_enable(0); + + gyro_read_state = GYRO_READING; + + 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: { + mpu6xxx_start_read(gyro_set_ready); + break; + } + + case GYRO_TYPE_ICM42605: + case GYRO_TYPE_ICM42688P: { + // icm42605_start_read(); + break; + } + + case GYRO_TYPE_BMI270: { + // bmi270_start_read(); + break; + } + case GYRO_TYPE_BMI323: { + // bmi323_start_read(); + break; + } + + default: + break; + } +} + +void gyro_wait_for_ready() { + while (gyro_read_state != GYRO_DATA_READY) + ; +} + gyro_data_t gyro_read() { + // if (gyro_read_state == GYRO_IDLE) { + // gyro_start_read(); + // } + while (gyro_read_state != GYRO_DATA_READY) + ; + gyro_read_state = GYRO_IDLE; + static gyro_data_t data; switch (gyro_type) { @@ -179,6 +238,10 @@ gyro_data_t gyro_read() { return data; } +void gyro_handle_exti(bool level) { + gyro_start_read(); +} + void gyro_calibrate() { switch (gyro_type) { case GYRO_TYPE_BMI270: { diff --git a/src/driver/gyro/gyro.h b/src/driver/gyro/gyro.h index 201049a3a..999ed3ca8 100644 --- a/src/driver/gyro/gyro.h +++ b/src/driver/gyro/gyro.h @@ -1,5 +1,6 @@ #pragma once +#include "driver/spi.h" #include "util/vector.h" typedef enum { @@ -29,8 +30,8 @@ typedef struct { extern gyro_types_t gyro_type; float gyro_update_period(); -bool gyro_exti_state(); uint8_t gyro_int(); gyro_data_t gyro_read(); -void gyro_calibrate(); \ No newline at end of file +void gyro_calibrate(); +void gyro_wait_for_ready(); \ No newline at end of file diff --git a/src/driver/gyro/mpu6xxx.c b/src/driver/gyro/mpu6xxx.c index b1c2e72e2..ec6bbe804 100644 --- a/src/driver/gyro/mpu6xxx.c +++ b/src/driver/gyro/mpu6xxx.c @@ -112,10 +112,19 @@ void mpu6xxx_write(uint8_t reg, uint8_t data) { spi_seg_submit_wait(&gyro_bus, segs); } -void mpu6xxx_read_gyro_data(gyro_data_t *data) { +void mpu6xxx_start_read(spi_txn_done_fn_t done_fn) { spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, mpu6xxx_fast_divider()); - spi_txn_wait(&gyro_bus); + const spi_txn_segment_t segs[] = { + spi_make_seg_const(MPU_RA_ACCEL_XOUT_H | 0x80), + spi_make_seg_buffer(gyro_buf, NULL, 14), + }; + spi_seg_submit(&gyro_bus, segs, .done_fn = done_fn); + while (!spi_txn_continue(&gyro_bus)) + ; +} + +void mpu6xxx_read_gyro_data(gyro_data_t *data) { data->accel.pitch = -(int16_t)((gyro_buf[0] << 8) | gyro_buf[1]); data->accel.roll = -(int16_t)((gyro_buf[2] << 8) | gyro_buf[3]); data->accel.yaw = (int16_t)((gyro_buf[4] << 8) | gyro_buf[5]); @@ -125,14 +134,6 @@ void mpu6xxx_read_gyro_data(gyro_data_t *data) { data->gyro.pitch = (int16_t)((gyro_buf[8] << 8) | gyro_buf[9]); data->gyro.roll = (int16_t)((gyro_buf[10] << 8) | gyro_buf[11]); data->gyro.yaw = (int16_t)((gyro_buf[12] << 8) | gyro_buf[13]); - - const spi_txn_segment_t segs[] = { - spi_make_seg_const(MPU_RA_ACCEL_XOUT_H | 0x80), - spi_make_seg_buffer(gyro_buf, NULL, 14), - }; - spi_seg_submit(&gyro_bus, segs); - while (!spi_txn_continue(&gyro_bus)) - ; } #endif \ No newline at end of file diff --git a/src/driver/gyro/mpu6xxx.h b/src/driver/gyro/mpu6xxx.h index e202e078c..ae7a0d6b9 100644 --- a/src/driver/gyro/mpu6xxx.h +++ b/src/driver/gyro/mpu6xxx.h @@ -133,4 +133,5 @@ void mpu6xxx_configure(); void mpu6xxx_write(uint8_t reg, uint8_t data); uint8_t mpu6xxx_read(uint8_t reg); -void mpu6xxx_read_gyro_data(gyro_data_t *data); \ No newline at end of file +void mpu6xxx_read_gyro_data(gyro_data_t *data); +void mpu6xxx_start_read(spi_txn_done_fn_t done_fn); \ No newline at end of file diff --git a/src/driver/mcu/stm32/exti.c b/src/driver/mcu/stm32/exti.c index e0f2e84cf..7b811bdda 100644 --- a/src/driver/mcu/stm32/exti.c +++ b/src/driver/mcu/stm32/exti.c @@ -103,6 +103,10 @@ const uint32_t exti_trigger_map[] = { }; void exti_enable(gpio_pins_t pin, exti_trigger_t trigger) { + if (pin == PIN_NONE) { + return; + } + exti_set_source(pin); LL_EXTI_ClearFlag_0_31(LINE.exti_line); @@ -139,6 +143,11 @@ bool exti_line_active(gpio_pins_t pin) { } static void handle_exit_isr() { + if (exti_line_active(target.gyro.exti)) { + extern void gyro_handle_exti(bool); + gyro_handle_exti(gpio_pin_read(target.gyro.exti)); + } + if (exti_line_active(target.rx_spi.exti)) { extern void rx_spi_handle_exti(bool); rx_spi_handle_exti(gpio_pin_read(target.rx_spi.exti)); diff --git a/src/flight/sixaxis.c b/src/flight/sixaxis.c index 34d7059cd..7411d2d8b 100644 --- a/src/flight/sixaxis.c +++ b/src/flight/sixaxis.c @@ -127,13 +127,12 @@ static vec3_t sixaxis_apply_matrix(vec3_t v) { } void sixaxis_read() { - sixaxis_compute_matrix(); + const gyro_data_t data = gyro_read(); filter_coeff(profile.filter.gyro[0].type, &filter[0], profile.filter.gyro[0].cutoff_freq); filter_coeff(profile.filter.gyro[1].type, &filter[1], profile.filter.gyro[1].cutoff_freq); - const gyro_data_t data = gyro_read(); - + sixaxis_compute_matrix(); const vec3_t accel = sixaxis_apply_matrix(data.accel); // swap pitch and roll to match gyro state.accel_raw.roll = (accel.pitch - flash_storage.accelcal[1]) * ACCEL_RANGE;