altos: Record all failed sensors and report status at power up

Use DATA bits to mark which sensors have failed, then report that in
beeps at startup time to help diagnose hardware failures while still
allowing the board to be used over USB.

Signed-off-by: Keith Packard <keithp@keithp.com>

7 files changed, 19 insertions, 10 deletions

diff --git a/src/drivers/ao_adxl375.c b/src/drivers/ao_adxl375.c
index 66d39ff9..4f6b8169 100644
--- a/src/drivers/ao_adxl375.c
+++ b/src/drivers/ao_adxl375.c
@@ -143,7 +143,7 @@ ao_adxl375_setup(void)
 
 	uint8_t	devid = ao_adxl375_reg_read(AO_ADXL375_DEVID);
 	if (devid != AO_ADXL375_DEVID_ID)
-		ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_ADXL375);
 
 	/* Set the data rate */
 	ao_adxl375_reg_write(AO_ADXL375_BW_RATE,
@@ -204,12 +204,12 @@ ao_adxl375_setup(void)
 	self_test_value = z_change;
 
 	if (z_change < MIN_SELF_TEST)
-		ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_ADXL375);
 
 	/* This check is commented out as maximum self test is unreliable
 
 	   if (z_change > MAX_SELF_TEST)
-	   	ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_ADXL375);
 
 	*/
 
diff --git a/src/drivers/ao_mma655x.c b/src/drivers/ao_mma655x.c
index e8eeea6e..b0217368 100644
--- a/src/drivers/ao_mma655x.c
+++ b/src/drivers/ao_mma655x.c
@@ -232,7 +232,7 @@ ao_mma655x_setup(void)
 		ao_delay(AO_ST_DELAY);
 	}
 	if (tries == AO_ST_TRIES)
-		ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_MMA655X);
 
 	ao_mma655x_reg_write(AO_MMA655X_DEVCFG,
 			     DEVCFG_VALUE | (1 << AO_MMA655X_DEVCFG_ENDINIT));
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index 8c85ab01..c894239e 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -305,7 +305,7 @@ _ao_mpu6000_setup(void)
 	}
 
 	if (st_tries == ST_TRIES)
-		ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_MPU6000);
 
 	/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
 	_ao_mpu6000_reg_write(MPU6000_CONFIG,
diff --git a/src/drivers/ao_mpu9250.c b/src/drivers/ao_mpu9250.c
index 74e444a9..1b31edf4 100644
--- a/src/drivers/ao_mpu9250.c
+++ b/src/drivers/ao_mpu9250.c
@@ -26,8 +26,6 @@
 
 static uint8_t	ao_mpu9250_configured;
 
-extern uint8_t ao_sensor_errors;
-
 #ifndef AO_MPU9250_I2C_INDEX
 #define AO_MPU9250_SPI	1
 #else
@@ -361,7 +359,7 @@ _ao_mpu9250_setup(void)
 	}
 
 	if (st_tries == ST_TRIES)
-		ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_MPU9250);
 
 	/* Set up the mag sensor */
 
@@ -372,7 +370,7 @@ _ao_mpu9250_setup(void)
 	}
 
 	if (mag_tries == MAG_TRIES)
-		ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_MPU9250);
 
 	/* Select continuous mode 2 (100Hz), 16 bit samples */
 
diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c
index febe0111..1ebba3e9 100644
--- a/src/drivers/ao_ms5607.c
+++ b/src/drivers/ao_ms5607.c
@@ -115,7 +115,7 @@ ao_ms5607_prom_read(struct ao_ms5607_prom *prom)
 
 	if (!ao_ms5607_prom_valid((uint8_t *) prom)) {
 #if HAS_SENSOR_ERRORS
-		ao_sensor_errors = 1;
+		AO_SENSOR_ERROR(AO_DATA_MS5607);
 #else
 		ao_panic(AO_PANIC_SELF_TEST_MS5607);
 #endif
diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index 5a340c3f..4fc9db8f 100644
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -143,6 +143,12 @@ extern volatile uint8_t		ao_data_count;
 #define AO_DATA_PRESENT(bit)	(ao_data_present |= (bit))
 
 /*
+ * Mark sensor failed, and unblock the sample collection code by
+ * marking the data as present
+ */
+#define AO_SENSOR_ERROR(bit)	(ao_data_present |= (ao_sensor_errors |= (bit)))
+
+/*
  * Wait until it is time to write a sensor sample; this is
  * signaled by the timer tick
  */
diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c
index 872dcd48..12c3a1e9 100644
--- a/src/kernel/ao_report.c
+++ b/src/kernel/ao_report.c
@@ -262,6 +262,11 @@ ao_report(void)
 		else
 #endif
 			ao_report_beep();
+#if HAS_SENSOR_ERRORS
+		if (ao_report_state == ao_flight_invalid && ao_sensor_errors)
+			ao_report_number(ao_sensor_errors);
+#endif
+
 		if (ao_report_state == ao_flight_landed) {
 			ao_report_altitude();
 #if HAS_FLIGHT