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
// @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++;
}

143
libraries/AP_Baro/AP_Baro.h
View File

@ -3,85 +3,92 @@
#ifndef __AP_BARO_H__
#define __AP_BARO_H__
#include <AP_HAL.h>
#include <AP_Param.h>
#include <Filter.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
{
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__

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 -*-
#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

1
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -25,7 +25,6 @@ public:
ac4(0), ac5(0), ac6(0),
_retry_time(0)
{
_pressure_samples = 1;
}; // 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 -*-
#include <AP_Baro.h>
#include "AP_Baro_HIL.h"
#include <AP_HAL.h>
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();
}

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 -*-
/*
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__

385
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -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 <AP_HAL.h>
#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, &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);
}
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);
}

121
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -6,15 +6,10 @@
#include <AP_HAL.h>
#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__

101
libraries/AP_Baro/AP_Baro_PX4.cpp
View File

@ -2,7 +2,7 @@
#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_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

28
libraries/AP_Baro/AP_Baro_PX4.h
View File

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

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
#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();
}
}

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);
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();
}
}