summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/core/ao_flight.c8
-rw-r--r--src/core/ao_flight.h4
-rw-r--r--src/drivers/ao_mpu6000.c104
-rw-r--r--src/stm/ao_usb_stm.c2
-rw-r--r--src/telemega-v0.1/ao_pins.h1
-rw-r--r--src/telemega-v1.0/ao_pins.h3
6 files changed, 71 insertions, 51 deletions
diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index 4a53bdc6..463ff4a2 100644
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -46,6 +46,11 @@ __pdata enum ao_flight_state ao_flight_state; /* current flight state */
__pdata uint16_t ao_boost_tick; /* time of launch detect */
__pdata uint16_t ao_motor_number; /* number of motors burned so far */
+#if HAS_IMU
+/* Any sensor can set this to mark the flight computer as 'broken' */
+__xdata uint8_t ao_sensor_errors;
+#endif
+
/*
* track min/max data over a long interval to detect
* resting
@@ -99,6 +104,9 @@ ao_flight(void)
ao_config.accel_minus_g == 0 ||
ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP ||
+#if HAS_IMU
+ ao_sensor_errors ||
+#endif
ao_ground_height < -1000 ||
ao_ground_height > 7000)
{
diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h
index ac3e9cfb..c7c02ccf 100644
--- a/src/core/ao_flight.h
+++ b/src/core/ao_flight.h
@@ -41,6 +41,10 @@ extern __pdata enum ao_flight_state ao_flight_state;
extern __pdata uint16_t ao_boost_tick;
extern __pdata uint16_t ao_motor_number;
+#if HAS_IMU
+extern __xdata uint8_t ao_sensor_errors;
+#endif
+
extern __pdata uint16_t ao_launch_time;
extern __pdata uint8_t ao_flight_force_idle;
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index 5e75b78a..f8ce7346 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -137,10 +137,10 @@ ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
{
int16_t diff = test - normal;
- if (diff < MPU6000_ST_ACCEL(16) / 2) {
+ if (diff < MPU6000_ST_ACCEL(16) / 4) {
return 1;
}
- if (diff > MPU6000_ST_ACCEL(16) * 2) {
+ if (diff > MPU6000_ST_ACCEL(16) * 4) {
return 1;
}
return 0;
@@ -153,10 +153,10 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
if (diff < 0)
diff = -diff;
- if (diff < MPU6000_ST_GYRO(2000) / 2) {
+ if (diff < MPU6000_ST_GYRO(2000) / 4) {
return 1;
}
- if (diff > MPU6000_ST_GYRO(2000) * 2) {
+ if (diff > MPU6000_ST_GYRO(2000) * 4) {
return 1;
}
return 0;
@@ -177,11 +177,14 @@ _ao_mpu6000_wait_alive(void)
ao_panic(AO_PANIC_SELF_TEST_MPU6000);
}
+#define ST_TRIES 10
+
static void
_ao_mpu6000_setup(void)
{
struct ao_mpu6000_sample normal_mode, test_mode;
- int errors =0;
+ int errors;
+ int st_tries;
if (ao_mpu6000_configured)
return;
@@ -236,23 +239,6 @@ _ao_mpu6000_setup(void)
(MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
(MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
- /* Configure accelerometer to +/-16G in self-test mode */
- _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
- (1 << MPU600_ACCEL_CONFIG_XA_ST) |
- (1 << MPU600_ACCEL_CONFIG_YA_ST) |
- (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
- (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
- /* Configure gyro to +/- 2000°/s in self-test mode */
- _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
- (1 << MPU600_GYRO_CONFIG_XG_ST) |
- (1 << MPU600_GYRO_CONFIG_YG_ST) |
- (1 << MPU600_GYRO_CONFIG_ZG_ST) |
- (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
- ao_delay(AO_MS_TO_TICKS(200));
- _ao_mpu6000_sample(&test_mode);
-
#if TRIDGE
// read the product ID rev c has 1/2 the sensitivity of rev d
_mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
@@ -268,36 +254,58 @@ _ao_mpu6000_setup(void)
register_write(MPUREG_ACCEL_CONFIG,2<<3);
}
hal.scheduler->delay(1);
-
#endif
- /* Configure accelerometer to +/-16G */
- _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
- (0 << MPU600_ACCEL_CONFIG_XA_ST) |
- (0 << MPU600_ACCEL_CONFIG_YA_ST) |
- (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
- (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
- /* Configure gyro to +/- 2000°/s */
- _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
- (0 << MPU600_GYRO_CONFIG_XG_ST) |
- (0 << MPU600_GYRO_CONFIG_YG_ST) |
- (0 << MPU600_GYRO_CONFIG_ZG_ST) |
- (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
- ao_delay(AO_MS_TO_TICKS(10));
- _ao_mpu6000_sample(&normal_mode);
+ for (st_tries = 0; st_tries < ST_TRIES; st_tries++) {
+ errors = 0;
+
+ /* Configure accelerometer to +/-16G in self-test mode */
+ _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+ (1 << MPU600_ACCEL_CONFIG_XA_ST) |
+ (1 << MPU600_ACCEL_CONFIG_YA_ST) |
+ (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
+ (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+ /* Configure gyro to +/- 2000°/s in self-test mode */
+ _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+ (1 << MPU600_GYRO_CONFIG_XG_ST) |
+ (1 << MPU600_GYRO_CONFIG_YG_ST) |
+ (1 << MPU600_GYRO_CONFIG_ZG_ST) |
+ (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+ ao_delay(AO_MS_TO_TICKS(200));
+ _ao_mpu6000_sample(&test_mode);
+
+ /* Configure accelerometer to +/-16G */
+ _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+ (0 << MPU600_ACCEL_CONFIG_XA_ST) |
+ (0 << MPU600_ACCEL_CONFIG_YA_ST) |
+ (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
+ (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+ /* Configure gyro to +/- 2000°/s */
+ _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+ (0 << MPU600_GYRO_CONFIG_XG_ST) |
+ (0 << MPU600_GYRO_CONFIG_YG_ST) |
+ (0 << MPU600_GYRO_CONFIG_ZG_ST) |
+ (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+ ao_delay(AO_MS_TO_TICKS(200));
+ _ao_mpu6000_sample(&normal_mode);
- errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
- errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
- errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
-
- errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
- errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
- errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
+
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+ if (!errors)
+ break;
+ }
- if (errors)
- ao_panic(AO_PANIC_SELF_TEST_MPU6000);
+ if (st_tries == ST_TRIES)
+ ao_sensor_errors = 1;
/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
_ao_mpu6000_reg_write(MPU6000_CONFIG,
diff --git a/src/stm/ao_usb_stm.c b/src/stm/ao_usb_stm.c
index b00390ec..28a9f9f3 100644
--- a/src/stm/ao_usb_stm.c
+++ b/src/stm/ao_usb_stm.c
@@ -965,7 +965,7 @@ ao_usb_disable(void)
stm_usb.cntr = (1 << STM_USB_CNTR_PDWN) | (1 << STM_USB_CNTR_FRES);
/* Disable the interface */
- stm_rcc.apb1enr &+ ~(1 << STM_RCC_APB1ENR_USBEN);
+ stm_rcc.apb1enr &= ~(1 << STM_RCC_APB1ENR_USBEN);
ao_arch_release_interrupts();
}
diff --git a/src/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h
index 7ba3a1a7..daeb9f17 100644
--- a/src/telemega-v0.1/ao_pins.h
+++ b/src/telemega-v0.1/ao_pins.h
@@ -316,6 +316,7 @@ struct ao_adc {
#define AO_MPU6000_INT_PORT (&stm_gpioc)
#define AO_MPU6000_INT_PIN 13
#define AO_MPU6000_I2C_INDEX STM_I2C_INDEX(1)
+#define HAS_IMU 1
/*
* mma655x
diff --git a/src/telemega-v1.0/ao_pins.h b/src/telemega-v1.0/ao_pins.h
index 7ff676ea..95644dae 100644
--- a/src/telemega-v1.0/ao_pins.h
+++ b/src/telemega-v1.0/ao_pins.h
@@ -318,8 +318,7 @@ struct ao_adc {
#define AO_MPU6000_SPI_BUS AO_SPI_1_PE13_PE14_PE15
#define AO_MPU6000_SPI_CS_PORT (&stm_gpiod)
#define AO_MPU6000_SPI_CS_PIN 2
-
-#define HAS_HIGHG_ACCEL 1
+#define HAS_IMU 1
/*
* mma655x
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-16 06:16:00 +0000