Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

sensors: throttle vehicle_air_data publication (SENS_BARO_RATE)

Browse Source 
- don't bother running baro aggregator if SYS_HAS_BARO disabled
This commit is contained in:
Daniel Agar
2020-07-12 10:46:25 -04:00
parent 67ae40e922
commit 19059a80bd
4 changed files with 109 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+36 -11
View File
@@ -168,7 +168,7 @@ private:
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAirData _vehicle_air_data;
VehicleAirData *_vehicle_air_data{nullptr};
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -200,8 +200,12 @@ private:
*/
void adc_poll();
void InitializeVehicleAirData();
void InitializeVehicleIMU();
DEFINE_PARAMETERS(
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
)
};
Sensors::Sensors(bool hil_enabled) :
@@ -218,9 +222,6 @@ Sensors::Sensors(bool hil_enabled) :
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
_vehicle_air_data.Start();
InitializeVehicleIMU();
}
Sensors::~Sensors()
@@ -232,11 +233,16 @@ Sensors::~Sensors()
_vehicle_acceleration.Stop();
_vehicle_angular_velocity.Stop();
_vehicle_air_data.Stop();
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
i->Stop();
if (_vehicle_air_data) {
_vehicle_air_data->Stop();
delete _vehicle_air_data;
}
for (auto &vehicle_imu : _vehicle_imu_list) {
if (vehicle_imu) {
vehicle_imu->Stop();
delete vehicle_imu;
}
}
@@ -418,6 +424,21 @@ void Sensors::adc_poll()
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
}
void Sensors::InitializeVehicleAirData()
{
if (_param_sys_has_baro.get()) {
if (_vehicle_air_data == nullptr) {
if (orb_exists(ORB_ID(sensor_baro), 0) == PX4_OK) {
_vehicle_air_data = new VehicleAirData();
if (_vehicle_air_data) {
_vehicle_air_data->Start();
}
}
}
}
}
void Sensors::InitializeVehicleIMU()
{
// create a VehicleIMU instance for each accel/gyro pair
@@ -439,7 +460,6 @@ void Sensors::InitializeVehicleIMU()
// Start VehicleIMU instance and store
if (imu->Start()) {
_vehicle_imu_list[i] = imu;
ScheduleNow();
} else {
delete imu;
@@ -468,6 +488,8 @@ void Sensors::Run()
// run once
if (_last_config_update == 0) {
InitializeVehicleAirData();
InitializeVehicleIMU();
_voted_sensors_update.init(_sensor_combined);
parameter_update_poll(true);
}
@@ -555,6 +577,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();
InitializeVehicleAirData();
InitializeVehicleIMU();
_last_config_update = hrt_absolute_time();
@@ -621,8 +644,10 @@ int Sensors::print_status()
{
_voted_sensors_update.printStatus();
PX4_INFO_RAW("\n");
_vehicle_air_data.PrintStatus();
if (_vehicle_air_data) {
PX4_INFO_RAW("\n");
_vehicle_air_data->PrintStatus();
}
PX4_INFO_RAW("\n");
PX4_INFO("Airspeed status:");
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
+50 -32
View File
@@ -200,46 +200,64 @@ void VehicleAirData::Run()
const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
// populate vehicle_air_data with primary baro and publish
vehicle_air_data_s out{};
out.timestamp_sample = baro.timestamp; // TODO: baro.timestamp_sample;
out.baro_device_id = baro.device_id;
out.baro_temp_celcius = baro.temperature;
_baro_timestamp_sum += baro.timestamp;
_baro_sum += baro.pressure;
_baro_sum_count++;
// Convert from millibar to Pa and apply temperature compensation
out.baro_pressure_pa = 100.0f * baro.pressure - _thermal_offset[_selected_sensor_sub_index];
if ((_param_sens_baro_rate.get() > 0)
&& hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())) {
// calculate altitude using the hypsometric equation
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
const float pressure = _baro_sum / _baro_sum_count;
const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count;
// current pressure at MSL in kPa (QNH in hPa)
const float p1 = _param_sens_baro_qnh.get() * 0.1f;
// reset
_baro_timestamp_sum = 0;
_baro_sum = 0.f;
_baro_sum_count = 0;
// measured pressure in kPa
const float p = out.baro_pressure_pa * 0.001f;
// populate vehicle_air_data with primary baro and publish
vehicle_air_data_s out{};
out.timestamp_sample = timestamp_sample; // TODO: baro.timestamp_sample;
out.baro_device_id = baro.device_id;
out.baro_temp_celcius = baro.temperature;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
// Convert from millibar to Pa and apply temperature compensation
out.baro_pressure_pa = 100.0f * pressure - _thermal_offset[_selected_sensor_sub_index];
// 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));
// calculate altitude using the hypsometric equation
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
out.rho = pressure_to_density * out.baro_pressure_pa;
// current pressure at MSL in kPa (QNH in hPa)
const float p1 = _param_sens_baro_qnh.get() * 0.1f;
out.timestamp = hrt_absolute_time();
_vehicle_air_data_pub.publish(out);
// measured pressure in kPa
const float p = out.baro_pressure_pa * 0.001f;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
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.timestamp = hrt_absolute_time();
_vehicle_air_data_pub.publish(out);
_last_publication_timestamp = out.timestamp;
}
}
// check failover and report
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
+7 -1
View File
@@ -86,12 +86,17 @@ private:
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _last_publication_timestamp{0};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
DataValidatorGroup _voter{1};
unsigned _last_failover_count{0};
uint64_t _baro_timestamp_sum{0};
float _baro_sum{0.f};
int _baro_sum_count{0};
sensor_baro_s _last_data[MAX_SENSOR_COUNT] {};
bool _advertised[MAX_SENSOR_COUNT] {};
@@ -102,7 +107,8 @@ private:
int8_t _selected_sensor_sub_index{-1};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh
(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate
)
};
}; // namespace sensors
src/modules/sensors/vehicle_air_data/params.c
+16
View File
@@ -43,3 +43,19 @@
*
*/
PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
/**
* Baro max rate.
*
* Barometric air data maximum publication rate. This is an upper bound,
* actual barometric data rate is still dependant on the sensor.
*
* @min 1
* @max 200
* @group Sensors
* @unit Hz
*
* @reboot_required true
*
*/
PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f);