I2C: convert barometer library to new I2C library

this also adds a healthy attribute and error checking
This commit is contained in:
Andrew Tridgell 2011-12-28 20:32:21 +11:00
parent acf4e9b61d
commit 7ba744a11a
8 changed files with 109 additions and 115 deletions

View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_H__
#define __AP_BARO_H__
@ -7,15 +8,16 @@
class AP_Baro
{
public:
AP_Baro() {}
virtual void init(AP_PeriodicProcess *scheduler)=0;
virtual uint8_t read() = 0;
virtual int32_t get_pressure() = 0;
virtual int16_t get_temperature() = 0;
virtual float get_altitude() = 0;
virtual int32_t get_raw_pressure() = 0;
virtual int32_t get_raw_temp() = 0;
bool healthy;
AP_Baro() {}
virtual bool init(AP_PeriodicProcess *scheduler)=0;
virtual uint8_t read() = 0;
virtual int32_t get_pressure() = 0;
virtual int16_t get_temperature() = 0;
virtual float get_altitude() = 0;
virtual int32_t get_raw_pressure() = 0;
virtual int32_t get_raw_temp() = 0;
};
#include "AP_Baro_MS5611.h"

View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
@ -41,7 +42,9 @@ extern "C" {
#include "WConstants.h"
}
#include <Wire.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <I2C.h>
#include "AP_Baro_BMP085.h"
#define BMP085_ADDRESS 0x77 //(0xEE >> 1)
@ -55,34 +58,19 @@ extern "C" {
// Public Methods //////////////////////////////////////////////////////////////
void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
{
byte buff[22];
int i = 0;
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
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);
if (Wire.endTransmission() != 0) {
// Error!
return;
}
Wire.requestFrom(BMP085_ADDRESS, 22);
//Wire.endTransmission();
while(Wire.available()){
buff[i] = Wire.receive(); // receive one byte
i++;
}
if (i != 22) {
// Error!
return;
// We read the calibration data registers
if (I2c.read(BMP085_ADDRESS, 0xAA, 22, buff) != 0) {
healthy = false;
return false;
}
ac1 = ((int)buff[0] << 8) | buff[1];
@ -97,10 +85,12 @@ void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
mc = ((int)buff[18] << 8) | buff[19];
md = ((int)buff[20] << 8) | buff[21];
//Send a command to read Temp
//Send a command to read Temp
Command_ReadTemp();
BMP085_State = 1;
return;
healthy = true;
return true;
}
// Read the sensor. This is a state machine
@ -152,48 +142,27 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
// Send command to Read Pressure
void AP_Baro_BMP085::Command_ReadPress()
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x34+(oss << 6)); // write_register(0xF4, 0x34+(oversampling_setting << 6));
Wire.endTransmission();
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) {
healthy = false;
}
}
// Read Raw Pressure values
void AP_Baro_BMP085::ReadPress()
{
byte msb;
byte lsb;
byte xlsb;
uint8_t buf[3];
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
while(!Wire.available()) {
// waiting
if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
healthy = false;
return;
}
msb = Wire.receive();
while(!Wire.available()) {
// waiting
}
lsb = Wire.receive();
while(!Wire.available()) {
// waiting
}
xlsb = Wire.receive();
RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss);
RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss);
if(_offset_press == 0){
_offset_press = RawPress;
RawPress = 0;
}else{
} else{
RawPress -= _offset_press;
}
// filter
@ -203,8 +172,9 @@ void AP_Baro_BMP085::ReadPress()
_press_index = 0;
RawPress = 0;
// sum our filter
for(uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
RawPress += _press_filter[i];
}
@ -217,35 +187,27 @@ void AP_Baro_BMP085::ReadPress()
// Send Command to Read Temperature
void AP_Baro_BMP085::Command_ReadTemp()
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x2E);
Wire.endTransmission();
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x2E) != 0) {
healthy = false;
}
}
// Read Raw Temperature values
void AP_Baro_BMP085::ReadTemp()
{
byte tmp;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
uint8_t buf[2];
Wire.beginTransmission(BMP085_ADDRESS);
Wire.requestFrom(BMP085_ADDRESS,2);
if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
healthy = false;
return;
}
RawTemp = buf[0];
RawTemp = (RawTemp << 8) | buf[1];
while(!Wire.available()); // wait
RawTemp = Wire.receive();
while(!Wire.available()); // wait
tmp = Wire.receive();
RawTemp = RawTemp << 8 | tmp;
if(_offset_temp == 0){
if (_offset_temp == 0){
_offset_temp = RawTemp;
RawTemp = 0;
}else{
} else {
RawTemp -= _offset_temp;
}

View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_BMP085_H__
#define __AP_BARO_BMP085_H__
@ -16,7 +17,7 @@ class AP_Baro_BMP085 : public AP_Baro
/* AP_Baro public interface: */
void init(AP_PeriodicProcess * scheduler);
bool init(AP_PeriodicProcess * scheduler);
uint8_t read();
int32_t get_pressure();
int16_t get_temperature();

View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
extern "C" {
// AVR LibC Includes
@ -14,9 +15,10 @@ AP_Baro_BMP085_HIL::AP_Baro_BMP085_HIL()
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler)
bool AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler)
{
BMP085_State=1;
BMP085_State=1;
return true;
}
@ -46,6 +48,7 @@ void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press)
// TODO: map floats to raw
Temp = _Temp;
Press = _Press;
healthy = true;
}
int32_t AP_Baro_BMP085_HIL::get_pressure() {

View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_BMP085_HIL_H__
#define __AP_BARO_BMP085_HIL_H__
@ -6,16 +7,18 @@
class AP_Baro_BMP085_HIL
{
private:
uint8_t BMP085_State;
private:
uint8_t BMP085_State;
int16_t Temp;
int32_t Press;
public:
public:
AP_Baro_BMP085_HIL(); // Constructor
//int Altitude;
bool healthy;
uint8_t oss;
void init(AP_PeriodicProcess * scheduler);
bool init(AP_PeriodicProcess * scheduler);
uint8_t read();
int32_t get_pressure();
int16_t get_temperature();

View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com
@ -110,12 +111,12 @@ void AP_Baro_MS5611::_spi_write(uint8_t reg)
// Public Methods //////////////////////////////////////////////////////////////
// SPI should be initialized externally
void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
{
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
digitalWrite(MS5611_CS, HIGH);
delay(1);
digitalWrite(MS5611_CS, HIGH);
delay(1);
_spi_write(CMD_MS5611_RESET);
delay(4);
@ -128,14 +129,17 @@ void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
C6 = _spi_read_16bits(CMD_MS5611_PROM_C6);
//Send a command to read Temp first
//Send a command to read Temp first
_spi_write(CMD_CONVERT_D2_OSR4096);
_timer = micros();
_state = 1;
Temp=0;
Press=0;
Temp=0;
Press=0;
scheduler->register_process( AP_Baro_MS5611::_update );
scheduler->register_process( AP_Baro_MS5611::_update );
healthy = true;
return true;
}

View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_MS5611_H__
#define __AP_BARO_MS5611_H__
@ -10,7 +11,7 @@ class AP_Baro_MS5611 : public AP_Baro
AP_Baro_MS5611() {} // Constructor
/* AP_Baro public interface: */
void init(AP_PeriodicProcess *scheduler);
bool init(AP_PeriodicProcess *scheduler);
uint8_t read();
int32_t get_pressure(); // in mbar*100 units
int16_t get_temperature(); // in celsius degrees * 100 units

View File

@ -4,27 +4,42 @@
*/
#include <FastSerial.h>
#include <Wire.h>
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <I2C.h>
#include <SPI.h>
#include <AP_Baro.h> // ArduPilot Mega BMP085 Library
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h>
#include <AP_Math.h>
#include <AP_Common.h>
APM_BMP085_Class APM_BMP085;
#ifndef APM2_HARDWARE
# define APM2_HARDWARE 0
#endif
AP_Baro_BMP085 APM_BMP085(APM2_HARDWARE);
unsigned long timer;
FastSerialPort0(Serial);
#ifdef APM2_HARDWARE
static bool apm2_hardware = true;
#else
static bool apm2_hardware = false;
#endif
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
void setup()
{
Serial.begin(115200);
Serial.println("ArduPilot Mega BMP085 library test");
Serial.println("Initialising barometer..."); delay(100);
if (!APM_BMP085.Init(1, apm2_hardware)) {
I2c.begin();
I2c.timeOut(20);
isr_registry.init();
scheduler.init(&isr_registry);
if (!APM_BMP085.init(&scheduler)) {
Serial.println("Barometer initialisation FAILED\n");
}
Serial.println("initialisation complete."); delay(100);
@ -34,19 +49,22 @@ void setup()
void loop()
{
int ch;
float tmp_float;
float Altitude;
if((millis()- timer) > 50){
timer = millis();
APM_BMP085.Read();
APM_BMP085.read();
if (!APM_BMP085.healthy) {
Serial.println("not healthy");
return;
}
Serial.print("Pressure:");
Serial.print(APM_BMP085.Press);
Serial.print(APM_BMP085.get_pressure());
Serial.print(" Temperature:");
Serial.print(APM_BMP085.Temp / 10.0);
Serial.print(APM_BMP085.get_temperature());
Serial.print(" Altitude:");
tmp_float = (APM_BMP085.Press / 101325.0);
tmp_float = (APM_BMP085.get_pressure() / 101325.0);
tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330 * (1.0 - tmp_float);
Serial.print(Altitude);