From d125abf3352e8cde1b4c7288611447facd051d85 Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Wed, 18 Apr 2012 10:48:31 +0200
Subject: [PATCH] Baro calibration

---
 flight/Modules/Airspeed/revolution/airspeed.c | 51 ++++++++++++++++---
 flight/PiOS/Common/pios_etasv3.c              | 16 +-----
 flight/PiOS/inc/pios_etasv3.h                 |  1 -
 flight/PiOS/pios.h                            |  1 +
 shared/uavobjectdefinition/baroairspeed.xml   |  4 ++
 5 files changed, 52 insertions(+), 21 deletions(-)

diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c
index eec3f015b..d500e449e 100644
--- a/flight/Modules/Airspeed/revolution/airspeed.c
+++ b/flight/Modules/Airspeed/revolution/airspeed.c
@@ -48,6 +48,9 @@
 #define STACK_SIZE_BYTES 500
 #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
 #define SAMPLING_DELAY_MS 50
+#define CALIBRATION_IDLE 40
+#define CALIBRATION_COUNT 40
+#define ETS_AIRSPEED_SCALE 1.8f
 
 // Private types
 
@@ -111,18 +114,54 @@ static void airspeedTask(void *parameters)
 {
 	BaroAirspeedData data;
 	
-	// TODO: Check the pressure sensor and set a warning if it fails test
+	uint8_t calibrationCount = 0;
+	uint32_t calibrationSum = 0;
+	uint16_t calibrationMin = 0;
+	uint16_t calibrationMax = 0;
 	
 	// Main task loop
 	while (1)
 	{
-		float airspeed;
-		
 		// Update the airspeed
 		vTaskDelay(SAMPLING_DELAY_MS);
-		airspeed = PIOS_ETASV3_ReadAirspeed();
-		
-		data.Airspeed = airspeed;
+
+		BaroAirspeedGet(&data);
+		data.SensorValue = PIOS_ETASV3_ReadAirspeed();
+		if (data.SensorValue==-1) {
+			data.Connected = BAROAIRSPEED_CONNECTED_FALSE;
+			data.Airspeed = 0;
+			BaroAirspeedSet(&data);
+			continue;
+		}
+
+		if (calibrationCount<CALIBRATION_IDLE) {
+			calibrationMin=data.SensorValue;
+			calibrationMax=data.SensorValue;
+			calibrationCount++;
+		} else if (calibrationCount<CALIBRATION_IDLE + CALIBRATION_COUNT) {
+			if (data.SensorValue<calibrationMin)
+				calibrationMin = data.SensorValue;
+			if (data.SensorValue>calibrationMax)
+				calibrationMax = data.SensorValue;
+			calibrationCount++;
+			calibrationSum +=  data.SensorValue;
+			if (calibrationCount==CALIBRATION_IDLE+CALIBRATION_COUNT) {
+				data.ZeroPoint = calibrationSum / CALIBRATION_COUNT;
+				data.ZeroZone = (calibrationMax - calibrationMin) / 2;
+			} else {
+				continue;
+			}
+		}
+
+		data.Connected = BAROAIRSPEED_CONNECTED_TRUE;
+
+		int16_t tmp = abs(data.SensorValue - data.ZeroPoint) - data.ZeroZone;
+
+		if (tmp>0) {
+			data.Airspeed = ETS_AIRSPEED_SCALE * sqrtf((float)tmp);
+		} else {
+			data.Airspeed = 0;
+		}
 	
 		// Update the AirspeedActual UAVObject
 		BaroAirspeedSet(&data);
diff --git a/flight/PiOS/Common/pios_etasv3.c b/flight/PiOS/Common/pios_etasv3.c
index 87e400c81..2a0ca9b52 100644
--- a/flight/PiOS/Common/pios_etasv3.c
+++ b/flight/PiOS/Common/pios_etasv3.c
@@ -35,21 +35,9 @@
 
 #include "pios_etasv3.h"
 
-static bool PIOS_ETASV3_Read(uint8_t command, uint8_t * buffer, uint8_t len)
+static bool PIOS_ETASV3_Read(uint8_t * buffer, uint8_t len)
 {
-	uint8_t cmd_buffer[] = {
-		command,
-	};
-
 	const struct pios_i2c_txn txn_list[] = {
-		/*{
-		 .info = __func__,
-		 .addr = ETASV3_I2C_ADDR,
-		 .rw = PIOS_I2C_TXN_WRITE,
-		 .len = sizeof(cmd_buffer),
-		 .buf = cmd_buffer,
-		 }
-		,*/
 		{
 		 .info = __func__,
 		 .addr = ETASV3_I2C_ADDR,
@@ -66,7 +54,7 @@ int16_t PIOS_ETASV3_ReadAirspeed (void)
 {
 	uint8_t airspeed_raw[2];
 
-	if (PIOS_ETASV3_Read(ETASV3_I2C_READ_CMD, airspeed_raw, sizeof(airspeed_raw)) != 0) {
+	if (PIOS_ETASV3_Read(airspeed_raw, sizeof(airspeed_raw)) != 0) {
 		/* Failed to read airspeed */
 		return -1;
 	}
diff --git a/flight/PiOS/inc/pios_etasv3.h b/flight/PiOS/inc/pios_etasv3.h
index f816dc247..afa038d3d 100644
--- a/flight/PiOS/inc/pios_etasv3.h
+++ b/flight/PiOS/inc/pios_etasv3.h
@@ -29,7 +29,6 @@
  */
 
 #define ETASV3_I2C_ADDR			0x75
-#define ETASV3_I2C_READ_CMD		0x07
 
 
 int16_t PIOS_ETASV3_ReadAirspeed (void);
diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h
index 560eb9b39..6e6e2aa1d 100644
--- a/flight/PiOS/pios.h
+++ b/flight/PiOS/pios.h
@@ -100,6 +100,7 @@
 /* PIOS Hardware Includes (Common) */
 #include <pios_sdcard.h>
 #include <pios_com.h>
+#include <pios_etasv3.h>
 #if defined(PIOS_INCLUDE_BMP085)
 #include <pios_bmp085.h>
 #endif
diff --git a/shared/uavobjectdefinition/baroairspeed.xml b/shared/uavobjectdefinition/baroairspeed.xml
index 7b0f3a6c2..ea6ecfda8 100644
--- a/shared/uavobjectdefinition/baroairspeed.xml
+++ b/shared/uavobjectdefinition/baroairspeed.xml
@@ -1,6 +1,10 @@
 <xml>
     <object name="BaroAirspeed" singleinstance="true" settings="false">
         <description>The raw data from the dynamic pressure sensor with pressure, temperature and airspeed.</description>
+        <field name="Connected" units="" type="enum" elements="1" options="False,True"/>
+        <field name="SensorValue" units="raw" type="uint16" elements="1"/>
+        <field name="ZeroPoint" units="raw" type="uint16" elements="1"/>
+        <field name="ZeroZone" units="raw" type="uint16" elements="1"/>
         <field name="Airspeed" units="m/s" type="float" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>