From cd3729d993bb36ad9c80e7f576c1b988863ee7dd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 28 Dec 2011 20:34:11 +1100
Subject: [PATCH] APM: added error checking on barometer read

if the barometer is dead and the GPS has a fix, use the GPS for
altitude
---
 ArduPlane/ArduPlane.pde |  8 ++++++--
 ArduPlane/sensors.pde   | 11 ++++++-----
 ArduPlane/test.pde      | 10 +++++++---
 3 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index de4d2de2f9..f81c1efe4e 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -978,8 +978,12 @@ static void update_alt()
 		// this function is in place to potentially add a sonar sensor in the future
 		//altitude_sensor = BARO;
 
-		current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude;			// alt_MSL centimeters (meters * 100)
-		current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
+        if (barometer.healthy) {
+            current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude;			// alt_MSL centimeters (meters * 100)
+            current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
+        } else if (g_gps->fix) {
+            current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100)            
+        }
 	#endif
 
         geofence_check(true);
diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde
index b144db8b4f..cb96f16e2b 100644
--- a/ArduPlane/sensors.pde
+++ b/ArduPlane/sensors.pde
@@ -11,7 +11,7 @@ static void init_barometer(void)
 	long 			ground_pressure = 0;
 	int 			ground_temperature;
 
-	while(ground_pressure == 0){
+	while (ground_pressure == 0 || !barometer.healthy) {
 		barometer.read(); 					// Get initial data from absolute pressure sensor
 		ground_pressure 	= barometer.get_pressure();
 		ground_temperature 	= barometer.get_temperature();
@@ -22,10 +22,12 @@ static void init_barometer(void)
 	for(int i = 0; i < 30; i++){		// We take some readings...
 
 		#if HIL_MODE == HIL_MODE_SENSORS
-			gcs_update(); 				// look for inbound hil packets
+        gcs_update(); 				// look for inbound hil packets
 		#endif
-
-		barometer.read(); 				// Get initial data from absolute pressure sensor
+        
+        do {
+            barometer.read(); 				// Get pressure sensor
+        } while (!barometer.healthy);
 		ground_pressure		= (ground_pressure * 9l   + barometer.get_pressure()) / 10l;
 		ground_temperature	= (ground_temperature * 9 + barometer.get_temperature()) / 10;
 
@@ -59,7 +61,6 @@ static long read_barometer(void)
 
 	barometer.read();		// Get new data from absolute pressure sensor
 
-
 	//abs_pressure 			= (abs_pressure + barometer.get_pressure()) >> 1;		// Small filtering
 	abs_pressure 			= ((float)abs_pressure * .7) + ((float)barometer.get_pressure() * .3);		// large filtering
 	scaling 				= (float)g.ground_pressure / (float)abs_pressure;
diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde
index 1b899c8558..f55604454e 100644
--- a/ArduPlane/test.pde
+++ b/ArduPlane/test.pde
@@ -700,9 +700,13 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 		delay(100);
 		current_loc.alt = read_barometer() + home.alt;
 
-		Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
-						current_loc.alt / 100.0,
-						abs_pressure, 0.1*barometer.get_temperature());
+        if (!barometer.healthy) {
+            Serial.println_P(PSTR("not healthy"));
+        } else {
+            Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
+                            current_loc.alt / 100.0,
+                            abs_pressure, 0.1*barometer.get_temperature());
+        }
 
 		if(Serial.available() > 0){
 			return (0);