5 files changed, 17 insertions, 2 deletions
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index df400fcb..065ed221 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -246,6 +246,7 @@ ao_mpu6000_setup(void)
 }
 
 struct ao_mpu6000_sample ao_mpu6000_current;
+uint8_t ao_mpu6000_valid;
 
 static void
 ao_mpu6000(void)
@@ -257,6 +258,7 @@ ao_mpu6000(void)
 		ao_mpu6000_sample(&ao_mpu6000_next);
 		ao_arch_critical(
 			ao_mpu6000_current = ao_mpu6000_next;
+			ao_mpu6000_valid = 1;
 			);
 		ao_delay(0);
 	}
@@ -288,6 +290,7 @@ void
 ao_mpu6000_init(void)
 {
 	ao_mpu6000_configured = 0;
+	ao_mpu6000_valid = 0;
 
 	ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
 	ao_cmd_register(&ao_mpu6000_cmds[0]);
diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index 5e52148d..fc7af1e0 100644
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -156,6 +156,7 @@ struct ao_mpu6000_sample {
 };
 
 extern struct ao_mpu6000_sample ao_mpu6000_current;
+extern uint8_t ao_mpu6000_valid;
 
 void
 ao_mpu6000_init(void);
diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c
index f79c315a..7db7022f 100644
--- a/src/drivers/ao_ms5607.c
+++ b/src/drivers/ao_ms5607.c
@@ -195,6 +195,7 @@ ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value
 }
 
 struct ao_ms5607_sample	ao_ms5607_current;
+uint8_t ao_ms5607_valid;
 
 static void
 ao_ms5607(void)
@@ -206,6 +207,7 @@ ao_ms5607(void)
 		ao_ms5607_sample(&ao_ms5607_next);
 		ao_arch_critical(
 			ao_ms5607_current = ao_ms5607_next;
+			ao_ms5607_valid = 1;
 			);
 		ao_delay(0);
 	}
@@ -244,6 +246,7 @@ void
 ao_ms5607_init(void)
 {
 	ms5607_configured = 0;
+	ao_ms5607_valid = 0;
 	ao_cmd_register(&ao_ms5607_cmds[0]);
 	ao_spi_init_cs(AO_MS5607_CS_GPIO, (1 << AO_MS5607_CS));
 
diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h
index fd5bc984..1384d3a2 100644
--- a/src/drivers/ao_ms5607.h
+++ b/src/drivers/ao_ms5607.h
@@ -51,6 +51,7 @@ struct ao_ms5607_sample {
 	uint32_t	temp;	/* raw 24 bit sensor */
 };
 
+extern uint8_t ao_ms5607_valid;
 extern struct ao_ms5607_sample ao_ms5607_current;
 
 struct ao_ms5607_value {
diff --git a/src/stm/ao_adc_stm.c b/src/stm/ao_adc_stm.c
index 576dbf5d..02187205 100644
--- a/src/stm/ao_adc_stm.c
+++ b/src/stm/ao_adc_stm.c
@@ -50,15 +50,22 @@ static uint8_t			ao_adc_ready;
  */
 static void ao_adc_done(int index)
 {
+	uint8_t	step = 1;
 	ao_data_ring[ao_data_head].tick = ao_time();
 #if HAS_MPU6000
+	if (!ao_mpu6000_valid)
+		step = 0;
 	ao_data_ring[ao_data_head].mpu6000 = ao_mpu6000_current;
 #endif
 #if HAS_MS5607
+	if (!ao_ms5607_valid)
+		step = 0;
 	ao_data_ring[ao_data_head].ms5607 = ao_ms5607_current;
 #endif	
-	ao_data_head = ao_data_ring_next(ao_data_head);
-	ao_wakeup((void *) &ao_data_head);
+	if (step) {
+		ao_data_head = ao_data_ring_next(ao_data_head);
+		ao_wakeup((void *) &ao_data_head);
+	}
 	ao_dma_done_transfer(STM_DMA_INDEX(STM_DMA_CHANNEL_ADC1));
 	ao_adc_ready = 1;
 }