forked from Archive/PX4-Autopilot
move pressure altitude from baros to sensors module
This commit is contained in:
parent
bdf4f19867
commit
5dc23def2a
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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'");
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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'");
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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 */
|
||||
|
||||
|
|
|
@ -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++) {
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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 ¶meters, 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,15 +785,13 @@ 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];
|
||||
if (_selection.mag_device_id != _mag_device_id[best_index]) {
|
||||
_selection_changed = true;
|
||||
_selection.mag_device_id = _mag_device_id[best_index];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -79,7 +79,6 @@ private:
|
|||
|
||||
ringbuffer::RingBuffer _reports;
|
||||
|
||||
unsigned _msl_pressure = 101325;
|
||||
float last_temperature_kelvin = 0.0f;
|
||||
|
||||
};
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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)) {
|
||||
|
||||
|
|
|
@ -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)) {
|
||||
|
||||
|
|
|
@ -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)) {
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
|
Loading…
Reference in New Issue