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

apply differential pressure calibration (SENS_DPRES_OFF) centrally 

- remove drv_airspeed.h and ioctls
This commit is contained in:
Daniel Agar 2021-08-25 10:57:21 -04:00
parent c6ab4c466e
commit 258f558fea
12 changed files with 29 additions and 226 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/differential_pressure/ets/ets_airspeed.cpp
View File

@ -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.

4
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
View File

@ -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();

4
src/drivers/differential_pressure/ms5525/MS5525.cpp
View File

@ -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();

4
src/drivers/differential_pressure/sdp3x/SDP3X.cpp
View File

@ -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();

72
src/drivers/drv_airspeed.h
View File

@ -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 <stdint.h>
#include <sys/ioctl.h>
#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 */

1
src/drivers/uavcan/sensors/airspeed.hpp
View File

@ -38,7 +38,6 @@
#pragma once
#include "sensor_bridge.hpp"
#include <drivers/drv_airspeed.h>
#include <uORB/topics/airspeed.h>
#include <uavcan/equipment/air_data/IndicatedAirspeed.hpp>

8
src/drivers/uavcan/sensors/differential_pressure.cpp
View File

@ -37,7 +37,6 @@
#include "differential_pressure.hpp"
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <parameters/param.h>
@ -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();

5
src/drivers/uavcan/sensors/differential_pressure.hpp
View File

@ -37,9 +37,8 @@
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <uORB/topics/differential_pressure.h>
#include "sensor_bridge.hpp"
@ -57,8 +56,6 @@ public:
int init() override;
private:
float _diff_pres_offset{0.f};
math::LowPassFilter2p<float> _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
void air_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);

29
src/lib/drivers/airspeed/airspeed.cpp
View File

@ -47,10 +47,8 @@
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <drivers/airspeed/airspeed.h>
@ -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);
}
}

8
src/lib/drivers/airspeed/airspeed.h
View File

@ -35,7 +35,6 @@
#include <string.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
@ -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<differential_pressure_s> _airspeed_pub{ORB_ID(differential_pressure)};
int _airspeed_orb_class_instance;
int _class_instance;
unsigned _conversion_interval;
perf_counter_t _sample_perf;

87
src/modules/commander/airspeed_calibration.cpp
View File

@ -49,7 +49,7 @@
#include <fcntl.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
@ -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<differential_pressure_s> 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);

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

@ -42,8 +42,8 @@
*/
#include <drivers/drv_adc.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_sensor.h>
#include <lib/airspeed/airspeed.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
@ -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;