From f1891cea1f2809dff12e2ee07e397b1acf2e5069 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 5 Jan 2015 22:00:32 +1100
Subject: [PATCH] AP_Baro: BMP085 driver done, but untested

---
 libraries/AP_Baro/AP_Baro.cpp        |   5 +
 libraries/AP_Baro/AP_Baro.h          |   2 +-
 libraries/AP_Baro/AP_Baro_BMP085.cpp | 136 +++++++++------------------
 libraries/AP_Baro/AP_Baro_BMP085.h   |  45 +++------
 4 files changed, 63 insertions(+), 125 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index dc861b6f5..65d50197e 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -241,6 +241,11 @@ void AP_Baro::init(void)
 #elif HAL_BARO_DEFAULT == HAL_BARO_HIL
     drivers[0] = new AP_Baro_HIL(*this);
     _num_drivers = 1;
+#elif HAL_BARO_DEFAULT == HAL_BARO_BMP085
+    {
+        drivers[0] = new AP_Baro_BMP085(*this);
+        _num_drivers = 1;
+    }
 #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611
     {
         drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(MS5611_I2C_ADDR));
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index b65426c13..9825b835b 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -149,7 +149,7 @@ private:
 
 #include "AP_Baro_Backend.h"
 #include "AP_Baro_MS5611.h"
-//#include "AP_Baro_BMP085.h"
+#include "AP_Baro_BMP085.h"
 #include "AP_Baro_HIL.h"
 #include "AP_Baro_PX4.h"
 
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp
index 6efbf79a9..c52e59153 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.cpp
+++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp
@@ -1,5 +1,4 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-#if 0
 /*
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
@@ -15,42 +14,16 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
- *       APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
- *       Code by Jordi Mu�oz and Jose Julio. DIYDrones.com
- *       Sensor is conected to I2C port
- *       Sensor End of Conversion (EOC) pin is PC7 (30)
- *
- *       Variables:
- *               RawTemp : Raw temperature data
- *               RawPress : Raw pressure data
- *
- *               Temp : Calculated temperature (in 0.1�C units)
- *               Press : Calculated pressure   (in Pa units)
- *
- *       Methods:
- *               Init() : Initialization of I2C and read sensor calibration data
- *               Read() : Read sensor data and calculate Temperature and Pressure
- *                        This function is optimized so the main host don�t need to wait
- *                                You can call this function in your main loop
- *                                It returns a 1 if there are new data.
- *
- *       Internal functions:
- *               Command_ReadTemp(): Send commando to read temperature
- *               Command_ReadPress(): Send commando to read Pressure
- *               ReadTemp() : Read temp register
- *               ReadPress() : Read press register
- *
- *
- */
+  BMP085 barometer driver. Based on original code by Jordi Munoz and
+  Jose Julio
 
-// AVR LibC Includes
-#include <inttypes.h>
+  Substantially modified by Andrew Tridgell
+*/
 
+#include <AP_HAL.h>
 #include <AP_Common.h>
-#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 
-#include <AP_HAL.h>
-#include "AP_Baro_BMP085.h"
+#include "AP_Baro.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -74,25 +47,36 @@ extern const AP_HAL::HAL& hal;
 // oversampling 3 gives 26ms conversion time. We then average
 #define OVERSAMPLING 3
 
-// Public Methods //////////////////////////////////////////////////////////////
-bool AP_Baro_BMP085::init()
+/*
+  constructor
+ */
+AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) :
+    AP_Baro_Backend(baro),
+    _instance(0),
+    _temp_sum(0),
+    _press_sum(0),
+    _count(0),
+    BMP085_State(0),
+    ac1(0), ac2(0), ac3(0), b1(0), b2(0), mb(0), mc(0), md(0),
+    ac4(0), ac5(0), ac6(0),
+    _retry_time(0)
 {
     uint8_t buff[22];
 
     // get pointer to i2c bus semaphore
-    AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
+    AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
 
     // take i2c bus sempahore
-    if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))
-        return false;
+    if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        hal.scheduler->panic(PSTR("BMP085: unable to get semaphore"));
+    }
 
-    hal.gpio->pinMode(BMP085_EOC, HAL_GPIO_INPUT);// End Of Conversion (PC7) input
+    // End Of Conversion (PC7) input
+    hal.gpio->pinMode(BMP085_EOC, HAL_GPIO_INPUT);
 
     // We read the calibration data registers
     if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xAA, 22, buff) != 0) {
-        _flags.healthy = false;
-        i2c_sem->give();
-        return false;
+        hal.scheduler->panic(PSTR("BMP085: bad calibration registers"));
     }
 
     ac1 = ((int16_t)buff[0] << 8) | buff[1];
@@ -110,17 +94,14 @@ bool AP_Baro_BMP085::init()
     _last_press_read_command_time = 0;
     _last_temp_read_command_time = 0;
 
+    _instance = _frontend.register_sensor();
+
     //Send a command to read Temp
     Command_ReadTemp();
     
     BMP085_State = 0;
 
-    // init raw temo
-    RawTemp = 0;
-
-    _flags.healthy = true;
     i2c_sem->give();
-    return true;
 }
 
 // Read the sensor. This is a state machine
@@ -141,8 +122,9 @@ void AP_Baro_BMP085::accumulate(void)
     if (BMP085_State == 0) {
         ReadTemp();
     } else {
-        ReadPress();
-        Calculate();
+        if (ReadPress()) {
+            Calculate();
+        }
     }
     BMP085_State++;
     if (BMP085_State == 5) {
@@ -156,76 +138,58 @@ void AP_Baro_BMP085::accumulate(void)
 }
 
 
-// Read the sensor using accumulated data
-uint8_t AP_Baro_BMP085::read()
+/*
+  transfer data to the frontend
+ */
+void AP_Baro_BMP085::update(void)
 {
     if (_count == 0 && BMP_DATA_READY()) {
         accumulate();
     }
     if (_count == 0) {
-        return 0;
+        return;
     }
-    _last_update = hal.scheduler->millis();
 
-    Temp = 0.1f * _temp_sum / _count;
-    Press = _press_sum / _count;
+    float temperature = 0.1f * _temp_sum / _count;
+    float pressure = _press_sum / _count;
 
     _count = 0;
     _temp_sum = 0;
     _press_sum = 0;
 
-    return 1;
-}
-
-float AP_Baro_BMP085::get_pressure() {
-    return Press;
+    _copy_to_frontend(_instance, pressure, temperature);
 }
 
-float AP_Baro_BMP085::get_temperature() const {
-    return Temp;
-}
-
-// Private functions: /////////////////////////////////////////////////////////
-
 // Send command to Read Pressure
 void AP_Baro_BMP085::Command_ReadPress()
 {
     // Mode 0x34+(OVERSAMPLING << 6) is osrs=3 when OVERSAMPLING=3 => 25.5ms conversion time
-    uint8_t res = hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4,
-            0x34+(OVERSAMPLING << 6));
+    hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4,
+                           0x34+(OVERSAMPLING << 6));
     _last_press_read_command_time = hal.scheduler->millis();
-    if (res != 0) {
-        _flags.healthy = false;
-    }
 }
 
 // Read Raw Pressure values
-void AP_Baro_BMP085::ReadPress()
+bool AP_Baro_BMP085::ReadPress()
 {
     uint8_t buf[3];
 
-    if (!_flags.healthy && hal.scheduler->millis() < _retry_time) {
-        return;
-    }
-
     if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
         _retry_time = hal.scheduler->millis() + 1000;
         hal.i2c->setHighSpeed(false);
-        _flags.healthy = false;
-        return;
+        return false;
     }
 
     RawPress = (((uint32_t)buf[0] << 16) 
              | ((uint32_t)buf[1] << 8)
              | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
+    return true;
 }
 
 // Send Command to Read Temperature
 void AP_Baro_BMP085::Command_ReadTemp()
 {
-    if (hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E) != 0) {
-        _flags.healthy = false;
-    }
+    hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E);
     _last_temp_read_command_time = hal.scheduler->millis();
 }
 
@@ -235,20 +199,14 @@ void AP_Baro_BMP085::ReadTemp()
     uint8_t buf[2];
     int32_t _temp_sensor;
 
-    if (!_flags.healthy && hal.scheduler->millis() < _retry_time) {
-        return;
-    }
-
     if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
-        _retry_time = hal.scheduler->millis() + 1000;
         hal.i2c->setHighSpeed(false);
-        _flags.healthy = false;
         return;
     }
     _temp_sensor = buf[0];
     _temp_sensor = (_temp_sensor << 8) | buf[1];
 
-    RawTemp = _temp_filter.apply(_temp_sensor);
+    RawTemp = _temp_sensor;
 }
 
 
@@ -296,5 +254,3 @@ void AP_Baro_BMP085::Calculate()
         _count /= 2;
     }
 }
-
-#endif
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h
index 85ee991b4..065ca7179 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.h
+++ b/libraries/AP_Baro/AP_Baro_BMP085.h
@@ -2,65 +2,42 @@
 #ifndef __AP_BARO_BMP085_H__
 #define __AP_BARO_BMP085_H__
 
-#define PRESS_FILTER_SIZE 2
-
 #include "AP_Baro.h"
-#include <AverageFilter.h>
 
-class AP_Baro_BMP085 : public AP_Baro
+class AP_Baro_BMP085 : public AP_Baro_Backend
 {
 public:
-    AP_Baro_BMP085() :
-        RawPress(0),
-        RawTemp(0),
-        _temp_sum(0.0f),
-        _press_sum(0.0f),
-        _count(0),
-        Temp(0.0f),
-        Press(0.0f),
-        _last_press_read_command_time(0),
-        _last_temp_read_command_time(0),
-        BMP085_State(0),
-        ac1(0), ac2(0), ac3(0), b1(0), b2(0), mb(0), mc(0), md(0),
-        ac4(0), ac5(0), ac6(0),
-        _retry_time(0)
-    {
-    };       // Constructor
-
+    // Constructor
+    AP_Baro_BMP085(AP_Baro &baro);
 
     /* AP_Baro public interface: */
-    bool            init();
-    uint8_t         read();
-    void 			accumulate(void);
-    float           get_pressure();
-    float           get_temperature() const;
+    void update();
+    void accumulate(void);
 
 private:
-    int32_t         RawPress;
-    int32_t         RawTemp;
+    uint8_t         _instance;
     float		    _temp_sum;
     float			_press_sum;
     uint8_t			_count;
-    float           Temp;
-    float           Press;
+
     // Flymaple has no EOC pin, so use times instead
     uint32_t        _last_press_read_command_time;
     uint32_t        _last_temp_read_command_time;
-
     
     // State machine
     uint8_t                         BMP085_State;
+
     // Internal calibration registers
     int16_t                         ac1, ac2, ac3, b1, b2, mb, mc, md;
     uint16_t                        ac4, ac5, ac6;
 
-    AverageFilterInt32_Size4        _temp_filter;
-
     uint32_t                        _retry_time;
+    int32_t                         RawPress;
+    int32_t                         RawTemp;
 
     void                            Command_ReadPress();
     void                            Command_ReadTemp();
-    void                            ReadPress();
+    bool                            ReadPress();
     void                            ReadTemp();
     void                            Calculate();
 };
-- 
GitLab