Adjusted the state machine to read the temperature less often (from twice a second to every 4 seconds). Added a small smoothing filter.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2552 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/APM_BMP085/APM_BMP085.cpp

@@ -54,43 +54,45 @@ APM_BMP085_Class::APM_BMP085_Class()
 // Public Methods //////////////////////////////////////////////////////////////
 void APM_BMP085_Class::Init(int initialiseWireLib)
 {
-  byte buff[22];
-  int i=0;
+	byte buff[22];
+	int i = 0;
 
-  pinMode(BMP085_EOC,INPUT);   // End Of Conversion (PC7) input
+	pinMode(BMP085_EOC, INPUT);	 // End Of Conversion (PC7) input
 
-  if( initialiseWireLib != 0 )
-      Wire.begin();
-  oss = 3;           // Over Sampling setting 3 = High resolution
-  BMP085_State = 0;     // Initial state
+	if( initialiseWireLib != 0 )
+			Wire.begin();
+
+	oss = 3;					 // Over Sampling setting 3 = High resolution
+	BMP085_State = 0;		 // Initial state
 
   // We read the calibration data registers
-  Wire.beginTransmission(BMP085_ADDRESS);
-  Wire.send(0xAA);
-  Wire.endTransmission();
+	Wire.beginTransmission(BMP085_ADDRESS);
+	Wire.send(0xAA);
+	Wire.endTransmission();
+
+	Wire.requestFrom(BMP085_ADDRESS, 22);
 
-  Wire.requestFrom(BMP085_ADDRESS, 22);
   //Wire.endTransmission();
-  while(Wire.available())
-  { 
-    buff[i] = Wire.receive();  // receive one byte
-    i++;
-  }
-  ac1 = ((int)buff[0] << 8) | buff[1];
-  ac2 = ((int)buff[2] << 8) | buff[3];
-  ac3 = ((int)buff[4] << 8) | buff[5];
-  ac4 = ((int)buff[6] << 8) | buff[7];
-  ac5 = ((int)buff[8] << 8) | buff[9];
-  ac6 = ((int)buff[10] << 8) | buff[11];
-  b1 = ((int)buff[12] << 8) | buff[13];
-  b2 = ((int)buff[14] << 8) | buff[15];
-  mb = ((int)buff[16] << 8) | buff[17];
-  mc = ((int)buff[18] << 8) | buff[19];
-  md = ((int)buff[20] << 8) | buff[21];
+	while(Wire.available()){
+		buff[i] = Wire.receive();	// receive one byte
+		i++;
+	}
+
+	ac1 = ((int)buff[0] << 8) | buff[1];
+	ac2 = ((int)buff[2] << 8) | buff[3];
+	ac3 = ((int)buff[4] << 8) | buff[5];
+	ac4 = ((int)buff[6] << 8) | buff[7];
+	ac5 = ((int)buff[8] << 8) | buff[9];
+	ac6 = ((int)buff[10] << 8) | buff[11];
+	b1 = ((int)buff[12] << 8) | buff[13];
+	b2 = ((int)buff[14] << 8) | buff[15];
+	mb = ((int)buff[16] << 8) | buff[17];
+	mc = ((int)buff[18] << 8) | buff[19];
+	md = ((int)buff[20] << 8) | buff[21];
 
   //Send a command to read Temp
-  Command_ReadTemp();
-  BMP085_State=1;
+	Command_ReadTemp();
+	BMP085_State = 1;
 }
 
 
@@ -98,80 +100,78 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
 // We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
 uint8_t APM_BMP085_Class::Read()
 {
-uint8_t result=0;
+	uint8_t result = 0;
 
-	if (BMP085_State==1)
-	{
-		if (digitalRead(BMP085_EOC))
-		{
-			ReadTemp();             // On state 1 we read temp
+	if (BMP085_State == 1){
+		if (digitalRead(BMP085_EOC)){
+			ReadTemp();						 // On state 1 we read temp
 			BMP085_State++;
 			Command_ReadPress();
 		}
-	}
-	else
-	{
-		if (BMP085_State==5)
-		{
-			if (digitalRead(BMP085_EOC))
-			{
+	}else{
+		if (BMP085_State == 40){
+			if (digitalRead(BMP085_EOC)){
 				ReadPress();
 				Calculate();
-				BMP085_State = 1;    // Start again from state=1
-				Command_ReadTemp();  // Read Temp
-				result=1;            // New pressure reading
+
+				BMP085_State = 1;			// Start again from state = 1
+				Command_ReadTemp();			// Read Temp
+				result = 1;					// New pressure reading
 			}
-		}
-		else
-		{
-			if (digitalRead(BMP085_EOC))
-			{
+		}else{
+			if (digitalRead(BMP085_EOC)){
 				ReadPress();
 				Calculate();
 				BMP085_State++;
 				Command_ReadPress();
-				result=1;            // New pressure reading
+				result = 1;					// New pressure reading
 			}
 		}
 	}
-  return(result);
+	return(result);
 }
 
 
 // Send command to Read Pressure
 void APM_BMP085_Class::Command_ReadPress()
 {
-  Wire.beginTransmission(BMP085_ADDRESS);
-  Wire.send(0xF4);
-  Wire.send(0x34+(oss<<6));  //write_register(0xF4,0x34+(oversampling_setting<<6));
-  Wire.endTransmission();
+	Wire.beginTransmission(BMP085_ADDRESS);
+	Wire.send(0xF4);
+	Wire.send(0x34+(oss << 6));	// write_register(0xF4, 0x34+(oversampling_setting << 6));
+	Wire.endTransmission();
 }
 
 // Read Raw Pressure values
 void APM_BMP085_Class::ReadPress()
 {
-  byte msb;
-  byte lsb;
-  byte xlsb;
+	byte msb;
+	byte lsb;
+	byte xlsb;
 
-  Wire.beginTransmission(BMP085_ADDRESS);
-  Wire.send(0xF6);
-  Wire.endTransmission();
+	Wire.beginTransmission(BMP085_ADDRESS);
+	Wire.send(0xF6);
+	Wire.endTransmission();
 
-  Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
-  while(!Wire.available()) {
+	Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
+
+	while(!Wire.available()) {
     // waiting
-  }
-  msb = Wire.receive();
-  while(!Wire.available()) {
+	}
+
+	msb = Wire.receive();
+
+	while(!Wire.available()) {
     // waiting
-  }
-  lsb = Wire.receive();
-  while(!Wire.available()) {
+	}
+
+	lsb = Wire.receive();
+
+	while(!Wire.available()) {
     // waiting
-  }
-  xlsb = Wire.receive();
-  RawPress = (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >> (8-oss);
+	}
+
+	xlsb = Wire.receive();
+	RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss);
 }
 
 // Send Command to Read Temperature
@@ -186,19 +186,21 @@ void APM_BMP085_Class::Command_ReadTemp()
 // Read Raw Temperature values
 void APM_BMP085_Class::ReadTemp()
 {
-  byte tmp;
+	byte tmp;
+	int16_t tmp2;
 
-  Wire.beginTransmission(BMP085_ADDRESS);
-  Wire.send(0xF6);
-  Wire.endTransmission();
+	Wire.beginTransmission(BMP085_ADDRESS);
+	Wire.send(0xF6);
+	Wire.endTransmission();
 
-  Wire.beginTransmission(BMP085_ADDRESS);
-  Wire.requestFrom(BMP085_ADDRESS,2);
-  while(!Wire.available());  // wait
-  RawTemp = Wire.receive();
-  while(!Wire.available());  // wait
-  tmp = Wire.receive();
-  RawTemp = RawTemp<<8 | tmp;
+	Wire.beginTransmission(BMP085_ADDRESS);
+	Wire.requestFrom(BMP085_ADDRESS, 2);
+	while(!Wire.available());	// wait
+	tmp2 = Wire.receive();
+	while(!Wire.available());	// wait
+	tmp = Wire.receive();
+	RawTemp += tmp2 << 8 | tmp;
+	RawTemp = RawTemp >> 1;
 }
 
 // Calculate Temperature and Pressure in real units.
@@ -252,34 +254,29 @@ void APM_BMP085_HIL_Class::Init(int initialiseWireLib)
 
 
 // Read the sensor. This is a state machine
-// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
+// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5)
 uint8_t APM_BMP085_HIL_Class::Read()
 {
-    uint8_t result=0;
+	uint8_t result = 0;
 
-	if (BMP085_State==1)
-	{
-        BMP085_State++;
-	}
-	else
-	{
-		if (BMP085_State==5)
-		{
-            BMP085_State = 1;    // Start again from state=1
-            result=1;            // New pressure reading
-		}
-		else
-		{
-            BMP085_State++;
-            result=1;            // New pressure reading
+	if (BMP085_State == 1){
+		BMP085_State++;
+	}else{
+
+		if (BMP085_State == 5){
+			BMP085_State = 1;				// Start again from state = 1
+			result = 1;						// New pressure reading
+		}else{
+			BMP085_State++;
+			result = 1;						// New pressure reading
 		}
 	}
-    return(result);
+	return(result);
 }
 
 void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press)
 {
     // TODO: map floats to raw
-    Temp = _Temp;
-    Press = _Press;
+	Temp 	= _Temp;
+	Press 	= _Press;
 }

libraries/APM_BMP085/APM_BMP085.h

@@ -15,6 +15,7 @@ class APM_BMP085_Class
 	void ReadPress();
 	void ReadTemp();
 	void Calculate();
+
   public:
 	int32_t RawPress;
 	int32_t RawTemp;