fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: move differential pressure aggregation to sensors/airspeed WorkItem

Browse Source
This commit is contained in:
Daniel Agar 2020-09-29 11:31:26 -04:00
parent d65d06f82d
commit dfb342bdac
9 changed files with 678 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/airspeed.msg
View File

@ -1,9 +1,10 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
float32 indicated_airspeed_m_s # indicated airspeed in m/s
float32 true_airspeed_m_s # true filtered airspeed in m/s
float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown
float32 air_temperature_celsius # air temperature in degrees celsius, NAN if unknown
float32 confidence # confidence value from 0 to 1 for this sensor

1
src/lib/airspeed/airspeed.cpp
View File

@ -183,7 +183,6 @@ float calc_IAS(float differential_pressure)
} else {
return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
}
float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float temperature_celsius)

2
src/modules/sensors/CMakeLists.txt
View File

@ -34,6 +34,7 @@
add_subdirectory(data_validator)
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_subdirectory(airspeed)
add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_air_data)
@ -55,6 +56,7 @@ px4_add_module(
git_ecl
mathlib
sensor_calibration
sensors_airspeed
vehicle_acceleration
vehicle_angular_velocity
vehicle_air_data

352
src/modules/sensors/airspeed/Airspeed.cpp Normal file
View File

@ -0,0 +1,352 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#include "Airspeed.hpp"
#include <px4_platform_common/log.h>
#include <drivers/drv_airspeed.h>
#include <lib/ecl/geo/geo.h>
#include <lib/airspeed/airspeed.h>
namespace sensors
{
using namespace matrix;
using namespace time_literals;
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
/**
* HACK - true temperature is much less than indicated temperature in baro,
* subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
*/
#define PCB_TEMP_ESTIMATE_DEG 5.0f
Airspeed::Airspeed() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_voter.set_timeout(SENSOR_TIMEOUT);
_voter.set_equal_value_threshold(100);
ParametersUpdate(true);
}
Airspeed::~Airspeed()
{
Stop();
perf_free(_cycle_perf);
}
bool Airspeed::Start()
{
ScheduleNow();
return true;
}
void Airspeed::Stop()
{
Deinit();
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
}
void Airspeed::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_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 = {
_param_sens_dpres_off.get(),
1.0f,
};
if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
PX4_ERR("failed to set offset for differential pressure sensor");
}
px4_close(fd);
}
}
}
void Airspeed::Run()
{
perf_begin(_cycle_perf);
ParametersUpdate();
if (_vehicle_air_data_sub.updated()) {
vehicle_air_data_s air_data;
if (_vehicle_air_data_sub.copy(&air_data)) {
if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
&& (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
_baro_air_temperature = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
}
}
}
bool updated[MAX_SENSOR_COUNT] {};
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
if (!_advertised[uorb_index]) {
// use data's timestamp to throttle advertisement checks
if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) {
if (_sensor_sub[uorb_index].advertised()) {
if (uorb_index > 0) {
/* the first always exists, but for each further sensor, add a new validator */
if (!_voter.add_new_validator()) {
PX4_ERR("failed to add validator for %s %i", "DPRES", uorb_index);
}
}
_advertised[uorb_index] = true;
// advertise outputs in order if publishing all
if (!_param_sens_dpres_mode.get()) {
for (int instance = 0; instance < uorb_index; instance++) {
_airspeed_multi_pub[instance].advertise();
}
}
if (_selected_sensor_sub_index < 0) {
_sensor_sub[uorb_index].registerCallback();
}
} else {
_last_data[uorb_index].timestamp = hrt_absolute_time();
}
}
}
if (_advertised[uorb_index]) {
differential_pressure_s diff_pres;
while (_sensor_sub[uorb_index].update(&diff_pres)) {
updated[uorb_index] = true;
_device_id[uorb_index] = diff_pres.device_id;
float vect[3] {diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.f};
_voter.put(uorb_index, diff_pres.timestamp, vect, diff_pres.error_count, _priority[uorb_index]);
float air_temperature_celsius = NAN;
// assume anything outside of a (generous) operating range of -40C to 125C is invalid
if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
air_temperature_celsius = diff_pres.temperature;
} else {
// differential pressure temperature invalid, use barometer temperature if available
if (PX4_ISFINITE(_baro_air_temperature)) {
air_temperature_celsius = _baro_air_temperature;
}
}
// average raw data for all instances
_timestamp_sample_sum[uorb_index] += diff_pres.timestamp;
_differential_pressure_sum[uorb_index] += diff_pres.differential_pressure_filtered_pa;
_temperature_sum[uorb_index] += air_temperature_celsius;
_sum_count[uorb_index]++;
}
}
}
// check for the current best sensor
int best_index = 0;
_voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
if (_selected_sensor_sub_index != best_index) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
if (_param_sens_dpres_mode.get()) {
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("%s switch from #%u -> #%d", "DPRES", _selected_sensor_sub_index, best_index);
}
}
_selected_sensor_sub_index = best_index;
_sensor_sub[_selected_sensor_sub_index].registerCallback();
}
}
// Publish
if (_param_sens_dpres_mode.get()) {
// publish only best sensor
if ((_selected_sensor_sub_index >= 0)
&& (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
&& updated[_selected_sensor_sub_index]) {
Publish(_selected_sensor_sub_index);
}
} else {
// publish all
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
// publish all sensors as separate instances
if (updated[uorb_index] && (_device_id[uorb_index] != 0)) {
Publish(uorb_index, true);
}
}
}
// check failover and report
if (_param_sens_dpres_mode.get()) {
if (_last_failover_count != _voter.failover_count()) {
uint32_t flags = _voter.failover_state();
int failover_index = _voter.failover_index();
if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
if (failover_index != -1) {
const hrt_abstime now = hrt_absolute_time();
if (now - _last_error_message > 3_s) {
mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!",
"DPRES",
failover_index,
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
_last_error_message = now;
}
// reduce priority of failed sensor to the minimum
_priority[failover_index] = 1;
}
}
_last_failover_count = _voter.failover_count();
}
}
// reschedule timeout
ScheduleDelayed(100_ms);
perf_end(_cycle_perf);
}
void Airspeed::Publish(uint8_t instance, bool multi)
{
if ((_param_sens_dpres_rate.get() > 0)
&& hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_dpres_rate.get())) {
const float differential_pressure = _differential_pressure_sum[instance] / _sum_count[instance];
const float temperature = _temperature_sum[instance] / _sum_count[instance];
const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _sum_count[instance];
// reset
_timestamp_sample_sum[instance] = 0;
_differential_pressure_sum[instance] = 0;
_temperature_sum[instance] = 0;
_sum_count[instance] = 0;
airspeed_s out{};
out.timestamp_sample = timestamp_sample;
out.air_temperature_celsius = temperature;
out.confidence = 1.f; // TODO
switch ((_device_id[instance] >> 16) & 0xFF) {
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
// fallthrough
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
out.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) _param_cal_air_cmodel.get(),
AIRSPEED_SENSOR_MODEL_SDP3X, _param_cal_air_tubelen.get(), _param_cal_air_tubed_mm.get(),
differential_pressure, _baro_pressure_pa, temperature);
break;
default:
out.indicated_airspeed_m_s = calc_IAS(differential_pressure);
break;
}
// assume that CAS = IAS as we don't have an CAS-scale here
out.true_airspeed_m_s = calc_TAS_from_CAS(out.indicated_airspeed_m_s, _baro_pressure_pa, temperature);
if (PX4_ISFINITE(out.indicated_airspeed_m_s) && PX4_ISFINITE(out.true_airspeed_m_s)) {
out.timestamp = hrt_absolute_time();
if (multi) {
_airspeed_multi_pub[instance].publish(out);
} else {
// otherwise only ever publish the first instance
_airspeed_multi_pub[0].publish(out);
}
_last_publication_timestamp[instance] = out.timestamp;
}
}
}
void Airspeed::PrintStatus()
{
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("selected differential pressure: %d (%d)", _device_id[_selected_sensor_sub_index], _selected_sensor_sub_index);
}
_voter.print();
}
}; // namespace sensors

131
src/modules/sensors/airspeed/Airspeed.hpp Normal file
View File

@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#pragma once
#include "data_validator/DataValidatorGroup.hpp"
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_air_data.h>
using namespace time_literals;
namespace sensors
{
class Airspeed : public ModuleParams, public px4::ScheduledWorkItem
{
public:
Airspeed();
~Airspeed() override;
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate(bool force = false);
void Publish(uint8_t instance, bool multi = false);
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::PublicationMulti<airspeed_s> _airspeed_multi_pub[MAX_SENSOR_COUNT] {
{ORB_ID(airspeed)},
{ORB_ID(airspeed)},
{ORB_ID(airspeed)},
{ORB_ID(airspeed)},
};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(differential_pressure), 0},
{this, ORB_ID(differential_pressure), 1},
{this, ORB_ID(differential_pressure), 2},
{this, ORB_ID(differential_pressure), 3},
};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
DataValidatorGroup _voter{1};
unsigned _last_failover_count{0};
uint32_t _device_id[MAX_SENSOR_COUNT] {};
uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
float _differential_pressure_sum[MAX_SENSOR_COUNT] {};
float _temperature_sum[MAX_SENSOR_COUNT] {};
int _sum_count[MAX_SENSOR_COUNT] {};
differential_pressure_s _last_data[MAX_SENSOR_COUNT] {};
bool _advertised[MAX_SENSOR_COUNT] {};
uint8_t _priority[MAX_SENSOR_COUNT] {100, 100, 100, 100};
int8_t _selected_sensor_sub_index{-1};
float _baro_pressure_pa{101325.f}; // Sea level standard atmospheric pressure
float _baro_air_temperature{15.f};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_DPRES_MODE>) _param_sens_dpres_mode,
(ParamFloat<px4::params::SENS_DPRES_OFF>) _param_sens_dpres_off,
(ParamFloat<px4::params::SENS_DPRES_RATE>) _param_sens_dpres_rate,
(ParamInt<px4::params::CAL_AIR_CMODEL>) _param_cal_air_cmodel,
(ParamFloat<px4::params::CAL_AIR_TUBELEN>) _param_cal_air_tubelen,
(ParamFloat<px4::params::CAL_AIR_TUBED_MM>) _param_cal_air_tubed_mm
)
};
}; // namespace sensors

42
src/modules/sensors/airspeed/CMakeLists.txt Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2020 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.
#
############################################################################
px4_add_library(sensors_airspeed
Airspeed.cpp
Airspeed.hpp
)
target_link_libraries(sensors_airspeed
PRIVATE
airspeed
px4_work_queue
)

115
src/modules/sensors/airspeed/params.c Normal file
View File

@ -0,0 +1,115 @@
/****************************************************************************
*
* Copyright (c) 2012-2020 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.
*
****************************************************************************/
/**
* Airspeed sensor compensation model for the SDP3x
*
* Model with Pitot
* CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
* CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
* Model without Pitot (1.5 mm tubes)
* CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
* CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
* Tube Pressure Drop
* CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter.
* CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot.
*
* @value 0 Model with Pitot
* @value 1 Model without Pitot (1.5 mm tubes)
* @value 2 Tube Pressure Drop
*
* @group Sensors
*/
PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0);
/**
* Airspeed sensor tube length.
*
* See the CAL_AIR_CMODEL explanation on how this parameter should be set.
*
* @min 0.01
* @max 2.00
* @unit m
*
* @group Sensors
*/
PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f);
/**
* Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation.
*
* @min 0.1
* @max 100
* @unit mm
*
* @group Sensors
*/
PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f);
/**
* Differential pressure sensor offset
*
* The offset (zero-reading) in Pascal
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
/**
* Sensors hub differential pressure mode
*
* @value 0 Publish all airspeeds
* @value 1 Publish primary airspeed
*
* @category system
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_DPRES_MODE, 1);
/**
* Differential pressure (airspeed) max rate.
*
* Airspeed data maximum publication rate. This is an upper bound,
* actual differential pressure data rate is still dependant on the sensor.
*
* @min 1
* @max 100
* @group Sensors
* @unit Hz
*
* @reboot_required true
*
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_RATE, 10.0f);

58
src/modules/sensors/sensor_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2021 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
@ -31,62 +31,6 @@
*
****************************************************************************/
/**
* Airspeed sensor compensation model for the SDP3x
*
* Model with Pitot
* CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
* CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
* Model without Pitot (1.5 mm tubes)
* CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
* CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
* Tube Pressure Drop
* CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter.
* CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot.
*
* @value 0 Model with Pitot
* @value 1 Model without Pitot (1.5 mm tubes)
* @value 2 Tube Pressure Drop
*
* @group Sensors
*/
PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0);
/**
* Airspeed sensor tube length.
*
* See the CAL_AIR_CMODEL explanation on how this parameter should be set.
*
* @min 0.01
* @max 2.00
* @unit m
*
* @group Sensors
*/
PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f);
/**
* Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation.
*
* @min 0.1
* @max 100
* @unit mm
*
* @group Sensors
*/
PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f);
/**
* Differential pressure sensor offset
*
* The offset (zero-reading) in Pascal
*
* @category system
* @group Sensor Calibration
* @volatile
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
/**
* Differential pressure sensor analog scaling
*

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

@ -44,7 +44,6 @@
#include <drivers/drv_adc.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <lib/airspeed/airspeed.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@ -61,7 +60,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensors_status_imu.h>
@ -70,6 +68,7 @@
#include <uORB/topics/vehicle_imu.h>
#include "voted_sensors_update.h"
#include "airspeed/Airspeed.hpp"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_air_data/VehicleAirData.hpp"
@ -80,11 +79,6 @@
using namespace sensors;
using namespace time_literals;
/**
* HACK - true temperature is much less than indicated temperature in baro,
* subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
*/
#define PCB_TEMP_ESTIMATE_DEG 5.0f
class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@ -126,17 +120,12 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
perf_counter_t _loop_perf; /**< loop performance counter */
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */
@ -153,9 +142,6 @@ private:
float diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
int32_t air_cmodel;
float air_tube_length;
float air_tube_diameter_mm;
} _parameters{}; /**< local copies of interesting parameters */
struct ParameterHandles {
@ -164,15 +150,14 @@ private:
param_t diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_t air_cmodel;
param_t air_tube_length;
param_t air_tube_diameter_mm;
} _parameter_handles{}; /**< handles for interesting parameters */
VotedSensorsUpdate _voted_sensors_update;
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
Airspeed *_airspeed{nullptr};
VehicleAirData *_vehicle_air_data{nullptr};
VehicleMagnetometer *_vehicle_magnetometer{nullptr};
VehicleGPSPosition *_vehicle_gps_position{nullptr};
@ -184,14 +169,6 @@ private:
*/
int parameters_update();
/**
* Poll the differential pressure sensor for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void diff_pres_poll();
/**
* Check for changes in parameters.
*/
@ -205,6 +182,7 @@ private:
*/
void adc_poll();
void InitializeAirspeed();
void InitializeVehicleAirData();
void InitializeVehicleGPSPosition();
void InitializeVehicleIMU();
@ -230,10 +208,6 @@ Sensors::Sensors(bool hil_enabled) :
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
_parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
param_find("SYS_FAC_CAL_MODE");
// Parameters controlling the on-board sensor thermal calibrator
@ -241,9 +215,6 @@ Sensors::Sensors(bool hil_enabled) :
param_find("SYS_CAL_TMAX");
param_find("SYS_CAL_TMIN");
_airspeed_validator.set_timeout(300000);
_airspeed_validator.set_equal_value_threshold(100);
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
}
@ -258,6 +229,11 @@ Sensors::~Sensors()
_vehicle_acceleration.Stop();
_vehicle_angular_velocity.Stop();
if (_airspeed) {
_airspeed->Stop();
delete _airspeed;
}
if (_vehicle_air_data) {
_vehicle_air_data->Stop();
delete _vehicle_air_data;
@ -302,88 +278,11 @@ int Sensors::parameters_update()
param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel);
param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length);
param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);
_voted_sensors_update.parametersUpdate();
return PX4_OK;
}
void Sensors::diff_pres_poll()
{
differential_pressure_s diff_pres{};
if (_diff_pres_sub.update(&diff_pres)) {
vehicle_air_data_s air_data{};
_vehicle_air_data_sub.copy(&air_data);
float air_temperature_celsius = NAN;
// assume anything outside of a (generous) operating range of -40C to 125C is invalid
if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
air_temperature_celsius = diff_pres.temperature;
} else {
// differential pressure temperature invalid, check barometer
if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
&& (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
}
}
airspeed_s airspeed{};
airspeed.timestamp = diff_pres.timestamp;
/* push data into validator */
float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
enum AIRSPEED_SENSOR_MODEL smodel;
switch ((diff_pres.device_id >> 16) & 0xFF) {
case DRV_DIFF_PRESS_DEVTYPE_SDP31:
/* fallthrough */
case DRV_DIFF_PRESS_DEVTYPE_SDP32:
/* fallthrough */
case DRV_DIFF_PRESS_DEVTYPE_SDP33:
/* fallthrough */
smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
break;
default:
smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
break;
}
/* don't risk to feed negative airspeed into the system */
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,
air_temperature_celsius);
airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
airspeed.air_temperature_celsius = air_temperature_celsius;
if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) {
_airspeed_pub.publish(airspeed);
}
}
}
void
Sensors::parameter_update_poll(bool forced)
{
@ -396,23 +295,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);
}
}
}
@ -427,7 +309,7 @@ void Sensors::adc_poll()
if (_parameters.diff_pres_analog_scale > 0.0f) {
hrt_abstime t = hrt_absolute_time();
const hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (t - _last_adc >= 10000) {
@ -456,11 +338,12 @@ void Sensors::adc_poll()
if (voltage > 0.4f) {
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
(diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
_diff_pres.temperature = NAN;
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres_pub.publish(_diff_pres);
}
@ -475,6 +358,19 @@ void Sensors::adc_poll()
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
}
void Sensors::InitializeAirspeed()
{
if (_airspeed == nullptr) {
if (orb_exists(ORB_ID(differential_pressure), 0) == PX4_OK) {
_airspeed = new Airspeed();
if (_airspeed) {
_airspeed->Start();
}
}
}
}
void Sensors::InitializeVehicleAirData()
{
if (_param_sys_has_baro.get()) {
@ -572,6 +468,7 @@ void Sensors::Run()
// run once
if (_last_config_update == 0) {
InitializeAirspeed();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
@ -599,8 +496,6 @@ void Sensors::Run()
// check analog airspeed
adc_poll();
diff_pres_poll();
if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
@ -612,6 +507,7 @@ void Sensors::Run()
// when not adding sensors poll for param updates
if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) {
_voted_sensors_update.initializeSensors();
InitializeAirspeed();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
@ -691,9 +587,10 @@ int Sensors::print_status()
_vehicle_air_data->PrintStatus();
}
PX4_INFO_RAW("\n");
PX4_INFO("Airspeed status:");
_airspeed_validator.print();
if (_airspeed) {
PX4_INFO_RAW("\n");
_airspeed->PrintStatus();
}
PX4_INFO_RAW("\n");
_vehicle_acceleration.PrintStatus();