barometer: add purple_hardware option to Init()

this allows selection of right hardware at Init() time
This commit is contained in:
Pat Hickey 2011-11-13 13:45:23 +11:00
parent f66a6b4308
commit 004a4425cd
5 changed files with 35 additions and 10 deletions

View File

@ -33,6 +33,7 @@
*/ */
extern "C" { extern "C" {
// AVR LibC Includes // AVR LibC Includes
#include <inttypes.h> #include <inttypes.h>
@ -51,12 +52,21 @@ extern "C" {
//{ //{
//} //}
// the purple hardware needs to check the state of the
// chip using a direct IO port
// On Purple prerelease hw, the data ready port is hooked up to PE7, which
// is not available to the arduino digitalRead function.
#define BMP_DATA_READY() (_purple_hardware?(PINE&0x80):digitalRead(BMP085_EOC))
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void APM_BMP085_Class::Init(int initialiseWireLib) bool APM_BMP085_Class::Init(int initialiseWireLib, bool purple_hardware)
{ {
byte buff[22]; byte buff[22];
int i = 0; int i = 0;
_purple_hardware = purple_hardware;
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
if( initialiseWireLib != 0 ) if( initialiseWireLib != 0 )
@ -68,7 +78,9 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
// We read the calibration data registers // We read the calibration data registers
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xAA); Wire.send(0xAA);
Wire.endTransmission(); if (Wire.endTransmission() != 0) {
return false;
}
Wire.requestFrom(BMP085_ADDRESS, 22); Wire.requestFrom(BMP085_ADDRESS, 22);
@ -77,6 +89,9 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
buff[i] = Wire.receive(); // receive one byte buff[i] = Wire.receive(); // receive one byte
i++; i++;
} }
if (i != 22) {
return false;
}
ac1 = ((int)buff[0] << 8) | buff[1]; ac1 = ((int)buff[0] << 8) | buff[1];
ac2 = ((int)buff[2] << 8) | buff[3]; ac2 = ((int)buff[2] << 8) | buff[3];
@ -93,6 +108,7 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
//Send a command to read Temp //Send a command to read Temp
Command_ReadTemp(); Command_ReadTemp();
BMP085_State = 1; BMP085_State = 1;
return true;
} }
/* /*
@ -103,14 +119,14 @@ uint8_t APM_BMP085_Class::Read()
uint8_t result = 0; uint8_t result = 0;
if (BMP085_State == 1){ if (BMP085_State == 1){
if (digitalRead(BMP085_EOC)){ if (BMP_DATA_READY()) {
ReadTemp(); // On state 1 we read temp ReadTemp(); // On state 1 we read temp
BMP085_State++; BMP085_State++;
Command_ReadPress(); Command_ReadPress();
} }
}else{ }else{
if (BMP085_State == 5){ if (BMP085_State == 5){
if (digitalRead(BMP085_EOC)){ if (BMP_DATA_READY()){
ReadPress(); ReadPress();
Calculate(); Calculate();
@ -119,7 +135,7 @@ uint8_t APM_BMP085_Class::Read()
result = 1; // New pressure reading result = 1; // New pressure reading
} }
}else{ }else{
if (digitalRead(BMP085_EOC)){ if (BMP_DATA_READY()){
ReadPress(); ReadPress();
Calculate(); Calculate();
BMP085_State++; BMP085_State++;
@ -310,4 +326,4 @@ void APM_BMP085_Class::Calculate()
x1 = (x1 * 3038) >> 16; x1 = (x1 * 3038) >> 16;
x2 = (-7357 * p) >> 16; x2 = (-7357 * p) >> 16;
Press = p + ((x1 + x2 + 3791) >> 4); Press = p + ((x1 + x2 + 3791) >> 4);
} }

View File

@ -19,9 +19,10 @@ class APM_BMP085_Class
int32_t Press; int32_t Press;
//int Altitude; //int Altitude;
uint8_t oss; uint8_t oss;
bool _purple_hardware;
//int32_t Press0; // Pressure at sea level //int32_t Press0; // Pressure at sea level
void Init(int initialiseWireLib = 1); bool Init(int initialiseWireLib = 1, bool purple_hardware=false);
uint8_t Read(); uint8_t Read();
private: private:

View File

@ -15,7 +15,7 @@ APM_BMP085_HIL_Class::APM_BMP085_HIL_Class()
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void APM_BMP085_HIL_Class::Init(int initialiseWireLib) void APM_BMP085_HIL_Class::Init(int initialiseWireLib, bool purple_hardware)
{ {
BMP085_State=1; BMP085_State=1;
} }

View File

@ -13,7 +13,7 @@ class APM_BMP085_HIL_Class
int32_t Press; int32_t Press;
//int Altitude; //int Altitude;
uint8_t oss; uint8_t oss;
void Init(int initialiseWireLib = 1); void Init(int initialiseWireLib = 1, bool purple_hardware=false);
uint8_t Read(); uint8_t Read();
void setHIL(float Temp, float Press); void setHIL(float Temp, float Press);
}; };

View File

@ -3,6 +3,7 @@
Code by Jordi MuÒoz and Jose Julio. DIYDrones.com Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
*/ */
#include <FastSerial.h>
#include <Wire.h> #include <Wire.h>
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library #include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
@ -10,12 +11,19 @@ APM_BMP085_Class APM_BMP085;
unsigned long timer; unsigned long timer;
FastSerialPort0(Serial);
// set this to true on purple
static bool purple_hardware = false;
void setup() void setup()
{ {
Serial.begin(115200); Serial.begin(115200);
Serial.println("ArduPilot Mega BMP085 library test"); Serial.println("ArduPilot Mega BMP085 library test");
Serial.println("Initialising barometer..."); delay(100); Serial.println("Initialising barometer..."); delay(100);
APM_BMP085.Init(); // APM ADC initialization if (!APM_BMP085.Init(1, purple_hardware)) {
Serial.println("Barometer initialisation FAILED\n");
}
Serial.println("initialisation complete."); delay(100); Serial.println("initialisation complete."); delay(100);
delay(1000); delay(1000);
timer = millis(); timer = millis();