yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Sensors: subscribe to differential_pressure in vehicle_air_data to calculate air density

Browse Source 
-constrain range to -20..35°C
-do not use temperature readings if exactly 0 (as likley not filled by driver at all in this case)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-05-20 11:42:51 +02:00
parent 973068ebfe
commit cf5e6e4133
2 changed files with 27 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@@ -110,6 +110,22 @@ void VehicleAirData::SensorCorrectionsUpdate(bool force)
}
}
void VehicleAirData::AirTemperatureUpdate()
{
differential_pressure_s differential_pressure;
static constexpr float temperature_min_celsius = -20.f;
static constexpr float temperature_max_celsius = 35.f;
// update air temperature if data from differential pressure sensor is finite and not exactly 0
// limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight
if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature)
&& fabsf(differential_pressure.temperature) > FLT_EPSILON) {
_air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius,
temperature_max_celsius);
}
}
void VehicleAirData::ParametersUpdate()
{
// Check if parameters have changed
@@ -130,6 +146,8 @@ void VehicleAirData::Run()
SensorCorrectionsUpdate();
AirTemperatureUpdate();
bool updated[MAX_SENSOR_COUNT] {};
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
@@ -250,12 +268,8 @@ void VehicleAirData::Run()
out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
// calculate air density
// estimate air density assuming typical 20degC ambient temperature
// TODO: use air temperature if available (differential pressure sensors)
static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
out.rho = pressure_to_density * out.baro_pressure_pa;
out.rho = out.baro_pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius -
CONSTANTS_ABSOLUTE_NULL_CELSIUS));
out.timestamp = hrt_absolute_time();
_vehicle_air_data_pub.publish(out);

7
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@@ -46,6 +46,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_correction.h>
@@ -72,6 +73,7 @@ private:
void ParametersUpdate();
void SensorCorrectionsUpdate(bool force = false);
void AirTemperatureUpdate();
static constexpr int MAX_SENSOR_COUNT = 4;
@@ -80,6 +82,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(sensor_baro), 0},
@@ -110,6 +113,8 @@ private:
int8_t _selected_sensor_sub_index{-1};
float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate