From 258f558feafcce7a92911b67bea51d7169f51bee Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 25 Aug 2021 10:57:21 -0400 Subject: [PATCH] apply differential pressure calibration (SENS_DPRES_OFF) centrally - remove drv_airspeed.h and ioctls --- .../ets/ets_airspeed.cpp | 3 - .../ms4525/ms4525_airspeed.cpp | 4 +- .../differential_pressure/ms5525/MS5525.cpp | 4 +- .../differential_pressure/sdp3x/SDP3X.cpp | 4 +- src/drivers/drv_airspeed.h | 72 --------------- src/drivers/uavcan/sensors/airspeed.hpp | 1 - .../uavcan/sensors/differential_pressure.cpp | 8 +- .../uavcan/sensors/differential_pressure.hpp | 5 +- src/lib/drivers/airspeed/airspeed.cpp | 29 ------- src/lib/drivers/airspeed/airspeed.h | 8 -- .../commander/airspeed_calibration.cpp | 87 +++---------------- src/modules/sensors/sensors.cpp | 30 ++----- 12 files changed, 29 insertions(+), 226 deletions(-) delete mode 100644 src/drivers/drv_airspeed.h diff --git a/src/drivers/differential_pressure/ets/ets_airspeed.cpp b/src/drivers/differential_pressure/ets/ets_airspeed.cpp index 2dbc4073a8..b409da9095 100644 --- a/src/drivers/differential_pressure/ets/ets_airspeed.cpp +++ b/src/drivers/differential_pressure/ets/ets_airspeed.cpp @@ -150,9 +150,6 @@ ETSAirspeed::collect() diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01); } - // The raw value still should be compensated for the known offset - diff_pres_pa_raw -= _diff_pres_offset; - report.error_count = perf_event_count(_comms_errors); // XXX we may want to smooth out the readings to remove noise. diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp index d9a50eb77f..1ded849d49 100644 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp @@ -212,8 +212,8 @@ MEASAirspeed::collect() report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; - report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.device_id = _device_id.devid; report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/differential_pressure/ms5525/MS5525.cpp b/src/drivers/differential_pressure/ms5525/MS5525.cpp index 21118e69b7..c1472f0f06 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.cpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.cpp @@ -265,8 +265,8 @@ MS5525::collect() differential_pressure_s diff_pressure{}; diff_pressure.error_count = perf_event_count(_comms_errors); - diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; - diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; + diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw; + diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); diff_pressure.temperature = temperature_c; diff_pressure.device_id = _device_id.devid; diff_pressure.timestamp = hrt_absolute_time(); diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index 72001f539a..30a8d31df0 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -172,8 +172,8 @@ SDP3X::collect() report.error_count = perf_event_count(_comms_errors); report.temperature = temperature_c; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; - report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.device_id = _device_id.devid; report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h deleted file mode 100644 index 763cab3524..0000000000 --- a/src/drivers/drv_airspeed.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_airspeed.h - * - * Airspeed driver interface. - * - * @author Simon Wilks - */ - -#ifndef _DRV_AIRSPEED_H -#define _DRV_AIRSPEED_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define AIRSPEED_BASE_DEVICE_PATH "/dev/airspeed" -#define AIRSPEED0_DEVICE_PATH "/dev/airspeed0" - -/* - * ioctl() definitions - * - * Airspeed drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ - -#define _AIRSPEEDIOCBASE (0x7700) -#define __AIRSPEEDIOC(_n) (_PX4_IOC(_AIRSPEEDIOCBASE, _n)) - -#define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0) - -/** airspeed scaling factors; out = (in * Vscale) + offset */ -struct airspeed_scale { - float offset_pa; - float scale; -}; - -#endif /* _DRV_AIRSPEED_H */ diff --git a/src/drivers/uavcan/sensors/airspeed.hpp b/src/drivers/uavcan/sensors/airspeed.hpp index c2f142d230..29f0194f06 100644 --- a/src/drivers/uavcan/sensors/airspeed.hpp +++ b/src/drivers/uavcan/sensors/airspeed.hpp @@ -38,7 +38,6 @@ #pragma once #include "sensor_bridge.hpp" -#include #include #include diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp index 8c9e6069ef..480a9f42ec 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.cpp +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -37,7 +37,6 @@ #include "differential_pressure.hpp" -#include #include #include #include @@ -53,9 +52,6 @@ UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode int UavcanDifferentialPressureBridge::init() { - // Initialize the calibration offset - param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - int res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb)); if (res < 0) { @@ -78,8 +74,8 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const if (PX4_ISFINITE(diff_press_pa)) { differential_pressure_s report{}; - report.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset; + report.differential_pressure_raw_pa = diff_press_pa; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); report.temperature = temperature_c; report.device_id = _device_id.devid; report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/uavcan/sensors/differential_pressure.hpp b/src/drivers/uavcan/sensors/differential_pressure.hpp index a862b2dd34..17eb6fccde 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.hpp +++ b/src/drivers/uavcan/sensors/differential_pressure.hpp @@ -37,9 +37,8 @@ #pragma once -#include -#include #include +#include #include "sensor_bridge.hpp" @@ -57,8 +56,6 @@ public: int init() override; private: - float _diff_pres_offset{0.f}; - math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver void air_sub_cb(const uavcan::ReceivedDataStructure &msg); diff --git a/src/lib/drivers/airspeed/airspeed.cpp b/src/lib/drivers/airspeed/airspeed.cpp index 4c9310ae16..61bfbc51a7 100644 --- a/src/lib/drivers/airspeed/airspeed.cpp +++ b/src/lib/drivers/airspeed/airspeed.cpp @@ -47,10 +47,8 @@ #include #include -#include #include -#include #include #include @@ -60,9 +58,6 @@ Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_ _sensor_ok(false), _measure_interval(conversion_interval), _collect_phase(false), - _diff_pres_offset(0.0f), - _airspeed_orb_class_instance(-1), - _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")), _comms_errors(perf_alloc(PC_COUNT, "aspd_com_err")) @@ -71,10 +66,6 @@ Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_ Airspeed::~Airspeed() { - if (_class_instance != -1) { - unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); - } - // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -88,9 +79,6 @@ Airspeed::init() return PX4_ERROR; } - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); - /* advertise sensor topic, measure manually to initialize valid report */ measure(); @@ -109,20 +97,3 @@ Airspeed::probe() return ret; } - -int -Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale *)arg; - _diff_pres_offset = s->offset_pa; - return OK; - } - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - diff --git a/src/lib/drivers/airspeed/airspeed.h b/src/lib/drivers/airspeed/airspeed.h index f60a59e511..b5b4d7ff78 100644 --- a/src/lib/drivers/airspeed/airspeed.h +++ b/src/lib/drivers/airspeed/airspeed.h @@ -35,7 +35,6 @@ #include #include -#include #include #include #include @@ -51,8 +50,6 @@ public: int init() override; - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - private: Airspeed(const Airspeed &) = delete; Airspeed &operator=(const Airspeed &) = delete; @@ -70,14 +67,9 @@ protected: bool _sensor_ok; int _measure_interval; bool _collect_phase; - float _diff_pres_offset; uORB::PublicationMulti _airspeed_pub{ORB_ID(differential_pressure)}; - int _airspeed_orb_class_instance; - - int _class_instance; - unsigned _conversion_interval; perf_counter_t _sample_perf; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 25b58b20d8..b2b359aafc 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -76,47 +76,13 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) const unsigned calibration_count = (maxcount * 2) / 3; - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; + uORB::SubscriptionBlocking diff_pres_sub{ORB_ID(differential_pressure)}; float diff_pres_offset = 0.0f; - /* Reset sensor parameters */ - struct airspeed_scale airscale = { - diff_pres_offset, - 1.0f, - }; - - bool paramreset_successful = false; - int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); - - if (fd >= 0) { - if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - paramreset_successful = true; - - } else { - calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed"); - } - - px4_close(fd); - } - - if (!paramreset_successful) { - - /* only warn if analog scaling is zero */ - float analog_scaling = 0.0f; - param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); - - if (fabsf(analog_scaling) < 0.1f) { - calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found"); - goto error_return; - } - - /* set scaling offset parameter */ - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - goto error_return; - } + if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset) != PX4_OK) { + calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); @@ -128,15 +94,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) goto error_return; } - /* wait blocking for new data */ - px4_pollfd_struct_t fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + differential_pressure_s diff_pres; - int poll_ret = px4_poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) { diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; @@ -154,7 +114,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } - } else if (poll_ret == 0) { + } else { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_log_pub); goto error_return; @@ -164,18 +124,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) diff_pres_offset = diff_pres_offset / calibration_count; if (PX4_ISFINITE(diff_pres_offset)) { - - int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); - airscale.offset_pa = diff_pres_offset; - - if (fd_scale >= 0) { - if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed"); - } - - px4_close(fd_scale); - } - // Prevent a completely zero param // since this is used to detect a missing calibration // This value is numerically down in the noise and has @@ -210,16 +158,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) goto error_return; } - /* wait blocking for new data */ - px4_pollfd_struct_t fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; - - int poll_ret = px4_poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + differential_pressure_s diff_pres; + if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) { if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) { if (diff_pres.differential_pressure_filtered_pa > 0) { calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", @@ -257,7 +198,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) calibration_counter++; - } else if (poll_ret == 0) { + } else { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_log_pub); goto error_return; @@ -274,13 +215,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); - /* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise - * the followup preflight checks might fail. */ - px4_usleep(2e6); - normal_return: - px4_close(diff_pres_sub); - // This give a chance for the log messages to go out of the queue before someone else stomps on then px4_sleep(1); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4a9f02fa51..fa1b6e5376 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -42,8 +42,8 @@ */ #include -#include #include +#include #include #include #include @@ -416,8 +416,13 @@ void Sensors::diff_pres_poll() airspeed_s airspeed{}; airspeed.timestamp = diff_pres.timestamp; + // apply calibration offset (SENS_DPRES_OFF) + const float differential_pressure_raw_pa = diff_pres.differential_pressure_raw_pa - _parameters.diff_pres_offset_pa; + const float differential_pressure_filtered_pa = diff_pres.differential_pressure_filtered_pa - + _parameters.diff_pres_offset_pa; + /* push data into validator */ - float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f }; + float airspeed_input[3] = { differential_pressure_raw_pa, diff_pres.temperature, 0.0f }; _airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? @@ -446,7 +451,7 @@ void Sensors::diff_pres_poll() airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) _parameters.air_cmodel, smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa, + differential_pressure_filtered_pa, air_data.baro_pressure_pa, air_temperature_celsius); airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa, @@ -472,23 +477,6 @@ Sensors::parameter_update_poll(bool forced) // update parameters from storage parameters_update(); updateParams(); - - /* update airspeed scale */ - int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); - - /* this sensor is optional, abort without error */ - if (fd >= 0) { - struct airspeed_scale airscale = { - _parameters.diff_pres_offset_pa, - 1.0f, - }; - - if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - } - - px4_close(fd); - } } } @@ -530,7 +518,7 @@ void Sensors::adc_poll() * vref. Those devices require no divider at all. */ if (voltage > 0.4f) { - const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale; _diff_pres.timestamp = t; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;