mirror of https://github.com/ArduPilot/ardupilot
barometer: add purple_hardware option to Init()
this allows selection of right hardware at Init() time
This commit is contained in:
parent
f66a6b4308
commit
004a4425cd
|
@ -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++;
|
||||||
|
|
|
@ -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:
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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);
|
||||||
};
|
};
|
||||||
|
|
|
@ -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();
|
||||||
|
|
Loading…
Reference in New Issue