/**************************************************************************** * * 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 * 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 "VehicleAirData.hpp" #include #include #include namespace sensors { using namespace matrix; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; VehicleAirData::VehicleAirData() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { _voter.set_timeout(SENSOR_TIMEOUT); } VehicleAirData::~VehicleAirData() { Stop(); perf_free(_cycle_perf); } bool VehicleAirData::Start() { ScheduleNow(); return true; } void VehicleAirData::Stop() { Deinit(); // clear all registered callbacks for (auto &sub : _sensor_sub) { sub.unregisterCallback(); } } void VehicleAirData::SensorCorrectionsUpdate(bool force) { if (_sensor_correction_sub.updated() || force) { sensor_correction_s corrections; if (_sensor_correction_sub.copy(&corrections)) { for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) { // find sensor (by device id) in sensor_correction const uint32_t device_id = _last_data[baro_index].device_id; if (device_id != 0) { for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) { if (corrections.baro_device_ids[correction_index] == device_id) { switch (correction_index) { case 0: _thermal_offset[baro_index] = corrections.baro_offset_0; break; case 1: _thermal_offset[baro_index] = corrections.baro_offset_1; break; case 2: _thermal_offset[baro_index] = corrections.baro_offset_2; break; case 3: _thermal_offset[baro_index] = corrections.baro_offset_3; break; } } } } } } } } 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 if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); updateParams(); } } void VehicleAirData::Run() { perf_begin(_cycle_perf); ParametersUpdate(); SensorCorrectionsUpdate(); AirTemperatureUpdate(); 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", "BARO", uorb_index); } } _advertised[uorb_index] = true; // force temperature correction update SensorCorrectionsUpdate(true); if (_selected_sensor_sub_index < 0) { _sensor_sub[uorb_index].registerCallback(); } } else { _last_data[uorb_index].timestamp = hrt_absolute_time(); } } } if (_advertised[uorb_index]) { updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]); if (updated[uorb_index]) { // millibar to Pa const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f; // pressure corrected with offset (if available) const float pressure_corrected = (raw_pressure_pascals - _thermal_offset[uorb_index]); float vect[3] {pressure_corrected, _last_data[uorb_index].temperature, 0.f}; _voter.put(uorb_index, _last_data[uorb_index].timestamp, vect, _last_data[uorb_index].error_count, _priority[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 (_selected_sensor_sub_index >= 0) { PX4_INFO("%s switch from #%" PRIu8 " -> #%d", "BARO", _selected_sensor_sub_index, best_index); } _selected_sensor_sub_index = best_index; _sensor_sub[_selected_sensor_sub_index].registerCallback(); } } if ((_selected_sensor_sub_index >= 0) && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR) && updated[_selected_sensor_sub_index]) { const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index]; _baro_timestamp_sum += baro.timestamp_sample; _baro_sum += baro.pressure; _baro_sum_count++; if ((_param_sens_baro_rate.get() > 0) && ((_last_publication_timestamp == 0) || (hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())))) { const float pressure = _baro_sum / _baro_sum_count; const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count; // reset _baro_timestamp_sum = 0; _baro_sum = 0.f; _baro_sum_count = 0; // populate vehicle_air_data with primary baro and publish vehicle_air_data_s out{}; out.timestamp_sample = timestamp_sample; out.baro_device_id = baro.device_id; out.baro_temp_celcius = baro.temperature; // Convert from millibar to Pa and apply temperature compensation out.baro_pressure_pa = 100.0f * pressure - _thermal_offset[_selected_sensor_sub_index]; // 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 // current pressure at MSL in kPa (QNH in hPa) const float p1 = _param_sens_baro_qnh.get() * 0.1f; // 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 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); _last_publication_timestamp = out.timestamp; } } // check failover and report 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!\t", "BARO", 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" : "")); events::px4::enums::sensor_failover_reason_t failover_reason{}; if (flags & DataValidator::ERROR_FLAG_NO_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::no_data; } if (flags & DataValidator::ERROR_FLAG_STALE_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::stale_data; } if (flags & DataValidator::ERROR_FLAG_TIMEOUT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::timeout; } if (flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_count; } if (flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_density; } /* EVENT * @description * Land immediately and check the system. */ events::send( events::ID("sensor_failover_baro"), events::Log::Emergency, "Baro sensor #{1} failure: {2}", failover_index, failover_reason); _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 VehicleAirData::PrintStatus() { if (_selected_sensor_sub_index >= 0) { PX4_INFO("selected barometer: %" PRIu32 " (%" PRId8 ")", _last_data[_selected_sensor_sub_index].device_id, _selected_sensor_sub_index); if (fabsf(_thermal_offset[_selected_sensor_sub_index]) > 0.f) { PX4_INFO("%" PRIu32 " temperature offset: %.4f", _last_data[_selected_sensor_sub_index].device_id, (double)_thermal_offset[_selected_sensor_sub_index]); } } _voter.print(); } }; // namespace sensors