dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

move pressure altitude from baros to sensors module

This commit is contained in:
Daniel Agar 2017-12-14 00:45:55 -05:00
parent bdf4f19867
commit 5dc23def2a
22 changed files with 55 additions and 794 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/sensor_baro.msg
View File

@ -1,5 +1,4 @@
float32 pressure # static pressure measurement in millibar
float32 altitude # ISA pressure altitude in meters
float32 temperature # static temperature measurement in deg C
uint64 error_count
uint32 device_id # Sensor ID that must be unique for each baro sensor and must not change

3
msg/sensor_combined.msg
View File

@ -20,5 +20,6 @@ int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relat
float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss
int32 baro_timestamp_relative # timestamp + baro_timestamp_relative = Barometer timestamp
float32 baro_alt_meter # Altitude, already temp. comp.
float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH.
float32 baro_temp_celcius # Temperature in degrees celsius
float32 baro_pressure_pa # Absolute pressure in pascals

139
src/drivers/barometer/bmp280/bmp280.cpp
View File

@ -117,9 +117,6 @@ private:
bool _collect_phase;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
int _orb_class_instance;
int _class_instance;
@ -157,7 +154,6 @@ BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) :
_report_ticks(0),
_reports(nullptr),
_collect_phase(false),
_msl_pressure(101325),
_baro_topic(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
@ -418,19 +414,6 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
*/
return OK;
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
default:
break;
}
@ -551,28 +534,6 @@ BMP280::collect()
report.temperature = _T;
report.pressure = _P / 100.0f; // to mbar
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
/* tropospheric properties (0-11km) for standard atmosphere */
const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */
const float a = -6.5f / 1000.0f; /* temperature gradient in degrees per metre */
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
float pK = _P / _msl_pressure;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
report.altitude = (((powf(pK, (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
if (!(_pub_blocked)) {
/* publish it */
@ -595,6 +556,7 @@ BMP280::print_info()
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u us \n", _report_ticks * USEC_PER_TICK);
_reports->print_info("report queue");
sensor_baro_s brp = {};
_reports->get(&brp);
@ -640,7 +602,6 @@ void start(enum BMP280_BUS busid);
void test(enum BMP280_BUS busid);
void reset(enum BMP280_BUS busid);
void info();
void calibrate(unsigned altitude, enum BMP280_BUS busid);
void usage();
@ -868,92 +829,10 @@ info()
exit(0);
}
/**
* Calculate actual MSL pressure given current altitude
*/
void
calibrate(unsigned altitude, enum BMP280_BUS busid)
{
struct bmp280_bus_option &bus = find_bus(busid);
struct baro_report report;
float pressure;
float p1;
int fd;
fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
PX4_ERR("open failed (try 'bmp280 start' if the driver is not running)");
exit(1);
}
/* start the sensor polling at max */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
PX4_ERR("failed to set poll rate");
exit(1);
}
/* average a few measurements */
pressure = 0.0f;
for (unsigned i = 0; i < 20; i++) {
struct pollfd fds;
int ret;
ssize_t sz;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 1000);
if (ret != 1) {
PX4_ERR("timed out waiting for sensor data");
exit(1);
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("sensor read failed");
exit(1);
}
pressure += report.pressure;
}
pressure /= 20; /* average */
pressure /= 10; /* scale from millibar to kPa */
/* tropospheric properties (0-11km) for standard atmosphere */
const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
PX4_WARN("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
PX4_WARN("calculated MSL pressure %10.4fkPa", (double)p1);
/* save as integer Pa */
p1 *= 1000.0f;
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
PX4_ERR("BAROIOCSMSLPRESSURE");
exit(1);
}
close(fd);
exit(0);
}
void
usage()
{
PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset'");
PX4_WARN("options:");
PX4_WARN(" -X (external I2C bus TODO)");
PX4_WARN(" -I (internal I2C bus TODO)");
@ -1026,20 +905,6 @@ bmp280_main(int argc, char *argv[])
bmp280::info();
}
/*
* Perform MSL pressure calibration given an altitude in metres
*/
if (!strcmp(verb, "calibrate")) {
if (argc < 2) {
PX4_ERR("missing altitude");
exit(1);
}
long altitude = strtol(argv[optind + 1], nullptr, 10);
bmp280::calibrate(altitude, busid);
}
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
exit(1);
}

73
src/drivers/barometer/lps25h/lps25h.cpp
View File

@ -217,9 +217,6 @@ private:
ringbuffer::RingBuffer *_reports;
bool _collect_phase;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
int _orb_class_instance;
int _class_instance;
@ -247,17 +244,6 @@ private:
*/
int reset();
/**
* Perform the on-sensor scale calibration routine.
*
* @note The sensor will continue to provide measurements, these
* will however reflect the uncalibrated sensor state until
* the calibration routine has been completed.
*
* @param enable set to 1 to enable self-test strap, 0 to disable
*/
int calibrate(struct file *filp, unsigned enable);
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
@ -329,7 +315,6 @@ LPS25H::LPS25H(device::Device *interface, const char *path) :
_measure_ticks(0),
_reports(nullptr),
_collect_phase(false),
_msl_pressure(101325),
_baro_topic(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
@ -555,19 +540,6 @@ LPS25H::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCRESET:
return reset();
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
case DEVIOCGDEVICEID:
return _interface->ioctl(cmd, dummy);
@ -729,19 +701,15 @@ LPS25H::collect()
}
/* get measurements from the device */
new_report.temperature = 42.5 + (report.t / 480);
new_report.temperature = 42.5f + (report.t / 480);
/* raw pressure */
uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16);
/* Pressure and MSL in mBar */
double p = raw / 4096.0;
double msl = _msl_pressure / 100.0;
double alt = (1.0 - pow(p / msl, 0.190263)) * 44330.8;
float p = raw / 4096.0f;
new_report.pressure = p;
new_report.altitude = alt;
/* get device ID */
new_report.device_id = _device_id.devid;
@ -836,7 +804,6 @@ struct lps25h_bus_option &find_bus(enum LPS25H_BUS busid);
void test(enum LPS25H_BUS busid);
void reset(enum LPS25H_BUS busid);
void info();
void calibrate(unsigned altitude, enum LPS25H_BUS busid);
void usage();
/**
@ -996,27 +963,6 @@ test(enum LPS25H_BUS busid)
errx(0, "PASS");
}
/**
* Calculate actual MSL pressure given current altitude
*/
void
calibrate(unsigned altitude, enum LPS25H_BUS busid)
{
struct lps25h_bus_option &bus = find_bus(busid);
const char *path = bus.devpath;
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'lps25h start' if the driver is not running", path);
}
// TODO: Implement calibration
close(fd);
}
/**
* Reset the driver.
*/
@ -1064,7 +1010,7 @@ info()
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset', 'calibrate'");
warnx("missing command: try 'start', 'info', 'test', 'reset'");
warnx("options:");
warnx(" -X (external I2C bus)");
warnx(" -I (internal I2C bus)");
@ -1133,18 +1079,5 @@ lps25h_main(int argc, char *argv[])
lps25h::info();
}
/*
* Perform MSL pressure calibration given an altitude in metres
*/
if (!strcmp(verb, "calibrate")) {
if (argc < 2) {
errx(1, "missing altitude");
}
long altitude = strtol(argv[optind + 1], nullptr, 10);
lps25h::calibrate(altitude, busid);
}
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
}

145
src/drivers/barometer/mpl3115a2/mpl3115a2.cpp
View File

@ -133,9 +133,6 @@ protected:
float _P;
float _T;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
int _orb_class_instance;
int _class_instance;
@ -209,7 +206,6 @@ MPL3115A2::MPL3115A2(device::Device *interface, const char *path) :
_collect_phase(false),
_P(0),
_T(0),
_msl_pressure(101325),
_baro_topic(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
@ -492,19 +488,6 @@ MPL3115A2::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
}
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
default:
break;
}
@ -674,41 +657,6 @@ MPL3115A2::collect()
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
/*
* PERFORMANCE HINT:
*
* The single precision calculation is 50 microseconds faster than the double
* precision variant. It is however not obvious if double precision is required.
* Pending more inspection and tests, we'll leave the double precision variant active.
*
* Measurements:
* double precision: mpl3115a2_read: 992 events, 258641us elapsed, min 202us max 305us
* single precision: mpl3115a2_read: 963 events, 208066us elapsed, min 202us max 241us
*/
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
double p1 = _msl_pressure / 1000.0;
/* measured pressure in kPa */
double p = (double) _P / 1000.0;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
if (!(_pub_blocked) && _baro_topic != nullptr) {
/* publish it */
@ -775,7 +723,6 @@ void start(enum MPL3115A2_BUS busid);
void test(enum MPL3115A2_BUS busid);
void reset(enum MPL3115A2_BUS busid);
void info();
void calibrate(unsigned altitude, enum MPL3115A2_BUS busid);
void usage();
/**
@ -982,87 +929,10 @@ info()
exit(0);
}
/**
* Calculate actual MSL pressure given current altitude
*/
void
calibrate(unsigned altitude, enum MPL3115A2_BUS busid)
{
struct mpl3115a2_bus_option &bus = find_bus(busid);
struct baro_report report;
float pressure;
float p1;
int fd;
fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
err(1, "open failed (try 'mpl3115a2 start' if the driver is not running)");
}
/* start the sensor polling at max */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
errx(1, "failed to set poll rate");
}
/* average a few measurements */
pressure = 0.0f;
for (unsigned i = 0; i < 20; i++) {
struct pollfd fds;
int ret;
ssize_t sz;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 1000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "sensor read failed");
}
pressure += report.pressure;
}
pressure /= 20; /* average */
pressure /= 10; /* scale from millibar to kPa */
/* tropospheric properties (0-11km) for standard atmosphere */
const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
warnx("calculated MSL pressure %10.4fkPa", (double)p1);
/* save as integer Pa */
p1 *= 1000.0f;
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
err(1, "BAROIOCSMSLPRESSURE");
}
close(fd);
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset', 'calibrate'");
warnx("missing command: try 'start', 'info', 'test', 'reset'");
warnx("options:");
warnx(" -X (external I2C bus)");
warnx(" -I (intternal I2C bus)");
@ -1126,19 +996,6 @@ mpl3115a2_main(int argc, char *argv[])
mpl3115a2::info();
}
/*
* Perform MSL pressure calibration given an altitude in metres
*/
if (!strcmp(verb, "calibrate")) {
if (argc < 2) {
errx(1, "missing altitude");
}
long altitude = strtol(argv[myoptind + 1], nullptr, 10);
mpl3115a2::calibrate(altitude, busid);
}
mpl3115a2::usage();
exit(1);
}

161
src/drivers/barometer/ms5611/ms5611.cpp
View File

@ -163,9 +163,6 @@ protected:
float _P;
float _T;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
int _orb_class_instance;
int _class_instance;
@ -243,7 +240,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *
_TEMP(0),
_OFF(0),
_SENS(0),
_msl_pressure(101325),
_baro_topic(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
@ -351,8 +347,6 @@ MS5611::init()
/* state machine will have generated a report, copy it out */
_reports->get(&brp);
// DEVICE_LOG("altitude (%u) = %.2f", _device_type, (double)brp.altitude);
if (autodetect) {
if (_device_type == MS5611_DEVICE) {
if (brp.altitude > 5300.f) {
@ -571,19 +565,6 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
*/
return OK;
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
default:
break;
}
@ -827,43 +808,6 @@ MS5611::collect()
/* return device ID */
report.device_id = _device_id.devid;
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
/*
* PERFORMANCE HINT:
*
* The single precision calculation is 50 microseconds faster than the double
* precision variant. It is however not obvious if double precision is required.
* Pending more inspection and tests, we'll leave the double precision variant active.
*
* Measurements:
* double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
* single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
*/
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
double p1 = _msl_pressure / 1000.0;
/* measured pressure in kPa */
double p = P / 1000.0;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
if (!(_pub_blocked) && _baro_topic != nullptr) {
/* publish it */
@ -892,18 +836,6 @@ MS5611::print_info()
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
printf("device: %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");
printf("TEMP: %d\n", _TEMP);
printf("SENS: %lld\n", _SENS);
printf("OFF: %lld\n", _OFF);
printf("factory_setup %u\n", _prom.factory_setup);
printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens);
printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset);
printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens);
printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset);
printf("c5_reference_temp %u\n", _prom.c5_reference_temp);
printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp);
printf("serial_and_crc %u\n", _prom.serial_and_crc);
sensor_baro_s brp = {};
_reports->get(&brp);
@ -947,7 +879,6 @@ void start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type);
void test(enum MS5611_BUS busid);
void reset(enum MS5611_BUS busid);
void info();
void calibrate(unsigned altitude, enum MS5611_BUS busid);
void usage();
/**
@ -1202,87 +1133,10 @@ info()
exit(0);
}
/**
* Calculate actual MSL pressure given current altitude
*/
void
calibrate(unsigned altitude, enum MS5611_BUS busid)
{
struct ms5611_bus_option &bus = find_bus(busid);
struct baro_report report;
float pressure;
float p1;
int fd;
fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
err(1, "open failed (try 'ms5611 start' if the driver is not running)");
}
/* start the sensor polling at max */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
errx(1, "failed to set poll rate");
}
/* average a few measurements */
pressure = 0.0f;
for (unsigned i = 0; i < 20; i++) {
struct pollfd fds;
int ret;
ssize_t sz;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 1000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "sensor read failed");
}
pressure += report.pressure;
}
pressure /= 20; /* average */
pressure /= 10; /* scale from millibar to kPa */
/* tropospheric properties (0-11km) for standard atmosphere */
const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
warnx("calculated MSL pressure %10.4fkPa", (double)p1);
/* save as integer Pa */
p1 *= 1000.0f;
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
err(1, "BAROIOCSMSLPRESSURE");
}
close(fd);
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset'");
warnx("options:");
warnx(" -X (external I2C bus)");
warnx(" -I (intternal I2C bus)");
@ -1378,18 +1232,5 @@ ms5611_main(int argc, char *argv[])
ms5611::info();
}
/*
* Perform MSL pressure calibration given an altitude in metres
*/
if (!strcmp(verb, "calibrate")) {
if (argc < 2) {
errx(1, "missing altitude");
}
long altitude = strtol(argv[optind + 1], nullptr, 10);
ms5611::calibrate(altitude, busid);
}
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
}

13
src/drivers/drv_baro.h
View File

@ -53,17 +53,4 @@
#include <uORB/topics/sensor_baro.h>
#define baro_report sensor_baro_s
/*
* ioctl() definitions
*/
#define _BAROIOCBASE (0x2200)
#define _BAROIOC(_n) (_PX4_IOC(_BAROIOCBASE, _n))
/** set corrected MSL pressure in pascals */
#define BAROIOCSMSLPRESSURE _BAROIOC(0)
/** get current MSL pressure in pascals */
#define BAROIOCGMSLPRESSURE _BAROIOC(1)
#endif /* _DRV_BARO_H */

17
src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c
View File

@ -59,7 +59,7 @@
#include <systemlib/err.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_combined.h>
#include <math.h> // NAN
#include "sPort_data.h"
@ -319,15 +319,11 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
PX4_INFO("sending FrSky SmartPort telemetry");
struct sensor_baro_s *sensor_baro = malloc(sizeof(struct sensor_baro_s));
if (sensor_baro == NULL) {
err(1, "could not allocate memory");
}
struct sensor_combined_s sensor_combined = {};
float filtered_alt = NAN;
float last_baro_alt = 0.f;
int sensor_sub = orb_subscribe(ORB_ID(sensor_baro));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
uint32_t lastBATV_ms = 0;
uint32_t lastCUR_ms = 0;
@ -372,13 +368,13 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
orb_check(sensor_sub, &sensor_updated);
if (sensor_updated) {
orb_copy(ORB_ID(sensor_baro), sensor_sub, sensor_baro);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_combined);
if (isnan(filtered_alt)) {
filtered_alt = sensor_baro->altitude;
filtered_alt = sensor_combined.baro_alt_meter;
} else {
filtered_alt = .05f * sensor_baro->altitude + .95f * filtered_alt;
filtered_alt = .05f * sensor_combined.baro_alt_meter + .95f * filtered_alt;
}
}
@ -546,7 +542,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
PX4_DEBUG("freeing sPort memory");
sPort_deinit();
free(sensor_baro);
/* either no traffic on the port (0=>timeout), or D type packet */

16
src/modules/ekf2/ekf2_main.cpp
View File

@ -57,7 +57,6 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_selection.h>
@ -521,7 +520,6 @@ void Ekf2::run()
int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
int status_sub = orb_subscribe(ORB_ID(vehicle_status));
int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
int landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose));
bool imu_bias_reset_request = false;
@ -554,8 +552,6 @@ void Ekf2::run()
vehicle_attitude_s ev_att = {};
vehicle_status_s vehicle_status = {};
sensor_selection_s sensor_selection = {};
sensor_baro_s sensor_baro = {};
sensor_baro.pressure = 1013.5f; // initialise pressure to sea level
landing_target_pose_s landing_target_pose = {};
while (!should_exit()) {
@ -588,7 +584,6 @@ void Ekf2::run()
bool gps_updated = false;
bool airspeed_updated = false;
bool baro_updated = false;
bool sensor_selection_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
@ -620,12 +615,6 @@ void Ekf2::run()
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
}
orb_check(sensor_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro);
}
orb_check(sensor_selection_sub, &sensor_selection_updated);
// Always update sensor selction first time through if time stamp is non zero
@ -808,8 +797,8 @@ void Ekf2::run()
float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
// estimate air density assuming typical 20degC ambient temperature
const float pressure_to_density = 100.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
const float rho = pressure_to_density * sensor_baro.pressure;
const float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
const float rho = pressure_to_density * sensors.baro_pressure_pa;
_ekf.set_air_density(rho);
// calculate static pressure error = Pmeas - Ptruth
@ -1454,7 +1443,6 @@ void Ekf2::run()
orb_unsubscribe(vehicle_land_detected_sub);
orb_unsubscribe(status_sub);
orb_unsubscribe(sensor_selection_sub);
orb_unsubscribe(sensor_baro_sub);
orb_unsubscribe(landing_target_pose_sub);
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {

1
src/modules/logger/logger.cpp
View File

@ -680,7 +680,6 @@ void Logger::add_estimator_replay_topics()
add_topic("airspeed");
add_topic("distance_sensor");
add_topic("optical_flow");
add_topic("sensor_baro");
add_topic("sensor_combined");
add_topic("sensor_selection");
add_topic("vehicle_gps_position");

1
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1965,7 +1965,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
baro.timestamp = timestamp;
baro.pressure = imu.abs_pressure;
baro.altitude = imu.pressure_alt;
baro.temperature = imu.temperature;
/* fake device ID */

33
src/modules/sensors/sensors.cpp
View File

@ -267,30 +267,6 @@ Sensors::parameters_update()
_rc_update.update_rc_functions();
_voted_sensors_update.parameters_update();
/* update barometer qnh setting */
DevHandle h_baro;
DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) && !defined(__PX4_POSIX_OCPOC)
// TODO: this needs fixing for QURT and Raspberry Pi
if (!h_baro.isValid()) {
if (!_hil_enabled) { // in HIL we don't have a baro
PX4_ERR("no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
ret = PX4_ERROR;
}
} else {
int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
if (baroret) {
PX4_ERR("qnh for baro could not be set");
ret = PX4_ERROR;
}
}
#endif
return ret;
}
@ -359,16 +335,15 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
airspeed.indicated_airspeed_m_s = math::max(0.0f,
calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, _voted_sensors_update.baro_pressure(),
diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa,
air_temperature_celsius));
airspeed.true_airspeed_m_s = math::max(0.0f,
calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s,
_voted_sensors_update.baro_pressure(), air_temperature_celsius));
calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, air_temperature_celsius));
airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
calc_true_airspeed(diff_pres.differential_pressure_raw_pa + _voted_sensors_update.baro_pressure(),
_voted_sensors_update.baro_pressure(), air_temperature_celsius));
calc_true_airspeed(diff_pres.differential_pressure_raw_pa + raw.baro_pressure_pa, raw.baro_pressure_pa,
air_temperature_celsius));
airspeed.air_temperature_celsius = air_temperature_celsius;

82
src/modules/sensors/voted_sensors_update.cpp
View File

@ -42,17 +42,14 @@
#include <systemlib/mavlink_log.h>
#include <conversion/rotation.h>
#include <geo/geo.h>
#define MAG_ROT_VAL_INTERNAL -1
#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
using namespace sensors;
using namespace DriverFramework;
const double VotedSensorsUpdate::_msl_pressure = 101.325;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
: _parameters(parameters), _hil_enabled(hil_enabled)
{
@ -551,9 +548,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
if (accel_updated && _accel.enabled[uorb_index]) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
if (accel_report.timestamp == 0) {
if (ret != PX4_OK || accel_report.timestamp == 0) {
continue; //ignore invalid data
}
@ -629,6 +626,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
// write the best sensor data to the output variables
if (best_index >= 0) {
raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2));
if (best_index != _accel.last_best_vote) {
_accel.last_best_vote = (uint8_t)best_index;
@ -640,10 +638,6 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
_selection_changed = true;
_selection.accel_device_id = _accel_device_id[best_index];
}
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
raw.accelerometer_m_s2[axis_index] = _last_sensor_data[best_index].accelerometer_m_s2[axis_index];
}
}
}
@ -659,9 +653,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
if (gyro_updated && _gyro.enabled[uorb_index]) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
if (gyro_report.timestamp == 0) {
if (ret != PX4_OK || gyro_report.timestamp == 0) {
continue; //ignore invalid data
}
@ -736,8 +730,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
// write data for the best sensor to output variables
if (best_index >= 0) {
raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
raw.timestamp = _last_sensor_data[best_index].timestamp;
raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
memcpy(&raw.gyro_rad, &_last_sensor_data[best_index].gyro_rad, sizeof(raw.gyro_rad));
if (_gyro.last_best_vote != best_index) {
_gyro.last_best_vote = (uint8_t)best_index;
@ -749,10 +744,6 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
_selection_changed = true;
_selection.gyro_device_id = _gyro_device_id[best_index];
}
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index];
}
}
}
@ -765,19 +756,14 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
if (mag_updated && _mag.enabled[uorb_index]) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report);
int ret = orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report);
if (mag_report.timestamp == 0) {
if (ret != PX4_OK || mag_report.timestamp == 0) {
continue; //ignore invalid data
}
// First publication with data
if (_mag.priority[uorb_index] == 0) {
// Parameters update to get offsets, scaling & mag rotation loaded (if not already loaded)
parameters_update();
// Set device priority for the voter
int32_t priority = 0;
orb_priority(_mag.subscription[uorb_index], &priority);
_mag.priority[uorb_index] = (uint8_t)priority;
@ -799,16 +785,14 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
_mag.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0];
raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1];
raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
memcpy(&raw.magnetometer_ga, &_last_sensor_data[best_index].magnetometer_ga, sizeof(raw.magnetometer_ga));
_mag.last_best_vote = (uint8_t)best_index;
}
if (_selection.mag_device_id != _mag_device_id[best_index]) {
_selection_changed = true;
_selection.mag_device_id = _mag_device_id[best_index];
}
}
}
void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
@ -824,9 +808,9 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
if (baro_updated) {
struct baro_report baro_report;
orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
int ret = orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
if (baro_report.timestamp == 0) {
if (ret != PX4_OK || baro_report.timestamp == 0) {
continue; //ignore invalid data
}
@ -851,14 +835,13 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
_baro_device_id[uorb_index] = baro_report.device_id;
got_update = true;
matrix::Vector3f vect(baro_report.altitude, 0.f, 0.f);
matrix::Vector3f vect(baro_report.pressure, 0.f, 0.f);
_last_sensor_data[uorb_index].baro_alt_meter = baro_report.altitude;
_last_sensor_data[uorb_index].baro_temp_celcius = baro_report.temperature;
_last_baro_pressure[uorb_index] = corrected_pressure;
_last_sensor_data[uorb_index].baro_pressure_pa = corrected_pressure;
_last_baro_timestamp[uorb_index] = baro_report.timestamp;
_baro.voter.put(uorb_index, baro_report.timestamp, vect.data(), baro_report.error_count, _baro.priority[uorb_index]);
_baro.voter.put(uorb_index, baro_report.timestamp, vect.data, baro_report.error_count, _baro.priority[uorb_index]);
}
}
@ -868,7 +851,7 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
if (best_index >= 0) {
raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
_last_best_baro_pressure = _last_baro_pressure[best_index];
raw.baro_pressure_pa = _last_sensor_data[best_index].baro_pressure_pa;
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
@ -881,31 +864,16 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
_selection.baro_device_id = _baro_device_id[best_index];
}
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
// calculate altitude using the hypsometric equation
/*
* PERFORMANCE HINT:
*
* The single precision calculation is 50 microseconds faster than the double
* precision variant. It is however not obvious if double precision is required.
* Pending more inspection and tests, we'll leave the double precision variant active.
*
* Measurements:
* double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
* single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
*/
static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; /* temperature at base height in Kelvin */
static constexpr float a = -6.5f / 1000.0f; /* temperature gradient in degrees per metre */
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
const double p1 = _msl_pressure;
/* current pressure at MSL in kPa (QNH in hPa)*/
const float p1 = _parameters.baro_qnh * 0.1f;
/* measured pressure in kPa */
const double p = 0.001f * _last_best_baro_pressure;
const float p = raw.baro_pressure_pa * 0.001f;
/*
* Solve:
@ -916,7 +884,7 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
* h = ------------------------------- + h1
* a
*/
raw.baro_alt_meter = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
raw.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
}
}

8
src/modules/sensors/voted_sensors_update.h
View File

@ -97,9 +97,6 @@ public:
void print_status();
/** get the latest baro pressure */
float baro_pressure() const { return _last_best_baro_pressure; }
/**
* call this whenever parameters got updated. Make sure to have initialize_sensors() called at least
* once before calling this.
@ -246,9 +243,8 @@ private:
orb_advert_t _mavlink_log_pub = nullptr;
float _last_baro_pressure[BARO_COUNT_MAX]; /**< pressure from last baro sensors */
float _last_best_baro_pressure = 0.0f; /**< pressure from last best baro */
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]; /**< latest full timestamp */
uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */
uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */
@ -273,12 +269,12 @@ private:
struct sensor_selection_s _selection = {}; /**< struct containing the sensor selection to be published to the uORB*/
orb_advert_t _sensor_selection_pub = nullptr; /**< handle to the sensor selection uORB topic */
bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */
uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; /**< accel driver device id for each uorb instance */
uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {};
uint32_t _gyro_device_id[SENSOR_COUNT_MAX] = {};
uint32_t _mag_device_id[SENSOR_COUNT_MAX] = {};
static const double _msl_pressure; /** average sea-level pressure in kPa */
};

1
src/modules/simulator/simulator_mavlink.cpp
View File

@ -1073,7 +1073,6 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
baro.timestamp = timestamp;
baro.pressure = imu->abs_pressure;
baro.altitude = imu->pressure_alt;
baro.temperature = imu->temperature;
int baro_multi;

29
src/modules/uavcan/sensors/baro.cpp
View File

@ -98,21 +98,6 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl
int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case BAROIOCSMSLPRESSURE: {
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
} else {
DEVICE_LOG("new msl pressure %u", _msl_pressure);
_msl_pressure = arg;
return OK;
}
}
case BAROIOCGMSLPRESSURE: {
return _msl_pressure;
}
case SENSORIOCSPOLLRATE: {
// not supported yet, pretend that everything is ok
return OK;
@ -168,20 +153,6 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
/* TODO get device ID for sensor */
report.device_id = 0;
/*
* Altitude computation
* Refer to the MS5611 driver for details
*/
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
const double g = 9.80665; // gravity constant in m/s/s
const double R = 287.05; // ideal gas constant in J/kg/K
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// add to the ring buffer
_reports.force(&report);

1
src/modules/uavcan/sensors/baro.hpp
View File

@ -79,7 +79,6 @@ private:
ringbuffer::RingBuffer _reports;
unsigned _msl_pressure = 101325;
float last_temperature_kelvin = 0.0f;
};

26
src/platforms/posix/drivers/barosim/baro.cpp
View File

@ -101,9 +101,6 @@ protected:
/* last report */
struct baro_report report;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
int _orb_class_instance;
@ -136,7 +133,6 @@ BAROSIM::BAROSIM(const char *path) :
VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, BAROSIM_MEASURE_INTERVAL_US),
_reports(nullptr),
report{},
_msl_pressure(101325),
_baro_topic(nullptr),
_orb_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")),
@ -202,27 +198,6 @@ out:
int
BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
{
//PX4_WARN("baro IOCTL %" PRIu64 , hrt_absolute_time());
switch (cmd) {
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
default:
break;
}
/* give it to the bus-specific superclass */
// return bus_ioctl(filp, cmd, arg);
return VirtDevObj::devIOCTL(cmd, arg);
@ -264,7 +239,6 @@ BAROSIM::collect()
}
report.pressure = raw_baro.pressure;
report.altitude = raw_baro.altitude;
report.temperature = raw_baro.temperature;
/* fake device ID */

30
src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
View File

@ -150,40 +150,12 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
{
perf_begin(_baro_sample_perf);
baro_report baro_report = {};
baro_report baro_report;
baro_report.timestamp = hrt_absolute_time();
baro_report.pressure = data.pressure_pa / 100.0f; // to mbar
baro_report.temperature = data.temperature_c;
// TODO: verify this, it's just copied from the MS5611 driver.
// Constant for now
const double MSL_PRESSURE_KPA = 101325.0 / 1000.0;
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
double p1 = MSL_PRESSURE_KPA;
/* measured pressure in kPa */
double p = static_cast<double>(data.pressure_pa) / 1000.0;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {

30
src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
View File

@ -148,40 +148,12 @@ int DfMS5607Wrapper::_publish(struct baro_sensor_data &data)
{
perf_begin(_baro_sample_perf);
baro_report baro_report = {};
baro_report baro_report;
baro_report.timestamp = hrt_absolute_time();
baro_report.pressure = data.pressure_pa;
baro_report.temperature = data.temperature_c;
// TODO: verify this, it's just copied from the MS5611 driver.
// Constant for now
const double MSL_PRESSURE_KPA = 101325.0 / 1000.0;
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
double p1 = MSL_PRESSURE_KPA;
/* measured pressure in kPa */
double p = static_cast<double>(data.pressure_pa) / 1000.0;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {

30
src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp
View File

@ -148,40 +148,12 @@ int DfMS5611Wrapper::_publish(struct baro_sensor_data &data)
{
perf_begin(_baro_sample_perf);
baro_report baro_report = {};
baro_report baro_report;
baro_report.timestamp = hrt_absolute_time();
baro_report.pressure = data.pressure_pa / 100.0f; // convert to mbar
baro_report.temperature = data.temperature_c;
// TODO: verify this, it's just copied from the MS5611 driver.
// Constant for now
const double MSL_PRESSURE_KPA = 101325.0 / 1000.0;
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
double p1 = MSL_PRESSURE_KPA;
/* measured pressure in kPa */
double p = static_cast<double>(data.pressure_pa) / 1000.0;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {

3
src/systemcmds/tests/test_sensors.c
View File

@ -251,8 +251,7 @@ baro(int argc, char *argv[], const char *path)
return ERROR;
} else {
printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude,
(double)buf.temperature);
printf("\tBARO pressure: %8.4f mbar\t temp: %8.4f deg C\n", (double)buf.pressure, (double)buf.temperature);
}
/* Let user know everything is ok */