From d018d93e8883d40cb6f934fe850372cf49dfec03 Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Mon, 13 Feb 2012 01:49:54 -0600
Subject: [PATCH] Clean up previous commit a bit and make the sampling rate a
 variable (and adapt the sampling time accordingly).

---
 flight/Modules/Altitude/revolution/altitude.c |  4 +-
 flight/PiOS/Common/pios_ms5611.c              | 48 +++++++++++++------
 flight/PiOS/inc/pios_ms5611.h                 |  9 ++++
 3 files changed, 45 insertions(+), 16 deletions(-)

diff --git a/flight/Modules/Altitude/revolution/altitude.c b/flight/Modules/Altitude/revolution/altitude.c
index d8e92ea08..4f64bed70 100644
--- a/flight/Modules/Altitude/revolution/altitude.c
+++ b/flight/Modules/Altitude/revolution/altitude.c
@@ -133,12 +133,12 @@ static void altitudeTask(void *parameters)
 		
 		// Update the temperature data
 		PIOS_MS5611_StartADC(TemperatureConv);
-		vTaskDelay(5);
+		vTaskDelay(PIOS_MS5611_GetDelay());
 		PIOS_MS5611_ReadADC();
 		
 		// Update the pressure data
 		PIOS_MS5611_StartADC(PressureConv);
-		vTaskDelay(5);
+		vTaskDelay(PIOS_MS5611_GetDelay());
 		PIOS_MS5611_ReadADC();
 
 		
diff --git a/flight/PiOS/Common/pios_ms5611.c b/flight/PiOS/Common/pios_ms5611.c
index d47ebfc60..b1c50e6e6 100644
--- a/flight/PiOS/Common/pios_ms5611.c
+++ b/flight/PiOS/Common/pios_ms5611.c
@@ -45,10 +45,10 @@ MS5611CalibDataTypeDef CalibData;
 /* Straight from the datasheet */
 static int32_t X1, X2, X3, B3, B5, B6, P;
 static uint32_t B4, B7;
-static volatile uint32_t RawTemperature;
-static volatile uint32_t RawPressure;
-static volatile int64_t Pressure;
-static volatile int64_t Temperature;
+static uint32_t RawTemperature;
+static uint32_t RawPressure;
+static int64_t Pressure;
+static int64_t Temperature;
 
 static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t * buffer, uint8_t len);
 static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
@@ -58,9 +58,11 @@ static uint32_t oversampling;
 static const struct pios_ms5611_cfg * dev_cfg;
 static int32_t i2c_id;
 
+static enum pios_ms5611_osr osr = MS5611_OSR_256;
+
 /**
-* Initialise the BMP085 sensor
-*/
+ * Initialise the MS5611 sensor
+ */
 int32_t ms5611_read_flag;
 void PIOS_MS5611_Init(const struct pios_ms5611_cfg * cfg, int32_t i2c_device)
 {
@@ -90,10 +92,10 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
 {
 	/* Start the conversion */
 	if (Type == TemperatureConv) {
-		while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR) != 0)
+		while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + osr) != 0)
 			continue;
 	} else if (Type == PressureConv) {
-		while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR) != 0)
+		while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + osr) != 0)
 			continue;
 	}
 
@@ -102,21 +104,39 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
 	return 0;
 }
 
+/**
+ * @brief Return the delay for the current osr
+ */
+int32_t PIOS_MS5611_GetDelay() {
+	switch(osr) {
+		case MS5611_OSR_256:
+			return 2;
+		case MS5611_OSR_512:
+			return 2;
+		case MS5611_OSR_1024:
+			return 3;
+		case MS5611_OSR_2048:
+			return 5;
+		case MS5611_OSR_4096:
+			return 10;
+		default:
+			break;
+	}
+	return 10;
+}
 /**
 * Read the ADC conversion value (once ADC conversion has completed)
 * \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
 * \return 0 if successfully read the ADC, -1 if failed
 */
-volatile int64_t Offset;
-volatile int64_t Sens;
-volatile int64_t deltaTemp;
-
 int32_t PIOS_MS5611_ReadADC(void)
 {
 	uint8_t Data[3];
 	Data[0] = 0;
 	Data[1] = 0;
 	Data[2] = 0;
+	
+	static int64_t deltaTemp;
 
 	/* Read and store the 16bit result */
 	if (CurrentRead == TemperatureConv) {
@@ -128,9 +148,10 @@ int32_t PIOS_MS5611_ReadADC(void)
 		
 		deltaTemp = RawTemperature - (CalibData.C[4] << 8);
 		Temperature = ((2000l + deltaTemp * CalibData.C[5]) >> 23);
-//		Temperature /= 100.0f;
 
 	} else {	
+		int64_t Offset;
+		int64_t Sens;
 
 		/* Read the pressure conversion */
 		if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0)
@@ -142,7 +163,6 @@ int32_t PIOS_MS5611_ReadADC(void)
 		Sens = Sens + ((((int64_t) CalibData.C[2]) * deltaTemp) >> 8);
 		
 		Pressure = (((((int64_t) RawPressure) * Sens) >> 21) - Offset) >> 15; 
-//		Pressure /= 1000.0f;
 	}
 	return 0;
 }
diff --git a/flight/PiOS/inc/pios_ms5611.h b/flight/PiOS/inc/pios_ms5611.h
index 83f3f4855..7068988c4 100644
--- a/flight/PiOS/inc/pios_ms5611.h
+++ b/flight/PiOS/inc/pios_ms5611.h
@@ -58,6 +58,14 @@ struct pios_ms5611_cfg {
 	uint32_t oversampling;
 };
 
+enum pios_ms5611_osr {
+	MS5611_OSR_256   = 0,
+	MS5611_OSR_512   = 2,
+	MS5611_OSR_1024  = 4,
+	MS5611_OSR_2048  = 6,
+	MS5611_OSR_4096  = 8,
+};
+
 /* Public Functions */
 extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg * cfg, int32_t i2c_device);
 extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type);
@@ -65,6 +73,7 @@ extern int32_t PIOS_MS5611_ReadADC(void);
 extern float PIOS_MS5611_GetTemperature(void);
 extern float PIOS_MS5611_GetPressure(void);
 extern int32_t PIOS_MS5611_Test();
+extern int32_t PIOS_MS5611_GetDelay();
 
 #endif /* PIOS_MS5611_H */