From fed48732828c85ae56106cd72c5aeaaad47c552f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 11 Jun 2017 22:31:17 -0700 Subject: [PATCH] altos: Update sensor data atomically Read data into a temp variable, block interrupts, then update the published value. The bug is easy to see with the HMC5883 which has to byte-swap the output of the chip, and hence can occasionally get caught with the wrong byte order data. Signed-off-by: Keith Packard --- src/drivers/ao_hmc5883.c | 12 +++++++----- src/drivers/ao_mpu6000.c | 39 +++++++++++++++++++-------------------- src/drivers/ao_ms5607.c | 4 +++- 3 files changed, 29 insertions(+), 26 deletions(-) diff --git a/src/drivers/ao_hmc5883.c b/src/drivers/ao_hmc5883.c index f668fb66..9f1131d6 100644 --- a/src/drivers/ao_hmc5883.c +++ b/src/drivers/ao_hmc5883.c @@ -126,13 +126,15 @@ struct ao_hmc5883_sample ao_hmc5883_current; static void ao_hmc5883(void) { + struct ao_hmc5883_sample sample; ao_hmc5883_setup(); for (;;) { - ao_hmc5883_sample(&ao_hmc5883_current); - ao_arch_critical( - AO_DATA_PRESENT(AO_DATA_HMC5883); - AO_DATA_WAIT(); - ); + ao_hmc5883_sample(&sample); + ao_arch_block_interrupts(); + ao_hmc5883_current = sample; + AO_DATA_PRESENT(AO_DATA_HMC5883); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); } } diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 650407ad..81d3c16c 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -192,7 +192,7 @@ _ao_mpu6000_setup(void) _ao_mpu6000_wait_alive(); /* Reset the whole chip */ - + _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET)); @@ -292,7 +292,7 @@ _ao_mpu6000_setup(void) 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); errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y); errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z); @@ -315,7 +315,7 @@ _ao_mpu6000_setup(void) /* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */ _ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 1000 / 200 - 1); - + ao_delay(AO_MS_TO_TICKS(100)); ao_mpu6000_configured = 1; } @@ -325,6 +325,7 @@ struct ao_mpu6000_sample ao_mpu6000_current; static void ao_mpu6000(void) { + struct ao_mpu6000_sample sample; /* ao_mpu6000_init already grabbed the SPI bus and mutex */ _ao_mpu6000_setup(); #if AO_MPU6000_SPI @@ -335,14 +336,15 @@ ao_mpu6000(void) #if AO_MPU6000_SPI ao_mpu6000_spi_get(); #endif - _ao_mpu6000_sample(&ao_mpu6000_current); + _ao_mpu6000_sample(&sample); #if AO_MPU6000_SPI ao_mpu6000_spi_put(); -#endif - ao_arch_critical( - AO_DATA_PRESENT(AO_DATA_MPU6000); - AO_DATA_WAIT(); - ); +#endif + ao_arch_block_interrupts(); + ao_mpu6000_current = sample; + AO_DATA_PRESENT(AO_DATA_MPU6000); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); } } @@ -351,16 +353,13 @@ static struct ao_task ao_mpu6000_task; static void ao_mpu6000_show(void) { - struct ao_data sample; - - ao_data_get(&sample); printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n", - sample.mpu6000.accel_x, - sample.mpu6000.accel_y, - sample.mpu6000.accel_z, - sample.mpu6000.gyro_x, - sample.mpu6000.gyro_y, - sample.mpu6000.gyro_z); + ao_mpu6000_current.accel_x, + ao_mpu6000_current.accel_y, + ao_mpu6000_current.accel_z, + ao_mpu6000_current.gyro_x, + ao_mpu6000_current.gyro_y, + ao_mpu6000_current.gyro_z); } static const struct ao_cmds ao_mpu6000_cmds[] = { @@ -374,7 +373,7 @@ ao_mpu6000_init(void) ao_mpu6000_configured = 0; ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000"); - + #if AO_MPU6000_SPI ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN)); @@ -386,7 +385,7 @@ ao_mpu6000_init(void) ao_cur_task = &ao_mpu6000_task; ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz); ao_cur_task = NULL; -#endif +#endif ao_cmd_register(&ao_mpu6000_cmds[0]); } diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 0afdf012..914e0c1b 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -201,11 +201,13 @@ __xdata struct ao_ms5607_sample ao_ms5607_current; static void ao_ms5607(void) { + struct ao_ms5607_sample sample; ao_ms5607_setup(); for (;;) { - ao_ms5607_sample(&ao_ms5607_current); + ao_ms5607_sample(&sample); ao_arch_block_interrupts(); + ao_ms5607_current = sample; AO_DATA_PRESENT(AO_DATA_MS5607); AO_DATA_WAIT(); ao_arch_release_interrupts(); -- 2.30.2