diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg index 2d0d0528b7..ebab887df1 100644 --- a/msg/sensor_baro.msg +++ b/msg/sensor_baro.msg @@ -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 diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 7b6fc7b51c..4a18acf3b3 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -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 diff --git a/src/drivers/barometer/bmp280/bmp280.cpp b/src/drivers/barometer/bmp280/bmp280.cpp index a5752cb21d..cdd38aa4d5 100644 --- a/src/drivers/barometer/bmp280/bmp280.cpp +++ b/src/drivers/barometer/bmp280/bmp280.cpp @@ -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); } diff --git a/src/drivers/barometer/lps25h/lps25h.cpp b/src/drivers/barometer/lps25h/lps25h.cpp index 0849a6b834..3a0438b7d0 100644 --- a/src/drivers/barometer/lps25h/lps25h.cpp +++ b/src/drivers/barometer/lps25h/lps25h.cpp @@ -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'"); } diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp b/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp index 3d169d7ad2..6307c74793 100644 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp +++ b/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp @@ -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); } diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index 2082a5a591..414ed01527 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -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'"); } diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 87bf1000ec..2e5bf7ff15 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -53,17 +53,4 @@ #include #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 */ diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c index 3e14dfccb4..ba83602537 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.c @@ -59,7 +59,7 @@ #include #include #include -#include +#include #include // 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 */ diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 4ba9ad0e3a..cd8c934c76 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -57,7 +57,6 @@ #include #include #include -#include #include #include #include @@ -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++) { diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index c551126e63..b4ebca9172 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -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"); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fc17a3bc19..e4db48f069 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 59ad5cec83..e743ded83f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 8fed9ca361..1d60e521f8 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -42,17 +42,14 @@ #include #include +#include #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; } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index f4ee619565..259178402b 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -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 */ }; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 365440f786..6979b5c454 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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; diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 9c547e58ab..1333d8b526 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -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); diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index ed1e308515..b3e8b14aa6 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -79,7 +79,6 @@ private: ringbuffer::RingBuffer _reports; - unsigned _msl_pressure = 101325; float last_temperature_kelvin = 0.0f; }; diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 0cc51a4aaf..4cacb5f948 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -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 */ diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp index 4682b63167..5f1be27da3 100644 --- a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp +++ b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp @@ -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(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)) { diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp index 1e65921145..eb2658a569 100644 --- a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp +++ b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp @@ -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(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)) { diff --git a/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp b/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp index 1f0a592e2b..9830d0738a 100644 --- a/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp +++ b/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp @@ -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(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)) { diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index ee0d8d2579..37faa39318 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -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 */