mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 23:13:33 +08:00
move pressure altitude from baros to sensors module
This commit is contained in:
Daniel Agar
@@ -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 */
|
||||
|
||||
|
||||
Reference in New Issue
Block a user