wip

src/config/config.h

@@ -261,7 +261,7 @@
 #define FAILSAFE_LOCK_TIME_MS 5000
 
 // debug things ( debug struct and other)
-// #define DEBUG
+#define DEBUG
 // #define DEBUG_LOGGING
 // #define RESET_ON_FAULT
 
@@ -272,4 +272,7 @@
 // #define NOMOTORS
 
 // change mixer to a plus configuration
-// #define MOTOR_PLUS_CONFIGURATION
+// #define MOTOR_PLUS_CONFIGURATION
+
+#define DEBUG_PIN0 PIN_A10
+#define DEBUG_PIN1 PIN_A9

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();

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: {

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();
+void gyro_calibrate();
+void gyro_wait_for_ready();

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

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);
+void mpu6xxx_read_gyro_data(gyro_data_t *data);
+void mpu6xxx_start_read(spi_txn_done_fn_t done_fn);

src/driver/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));

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;