From 7eb150a2f08406d7a792f215f9818e1cad3c9e41 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 20 Jun 2012 15:17:15 +1000
Subject: [PATCH] ACM: adapt the ArduCopter code for new barometer interface

---
 ArduCopter/ArduCopter.pde  |  4 ---
 ArduCopter/GCS_Mavlink.pde |  2 +-
 ArduCopter/sensors.pde     | 62 +++-----------------------------------
 3 files changed, 6 insertions(+), 62 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index db530d030f..7eeab480f2 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -629,10 +629,6 @@ static bool		low_batt = false;
 ////////////////////////////////////////////////////////////////////////////////
 // Altitude
 ////////////////////////////////////////////////////////////////////////////////
-// The pressure at home location - calibrated at arming
-static int32_t 	ground_pressure;
-// The ground temperature at home location - calibrated at arming
-static int16_t 	ground_temperature;
 // The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP
 static int32_t		altitude_error;
 // The cm/s we are moving up or down based on sensor data - Positive = UP
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 7d5d74204f..f78ff63d40 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -493,7 +493,7 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
         chan,
         micros(),
         (float)barometer.get_pressure()/100.0,
-        (float)(barometer.get_pressure()-ground_pressure)/100.0,
+        (float)(barometer.get_pressure() - barometer.get_ground_pressure())/100.0,
         (int)(barometer.get_temperature()*10));
 }
 
diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
index 9c6dab766c..db90c58100 100644
--- a/ArduCopter/sensors.pde
+++ b/ArduCopter/sensors.pde
@@ -18,68 +18,16 @@ static void init_sonar(void)
 
 static void init_barometer(void)
 {
-	#if HIL_MODE == HIL_MODE_SENSORS
-		gcs_update();					// look for inbound hil packets for initialization
-	#endif
-
-	ground_temperature = barometer.get_temperature();
-	int i;
-
-	// We take some readings...
-	for(i = 0; i < 60; i++){
-		delay(20);
-
-		// get new data from absolute pressure sensor
-		barometer.read();
-
-		//Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress,  barometer.Press);
-	}
-
-	for(i = 0; i < 40; i++){
-		delay(20);
-
-		#if HIL_MODE == HIL_MODE_SENSORS
-			gcs_update(); 				// look for inbound hil packets
-		#endif
-
-		// Get initial data from absolute pressure sensor
-		barometer.read();
-		ground_pressure 	= baro_filter.apply(barometer.get_pressure());
-
-		//Serial.printf("t: %ld, p: %d\n", ground_pressure, ground_temperature);
-
-		/*Serial.printf("init %d, %d, -, %d, %d, -, %d, %d\n",
-				barometer.RawTemp,
-				barometer.Temp,
-				barometer.RawPress,
-				barometer.Press,
-				ground_temperature,
-				ground_pressure);*/
-	}
-	// save our ground temp
-	ground_temperature	= barometer.get_temperature();
-}
-
-static void reset_baro(void)
-{
-	ground_pressure 	= baro_filter.apply(barometer.get_pressure());
-	ground_temperature	= barometer.get_temperature();
+    barometer.calibrate(mavlink_delay);
+    ahrs.set_barometer(&barometer);
+    gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
 }
 
+// return barometric altitude in centimeters
 static int32_t read_barometer(void)
 {
- 	float x, scaling, temp;
-
 	barometer.read();
-	float abs_pressure = baro_filter.apply(barometer.get_pressure());
-
-
-	//Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure);
-
-	scaling 				= (float)ground_pressure / abs_pressure;
-	temp 					= ((float)ground_temperature / 10.0f) + 273.15f;
-	x 						= log(scaling) * temp * 29271.267f;
-	return 	(x / 10);
+    return baro_filter.apply(barometer.get_altitude() * 100.0);
 }