From 7ba744a11a573d1d89f10959513594b55c12747c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Dec 2011 20:32:21 +1100 Subject: [PATCH] I2C: convert barometer library to new I2C library this also adds a healthy attribute and error checking --- libraries/AP_Baro/AP_Baro.h | 20 ++-- libraries/AP_Baro/AP_Baro_BMP085.cpp | 112 ++++++------------ libraries/AP_Baro/AP_Baro_BMP085.h | 3 +- libraries/AP_Baro/AP_Baro_BMP085_hil.cpp | 7 +- libraries/AP_Baro/AP_Baro_BMP085_hil.h | 13 +- libraries/AP_Baro/AP_Baro_MS5611.cpp | 20 ++-- libraries/AP_Baro/AP_Baro_MS5611.h | 3 +- .../AP_Baro_BMP085_test.pde | 46 ++++--- 8 files changed, 109 insertions(+), 115 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 871ab3a8bc..49b4b00075 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -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" diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 60480ec82e..e88016f362 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -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�oz and Jose Julio. DIYDrones.com @@ -41,7 +42,9 @@ extern "C" { #include "WConstants.h" } -#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include #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; } diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index f894ab4395..ea9e5742b0 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -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(); diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp b/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp index 2af3da2d3d..795326cb4f 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp @@ -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() { diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.h b/libraries/AP_Baro/AP_Baro_BMP085_hil.h index fbcef71086..d7dbfd7618 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085_hil.h +++ b/libraries/AP_Baro/AP_Baro_BMP085_hil.h @@ -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(); diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index ab424c7de8..497fb11d45 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -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; } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 92cec87c26..f1a72e2681 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -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 diff --git a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde index ffbd13587e..8a4d4163cc 100644 --- a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde @@ -4,27 +4,42 @@ */ #include -#include -#include // ArduPilot Mega BMP085 Library +#include +#include +#include // ArduPilot Mega BMP085 Library +#include +#include +#include +#include +#include -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);