• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

APM_BMP085.cpp

Go to the documentation of this file.
00001 /*
00002         APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
00003         Code by Jordi Muņoz and Jose Julio. DIYDrones.com
00004 
00005         This library is free software; you can redistribute it and/or
00006     modify it under the terms of the GNU Lesser General Public
00007     License as published by the Free Software Foundation; either
00008     version 2.1 of the License, or (at your option) any later version.
00009 
00010         Sensor is conected to I2C port
00011         Sensor End of Conversion (EOC) pin is PC7 (30)
00012         
00013         Variables:
00014                 RawTemp : Raw temperature data
00015                 RawPress : Raw pressure data
00016 
00017                 Temp : Calculated temperature (in 0.1ēC units)
00018                 Press : Calculated pressure   (in Pa units)
00019         
00020         Methods:
00021                 Init() : Initialization of I2C and read sensor calibration data
00022                 Read() : Read sensor data and calculate Temperature and Pressure
00023                          This function is optimized so the main host don´t need to wait 
00024                                  You can call this function in your main loop
00025                                  It returns a 1 if there are new data.
00026     
00027         Internal functions:
00028                 Command_ReadTemp(): Send commando to read temperature
00029                 Command_ReadPress(): Send commando to read Pressure
00030                 ReadTemp() : Read temp register
00031                 ReadPress() : Read press register
00032                 Calculate() : Calculate Temperature and Pressure in real units
00033 
00034                 
00035 */
00036 extern "C" {
00037   // AVR LibC Includes
00038   #include <inttypes.h>
00039   #include <avr/interrupt.h>
00040   #include "WConstants.h"
00041 }
00042 
00043 #include <Wire.h>
00044 #include "APM_BMP085.h"
00045 
00046 #define BMP085_ADDRESS 0x77  //(0xEE >> 1)
00047 #define BMP085_EOC 30        // End of conversion pin PC7
00048 
00049 // Constructors ////////////////////////////////////////////////////////////////
00050 APM_BMP085_Class::APM_BMP085_Class()
00051 {
00052 }
00053 
00054 // Public Methods //////////////////////////////////////////////////////////////
00055 void APM_BMP085_Class::Init(int initialiseWireLib)
00056 {
00057   unsigned char tmp;
00058   byte buff[22];
00059   int i=0;
00060 
00061   pinMode(BMP085_EOC,INPUT);   // End Of Conversion (PC7) input
00062   
00063   if( initialiseWireLib != 0 )
00064       Wire.begin();
00065   oss = 3;           // Over Sampling setting 3 = High resolution
00066   BMP085_State = 0;     // Initial state
00067 
00068   // We read the calibration data registers
00069   Wire.beginTransmission(BMP085_ADDRESS);
00070   Wire.send(0xAA);
00071   Wire.endTransmission();
00072 
00073   Wire.requestFrom(BMP085_ADDRESS, 22);
00074   //Wire.endTransmission();
00075   while(Wire.available())
00076   { 
00077     buff[i] = Wire.receive();  // receive one byte
00078     i++;
00079   }
00080   ac1 = ((int)buff[0] << 8) | buff[1];
00081   ac2 = ((int)buff[2] << 8) | buff[3];
00082   ac3 = ((int)buff[4] << 8) | buff[5];
00083   ac4 = ((int)buff[6] << 8) | buff[7];
00084   ac5 = ((int)buff[8] << 8) | buff[9];
00085   ac6 = ((int)buff[10] << 8) | buff[11];
00086   b1 = ((int)buff[12] << 8) | buff[13];
00087   b2 = ((int)buff[14] << 8) | buff[15];
00088   mb = ((int)buff[16] << 8) | buff[17];
00089   mc = ((int)buff[18] << 8) | buff[19];
00090   md = ((int)buff[20] << 8) | buff[21];
00091 
00092   //Send a command to read Temp
00093   Command_ReadTemp();
00094   BMP085_State=1;
00095 }
00096 
00097 
00098 // Read the sensor. This is a state machine
00099 // We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
00100 uint8_t APM_BMP085_Class::Read()
00101 {
00102 uint8_t result=0;
00103 
00104         if (BMP085_State==1)
00105         {
00106                 if (digitalRead(BMP085_EOC))
00107                 {
00108                         ReadTemp();             // On state 1 we read temp
00109                         BMP085_State++;
00110                         Command_ReadPress();
00111                 }
00112         }
00113         else
00114         {
00115                 if (BMP085_State==5)
00116                 {
00117                         if (digitalRead(BMP085_EOC))
00118                         {
00119                                 ReadPress();
00120                                 Calculate();
00121                                 BMP085_State = 1;    // Start again from state=1
00122                                 Command_ReadTemp();  // Read Temp
00123                                 result=1;            // New pressure reading
00124                         }
00125                 }
00126                 else
00127                 {
00128                         if (digitalRead(BMP085_EOC))
00129                         {
00130                                 ReadPress();
00131                                 Calculate();
00132                                 BMP085_State++;
00133                                 Command_ReadPress();
00134                                 result=1;            // New pressure reading
00135                         }
00136                 }
00137         }
00138   return(result);
00139 }
00140 
00141 
00142 // Send command to Read Pressure
00143 void APM_BMP085_Class::Command_ReadPress()
00144 {
00145   Wire.beginTransmission(BMP085_ADDRESS);
00146   Wire.send(0xF4);
00147   Wire.send(0x34+(oss<<6));  //write_register(0xF4,0x34+(oversampling_setting<<6));
00148   Wire.endTransmission();
00149 }
00150 
00151 // Read Raw Pressure values
00152 void APM_BMP085_Class::ReadPress()
00153 {
00154   byte msb;
00155   byte lsb;
00156   byte xlsb;
00157 
00158   Wire.beginTransmission(BMP085_ADDRESS);
00159   Wire.send(0xF6);
00160   Wire.endTransmission();
00161 
00162   Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
00163   while(!Wire.available()) {
00164     // waiting
00165   }
00166   msb = Wire.receive();
00167   while(!Wire.available()) {
00168     // waiting
00169   }
00170   lsb = Wire.receive();
00171   while(!Wire.available()) {
00172     // waiting
00173   }
00174   xlsb = Wire.receive();
00175   RawPress = (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >> (8-oss);
00176 }
00177 
00178 // Send Command to Read Temperature
00179 void APM_BMP085_Class::Command_ReadTemp()
00180 {
00181   Wire.beginTransmission(BMP085_ADDRESS);
00182   Wire.send(0xF4);
00183   Wire.send(0x2E);
00184   Wire.endTransmission();
00185 }
00186 
00187 // Read Raw Temperature values
00188 void APM_BMP085_Class::ReadTemp()
00189 { 
00190   byte tmp;
00191 
00192   Wire.beginTransmission(BMP085_ADDRESS);
00193   Wire.send(0xF6);
00194   Wire.endTransmission();
00195 
00196   Wire.beginTransmission(BMP085_ADDRESS);
00197   Wire.requestFrom(BMP085_ADDRESS,2);
00198   while(!Wire.available());  // wait
00199   RawTemp = Wire.receive();
00200   while(!Wire.available());  // wait
00201   tmp = Wire.receive();
00202   RawTemp = RawTemp<<8 | tmp;
00203 }
00204 
00205 // Calculate Temperature and Pressure in real units.
00206 void APM_BMP085_Class::Calculate()
00207 {
00208   long x1, x2, x3, b3, b5, b6, p;
00209   unsigned long b4, b7;
00210   int32_t tmp;
00211   
00212   // See Datasheet page 13 for this formulas
00213   // Based also on Jee Labs BMP085 example code. Thanks for share.
00214   // Temperature calculations
00215   x1 = ((long)RawTemp - ac6) * ac5 >> 15;
00216   x2 = ((long) mc << 11) / (x1 + md);
00217   b5 = x1 + x2;
00218   Temp = (b5 + 8) >> 4;
00219 
00220   // Pressure calculations
00221   b6 = b5 - 4000;
00222   x1 = (b2 * (b6 * b6 >> 12)) >> 11; 
00223   x2 = ac2 * b6 >> 11;
00224   x3 = x1 + x2;
00225   //b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD
00226   //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2;  //OK for oss=0
00227   tmp = ac1;
00228   tmp = (tmp*4 + x3)<<oss;
00229   b3 = (tmp+2)/4;
00230   x1 = ac3 * b6 >> 13;
00231   x2 = (b1 * (b6 * b6 >> 12)) >> 16;
00232   x3 = ((x1 + x2) + 2) >> 2;
00233   b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
00234   b7 = ((uint32_t) RawPress - b3) * (50000 >> oss);
00235   p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
00236   
00237   x1 = (p >> 8) * (p >> 8);
00238   x1 = (x1 * 3038) >> 16;
00239   x2 = (-7357 * p) >> 16;
00240   Press = p + ((x1 + x2 + 3791) >> 4);
00241 }
00242 
00243 // Constructors ////////////////////////////////////////////////////////////////
00244 APM_BMP085_HIL_Class::APM_BMP085_HIL_Class()
00245 {
00246 }
00247 
00248 // Public Methods //////////////////////////////////////////////////////////////
00249 void APM_BMP085_HIL_Class::Init(int initialiseWireLib)
00250 {
00251   BMP085_State=1;
00252 }
00253 
00254 
00255 // Read the sensor. This is a state machine
00256 // We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
00257 uint8_t APM_BMP085_HIL_Class::Read()
00258 {
00259     uint8_t result=0;
00260 
00261         if (BMP085_State==1)
00262         {
00263         BMP085_State++;
00264         }
00265         else
00266         {
00267                 if (BMP085_State==5)
00268                 {
00269             BMP085_State = 1;    // Start again from state=1
00270             result=1;            // New pressure reading
00271                 }
00272                 else
00273                 {
00274             BMP085_State++;
00275             result=1;            // New pressure reading
00276                 }
00277         }
00278     return(result);
00279 }
00280 
00281 void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press)
00282 {
00283     // TODO: map floats to raw
00284     Temp = _Temp;
00285     Press = _Press;
00286 }

Generated for ArduPilot Libraries by doxygen