aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: split into frontend/backend

Browse Source 
this allows for support of multiple sensors on a board
This commit is contained in:
Andrew Tridgell 2014-10-20 06:22:51 +11:00 committed by Andrew Tridgell
parent 15c5e153c8
commit 5bb57a31f7
16 changed files with 587 additions and 757 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

238
libraries/AP_Baro/AP_Baro.cpp
View File

@ -36,14 +36,14 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
// @Description: calibrated ground pressure in Pascals // @Description: calibrated ground pressure in Pascals
// @Units: pascals // @Units: pascals
// @Increment: 1 // @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 // @Param: TEMP
// @DisplayName: ground temperature // @DisplayName: ground temperature
// @Description: calibrated ground temperature in degrees Celsius // @Description: calibrated ground temperature in degrees Celsius
// @Units: degrees celsius // @Units: degrees celsius
// @Increment: 1 // @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 // @Param: ALT_OFFSET
// @DisplayName: altitude offset // @DisplayName: altitude offset
@ -56,85 +56,95 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
AP_GROUPEND 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 // calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used // the altitude() or climb_rate() interfaces can be used
void AP_Baro::calibrate() void AP_Baro::calibrate()
{ {
float ground_pressure = 0;
float ground_temperature = 0;
// reset the altitude offset when we calibrate. The altitude // reset the altitude offset when we calibrate. The altitude
// offset is supposed to be for within a flight // offset is supposed to be for within a flight
_alt_offset.set_and_save(0); _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 // let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second, // the MS5611 reads quite a long way off for the first second,
// leading to about 1m of error if we don't wait // leading to about 1m of error if we don't wait
for (uint8_t i = 0; i < 10; i++) { for (uint8_t i = 0; i < 10; i++) {
uint32_t tstart = hal.scheduler->millis(); uint32_t tstart = hal.scheduler->millis();
do { do {
read(); update();
if (hal.scheduler->millis() - tstart > 500) { if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful " hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [2]\r\n")); "for more than 500ms in AP_Baro::calibrate [2]\r\n"));
} }
} while (!_flags.healthy); } while (!healthy());
ground_pressure = get_pressure();
ground_temperature = get_calibration_temperature();
hal.scheduler->delay(100); hal.scheduler->delay(100);
} }
// now average over 5 values for the ground pressure and // now average over 5 values for the ground pressure and
// temperature settings // 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(); uint32_t tstart = hal.scheduler->millis();
bool all_healthy = false;
do { do {
read(); update();
if (hal.scheduler->millis() - tstart > 500) { if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful " hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [3]\r\n")); "for more than 500ms in AP_Baro::calibrate [3]\r\n"));
} }
} while (!_flags.healthy); all_healthy = true;
ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f); for (uint8_t i=0; i<_num_sensors; i++) {
ground_temperature = (ground_temperature * 0.8f) + if (!healthy(i)) {
(get_calibration_temperature() * 0.2f); 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); hal.scheduler->delay(100);
} }
for (uint8_t i=0; i<_num_sensors; i++) {
_ground_pressure.set_and_save(ground_pressure); sensors[i].ground_pressure.set_and_save(sum_pressure[i] / num_samples);
_ground_temperature.set_and_save(ground_temperature); sensors[i].ground_temperature.set_and_save(sum_temperature[i] / num_samples);
}
} }
/** /*
update the barometer calibration update the barometer calibration
this updates the baro ground calibration to the current values. It this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated can be used before arming to keep the baro well calibrated
*/ */
void AP_Baro::update_calibration() void AP_Baro::update_calibration()
{ {
float pressure = get_pressure(); for (uint8_t i=0; i<_num_sensors; i++) {
_ground_pressure.set(pressure); if (healthy(i)) {
float last_temperature = _ground_temperature; sensors[i].ground_pressure.set(get_pressure(i));
_ground_temperature.set(get_calibration_temperature()); }
if (fabsf(last_temperature - _ground_temperature) > 3) { float last_temperature = sensors[i].ground_temperature;
// reset _EAS2TAS to force it to recalculate. This happens sensors[i].ground_temperature.set(get_calibration_temperature(i));
// when a digital airspeed sensor comes online if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
_EAS2TAS = 0; // 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 #if HAL_CPU_CLASS <= HAL_CPU_CLASS_16
// on slower CPUs use a less exact, but faster, calculation // on slower CPUs use a less exact, but faster, calculation
float scaling = base_pressure / pressure; float scaling = base_pressure / pressure;
float temp = _ground_temperature + 273.15f; float temp = get_calibration_temperature() + 273.15f;
ret = logf(scaling) * temp * 29.271267f; ret = logf(scaling) * temp * 29.271267f;
#else #else
// on faster CPUs use a more exact calculation // on faster CPUs use a more exact calculation
float scaling = pressure / base_pressure; 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 // This is an exact calculation that is within +-2.5m of the standard atmosphere tables
// in the troposphere (up to 11,000 m amsl). // 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 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 // return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL // valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate // assumes standard atmosphere lapse rate
float AP_Baro::get_EAS2TAS(void) 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 // not enough change to require re-calculating
return _EAS2TAS; 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))); _EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
_last_altitude_EAS2TAS = _altitude; _last_altitude_EAS2TAS = altitude;
return _EAS2TAS; 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 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 we have a recent external temperature then use it
if (_last_external_temperature_ms != 0 && hal.scheduler->millis() - _last_external_temperature_ms < 10000) { 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, // not just using the baro temperature is it tends to read high,
// often 30 degrees above the actual temperature. That means the // often 30 degrees above the actual temperature. That means the
// EAS2TAS tends to be off by quite a large margin // EAS2TAS tends to be off by quite a large margin
float ret = get_temperature(); float ret = get_temperature(instance);
if (ret > 25) { if (ret > 25) {
ret = 25; ret = 25;
} }
return ret; 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++;
}

143
libraries/AP_Baro/AP_Baro.h
View File

@ -3,85 +3,92 @@
#ifndef __AP_BARO_H__ #ifndef __AP_BARO_H__
#define __AP_BARO_H__ #define __AP_BARO_H__
#include <AP_HAL.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <Filter.h> #include <Filter.h>
#include <DerivativeFilter.h> #include <DerivativeFilter.h>
// 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 class AP_Baro
{ {
public: friend class AP_Baro_Backend;
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;
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 // healthy - returns true if sensor and derived altitude are good
bool healthy() const { return _flags.healthy && _flags.alt_ok; } bool healthy(void) const { return healthy(_primary); }
bool healthy(uint8_t instance) const { return sensors[instance].healthy && sensors[instance].alt_ok; }
virtual bool init()=0;
virtual uint8_t read() = 0;
// pressure in Pascal. Divide by 100 for millibars or hectopascals // 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 // 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 // accumulate a reading on sensors. Some backends without their
virtual void accumulate(void) {} // own thread or a timer may need this.
void accumulate(void);
// calibrate the barometer. This must be called on startup if the // calibrate the barometer. This must be called on startup if the
// altitude/climb_rate/acceleration interfaces are ever used // altitude/climb_rate/acceleration interfaces are ever used
// the callback is a delay() like routine void calibrate(void);
void calibrate();
// update the barometer calibration to the current pressure. Can // update the barometer calibration to the current pressure. Can
// be used for incremental preflight update of baro // 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 // get current altitude in meters relative to altitude at the time
// of the last calibrate() call // 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 // get altitude difference in meters relative given a base
// pressure in Pascal // pressure in Pascal
float get_altitude_difference(float base_pressure, float pressure) const; float get_altitude_difference(float base_pressure, float pressure) const;
// get scale factor required to convert equivalent to true airspeed // get scale factor required to convert equivalent to true airspeed
float get_EAS2TAS(void); 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;
}
// get current climb rate in meters/s. A positive number means // get current climb rate in meters/s. A positive number means
// going up // going up
float get_climb_rate(void); float get_climb_rate(void);
// ground temperature in degrees C // ground temperature in degrees C
// the ground values are only valid after calibration // the ground values are only valid after calibration
float get_ground_temperature(void) { float get_ground_temperature(void) const { return get_ground_temperature(_primary); }
return _ground_temperature.get(); float get_ground_temperature(uint8_t i) const { return sensors[i].ground_temperature.get(); }
}
// ground pressure in Pascal // ground pressure in Pascal
// the ground values are only valid after calibration // the ground values are only valid after calibration
float get_ground_pressure(void) { float get_ground_pressure(void) const { return get_ground_pressure(_primary); }
return _ground_pressure.get(); float get_ground_pressure(uint8_t i) const { return sensors[i].ground_pressure.get(); }
}
// set the temperature to be used for altitude calibration. This // set the temperature to be used for altitude calibration. This
// allows an external temperature source (such as a digital // allows an external temperature source (such as a digital
@ -89,41 +96,61 @@ public:
void set_external_temperature(float temperature); void set_external_temperature(float temperature);
// get last time sample was taken (in ms) // 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 { // HIL (and SITL) interface, setting altitude
uint8_t healthy :1; // true if sensor is healthy void setHIL(float altitude_msl);
uint8_t alt_ok :1; // true if calculated altitude is ok
} _flags;
uint32_t _last_update; // in ms // HIL (and SITL) interface, setting pressure and temperature
uint8_t _pressure_samples; 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: private:
// get the temperature to be used for altitude calibration // how many drivers do we have?
float get_calibration_temperature(void) const; 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; AP_Int8 _alt_offset;
float _altitude;
float _last_altitude_EAS2TAS; float _last_altitude_EAS2TAS;
float _EAS2TAS; float _EAS2TAS;
float _external_temperature; float _external_temperature;
uint32_t _last_altitude_t;
uint32_t _last_external_temperature_ms; uint32_t _last_external_temperature_ms;
DerivativeFilterFloat_Size7 _climb_rate_filter; 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_MS5611.h"
#include "AP_Baro_BMP085.h" //#include "AP_Baro_BMP085.h"
#include "AP_Baro_HIL.h" #include "AP_Baro_HIL.h"
#include "AP_Baro_PX4.h" #include "AP_Baro_PX4.h"
#include "AP_Baro_VRBRAIN.h"
#endif // __AP_BARO_H__ #endif // __AP_BARO_H__

3
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -1,4 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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 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 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; Temp = 0.1f * _temp_sum / _count;
Press = _press_sum / _count; Press = _press_sum / _count;
_pressure_samples = _count;
_count = 0; _count = 0;
_temp_sum = 0; _temp_sum = 0;
_press_sum = 0; _press_sum = 0;
@ -297,3 +297,4 @@ void AP_Baro_BMP085::Calculate()
} }
} }
#endif

1
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -25,7 +25,6 @@ public:
ac4(0), ac5(0), ac6(0), ac4(0), ac5(0), ac6(0),
_retry_time(0) _retry_time(0)
{ {
_pressure_samples = 1;
}; // Constructor }; // Constructor

23
libraries/AP_Baro/AP_Baro_Backend.cpp Normal file
View File

@ -0,0 +1,23 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Baro.h>
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();
}

29
libraries/AP_Baro/AP_Baro_Backend.h Normal file
View File

@ -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__

97
libraries/AP_Baro/AP_Baro_HIL.cpp
View File

@ -1,84 +1,51 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Baro.h> #include <AP_Baro.h>
#include "AP_Baro_HIL.h"
#include <AP_HAL.h> #include <AP_HAL.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// Public Methods ////////////////////////////////////////////////////////////// AP_Baro_HIL::AP_Baro_HIL(AP_Baro &baro) :
bool AP_Baro_HIL::init() AP_Baro_Backend(baro)
{ {
_flags.healthy = false; _instance = _frontend.register_sensor();
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)
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 // based on tables.cpp from http://www.pdas.com/atmosdownload.html
/* /*
Compute the temperature, density, and pressure in the standard atmosphere Compute the temperature, density, and pressure in the standard atmosphere
Correct to 20 km. Only approximate thereafter. Correct to 20 km. Only approximate thereafter.
*/ */
static void SimpleAtmosphere( void AP_Baro::SimpleAtmosphere(
const float alt, // geometric altitude, km. const float alt, // geometric altitude, km.
float& sigma, // density/sea-level standard density float& sigma, // density/sea-level standard density
float& delta, // pressure/sea-level standard pressure float& delta, // pressure/sea-level standard pressure
float& theta) // temperature/sea-level standard temperature float& theta) // temperature/sea-level standard temperature
{ {
const float REARTH = 6369.0f; // radius of the Earth (km) const float REARTH = 6369.0f; // radius of the Earth (km)
const float GMR = 34.163195f; // gas constant const float GMR = 34.163195f; // gas constant
float h=alt*REARTH/(alt+REARTH); // geometric to geopotential altitude float h=alt*REARTH/(alt+REARTH); // geometric to geopotential altitude
if (h<11.0f) if (h < 11.0f) {
{ // Troposphere // Troposphere
theta=(288.15f-6.5f*h)/288.15f; theta=(288.15f-6.5f*h)/288.15f;
delta=powf(theta, GMR/6.5f); delta=powf(theta, GMR/6.5f);
} } else {
else // Stratosphere
{ // Stratosphere theta=216.65f/288.15f;
theta=216.65f/288.15f; delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f);
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; float sigma, delta, theta;
const float p0 = 101325; const float p0 = 101325;
@ -87,13 +54,19 @@ void AP_Baro_HIL::setHIL(float altitude_msl)
float p = p0 * delta; float p = p0 * delta;
float T = 303.16f * theta - 273.16f; // Assume 30 degrees at sea level - converted to degrees Kelvin 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; set HIL pressure and temperature for an instance
} */
void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature)
float AP_Baro_HIL::get_temperature() const { {
return Temp; if (instance >= _num_sensors) {
// invalid
return;
}
sensors[instance].pressure = pressure;
sensors[instance].temperature = temperature;
sensors[instance].last_update_ms = hal.scheduler->millis();
} }

30
libraries/AP_Baro/AP_Baro_HIL.h
View File

@ -1,26 +1,22 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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__ #ifndef __AP_BARO_HIL_H__
#define __AP_BARO__HIL_H__ #define __AP_BARO_HIL_H__
#include "AP_Baro.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: public:
bool init(); AP_Baro_HIL(AP_Baro &baro);
uint8_t read(); void update() {}
float get_pressure();
float get_temperature() const; private:
void setHIL(float altitude_msl); uint8_t _instance;
void setHIL(float pressure, float temperature);
}; };
#endif // __AP_BARO__HIL_H__ #endif // __AP_BARO_HIL_H__

383
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -15,35 +15,13 @@
*/ */
/* /*
* APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor originally written by Jose Julio, Pat Hickey and Jordi Muñoz
* Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com
* Heavily modified by Andrew Tridgell
* 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
*
*
*/
#include <AP_HAL.h> #include <AP_HAL.h>
#include "AP_Baro_MS5611.h" #include "AP_Baro.h"
extern const AP_HAL::HAL& hal; 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_D1_OSR4096 0x48 // Maximum resolution (oversampling)
#define CMD_CONVERT_D2_OSR4096 0x58 // 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 ////////////////////////////////////////////////////////////////// // 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) { if (_spi == NULL) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get " hal.scheduler->panic(PSTR("did not get valid SPI device driver!"));
"valid SPI device driver!"));
return; /* never reached */
} }
_spi_sem = _spi->get_semaphore(); _spi_sem = _spi->get_semaphore();
if (_spi_sem == NULL) { if (_spi_sem == NULL) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get " hal.scheduler->panic(PSTR("AP_SerialBus_SPI did not get valid SPI semaphroe!"));
"valid SPI semaphroe!"));
return; /* never reached */
} }
_spi->set_bus_speed(_speed);
// 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);
} }
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]; uint8_t rx[3];
tx[0] = reg; tx[1] = 0; tx[2] = 0;
_spi->transaction(tx, rx, 3); _spi->transaction(tx, rx, 3);
return ((uint16_t) rx[1] << 8 ) | ( rx[2] ); 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]; uint8_t rx[4];
memset(tx, 0, 4); /* first byte is addr = 0 */
_spi->transaction(tx, rx, 4); _spi->transaction(tx, rx, 4);
return (((uint32_t)rx[1])<<16) | (((uint32_t)rx[2])<<8) | ((uint32_t)rx[3]); return (((uint32_t)rx[1])<<16) | (((uint32_t)rx[2])<<8) | ((uint32_t)rx[3]);
} }
void AP_SerialBus_SPI::write(uint8_t reg)
void AP_Baro_MS5611_SPI::write(uint8_t reg)
{ {
uint8_t tx[1]; uint8_t tx[1] = { reg };
tx[0] = reg;
_spi->transaction(tx, NULL, 1); _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); return _spi_sem->take(10);
} }
bool AP_Baro_MS5611_SPI::sem_take_nonblocking() bool AP_SerialBus_SPI::sem_take_nonblocking()
{ {
/** return _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;
} }
void AP_Baro_MS5611_SPI::sem_give() void AP_SerialBus_SPI::sem_give()
{ {
_spi_sem->give(); _spi_sem->give();
} }
// I2C Device //////////////////////////////////////////////////////////////////
#if MS5611_WITH_I2C
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO /// I2C SerialBus
#define MS5611_ADDR 0x77 AP_SerialBus_I2C::AP_SerialBus_I2C(uint8_t addr) :
#else _addr(addr),
#define MS5611_ADDR 0x76 /** I2C address of the MS5611 on the PX4 board. */ _i2c_sem(NULL)
#endif {
}
void AP_Baro_MS5611_I2C::init() void AP_SerialBus_I2C::init()
{ {
_i2c_sem = hal.i2c->get_semaphore(); _i2c_sem = hal.i2c->get_semaphore();
if (_i2c_sem == NULL) { if (_i2c_sem == NULL) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get " hal.scheduler->panic(PSTR("AP_SerialBus_I2C did not get valid I2C semaphroe!"));
"valid I2C semaphroe!"));
return; /* never reached */
} }
} }
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]; uint8_t buf[2];
if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
if (hal.i2c->readRegisters(MS5611_ADDR, reg, sizeof(buf), buf) == 0)
return (((uint16_t)(buf[0]) << 8) | buf[1]); return (((uint16_t)(buf[0]) << 8) | buf[1]);
}
return 0; return 0;
} }
uint32_t AP_Baro_MS5611_I2C::read_adc() uint32_t AP_SerialBus_I2C::read_24bits(uint8_t reg)
{ {
uint8_t buf[3]; uint8_t buf[3];
if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
if (hal.i2c->readRegisters(MS5611_ADDR, 0x00, sizeof(buf), buf) == 0)
return (((uint32_t)buf[0]) << 16) | (((uint32_t)buf[1]) << 8) | buf[2]; return (((uint32_t)buf[0]) << 16) | (((uint32_t)buf[1]) << 8) | buf[2];
}
return 0; 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, &reg); hal.i2c->write(_addr, 1, &reg);
} }
bool AP_Baro_MS5611_I2C::sem_take_blocking() { bool AP_SerialBus_I2C::sem_take_blocking()
{
return _i2c_sem->take(10); return _i2c_sem->take(10);
} }
bool AP_Baro_MS5611_I2C::sem_take_nonblocking() bool AP_SerialBus_I2C::sem_take_nonblocking()
{ {
/** return _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;
} }
void AP_Baro_MS5611_I2C::sem_give() void AP_SerialBus_I2C::sem_give()
{ {
_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 * MS5611 crc4 method based on PX4Firmware code
*/ */
bool AP_Baro_MS5611::check_crc(void) bool AP_Baro_MS5611::_check_crc(void)
{ {
int16_t cnt; int16_t cnt;
uint16_t n_rem; uint16_t n_rem;
@ -282,83 +244,17 @@ bool AP_Baro_MS5611::check_crc(void)
/* return true if CRCs match */ /* return true if CRCs match */
return (0x000F & crc_read) == (n_rem ^ 0x00); 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) Read the sensor. This is a state machine
// temperature does not change so quickly... We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
void AP_Baro_MS5611::_update(void) temperature does not change so quickly...
*/
void AP_Baro_MS5611::_timer(void)
{ {
// Throttle read rate to 100hz maximum. // Throttle read rate to 100hz maximum.
if (hal.scheduler->micros() - _timer < 10000) { if (hal.scheduler->micros() - _last_timer < 10000) {
return; return;
} }
@ -368,7 +264,7 @@ void AP_Baro_MS5611::_update(void)
if (_state == 0) { if (_state == 0) {
// On state 0 we read temp // On state 0 we read temp
uint32_t d2 = _serial->read_adc(); uint32_t d2 = _serial->read_24bits(0);
if (d2 != 0) { if (d2 != 0) {
_s_D2 += d2; _s_D2 += d2;
_d2_count++; _d2_count++;
@ -383,7 +279,7 @@ void AP_Baro_MS5611::_update(void)
_state++; _state++;
_serial->write(CMD_CONVERT_D1_OSR4096); // Command to read pressure _serial->write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
} else { } else {
uint32_t d1 = _serial->read_adc();; uint32_t d1 = _serial->read_24bits(0);;
if (d1 != 0) { if (d1 != 0) {
// occasional zero values have been seen on the PXF // occasional zero values have been seen on the PXF
// board. These may be SPI errors, but safest to ignore // 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(); _serial->sem_give();
} }
uint8_t AP_Baro_MS5611::read() void AP_Baro_MS5611::update()
{ {
bool updated = _updated; if (!_updated) {
if (updated) { return;
uint32_t sD1, sD2; }
uint8_t d1count, d2count; uint32_t sD1, sD2;
uint8_t d1count, d2count;
// Suspend timer procs because these variables are written to // Suspend timer procs because these variables are written to
// in "_update". // in "_update".
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
sD1 = _s_D1; _s_D1 = 0; sD1 = _s_D1; _s_D1 = 0;
sD2 = _s_D2; _s_D2 = 0; sD2 = _s_D2; _s_D2 = 0;
d1count = _d1_count; _d1_count = 0; d1count = _d1_count; _d1_count = 0;
d2count = _d2_count; _d2_count = 0; d2count = _d2_count; _d2_count = 0;
_updated = false; _updated = false;
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
if (d1count != 0) { if (d1count != 0) {
D1 = ((float)sD1) / d1count; D1 = ((float)sD1) / d1count;
} }
if (d2count != 0) { if (d2count != 0) {
D2 = ((float)sD2) / d2count; D2 = ((float)sD2) / d2count;
}
_pressure_samples = d1count;
_raw_press = D1;
_raw_temp = D2;
} }
_calculate(); _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). // 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 TEMP;
float OFF; float OFF;
float SENS; float SENS;
float P;
// Formulas from manufacturer datasheet // Formulas from manufacturer datasheet
// sub -20c temperature compensation is not included // sub -20c temperature compensation is not included
@ -478,18 +366,7 @@ void AP_Baro_MS5611::_calculate()
SENS = SENS - SENS2; SENS = SENS - SENS2;
} }
P = (D1*SENS/2097152 - OFF)/32768; float pressure = (D1*SENS/2097152 - OFF)/32768;
Temp = (TEMP + 2000) * 0.01f; float temperature = (TEMP + 2000) * 0.01f;
Press = P; _copy_to_frontend(_instance, pressure, temperature);
}
float AP_Baro_MS5611::get_pressure()
{
return Press;
}
float AP_Baro_MS5611::get_temperature() const
{
// temperature in degrees C units
return Temp;
} }

121
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -6,15 +6,10 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#include "AP_Baro.h" #include "AP_Baro.h"
#if CONFIG_HAL_BOARD != HAL_BOARD_APM2 && CONFIG_HAL_BOARD != HAL_BOARD_APM1 #define MS5611_I2C_ADDR 0x77
#define MS5611_WITH_I2C 1
#else
#define MS5611_WITH_I2C 0
#endif
/** Abstract serial bus device driver for I2C/SPI. */
/** Abstract serial device driver for MS5611. */ class AP_SerialBus
class AP_Baro_MS5611_Serial
{ {
public: public:
/** Initialize the driver. */ /** Initialize the driver. */
@ -23,107 +18,87 @@ public:
/** Read a 16-bit value from register "reg". */ /** Read a 16-bit value from register "reg". */
virtual uint16_t read_16bits(uint8_t reg) = 0; virtual uint16_t read_16bits(uint8_t reg) = 0;
/** Read a 24-bit value from the ADC. */ /** Read a 24-bit value */
virtual uint32_t read_adc() = 0; 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; virtual void write(uint8_t reg) = 0;
/** Acquire the internal semaphore for this device. /** Acquire the internal semaphore for this device.
* take_nonblocking should be used from the timer process, * take_nonblocking should be used from the timer process,
* take_blocking from synchronous code (i.e. init) */ * take_blocking from synchronous code (i.e. init) */
virtual bool sem_take_nonblocking() { return true; } virtual bool sem_take_nonblocking() = 0;
virtual bool sem_take_blocking() { return true; } virtual bool sem_take_blocking() = 0;
/** Release the internal semaphore for this device. */ /** Release the internal semaphore for this device. */
virtual void sem_give() {} virtual void sem_give() = 0;
}; };
/** SPI serial device. */ /** SPI serial device. */
class AP_Baro_MS5611_SPI : public AP_Baro_MS5611_Serial class AP_SerialBus_SPI : public AP_SerialBus
{ {
public: public:
virtual void init(); AP_SerialBus_SPI(enum AP_HAL::SPIDevice device, enum AP_HAL::SPIDeviceDriver::bus_speed speed);
virtual uint16_t read_16bits(uint8_t reg); void init();
virtual uint32_t read_adc(); uint16_t read_16bits(uint8_t reg);
virtual void write(uint8_t reg); uint32_t read_24bits(uint8_t reg);
virtual bool sem_take_nonblocking(); uint32_t read_adc(uint8_t reg);
virtual bool sem_take_blocking(); void write(uint8_t reg);
virtual void sem_give(); bool sem_take_nonblocking();
bool sem_take_blocking();
void sem_give();
private: private:
enum AP_HAL::SPIDeviceDriver::bus_speed _speed;
enum AP_HAL::SPIDevice _device;
AP_HAL::SPIDeviceDriver *_spi; AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem; AP_HAL::Semaphore *_spi_sem;
}; };
#if MS5611_WITH_I2C
/** I2C serial device. */ /** I2C serial device. */
class AP_Baro_MS5611_I2C : public AP_Baro_MS5611_Serial class AP_SerialBus_I2C : public AP_SerialBus
{ {
public: public:
virtual void init(); AP_SerialBus_I2C(uint8_t addr);
virtual uint16_t read_16bits(uint8_t reg); void init();
virtual uint32_t read_adc(); uint16_t read_16bits(uint8_t reg);
virtual void write(uint8_t reg); uint32_t read_24bits(uint8_t reg);
virtual bool sem_take_nonblocking(); void write(uint8_t reg);
virtual bool sem_take_blocking(); bool sem_take_nonblocking();
virtual void sem_give(); bool sem_take_blocking();
void sem_give();
private: private:
uint8_t _addr;
AP_HAL::Semaphore *_i2c_sem; 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: public:
AP_Baro_MS5611(AP_Baro_MS5611_Serial *serial) AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial);
{ void update();
_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
private: private:
void _calculate(); AP_SerialBus *_serial;
/* Asynchronous handler functions: */ uint8_t _instance;
void _update();
#if CONFIG_HAL_BOARD != HAL_BOARD_APM2 void _calculate();
bool check_crc(void); bool _check_crc();
#endif
void _timer();
/* Asynchronous state: */ /* Asynchronous state: */
static volatile bool _updated; volatile bool _updated;
static volatile uint8_t _d1_count; volatile uint8_t _d1_count;
static volatile uint8_t _d2_count; volatile uint8_t _d2_count;
static volatile uint32_t _s_D1, _s_D2; volatile uint32_t _s_D1, _s_D2;
static uint8_t _state; uint8_t _state;
static uint32_t _timer; uint32_t _last_timer;
static AP_Baro_MS5611_Serial *_serial;
/* Gates access to asynchronous state: */
static bool _sync_access;
float Temp;
float Press;
int32_t _raw_press;
int32_t _raw_temp;
// Internal calibration registers // Internal calibration registers
uint16_t C1,C2,C3,C4,C5,C6; uint16_t C1,C2,C3,C4,C5,C6;
float D1,D2; float D1,D2;
}; };
#endif // __AP_BARO_MS5611_H__ #endif // __AP_BARO_MS5611_H__

101
libraries/AP_Baro/AP_Baro_PX4.cpp
View File

@ -2,7 +2,7 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_Baro.h> #include <AP_Baro.h>
#include "AP_Baro_PX4.h" #include "AP_Baro_PX4.h"
@ -16,71 +16,62 @@
extern const AP_HAL::HAL& hal; 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) { memset(instances, 0, sizeof(instances));
_baro_fd = open(BARO_DEVICE_PATH, O_RDONLY); instances[0].fd = open(BARO_DEVICE_PATH, O_RDONLY);
if (_baro_fd < 0) { instances[1].fd = open(BARO_DEVICE_PATH"1", O_RDONLY);
hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH);
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 // 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 for (uint8_t i=0; i<_num_instances; i++) {
_accumulate(); struct baro_report baro_report;
struct px4_instance &instance = instances[i];
// consider the baro healthy if we got a reading in the last 0.2s while (::read(instance.fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) &&
_flags.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000); baro_report.timestamp != instance.last_timestamp) {
if (!_flags.healthy || _sum_count == 0) { instance.pressure_sum += baro_report.pressure; // Pressure in mbar
return _flags.healthy; instance.temperature_sum += baro_report.temperature; // degrees celcius
instance.sum_count++;
instance.last_timestamp = baro_report.timestamp;
}
} }
_pressure = (_pressure_sum / _sum_count) * 100.0f; for (uint8_t i=0; i<_num_instances; i++) {
_temperature = _temperature_sum / _sum_count; struct px4_instance &instance = instances[i];
_pressure_samples = _sum_count; if (instance.sum_count > 0) {
_last_update = (uint32_t)_last_timestamp/1000; float pressure = (instance.pressure_sum / instance.sum_count) * 100;
_pressure_sum = 0; float temperature = instance.temperature_sum / instance.sum_count;
_temperature_sum = 0; instance.pressure_sum = 0;
_sum_count = 0; instance.temperature_sum = 0;
instance.sum_count = 0;
return 1; _copy_to_frontend(instance.instance, pressure, temperature);
} }
// 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;
} }
} }
float AP_Baro_PX4::get_pressure() {
return _pressure;
}
float AP_Baro_PX4::get_temperature() const {
return _temperature;
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

28
libraries/AP_Baro/AP_Baro_PX4.h
View File

@ -5,25 +5,23 @@
#include "AP_Baro.h" #include "AP_Baro.h"
class AP_Baro_PX4 : public AP_Baro class AP_Baro_PX4 : public AP_Baro_Backend
{ {
public: public:
bool init(); AP_Baro_PX4(AP_Baro &);
uint8_t read(); void update();
float get_pressure();
float get_temperature() const;
private: private:
float _temperature; uint8_t _num_instances;
float _pressure;
float _pressure_sum; struct px4_instance {
float _temperature_sum; uint8_t instance;
uint32_t _sum_count; int fd;
void _accumulate(void); float pressure_sum;
void _baro_timer(uint32_t now); float temperature_sum;
uint64_t _last_timestamp; uint32_t sum_count;
// baro driver handle uint64_t last_timestamp;
int _baro_fd; } instances[BARO_MAX_INSTANCES];
}; };
#endif // __AP_BARO_PX4_H__ #endif // __AP_BARO_PX4_H__

86
libraries/AP_Baro/AP_Baro_VRBRAIN.cpp
View File

@ -1,86 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_Baro.h>
#include "AP_Baro_VRBRAIN.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h>
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

29
libraries/AP_Baro/AP_Baro_VRBRAIN.h
View File

@ -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__

25
libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde
View File

@ -38,21 +38,7 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
#define CONFIG_BARO HAL_BARO_DEFAULT #define CONFIG_BARO HAL_BARO_DEFAULT
#if CONFIG_BARO == HAL_BARO_BMP085 static AP_Baro barometer;
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 uint32_t timer; static uint32_t timer;
@ -78,7 +64,7 @@ void loop()
{ {
if((hal.scheduler->micros() - timer) > 100000UL) { if((hal.scheduler->micros() - timer) > 100000UL) {
timer = hal.scheduler->micros(); timer = hal.scheduler->micros();
barometer.read(); barometer.update();
uint32_t read_time = hal.scheduler->micros() - timer; uint32_t read_time = hal.scheduler->micros() - timer;
float alt = barometer.get_altitude(); float alt = barometer.get_altitude();
if (!barometer.healthy()) { if (!barometer.healthy()) {
@ -91,10 +77,9 @@ void loop()
hal.console->print(barometer.get_temperature()); hal.console->print(barometer.get_temperature());
hal.console->print(" Altitude:"); hal.console->print(" Altitude:");
hal.console->print(alt); hal.console->print(alt);
hal.console->printf(" climb=%.2f t=%u samples=%u", hal.console->printf(" climb=%.2f t=%u",
barometer.get_climb_rate(), barometer.get_climb_rate(),
(unsigned)read_time, (unsigned)read_time);
(unsigned)barometer.get_pressure_samples());
hal.console->println(); hal.console->println();
} }
} }

5
libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
View File

@ -75,9 +75,8 @@ void loop()
tmp_float = pow(tmp_float, 0.190295); tmp_float = pow(tmp_float, 0.190295);
float alt = 44330.0 * (1.0 - tmp_float); float alt = 44330.0 * (1.0 - tmp_float);
hal.console->print(alt); hal.console->print(alt);
hal.console->printf(" t=%lu samples=%u", hal.console->printf(" t=%lu",
read_time, read_time);
(unsigned)bmp085.get_pressure_samples());
hal.console->println(); hal.console->println();
} }
} }