From 5bb57a31f732bd00d886ec8edba9b0f6e0e5ba0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Oct 2014 06:22:51 +1100 Subject: [PATCH] AP_Baro: split into frontend/backend this allows for support of multiple sensors on a board --- libraries/AP_Baro/AP_Baro.cpp | 238 +++++++---- libraries/AP_Baro/AP_Baro.h | 143 ++++--- libraries/AP_Baro/AP_Baro_BMP085.cpp | 3 +- libraries/AP_Baro/AP_Baro_BMP085.h | 1 - libraries/AP_Baro/AP_Baro_Backend.cpp | 23 ++ libraries/AP_Baro/AP_Baro_Backend.h | 29 ++ libraries/AP_Baro/AP_Baro_HIL.cpp | 97 ++--- libraries/AP_Baro/AP_Baro_HIL.h | 30 +- libraries/AP_Baro/AP_Baro_MS5611.cpp | 385 ++++++------------ libraries/AP_Baro/AP_Baro_MS5611.h | 121 +++--- libraries/AP_Baro/AP_Baro_PX4.cpp | 101 +++-- libraries/AP_Baro/AP_Baro_PX4.h | 28 +- libraries/AP_Baro/AP_Baro_VRBRAIN.cpp | 86 ---- libraries/AP_Baro/AP_Baro_VRBRAIN.h | 29 -- .../examples/BARO_generic/BARO_generic.pde | 25 +- .../AP_Baro_BMP085_test.pde | 5 +- 16 files changed, 587 insertions(+), 757 deletions(-) create mode 100644 libraries/AP_Baro/AP_Baro_Backend.cpp create mode 100644 libraries/AP_Baro/AP_Baro_Backend.h delete mode 100644 libraries/AP_Baro/AP_Baro_VRBRAIN.cpp delete mode 100644 libraries/AP_Baro/AP_Baro_VRBRAIN.h diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 97697f4a3e..dc861b6f53 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -36,14 +36,14 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = { // @Description: calibrated ground pressure in Pascals // @Units: pascals // @Increment: 1 - AP_GROUPINFO("ABS_PRESS", 2, AP_Baro, _ground_pressure, 0), + AP_GROUPINFO("ABS_PRESS", 2, AP_Baro, sensors[0].ground_pressure, 0), // @Param: TEMP // @DisplayName: ground temperature // @Description: calibrated ground temperature in degrees Celsius // @Units: degrees celsius // @Increment: 1 - AP_GROUPINFO("TEMP", 3, AP_Baro, _ground_temperature, 0), + AP_GROUPINFO("TEMP", 3, AP_Baro, sensors[0].ground_temperature, 0), // @Param: ALT_OFFSET // @DisplayName: altitude offset @@ -56,85 +56,95 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = { AP_GROUPEND }; +/* + AP_Baro constructor + */ +AP_Baro::AP_Baro() : + _last_altitude_EAS2TAS(0.0f), + _EAS2TAS(0.0f), + _num_sensors(0), + _num_drivers(0) +{ + memset(sensors, 0, sizeof(sensors)); + + AP_Param::setup_object_defaults(this, var_info); +} + // calibrate the barometer. This must be called at least once before // the altitude() or climb_rate() interfaces can be used void AP_Baro::calibrate() { - float ground_pressure = 0; - float ground_temperature = 0; - // reset the altitude offset when we calibrate. The altitude // offset is supposed to be for within a flight _alt_offset.set_and_save(0); - { - uint32_t tstart = hal.scheduler->millis(); - while (ground_pressure == 0 || !_flags.healthy) { - read(); // Get initial data from absolute pressure sensor - if (hal.scheduler->millis() - tstart > 500) { - hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful " - "for more than 500ms in AP_Baro::calibrate [1]\r\n")); - } - ground_pressure = get_pressure(); - ground_temperature = get_calibration_temperature(); - hal.scheduler->delay(20); - } - } // let the barometer settle for a full second after startup // the MS5611 reads quite a long way off for the first second, // leading to about 1m of error if we don't wait for (uint8_t i = 0; i < 10; i++) { uint32_t tstart = hal.scheduler->millis(); do { - read(); + update(); if (hal.scheduler->millis() - tstart > 500) { hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful " "for more than 500ms in AP_Baro::calibrate [2]\r\n")); } - } while (!_flags.healthy); - ground_pressure = get_pressure(); - ground_temperature = get_calibration_temperature(); - + } while (!healthy()); hal.scheduler->delay(100); } // now average over 5 values for the ground pressure and // temperature settings - for (uint16_t i = 0; i < 5; i++) { + float sum_pressure[BARO_MAX_INSTANCES] = {0}; + float sum_temperature[BARO_MAX_INSTANCES] = {0}; + const uint8_t num_samples = 5; + + for (uint8_t c = 0; c < num_samples; c++) { uint32_t tstart = hal.scheduler->millis(); + bool all_healthy = false; do { - read(); + update(); if (hal.scheduler->millis() - tstart > 500) { hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful " "for more than 500ms in AP_Baro::calibrate [3]\r\n")); } - } while (!_flags.healthy); - ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f); - ground_temperature = (ground_temperature * 0.8f) + - (get_calibration_temperature() * 0.2f); - + all_healthy = true; + for (uint8_t i=0; i<_num_sensors; i++) { + if (!healthy(i)) { + all_healthy = false; + } + } + } while (!all_healthy); + for (uint8_t i=0; i<_num_sensors; i++) { + sum_pressure[i] += sensors[i].pressure; + sum_temperature[i] += sensors[i].temperature; + } hal.scheduler->delay(100); } - - _ground_pressure.set_and_save(ground_pressure); - _ground_temperature.set_and_save(ground_temperature); + for (uint8_t i=0; i<_num_sensors; i++) { + sensors[i].ground_pressure.set_and_save(sum_pressure[i] / num_samples); + sensors[i].ground_temperature.set_and_save(sum_temperature[i] / num_samples); + } } -/** +/* update the barometer calibration this updates the baro ground calibration to the current values. It can be used before arming to keep the baro well calibrated */ void AP_Baro::update_calibration() { - float pressure = get_pressure(); - _ground_pressure.set(pressure); - float last_temperature = _ground_temperature; - _ground_temperature.set(get_calibration_temperature()); - if (fabsf(last_temperature - _ground_temperature) > 3) { - // reset _EAS2TAS to force it to recalculate. This happens - // when a digital airspeed sensor comes online - _EAS2TAS = 0; + for (uint8_t i=0; i<_num_sensors; i++) { + if (healthy(i)) { + sensors[i].ground_pressure.set(get_pressure(i)); + } + float last_temperature = sensors[i].ground_temperature; + sensors[i].ground_temperature.set(get_calibration_temperature(i)); + if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) { + // reset _EAS2TAS to force it to recalculate. This happens + // when a digital airspeed sensor comes online + _EAS2TAS = 0; + } } } @@ -146,12 +156,12 @@ float AP_Baro::get_altitude_difference(float base_pressure, float pressure) cons #if HAL_CPU_CLASS <= HAL_CPU_CLASS_16 // on slower CPUs use a less exact, but faster, calculation float scaling = base_pressure / pressure; - float temp = _ground_temperature + 273.15f; + float temp = get_calibration_temperature() + 273.15f; ret = logf(scaling) * temp * 29.271267f; #else // on faster CPUs use a more exact calculation float scaling = pressure / base_pressure; - float temp = _ground_temperature + 273.15f; + float temp = get_calibration_temperature() + 273.15f; // This is an exact calculation that is within +-2.5m of the standard atmosphere tables // in the troposphere (up to 11,000 m amsl). @@ -160,54 +170,21 @@ float AP_Baro::get_altitude_difference(float base_pressure, float pressure) cons return ret; } -// return current altitude estimate relative to time that calibrate() -// was called. Returns altitude in meters -// note that this relies on read() being called regularly to get new data -float AP_Baro::get_altitude(void) -{ - if (_ground_pressure == 0) { - // called before initialisation - return 0; - } - - if (_last_altitude_t == _last_update) { - // no new information - return _altitude + _alt_offset; - } - - float pressure = get_pressure(); - float alt = get_altitude_difference(_ground_pressure, pressure); - - // record that we have consumed latest data - _last_altitude_t = _last_update; - - // sanity check altitude - if (isnan(alt) || isinf(alt)) { - _flags.alt_ok = false; - } else { - _altitude = alt; - _flags.alt_ok = true; - } - - // ensure the climb rate filter is updated - _climb_rate_filter.update(_altitude, _last_update); - - return _altitude + _alt_offset; -} // return current scale factor that converts from equivalent to true airspeed // valid for altitudes up to 10km AMSL // assumes standard atmosphere lapse rate float AP_Baro::get_EAS2TAS(void) { - if ((fabsf(_altitude - _last_altitude_EAS2TAS) < 100.0f) && (_EAS2TAS != 0.0f)) { + float altitude = get_altitude(); + if ((fabsf(altitude - _last_altitude_EAS2TAS) < 100.0f) && (_EAS2TAS != 0.0f)) { // not enough change to require re-calculating return _EAS2TAS; } - float tempK = ((float)_ground_temperature) + 273.15f - 0.0065f * _altitude; + float tempK = get_calibration_temperature() + 273.15f - 0.0065f * altitude; _EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK))); - _last_altitude_EAS2TAS = _altitude; + _last_altitude_EAS2TAS = altitude; return _EAS2TAS; } @@ -234,7 +211,7 @@ void AP_Baro::set_external_temperature(float temperature) /* get the temperature in degrees C to be used for calibration purposes */ -float AP_Baro::get_calibration_temperature(void) const +float AP_Baro::get_calibration_temperature(uint8_t instance) const { // if we have a recent external temperature then use it if (_last_external_temperature_ms != 0 && hal.scheduler->millis() - _last_external_temperature_ms < 10000) { @@ -245,9 +222,104 @@ float AP_Baro::get_calibration_temperature(void) const // not just using the baro temperature is it tends to read high, // often 30 degrees above the actual temperature. That means the // EAS2TAS tends to be off by quite a large margin - float ret = get_temperature(); + float ret = get_temperature(instance); if (ret > 25) { ret = 25; } return ret; } + + +/* + initialise the barometer object, loading backend drivers + */ +void AP_Baro::init(void) +{ +#if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN + drivers[0] = new AP_Baro_PX4(*this); + _num_drivers = 1; +#elif HAL_BARO_DEFAULT == HAL_BARO_HIL + drivers[0] = new AP_Baro_HIL(*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)); + _num_drivers = 1; + } +#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI + { + drivers[0] = new AP_Baro_MS5611(*this, + new AP_SerialBus_SPI(AP_HAL::SPIDevice_MS5611, + AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH)); + _num_drivers = 1; + } +#endif + if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) { + hal.scheduler->panic(PSTR("Baro: unable to initialise driver")); + } +} + + +/* + call update on all drivers + */ +void AP_Baro::update(void) +{ + for (uint8_t i=0; i<_num_drivers; i++) { + drivers[i]->update(); + } + + // consider a sensor as healthy if it has had an update in the + // last 0.5 seconds + uint32_t now = hal.scheduler->millis(); + for (uint8_t i=0; i<_num_sensors; i++) { + sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && sensors[i].pressure != 0; + } + + for (uint8_t i=0; i<_num_sensors; i++) { + if (sensors[i].healthy) { + // update altitude calculation + if (sensors[i].ground_pressure == 0) { + sensors[i].ground_pressure = sensors[i].pressure; + } + sensors[i].altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure); + // sanity check altitude + sensors[i].alt_ok = !(isnan(sensors[i].altitude) || isinf(sensors[i].altitude)); + } + } + + // choose primary sensor + _primary = 0; + for (uint8_t i=0; i<_num_sensors; i++) { + if (healthy(i)) { + _primary = i; + break; + } + } + + // ensure the climb rate filter is updated + _climb_rate_filter.update(get_altitude(), get_last_update()); +} + +/* + call accululate on all drivers + */ +void AP_Baro::accumulate(void) +{ + for (uint8_t i=0; i<_num_drivers; i++) { + drivers[i]->accumulate(); + } +} + + +/* register a new sensor, claiming a sensor slot. If we are out of + slots it will panic +*/ +uint8_t AP_Baro::register_sensor(void) +{ + if (_num_sensors >= BARO_MAX_INSTANCES) { + hal.scheduler->panic(PSTR("Too many barometers")); + } + return _num_sensors++; +} + diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 6e395dcfdd..b65426c135 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -3,85 +3,92 @@ #ifndef __AP_BARO_H__ #define __AP_BARO_H__ +#include #include #include #include +// maximum number of sensor instances +#if HAL_CPU_CLASS == HAL_CPU_CLASS_16 +#define BARO_MAX_INSTANCES 1 +#else +#define BARO_MAX_INSTANCES 2 +#endif + +// maximum number of drivers. Note that a single driver can provide +// multiple sensor instances +#if HAL_CPU_CLASS == HAL_CPU_CLASS_16 +#define BARO_MAX_DRIVERS 1 +#else +#define BARO_MAX_DRIVERS 2 +#endif + +class AP_Baro_Backend; + class AP_Baro { -public: - AP_Baro() : - _last_update(0), - _pressure_samples(0), - _altitude(0.0f), - _last_altitude_EAS2TAS(0.0f), - _EAS2TAS(0.0f), - _last_altitude_t(0), - _last_external_temperature_ms(0) - { - // initialise flags - _flags.healthy = false; - _flags.alt_ok = false; + friend class AP_Baro_Backend; - AP_Param::setup_object_defaults(this, var_info); - } +public: + // constructor + AP_Baro(); + + // initialise the barometer object, loading backend drivers + void init(void); + + // update the barometer object, asking backends to push data to + // the frontend + void update(void); // healthy - returns true if sensor and derived altitude are good - bool healthy() const { return _flags.healthy && _flags.alt_ok; } - - virtual bool init()=0; - virtual uint8_t read() = 0; + bool healthy(void) const { return healthy(_primary); } + bool healthy(uint8_t instance) const { return sensors[instance].healthy && sensors[instance].alt_ok; } // pressure in Pascal. Divide by 100 for millibars or hectopascals - virtual float get_pressure() = 0; + float get_pressure(void) const { return get_pressure(_primary); } + float get_pressure(uint8_t instance) const { return sensors[instance].pressure; } // temperature in degrees C - virtual float get_temperature() const = 0; + float get_temperature(void) const { return get_temperature(_primary); } + float get_temperature(uint8_t instance) const { return sensors[instance].temperature; } - // accumulate a reading - overridden in some drivers - virtual void accumulate(void) {} + // accumulate a reading on sensors. Some backends without their + // own thread or a timer may need this. + void accumulate(void); // calibrate the barometer. This must be called on startup if the // altitude/climb_rate/acceleration interfaces are ever used - // the callback is a delay() like routine - void calibrate(); + void calibrate(void); // update the barometer calibration to the current pressure. Can // be used for incremental preflight update of baro - void update_calibration(); + void update_calibration(void); // get current altitude in meters relative to altitude at the time // of the last calibrate() call - float get_altitude(void); + float get_altitude(void) const { return get_altitude(_primary); } + float get_altitude(uint8_t instance) const { return sensors[instance].altitude; } // get altitude difference in meters relative given a base // pressure in Pascal float get_altitude_difference(float base_pressure, float pressure) const; // get scale factor required to convert equivalent to true airspeed - float get_EAS2TAS(void); - - // return how many pressure samples were used to obtain - // the last pressure reading - uint8_t get_pressure_samples(void) { - return _pressure_samples; - } + float get_EAS2TAS(void); // get current climb rate in meters/s. A positive number means // going up - float get_climb_rate(void); + float get_climb_rate(void); // ground temperature in degrees C // the ground values are only valid after calibration - float get_ground_temperature(void) { - return _ground_temperature.get(); - } + float get_ground_temperature(void) const { return get_ground_temperature(_primary); } + float get_ground_temperature(uint8_t i) const { return sensors[i].ground_temperature.get(); } // ground pressure in Pascal // the ground values are only valid after calibration - float get_ground_pressure(void) { - return _ground_pressure.get(); - } + float get_ground_pressure(void) const { return get_ground_pressure(_primary); } + float get_ground_pressure(uint8_t i) const { return sensors[i].ground_pressure.get(); } // set the temperature to be used for altitude calibration. This // allows an external temperature source (such as a digital @@ -89,41 +96,61 @@ public: void set_external_temperature(float temperature); // get last time sample was taken (in ms) - uint32_t get_last_update() const { return _last_update; }; + uint32_t get_last_update(void) const { return get_last_update(_primary); } + uint32_t get_last_update(uint8_t instance) const { return sensors[_primary].last_update_ms; } - static const struct AP_Param::GroupInfo var_info[]; + // settable parameters + static const struct AP_Param::GroupInfo var_info[]; -protected: + float get_calibration_temperature(void) const { return get_calibration_temperature(_primary); } + float get_calibration_temperature(uint8_t instance) const; - struct Baro_flags { - uint8_t healthy :1; // true if sensor is healthy - uint8_t alt_ok :1; // true if calculated altitude is ok - } _flags; + // HIL (and SITL) interface, setting altitude + void setHIL(float altitude_msl); - uint32_t _last_update; // in ms - uint8_t _pressure_samples; + // HIL (and SITL) interface, setting pressure and temperature + void setHIL(uint8_t instance, float pressure, float temperature); + + // register a new sensor, claiming a sensor slot. If we are out of + // slots it will panic + uint8_t register_sensor(void); private: - // get the temperature to be used for altitude calibration - float get_calibration_temperature(void) const; + // how many drivers do we have? + uint8_t _num_drivers; + AP_Baro_Backend *drivers[BARO_MAX_DRIVERS]; + // how many sensors do we have? + uint8_t _num_sensors; + + // what is the primary sensor at the moment? + uint8_t _primary; + + struct sensor { + uint32_t last_update_ms; // last update time in ms + bool healthy:1; // true if sensor is healthy + bool alt_ok:1; // true if calculated altitude is ok + float pressure; // pressure in Pascal + float temperature; // temperature in degrees C + float altitude; // calculated altitude + AP_Float ground_temperature; + AP_Float ground_pressure; + } sensors[BARO_MAX_INSTANCES]; - AP_Float _ground_temperature; - AP_Float _ground_pressure; AP_Int8 _alt_offset; - float _altitude; float _last_altitude_EAS2TAS; float _EAS2TAS; float _external_temperature; - uint32_t _last_altitude_t; uint32_t _last_external_temperature_ms; DerivativeFilterFloat_Size7 _climb_rate_filter; + + void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta); }; +#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" -#include "AP_Baro_VRBRAIN.h" #endif // __AP_BARO_H__ diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 569375c9d9..6efbf79a9e 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -1,4 +1,5 @@ /// -*- 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 @@ -169,7 +170,6 @@ uint8_t AP_Baro_BMP085::read() Temp = 0.1f * _temp_sum / _count; Press = _press_sum / _count; - _pressure_samples = _count; _count = 0; _temp_sum = 0; _press_sum = 0; @@ -297,3 +297,4 @@ void AP_Baro_BMP085::Calculate() } } +#endif diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index bc53ddab8e..85ee991b45 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -25,7 +25,6 @@ public: ac4(0), ac5(0), ac6(0), _retry_time(0) { - _pressure_samples = 1; }; // Constructor diff --git a/libraries/AP_Baro/AP_Baro_Backend.cpp b/libraries/AP_Baro/AP_Baro_Backend.cpp new file mode 100644 index 0000000000..13f32c9bbe --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_Backend.cpp @@ -0,0 +1,23 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +extern const AP_HAL::HAL& hal; + +// constructor +AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) : + _frontend(baro) +{} + +/* + copy latest data to the frontend from a backend + */ +void AP_Baro_Backend::_copy_to_frontend(uint8_t instance, float pressure, float temperature) +{ + if (instance >= _frontend._num_sensors) { + return; + } + _frontend.sensors[instance].pressure = pressure; + _frontend.sensors[instance].temperature = temperature; + _frontend.sensors[instance].last_update_ms = hal.scheduler->millis(); +} diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h new file mode 100644 index 0000000000..b96f850541 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -0,0 +1,29 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_BARO_BACKEND_H__ +#define __AP_BARO_BACKEND_H__ + +#include "AP_Baro.h" + +class AP_Baro_Backend +{ +public: + AP_Baro_Backend(AP_Baro &baro); + + // each driver must provide an update method to copy accumulated + // data to the frontend + virtual void update() = 0; + + // accumulate function. This is used for backends that don't use a + // timer, and need to be called regularly by the main code to + // trigger them to read the sensor + virtual void accumulate(void) {} + +protected: + // reference to frontend object + AP_Baro &_frontend; + + void _copy_to_frontend(uint8_t instance, float pressure, float temperature); +}; + +#endif // __AP_BARO_BACKEND_H__ diff --git a/libraries/AP_Baro/AP_Baro_HIL.cpp b/libraries/AP_Baro/AP_Baro_HIL.cpp index c9e2773035..24f95bbe46 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.cpp +++ b/libraries/AP_Baro/AP_Baro_HIL.cpp @@ -1,84 +1,51 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include -#include "AP_Baro_HIL.h" #include extern const AP_HAL::HAL& hal; -// Public Methods ////////////////////////////////////////////////////////////// -bool AP_Baro_HIL::init() +AP_Baro_HIL::AP_Baro_HIL(AP_Baro &baro) : + AP_Baro_Backend(baro) { - _flags.healthy = false; - return true; + _instance = _frontend.register_sensor(); } -// Read the sensor. This is a state machine -// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5) -uint8_t AP_Baro_HIL::read() -{ - uint8_t result = 0; - - if (_count != 0) { - hal.scheduler->suspend_timer_procs(); - result = 1; - Press = _pressure_sum / _count; - Temp = _temperature_sum / _count; - _pressure_samples = _count; - _count = 0; - _pressure_sum = 0; - _temperature_sum = 0; - hal.scheduler->resume_timer_procs(); - } - - return result; -} - -void AP_Baro_HIL::setHIL(float pressure, float temperature) -{ - if (pressure > 0) { - _count = 1; - _pressure_sum = pressure; - _temperature_sum = temperature; - _last_update = hal.scheduler->millis(); - _flags.healthy = true; - } -} - - // ========================================================================== // based on tables.cpp from http://www.pdas.com/atmosdownload.html /* - Compute the temperature, density, and pressure in the standard atmosphere - Correct to 20 km. Only approximate thereafter. + Compute the temperature, density, and pressure in the standard atmosphere + Correct to 20 km. Only approximate thereafter. */ -static void SimpleAtmosphere( +void AP_Baro::SimpleAtmosphere( const float alt, // geometric altitude, km. float& sigma, // density/sea-level standard density float& delta, // pressure/sea-level standard pressure float& theta) // temperature/sea-level standard temperature { - const float REARTH = 6369.0f; // radius of the Earth (km) - const float GMR = 34.163195f; // gas constant - float h=alt*REARTH/(alt+REARTH); // geometric to geopotential altitude + const float REARTH = 6369.0f; // radius of the Earth (km) + const float GMR = 34.163195f; // gas constant + float h=alt*REARTH/(alt+REARTH); // geometric to geopotential altitude - if (h<11.0f) - { // Troposphere - theta=(288.15f-6.5f*h)/288.15f; - delta=powf(theta, GMR/6.5f); - } - else - { // Stratosphere - theta=216.65f/288.15f; - delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f); + if (h < 11.0f) { + // Troposphere + theta=(288.15f-6.5f*h)/288.15f; + delta=powf(theta, GMR/6.5f); + } else { + // Stratosphere + theta=216.65f/288.15f; + delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f); } - sigma=delta/theta; + sigma = delta/theta; } -void AP_Baro_HIL::setHIL(float altitude_msl) +/* + convert an altitude in meters above sea level to a presssure and temperature + */ +void AP_Baro::setHIL(float altitude_msl) { float sigma, delta, theta; const float p0 = 101325; @@ -87,13 +54,19 @@ void AP_Baro_HIL::setHIL(float altitude_msl) float p = p0 * delta; float T = 303.16f * theta - 273.16f; // Assume 30 degrees at sea level - converted to degrees Kelvin - setHIL(p, T); + setHIL(0, p, T); } -float AP_Baro_HIL::get_pressure() { - return Press; -} - -float AP_Baro_HIL::get_temperature() const { - return Temp; +/* + set HIL pressure and temperature for an instance + */ +void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature) +{ + if (instance >= _num_sensors) { + // invalid + return; + } + sensors[instance].pressure = pressure; + sensors[instance].temperature = temperature; + sensors[instance].last_update_ms = hal.scheduler->millis(); } diff --git a/libraries/AP_Baro/AP_Baro_HIL.h b/libraries/AP_Baro/AP_Baro_HIL.h index 9853bf2211..b88b9d1846 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.h +++ b/libraries/AP_Baro/AP_Baro_HIL.h @@ -1,26 +1,22 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + dummy backend for HIL (and SITL). This doesn't actually need to do + any work, as setHIL() is in the frontend + */ -#ifndef __AP_BARO__HIL_H__ -#define __AP_BARO__HIL_H__ +#ifndef __AP_BARO_HIL_H__ +#define __AP_BARO_HIL_H__ #include "AP_Baro.h" -class AP_Baro_HIL : public AP_Baro +class AP_Baro_HIL : public AP_Baro_Backend { -private: - float Temp; - float Press; - float _pressure_sum; - float _temperature_sum; - volatile uint8_t _count; - public: - bool init(); - uint8_t read(); - float get_pressure(); - float get_temperature() const; - void setHIL(float altitude_msl); - void setHIL(float pressure, float temperature); + AP_Baro_HIL(AP_Baro &baro); + void update() {} + +private: + uint8_t _instance; }; -#endif // __AP_BARO__HIL_H__ +#endif // __AP_BARO_HIL_H__ diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 9b180b0d72..b314e7ba6e 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -15,35 +15,13 @@ */ /* - * APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor - * Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com - * - * Sensor is conected to standard SPI port - * Chip Select pin: Analog2 (provisional until Jordi defines the pin)!! - * - * Variables: - * Temp : Calculated temperature (in Celsius degrees) - * Press : Calculated pressure (in mbar units * 100) - * - * - * Methods: - * init() : Initialization and sensor reset - * read() : Read sensor data and _calculate Temperature, Pressure - * This function is optimized so the main host don´t need to wait - * You can call this function in your main loop - * Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC - * It returns a 1 if there are new data. - * get_pressure() : return pressure in mbar*100 units - * get_temperature() : return temperature in celsius degrees*100 units - * - * Internal functions: - * _calculate() : Calculate Temperature and Pressure (temperature compensated) in real units - * - * - */ + originally written by Jose Julio, Pat Hickey and Jordi Muñoz + + Heavily modified by Andrew Tridgell +*/ #include -#include "AP_Baro_MS5611.h" +#include "AP_Baro.h" extern const AP_HAL::HAL& hal; @@ -59,187 +37,171 @@ extern const AP_HAL::HAL& hal; #define CMD_CONVERT_D1_OSR4096 0x48 // Maximum resolution (oversampling) #define CMD_CONVERT_D2_OSR4096 0x58 // Maximum resolution (oversampling) -uint32_t volatile AP_Baro_MS5611::_s_D1; -uint32_t volatile AP_Baro_MS5611::_s_D2; -uint8_t volatile AP_Baro_MS5611::_d1_count; -uint8_t volatile AP_Baro_MS5611::_d2_count; -uint8_t AP_Baro_MS5611::_state; -uint32_t AP_Baro_MS5611::_timer; -bool volatile AP_Baro_MS5611::_updated; - -AP_Baro_MS5611_Serial* AP_Baro_MS5611::_serial = NULL; -AP_Baro_MS5611_SPI AP_Baro_MS5611::spi; -#if MS5611_WITH_I2C -AP_Baro_MS5611_I2C AP_Baro_MS5611::i2c; -#endif - // SPI Device ////////////////////////////////////////////////////////////////// -void AP_Baro_MS5611_SPI::init() +AP_SerialBus_SPI::AP_SerialBus_SPI(enum AP_HAL::SPIDevice device, enum AP_HAL::SPIDeviceDriver::bus_speed speed) : + _device(device), + _speed(speed), + _spi(NULL), + _spi_sem(NULL) { - _spi = hal.spi->device(AP_HAL::SPIDevice_MS5611); +} + +void AP_SerialBus_SPI::init() +{ + _spi = hal.spi->device(_device); if (_spi == NULL) { - hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get " - "valid SPI device driver!")); - return; /* never reached */ + hal.scheduler->panic(PSTR("did not get valid SPI device driver!")); } _spi_sem = _spi->get_semaphore(); if (_spi_sem == NULL) { - hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get " - "valid SPI semaphroe!")); - return; /* never reached */ - + hal.scheduler->panic(PSTR("AP_SerialBus_SPI did not get valid SPI semaphroe!")); } - - // now that we have initialised, we set the SPI bus speed to high - // (8MHz on APM2) - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + _spi->set_bus_speed(_speed); } -uint16_t AP_Baro_MS5611_SPI::read_16bits(uint8_t reg) +uint16_t AP_SerialBus_SPI::read_16bits(uint8_t reg) { - uint8_t tx[3]; + uint8_t tx[3] = { reg, 0, 0 }; uint8_t rx[3]; - tx[0] = reg; tx[1] = 0; tx[2] = 0; _spi->transaction(tx, rx, 3); return ((uint16_t) rx[1] << 8 ) | ( rx[2] ); } -uint32_t AP_Baro_MS5611_SPI::read_adc() +uint32_t AP_SerialBus_SPI::read_24bits(uint8_t reg) { - uint8_t tx[4]; + uint8_t tx[4] = { reg, 0, 0, 0 }; uint8_t rx[4]; - memset(tx, 0, 4); /* first byte is addr = 0 */ _spi->transaction(tx, rx, 4); return (((uint32_t)rx[1])<<16) | (((uint32_t)rx[2])<<8) | ((uint32_t)rx[3]); } - -void AP_Baro_MS5611_SPI::write(uint8_t reg) +void AP_SerialBus_SPI::write(uint8_t reg) { - uint8_t tx[1]; - tx[0] = reg; + uint8_t tx[1] = { reg }; _spi->transaction(tx, NULL, 1); } -bool AP_Baro_MS5611_SPI::sem_take_blocking() { +bool AP_SerialBus_SPI::sem_take_blocking() +{ return _spi_sem->take(10); } -bool AP_Baro_MS5611_SPI::sem_take_nonblocking() +bool AP_SerialBus_SPI::sem_take_nonblocking() { - /** - * Take nonblocking from a TimerProcess context & - * monitor for bad failures - */ - static int semfail_ctr = 0; - bool got = _spi_sem->take_nonblocking(); - if (!got) { - if (!hal.scheduler->system_initializing()) { - semfail_ctr++; - if (semfail_ctr > 100) { - hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem " - "100 times in a row, in " - "AP_Baro_MS5611::_update")); - } - } - return false; /* never reached */ - } else { - semfail_ctr = 0; - } - return got; + return _spi_sem->take_nonblocking(); } -void AP_Baro_MS5611_SPI::sem_give() +void AP_SerialBus_SPI::sem_give() { _spi_sem->give(); } -// I2C Device ////////////////////////////////////////////////////////////////// -#if MS5611_WITH_I2C -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO -#define MS5611_ADDR 0x77 -#else -#define MS5611_ADDR 0x76 /** I2C address of the MS5611 on the PX4 board. */ -#endif +/// I2C SerialBus +AP_SerialBus_I2C::AP_SerialBus_I2C(uint8_t addr) : + _addr(addr), + _i2c_sem(NULL) +{ +} -void AP_Baro_MS5611_I2C::init() +void AP_SerialBus_I2C::init() { _i2c_sem = hal.i2c->get_semaphore(); if (_i2c_sem == NULL) { - hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get " - "valid I2C semaphroe!")); - return; /* never reached */ + hal.scheduler->panic(PSTR("AP_SerialBus_I2C did not get valid I2C semaphroe!")); } } -uint16_t AP_Baro_MS5611_I2C::read_16bits(uint8_t reg) +uint16_t AP_SerialBus_I2C::read_16bits(uint8_t reg) { uint8_t buf[2]; - - if (hal.i2c->readRegisters(MS5611_ADDR, reg, sizeof(buf), buf) == 0) + if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) { return (((uint16_t)(buf[0]) << 8) | buf[1]); - + } return 0; } -uint32_t AP_Baro_MS5611_I2C::read_adc() +uint32_t AP_SerialBus_I2C::read_24bits(uint8_t reg) { uint8_t buf[3]; - - if (hal.i2c->readRegisters(MS5611_ADDR, 0x00, sizeof(buf), buf) == 0) + if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) { return (((uint32_t)buf[0]) << 16) | (((uint32_t)buf[1]) << 8) | buf[2]; - + } return 0; } -void AP_Baro_MS5611_I2C::write(uint8_t reg) +void AP_SerialBus_I2C::write(uint8_t reg) { - hal.i2c->write(MS5611_ADDR, 1, ®); + hal.i2c->write(_addr, 1, ®); } -bool AP_Baro_MS5611_I2C::sem_take_blocking() { +bool AP_SerialBus_I2C::sem_take_blocking() +{ return _i2c_sem->take(10); } -bool AP_Baro_MS5611_I2C::sem_take_nonblocking() +bool AP_SerialBus_I2C::sem_take_nonblocking() { - /** - * Take nonblocking from a TimerProcess context & - * monitor for bad failures - */ - static int semfail_ctr = 0; - bool got = _i2c_sem->take_nonblocking(); - if (!got) { - if (!hal.scheduler->system_initializing()) { - semfail_ctr++; - if (semfail_ctr > 100) { - hal.scheduler->panic(PSTR("PANIC: failed to take _i2c_sem " - "100 times in a row, in " - "AP_Baro_MS5611::_update")); - } - } - return false; /* never reached */ - } else { - semfail_ctr = 0; - } - return got; + return _i2c_sem->take_nonblocking(); } -void AP_Baro_MS5611_I2C::sem_give() +void AP_SerialBus_I2C::sem_give() { _i2c_sem->give(); } -#endif // MS5611_WITH_I2C -// Public Methods ////////////////////////////////////////////////////////////// +/* + constructor + */ +AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial) : + AP_Baro_Backend(baro), + _serial(serial), + _updated(false), + _state(0), + _last_timer(0) +{ + _instance = _frontend.register_sensor(); + _serial->init(); + if (!_serial->sem_take_blocking()){ + hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: failed to take serial semaphore for init")); + } + + _serial->write(CMD_MS5611_RESET); + hal.scheduler->delay(4); + + // We read the factory calibration + // The on-chip CRC is not used + C1 = _serial->read_16bits(CMD_MS5611_PROM_C1); + C2 = _serial->read_16bits(CMD_MS5611_PROM_C2); + C3 = _serial->read_16bits(CMD_MS5611_PROM_C3); + C4 = _serial->read_16bits(CMD_MS5611_PROM_C4); + C5 = _serial->read_16bits(CMD_MS5611_PROM_C5); + C6 = _serial->read_16bits(CMD_MS5611_PROM_C6); + + if (!_check_crc()) { + hal.scheduler->panic(PSTR("Bad CRC on MS5611")); + } + + // Send a command to read Temp first + _serial->write(CMD_CONVERT_D2_OSR4096); + _last_timer = hal.scheduler->micros(); + _state = 0; + + _s_D1 = 0; + _s_D2 = 0; + _d1_count = 0; + _d2_count = 0; + + _serial->sem_give(); + + hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_timer)); +} -#if CONFIG_HAL_BOARD != HAL_BOARD_APM2 /** * MS5611 crc4 method based on PX4Firmware code */ -bool AP_Baro_MS5611::check_crc(void) +bool AP_Baro_MS5611::_check_crc(void) { int16_t cnt; uint16_t n_rem; @@ -282,83 +244,17 @@ bool AP_Baro_MS5611::check_crc(void) /* return true if CRCs match */ return (0x000F & crc_read) == (n_rem ^ 0x00); } -#endif - -// SPI should be initialized externally -bool AP_Baro_MS5611::init() -{ - hal.scheduler->suspend_timer_procs(); - if (_serial == NULL) { - hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: NULL serial driver")); - return false; /* never reached */ - } - - _serial->init(); - if (!_serial->sem_take_blocking()){ - hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: failed to take " - "serial semaphore for init")); - return false; /* never reached */ - } - - _serial->write(CMD_MS5611_RESET); - hal.scheduler->delay(4); - - // We read the factory calibration - // The on-chip CRC is not used - C1 = _serial->read_16bits(CMD_MS5611_PROM_C1); - C2 = _serial->read_16bits(CMD_MS5611_PROM_C2); - C3 = _serial->read_16bits(CMD_MS5611_PROM_C3); - C4 = _serial->read_16bits(CMD_MS5611_PROM_C4); - C5 = _serial->read_16bits(CMD_MS5611_PROM_C5); - C6 = _serial->read_16bits(CMD_MS5611_PROM_C6); - - // if not on APM2 then check CRC -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 - if (!check_crc()) { - hal.scheduler->panic("Bad CRC on MS5611"); - } -#endif - - //Send a command to read Temp first - _serial->write(CMD_CONVERT_D2_OSR4096); - _timer = hal.scheduler->micros(); - _state = 0; - Temp=0; - Press=0; - - _s_D1 = 0; - _s_D2 = 0; - _d1_count = 0; - _d2_count = 0; - - hal.scheduler->resume_timer_procs(); - hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_update)); - _serial->sem_give(); - - // wait for at least one value to be read - uint32_t tstart = hal.scheduler->millis(); - while (!_updated) { - hal.scheduler->delay(10); - if (hal.scheduler->millis() - tstart > 1000) { - hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 took more than " - "1000ms to initialize")); - _flags.healthy = false; - return false; - } - } - - _flags.healthy = true; - return true; -} -// Read the sensor. This is a state machine -// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5) -// temperature does not change so quickly... -void AP_Baro_MS5611::_update(void) +/* + Read the sensor. This is a state machine + We read one time Temperature (state=1) and then 4 times Pressure (states 2-5) + temperature does not change so quickly... +*/ +void AP_Baro_MS5611::_timer(void) { // Throttle read rate to 100hz maximum. - if (hal.scheduler->micros() - _timer < 10000) { + if (hal.scheduler->micros() - _last_timer < 10000) { return; } @@ -368,7 +264,7 @@ void AP_Baro_MS5611::_update(void) if (_state == 0) { // On state 0 we read temp - uint32_t d2 = _serial->read_adc(); + uint32_t d2 = _serial->read_24bits(0); if (d2 != 0) { _s_D2 += d2; _d2_count++; @@ -383,7 +279,7 @@ void AP_Baro_MS5611::_update(void) _state++; _serial->write(CMD_CONVERT_D1_OSR4096); // Command to read pressure } else { - uint32_t d1 = _serial->read_adc();; + uint32_t d1 = _serial->read_24bits(0);; if (d1 != 0) { // occasional zero values have been seen on the PXF // board. These may be SPI errors, but safest to ignore @@ -408,42 +304,35 @@ void AP_Baro_MS5611::_update(void) } } - _timer = hal.scheduler->micros(); + _last_timer = hal.scheduler->micros(); _serial->sem_give(); } -uint8_t AP_Baro_MS5611::read() +void AP_Baro_MS5611::update() { - bool updated = _updated; - if (updated) { - uint32_t sD1, sD2; - uint8_t d1count, d2count; + if (!_updated) { + return; + } + uint32_t sD1, sD2; + uint8_t d1count, d2count; - // Suspend timer procs because these variables are written to - // in "_update". - hal.scheduler->suspend_timer_procs(); - sD1 = _s_D1; _s_D1 = 0; - sD2 = _s_D2; _s_D2 = 0; - d1count = _d1_count; _d1_count = 0; - d2count = _d2_count; _d2_count = 0; - _updated = false; - hal.scheduler->resume_timer_procs(); - - if (d1count != 0) { - D1 = ((float)sD1) / d1count; - } - if (d2count != 0) { - D2 = ((float)sD2) / d2count; - } - _pressure_samples = d1count; - _raw_press = D1; - _raw_temp = D2; + // Suspend timer procs because these variables are written to + // in "_update". + hal.scheduler->suspend_timer_procs(); + sD1 = _s_D1; _s_D1 = 0; + sD2 = _s_D2; _s_D2 = 0; + d1count = _d1_count; _d1_count = 0; + d2count = _d2_count; _d2_count = 0; + _updated = false; + hal.scheduler->resume_timer_procs(); + + if (d1count != 0) { + D1 = ((float)sD1) / d1count; + } + if (d2count != 0) { + D2 = ((float)sD2) / d2count; } _calculate(); - if (updated) { - _last_update = hal.scheduler->millis(); - } - return updated ? 1 : 0; } // Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100). @@ -453,7 +342,6 @@ void AP_Baro_MS5611::_calculate() float TEMP; float OFF; float SENS; - float P; // Formulas from manufacturer datasheet // sub -20c temperature compensation is not included @@ -478,18 +366,7 @@ void AP_Baro_MS5611::_calculate() SENS = SENS - SENS2; } - P = (D1*SENS/2097152 - OFF)/32768; - Temp = (TEMP + 2000) * 0.01f; - Press = P; -} - -float AP_Baro_MS5611::get_pressure() -{ - return Press; -} - -float AP_Baro_MS5611::get_temperature() const -{ - // temperature in degrees C units - return Temp; + float pressure = (D1*SENS/2097152 - OFF)/32768; + float temperature = (TEMP + 2000) * 0.01f; + _copy_to_frontend(_instance, pressure, temperature); } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 959dcb2ba7..abebe2d519 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -6,15 +6,10 @@ #include #include "AP_Baro.h" -#if CONFIG_HAL_BOARD != HAL_BOARD_APM2 && CONFIG_HAL_BOARD != HAL_BOARD_APM1 -#define MS5611_WITH_I2C 1 -#else -#define MS5611_WITH_I2C 0 -#endif +#define MS5611_I2C_ADDR 0x77 - -/** Abstract serial device driver for MS5611. */ -class AP_Baro_MS5611_Serial +/** Abstract serial bus device driver for I2C/SPI. */ +class AP_SerialBus { public: /** Initialize the driver. */ @@ -23,107 +18,87 @@ public: /** Read a 16-bit value from register "reg". */ virtual uint16_t read_16bits(uint8_t reg) = 0; - /** Read a 24-bit value from the ADC. */ - virtual uint32_t read_adc() = 0; + /** Read a 24-bit value */ + virtual uint32_t read_24bits(uint8_t reg) = 0; - /** Write a single byte command. */ + /** Write to a register with no data. */ virtual void write(uint8_t reg) = 0; /** Acquire the internal semaphore for this device. * take_nonblocking should be used from the timer process, * take_blocking from synchronous code (i.e. init) */ - virtual bool sem_take_nonblocking() { return true; } - virtual bool sem_take_blocking() { return true; } + virtual bool sem_take_nonblocking() = 0; + virtual bool sem_take_blocking() = 0; /** Release the internal semaphore for this device. */ - virtual void sem_give() {} + virtual void sem_give() = 0; }; /** SPI serial device. */ -class AP_Baro_MS5611_SPI : public AP_Baro_MS5611_Serial +class AP_SerialBus_SPI : public AP_SerialBus { public: - virtual void init(); - virtual uint16_t read_16bits(uint8_t reg); - virtual uint32_t read_adc(); - virtual void write(uint8_t reg); - virtual bool sem_take_nonblocking(); - virtual bool sem_take_blocking(); - virtual void sem_give(); + AP_SerialBus_SPI(enum AP_HAL::SPIDevice device, enum AP_HAL::SPIDeviceDriver::bus_speed speed); + void init(); + uint16_t read_16bits(uint8_t reg); + uint32_t read_24bits(uint8_t reg); + uint32_t read_adc(uint8_t reg); + void write(uint8_t reg); + bool sem_take_nonblocking(); + bool sem_take_blocking(); + void sem_give(); private: + enum AP_HAL::SPIDeviceDriver::bus_speed _speed; + enum AP_HAL::SPIDevice _device; AP_HAL::SPIDeviceDriver *_spi; AP_HAL::Semaphore *_spi_sem; }; -#if MS5611_WITH_I2C /** I2C serial device. */ -class AP_Baro_MS5611_I2C : public AP_Baro_MS5611_Serial +class AP_SerialBus_I2C : public AP_SerialBus { public: - virtual void init(); - virtual uint16_t read_16bits(uint8_t reg); - virtual uint32_t read_adc(); - virtual void write(uint8_t reg); - virtual bool sem_take_nonblocking(); - virtual bool sem_take_blocking(); - virtual void sem_give(); + AP_SerialBus_I2C(uint8_t addr); + void init(); + uint16_t read_16bits(uint8_t reg); + uint32_t read_24bits(uint8_t reg); + void write(uint8_t reg); + bool sem_take_nonblocking(); + bool sem_take_blocking(); + void sem_give(); private: + uint8_t _addr; AP_HAL::Semaphore *_i2c_sem; }; -#endif // MS5611_WITH_I2C -class AP_Baro_MS5611 : public AP_Baro +class AP_Baro_MS5611 : public AP_Baro_Backend { public: - AP_Baro_MS5611(AP_Baro_MS5611_Serial *serial) - { - _serial = serial; - } - - /* AP_Baro public interface: */ - bool init(); - uint8_t read(); - float get_pressure(); // in mbar*100 units - float get_temperature() const; // in celsius degrees - - - /* Serial port drivers to pass to "init". */ - static AP_Baro_MS5611_SPI spi; -#if MS5611_WITH_I2C - static AP_Baro_MS5611_I2C i2c; -#endif + AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial); + void update(); private: - void _calculate(); - /* Asynchronous handler functions: */ - void _update(); + AP_SerialBus *_serial; + uint8_t _instance; -#if CONFIG_HAL_BOARD != HAL_BOARD_APM2 - bool check_crc(void); -#endif + void _calculate(); + bool _check_crc(); + + void _timer(); /* Asynchronous state: */ - static volatile bool _updated; - static volatile uint8_t _d1_count; - static volatile uint8_t _d2_count; - static volatile uint32_t _s_D1, _s_D2; - static uint8_t _state; - static uint32_t _timer; - static AP_Baro_MS5611_Serial *_serial; - /* Gates access to asynchronous state: */ - static bool _sync_access; + volatile bool _updated; + volatile uint8_t _d1_count; + volatile uint8_t _d2_count; + volatile uint32_t _s_D1, _s_D2; + uint8_t _state; + uint32_t _last_timer; - float Temp; - float Press; - - int32_t _raw_press; - int32_t _raw_temp; // Internal calibration registers - uint16_t C1,C2,C3,C4,C5,C6; - float D1,D2; - + uint16_t C1,C2,C3,C4,C5,C6; + float D1,D2; }; #endif // __AP_BARO_MS5611_H__ diff --git a/libraries/AP_Baro/AP_Baro_PX4.cpp b/libraries/AP_Baro/AP_Baro_PX4.cpp index 24d046215c..8847c93f0c 100644 --- a/libraries/AP_Baro/AP_Baro_PX4.cpp +++ b/libraries/AP_Baro/AP_Baro_PX4.cpp @@ -2,7 +2,7 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include #include "AP_Baro_PX4.h" @@ -16,71 +16,62 @@ extern const AP_HAL::HAL& hal; -// Public Methods ////////////////////////////////////////////////////////////// -bool AP_Baro_PX4::init(void) +/* + constructor - opens the PX4 drivers + */ +AP_Baro_PX4::AP_Baro_PX4(AP_Baro &baro) : + AP_Baro_Backend(baro), + _num_instances(0) { - if (_baro_fd <= 0) { - _baro_fd = open(BARO_DEVICE_PATH, O_RDONLY); - if (_baro_fd < 0) { - hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH); + memset(instances, 0, sizeof(instances)); + instances[0].fd = open(BARO_DEVICE_PATH, O_RDONLY); + instances[1].fd = open(BARO_DEVICE_PATH"1", O_RDONLY); + + for (uint8_t i=0; i<2; i++) { + if (instances[i].fd != -1) { + _num_instances = i+1; + } else { + break; } - - /* set the driver to poll at 150Hz */ - ioctl(_baro_fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX); - - // average over up to 20 samples - ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 20); - - // give the timer a chance to run and gather one sample - hal.scheduler->delay(40); - _accumulate(); } - return true; + for (uint8_t i=0; i<_num_instances; i++) { + instances[i].instance = _frontend.register_sensor(); + + /* set the driver to poll at 150Hz */ + ioctl(instances[i].fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX); + + // average over up to 20 samples + ioctl(instances[i].fd, SENSORIOCSQUEUEDEPTH, 20); + } } // Read the sensor -uint8_t AP_Baro_PX4::read(void) +void AP_Baro_PX4::update(void) { - // try to accumulate one more sample, so we have the latest data - _accumulate(); - - // consider the baro healthy if we got a reading in the last 0.2s - _flags.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000); - if (!_flags.healthy || _sum_count == 0) { - return _flags.healthy; + for (uint8_t i=0; i<_num_instances; i++) { + struct baro_report baro_report; + struct px4_instance &instance = instances[i]; + while (::read(instance.fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) && + baro_report.timestamp != instance.last_timestamp) { + instance.pressure_sum += baro_report.pressure; // Pressure in mbar + instance.temperature_sum += baro_report.temperature; // degrees celcius + instance.sum_count++; + instance.last_timestamp = baro_report.timestamp; + } } - _pressure = (_pressure_sum / _sum_count) * 100.0f; - _temperature = _temperature_sum / _sum_count; - _pressure_samples = _sum_count; - _last_update = (uint32_t)_last_timestamp/1000; - _pressure_sum = 0; - _temperature_sum = 0; - _sum_count = 0; - - return 1; -} - -// accumulate sensor values -void AP_Baro_PX4::_accumulate(void) -{ - struct baro_report baro_report; - while (::read(_baro_fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) && - baro_report.timestamp != _last_timestamp) { - _pressure_sum += baro_report.pressure; // Pressure in mbar - _temperature_sum += baro_report.temperature; // degrees celcius - _sum_count++; - _last_timestamp = baro_report.timestamp; + for (uint8_t i=0; i<_num_instances; i++) { + struct px4_instance &instance = instances[i]; + if (instance.sum_count > 0) { + float pressure = (instance.pressure_sum / instance.sum_count) * 100; + float temperature = instance.temperature_sum / instance.sum_count; + instance.pressure_sum = 0; + instance.temperature_sum = 0; + instance.sum_count = 0; + _copy_to_frontend(instance.instance, pressure, temperature); + } } } -float AP_Baro_PX4::get_pressure() { - return _pressure; -} - -float AP_Baro_PX4::get_temperature() const { - return _temperature; -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Baro/AP_Baro_PX4.h b/libraries/AP_Baro/AP_Baro_PX4.h index c9f7a5b752..b7df09cb0d 100644 --- a/libraries/AP_Baro/AP_Baro_PX4.h +++ b/libraries/AP_Baro/AP_Baro_PX4.h @@ -5,25 +5,23 @@ #include "AP_Baro.h" -class AP_Baro_PX4 : public AP_Baro +class AP_Baro_PX4 : public AP_Baro_Backend { public: - bool init(); - uint8_t read(); - float get_pressure(); - float get_temperature() const; + AP_Baro_PX4(AP_Baro &); + void update(); private: - float _temperature; - float _pressure; - float _pressure_sum; - float _temperature_sum; - uint32_t _sum_count; - void _accumulate(void); - void _baro_timer(uint32_t now); - uint64_t _last_timestamp; - // baro driver handle - int _baro_fd; + uint8_t _num_instances; + + struct px4_instance { + uint8_t instance; + int fd; + float pressure_sum; + float temperature_sum; + uint32_t sum_count; + uint64_t last_timestamp; + } instances[BARO_MAX_INSTANCES]; }; #endif // __AP_BARO_PX4_H__ diff --git a/libraries/AP_Baro/AP_Baro_VRBRAIN.cpp b/libraries/AP_Baro/AP_Baro_VRBRAIN.cpp deleted file mode 100644 index ef27b63be7..0000000000 --- a/libraries/AP_Baro/AP_Baro_VRBRAIN.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#include -#include "AP_Baro_VRBRAIN.h" - -#include -#include -#include -#include - -#include -#include - -extern const AP_HAL::HAL& hal; - -// Public Methods ////////////////////////////////////////////////////////////// -bool AP_Baro_VRBRAIN::init(void) -{ - if (_baro_fd <= 0) { - _baro_fd = open(BARO_DEVICE_PATH, O_RDONLY); - if (_baro_fd < 0) { - hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH); - } - - /* set the driver to poll at 150Hz */ - ioctl(_baro_fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX); - - // average over up to 20 samples - ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 20); - - // give the timer a chance to run and gather one sample - hal.scheduler->delay(40); - _accumulate(); - } - - return true; -} - -// Read the sensor -uint8_t AP_Baro_VRBRAIN::read(void) -{ - // try to accumulate one more sample, so we have the latest data - _accumulate(); - - // consider the baro healthy if we got a reading in the last 0.2s - _flags.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000); - if (!_flags.healthy || _sum_count == 0) { - return _flags.healthy; - } - - _pressure = (_pressure_sum / _sum_count) * 100.0f; - _temperature = _temperature_sum / _sum_count; - _pressure_samples = _sum_count; - _last_update = (uint32_t)_last_timestamp/1000; - _pressure_sum = 0; - _temperature_sum = 0; - _sum_count = 0; - - return 1; -} - -// accumulate sensor values -void AP_Baro_VRBRAIN::_accumulate(void) -{ - struct baro_report baro_report; - while (::read(_baro_fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) && - baro_report.timestamp != _last_timestamp) { - _pressure_sum += baro_report.pressure; // Pressure in mbar - _temperature_sum += baro_report.temperature; // degrees celcius - _sum_count++; - _last_timestamp = baro_report.timestamp; - } -} - -float AP_Baro_VRBRAIN::get_pressure() { - return _pressure; -} - -float AP_Baro_VRBRAIN::get_temperature() const { - return _temperature; -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Baro/AP_Baro_VRBRAIN.h b/libraries/AP_Baro/AP_Baro_VRBRAIN.h deleted file mode 100644 index 794cddc87c..0000000000 --- a/libraries/AP_Baro/AP_Baro_VRBRAIN.h +++ /dev/null @@ -1,29 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_BARO_VRBRAIN_H__ -#define __AP_BARO_VRBRAIN_H__ - -#include "AP_Baro.h" - -class AP_Baro_VRBRAIN : public AP_Baro -{ -public: - bool init(); - uint8_t read(); - float get_pressure(); - float get_temperature() const; - -private: - float _temperature; - float _pressure; - float _pressure_sum; - float _temperature_sum; - uint32_t _sum_count; - void _accumulate(void); - void _baro_timer(uint32_t now); - uint64_t _last_timestamp; - // baro driver handle - int _baro_fd; -}; - -#endif // __AP_BARO_VRBRAIN_H__ diff --git a/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde b/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde index 6a9b2c9d8c..f6c08a4fd7 100644 --- a/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde +++ b/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde @@ -38,21 +38,7 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; #define CONFIG_BARO HAL_BARO_DEFAULT -#if CONFIG_BARO == HAL_BARO_BMP085 -static AP_Baro_BMP085 barometer; -#elif CONFIG_BARO == HAL_BARO_PX4 -static AP_Baro_PX4 barometer; -#elif CONFIG_BARO == HAL_BARO_VRBRAIN -static AP_Baro_VRBRAIN barometer; -#elif CONFIG_BARO == HAL_BARO_HIL -static AP_Baro_HIL barometer; -#elif CONFIG_BARO == HAL_BARO_MS5611 -static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c); -#elif CONFIG_BARO == HAL_BARO_MS5611_SPI -static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi); -#else - #error Unrecognized CONFIG_BARO setting -#endif +static AP_Baro barometer; static uint32_t timer; @@ -78,7 +64,7 @@ void loop() { if((hal.scheduler->micros() - timer) > 100000UL) { timer = hal.scheduler->micros(); - barometer.read(); + barometer.update(); uint32_t read_time = hal.scheduler->micros() - timer; float alt = barometer.get_altitude(); if (!barometer.healthy()) { @@ -91,10 +77,9 @@ void loop() hal.console->print(barometer.get_temperature()); hal.console->print(" Altitude:"); hal.console->print(alt); - hal.console->printf(" climb=%.2f t=%u samples=%u", - barometer.get_climb_rate(), - (unsigned)read_time, - (unsigned)barometer.get_pressure_samples()); + hal.console->printf(" climb=%.2f t=%u", + barometer.get_climb_rate(), + (unsigned)read_time); hal.console->println(); } } diff --git a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde index f41c8b3596..e8d681317f 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde +++ b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde @@ -75,9 +75,8 @@ void loop() tmp_float = pow(tmp_float, 0.190295); float alt = 44330.0 * (1.0 - tmp_float); hal.console->print(alt); - hal.console->printf(" t=%lu samples=%u", - read_time, - (unsigned)bmp085.get_pressure_samples()); + hal.console->printf(" t=%lu", + read_time); hal.console->println(); } }