From 0eca583ecf61f1c73a826f128a5157c13d6fa591 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jan 2020 23:28:30 -0500 Subject: [PATCH] sensors: move baro aggregation to new sensors/vehicle_air_data --- ROMFS/px4fmu_common/init.d-posix/rcS | 2 - msg/sensor_selection.msg | 1 - msg/vehicle_air_data.msg | 7 +- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 23 +- src/modules/commander/CMakeLists.txt | 1 - src/modules/commander/Commander.cpp | 1 - src/modules/commander/baro_calibration.cpp | 54 ---- src/modules/ekf2/ekf2_main.cpp | 2 +- src/modules/sensors/CMakeLists.txt | 2 + src/modules/sensors/common.h | 6 +- src/modules/sensors/parameters.cpp | 5 - src/modules/sensors/parameters.h | 4 - src/modules/sensors/sensor_params.c | 21 -- src/modules/sensors/sensors.cpp | 38 ++- .../sensors/vehicle_air_data/CMakeLists.txt | 38 +++ .../vehicle_air_data/VehicleAirData.cpp | 283 ++++++++++++++++++ .../vehicle_air_data/VehicleAirData.hpp | 105 +++++++ .../vehicle_air_data/params.c} | 24 +- src/modules/sensors/voted_sensors_update.cpp | 112 +------ src/modules/sensors/voted_sensors_update.h | 15 +- src/modules/uORB/Subscription.hpp | 1 + src/modules/uORB/SubscriptionInterval.hpp | 1 + src/modules/uORB/uORB.h | 3 +- src/modules/uORB/uORBDeviceMaster.cpp | 2 +- src/modules/uORB/uORBDeviceNode.hpp | 4 +- 25 files changed, 479 insertions(+), 276 deletions(-) delete mode 100644 src/modules/commander/baro_calibration.cpp create mode 100644 src/modules/sensors/vehicle_air_data/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_air_data/VehicleAirData.cpp create mode 100644 src/modules/sensors/vehicle_air_data/VehicleAirData.hpp rename src/modules/{commander/baro_calibration.h => sensors/vehicle_air_data/params.c} (83%) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index ca47b9939e..e723ee355b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -128,8 +128,6 @@ then param set CAL_MAG0_ID 197388 param set CAL_MAG_PRIME 197388 - param set CAL_BARO_PRIME 6620172 - param set CBRK_AIRSPD_CHK 0 param set COM_DISARM_LAND 0.5 diff --git a/msg/sensor_selection.msg b/msg/sensor_selection.msg index 1ccde7895a..08e75122b4 100644 --- a/msg/sensor_selection.msg +++ b/msg/sensor_selection.msg @@ -4,6 +4,5 @@ # uint64 timestamp # time since system start (microseconds) uint32 accel_device_id # unique device ID for the selected accelerometers -uint32 baro_device_id # unique device ID for the selected barometer uint32 gyro_device_id # unique device ID for the selected rate gyros uint32 mag_device_id # unique device ID for the selected magnetometer diff --git a/msg/vehicle_air_data.msg b/msg/vehicle_air_data.msg index 4b8e5988de..fd99236613 100644 --- a/msg/vehicle_air_data.msg +++ b/msg/vehicle_air_data.msg @@ -1,4 +1,9 @@ -uint64 timestamp # time since system start (microseconds) + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 baro_device_id # unique device ID for the selected barometer float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. float32 baro_temp_celcius # Temperature in degrees celsius diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 1e377f2a84..8e9bba4037 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -221,11 +221,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- BARO ---- */ if (checkSensors) { - //bool prime_found = false; - - int32_t prime_id = -1; - param_get(param_find("CAL_BARO_PRIME"), &prime_id); - int32_t sys_has_baro = 1; param_get(param_find("SYS_HAS_BARO"), &sys_has_baro); @@ -238,29 +233,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t device_id = -1; - if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { - if ((prime_id > 0) && (device_id == prime_id)) { - //prime_found = true; - } - - } else { + if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { if (required) { failed = true; baro_fail_reported = true; } } } - - // TODO there is no logic in place to calibrate the primary baro yet - // // check if the primary device is present - // if (false) { - // if (reportFailures && !failed) { - // mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational"); - // } - - // set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status); - // failed = true; - // } } /* ---- IMU CONSISTENCY ---- */ diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index d0717378b7..ad128743b5 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -41,7 +41,6 @@ px4_add_module( SRCS accelerometer_calibration.cpp airspeed_calibration.cpp - baro_calibration.cpp calibration_routines.cpp Commander.cpp commander_helper.cpp diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c3ce6afdb8..5e9a78593c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -50,7 +50,6 @@ #include "Arming/HealthFlags/HealthFlags.h" #include "accelerometer_calibration.h" #include "airspeed_calibration.h" -#include "baro_calibration.h" #include "calibration_routines.h" #include "commander_helper.h" #include "esc_calibration.h" diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp deleted file mode 100644 index e86f9abd29..0000000000 --- a/src/modules/commander/baro_calibration.cpp +++ /dev/null @@ -1,54 +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 baro_calibration.cpp - * Barometer calibration routine - */ - -#include "baro_calibration.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -int do_baro_calibration(orb_advert_t *mavlink_log_pub) -{ - // TODO implement this - return PX4_ERROR; -} diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 1a173a175e..1f6cfe0491 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -911,7 +911,7 @@ void Ekf2::Run() if (_airdata_sub.copy(&airdata)) { _ekf.set_air_density(airdata.rho); - const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp}; + const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp_sample}; _ekf.setBaroData(baro_sample); ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 177af3dc9f..0b68a2c23d 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -35,6 +35,7 @@ add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_ve include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) +add_subdirectory(vehicle_air_data) add_subdirectory(vehicle_imu) px4_add_module( @@ -54,5 +55,6 @@ px4_add_module( mathlib vehicle_acceleration vehicle_angular_velocity + vehicle_air_data vehicle_imu ) diff --git a/src/modules/sensors/common.h b/src/modules/sensors/common.h index 93e8589716..bb6e5a31a3 100644 --- a/src/modules/sensors/common.h +++ b/src/modules/sensors/common.h @@ -46,11 +46,7 @@ namespace sensors constexpr uint8_t MAG_COUNT_MAX = 4; constexpr uint8_t GYRO_COUNT_MAX = 3; constexpr uint8_t ACCEL_COUNT_MAX = 3; -constexpr uint8_t BARO_COUNT_MAX = 3; -constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, - math::max(GYRO_COUNT_MAX, - math::max(ACCEL_COUNT_MAX, - BARO_COUNT_MAX))); +constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX)); } /* namespace sensors */ diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 4d32966bb9..7d29a3035d 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -58,9 +58,6 @@ void initialize_parameter_handles(ParameterHandles ¶meter_handles) parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); - /* Barometer QNH */ - parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); - 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"); @@ -107,8 +104,6 @@ void update_parameters(const ParameterHandles ¶meter_handles, Parameters &pa param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1])); param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2])); - param_get(parameter_handles.baro_qnh, &(parameters.baro_qnh)); - param_get(parameter_handles.air_cmodel, ¶meters.air_cmodel); param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length); param_get(parameter_handles.air_tube_diameter_mm, ¶meters.air_tube_diameter_mm); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index eb821cd93e..f2870e3c2a 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -56,8 +56,6 @@ struct Parameters { float board_offset[3]; - float baro_qnh; - int32_t air_cmodel; float air_tube_length; float air_tube_diameter_mm; @@ -73,8 +71,6 @@ struct ParameterHandles { param_t board_offset[3]; - param_t baro_qnh; - param_t air_cmodel; param_t air_tube_length; param_t air_tube_diameter_mm; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index de08c78363..8e036e348d 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -31,14 +31,6 @@ * ****************************************************************************/ -/** - * Primary baro ID - * - * @category system - * @group Sensor Calibration - */ -PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0); - /** * Airspeed sensor compensation model for the SDP3x * @@ -108,19 +100,6 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); -/** - * QNH for barometer - * - * @min 500 - * @max 1500 - * @group Sensors - * @unit hPa - * - * @reboot_required true - * - */ -PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); - /** * Board rotation * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index eee6488aa5..cfe413967a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -74,6 +74,7 @@ #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" +#include "vehicle_air_data/VehicleAirData.hpp" #include "vehicle_imu/VehicleIMU.hpp" using namespace sensors; @@ -112,7 +113,6 @@ private: bool _armed{false}; /**< arming status of the vehicle */ hrt_abstime _last_config_update{0}; - hrt_abstime _airdata_prev_timestamp{0}; hrt_abstime _sensor_combined_prev_timestamp{0}; hrt_abstime _magnetometer_prev_timestamp{0}; @@ -123,11 +123,11 @@ private: uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */ uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */ uORB::Publication _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */ - uORB::Publication _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */ uORB::Publication _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */ uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub[GYRO_COUNT_MAX] { @@ -159,6 +159,7 @@ private: VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; + VehicleAirData _vehicle_air_data; static constexpr int MAX_SENSOR_COUNT = 3; VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; @@ -175,7 +176,7 @@ private: * @param raw Combined sensor data structure into which * data should be returned. */ - void diff_pres_poll(const vehicle_air_data_s &airdata); + void diff_pres_poll(); /** * Check for changes in parameters. @@ -213,6 +214,7 @@ Sensors::Sensors(bool hil_enabled) : _vehicle_acceleration.Start(); _vehicle_angular_velocity.Start(); + _vehicle_air_data.Start(); InitializeVehicleIMU(); } @@ -221,6 +223,7 @@ Sensors::~Sensors() { _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); + _vehicle_air_data.Stop(); for (auto &i : _vehicle_imu_list) { if (i != nullptr) { @@ -268,15 +271,17 @@ int Sensors::adc_init() return OK; } -void -Sensors::diff_pres_poll(const vehicle_air_data_s &raw) +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 = (diff_pres.temperature > -300.0f) ? diff_pres.temperature : - (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + (air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); airspeed_s airspeed{}; airspeed.timestamp = diff_pres.timestamp; @@ -312,10 +317,10 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw) 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, raw.baro_pressure_pa, + diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa, air_temperature_celsius); - airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, + airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa, air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here airspeed.air_temperature_celsius = air_temperature_celsius; @@ -482,14 +487,13 @@ void Sensors::Run() } } - vehicle_air_data_s airdata{}; vehicle_magnetometer_s magnetometer{}; - _voted_sensors_update.sensorsPoll(_sensor_combined, airdata, magnetometer); + _voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer); // check analog airspeed adc_poll(); - diff_pres_poll(airdata); + diff_pres_poll(); // check failover and update subscribed sensor (if necessary) @@ -517,11 +521,6 @@ void Sensors::Run() } } - if ((airdata.timestamp != 0) && (airdata.timestamp != _airdata_prev_timestamp)) { - _airdata_pub.publish(airdata); - _airdata_prev_timestamp = airdata.timestamp; - } - if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) { _magnetometer_pub.publish(magnetometer); _magnetometer_prev_timestamp = magnetometer.timestamp; @@ -618,6 +617,8 @@ int Sensors::print_status() { _voted_sensors_update.printStatus(); + PX4_INFO_RAW("\n"); + PX4_INFO("Airspeed status:"); _airspeed_validator.print(); @@ -627,6 +628,11 @@ int Sensors::print_status() PX4_INFO_RAW("\n"); _vehicle_angular_velocity.PrintStatus(); + PX4_INFO_RAW("\n"); + _vehicle_air_data.PrintStatus(); + + PX4_INFO_RAW("\n"); + for (auto &i : _vehicle_imu_list) { if (i != nullptr) { PX4_INFO_RAW("\n"); diff --git a/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/vehicle_air_data/CMakeLists.txt new file mode 100644 index 0000000000..5d297e1618 --- /dev/null +++ b/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# 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(vehicle_air_data + VehicleAirData.cpp + VehicleAirData.hpp +) +target_link_libraries(vehicle_air_data PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp new file mode 100644 index 0000000000..bc32a92545 --- /dev/null +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * 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 "VehicleAirData.hpp" + +#include +#include + +using namespace matrix; +using namespace time_literals; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +VehicleAirData::VehicleAirData() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl) +{ + _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() +{ + // check if the selected sensor has updated + sensor_correction_s corrections; + + if (_sensor_correction_sub.update(&corrections)) { + for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) { + const sensor_baro_s &baro = _last_data[baro_index]; + + // find sensor (by device id) in sensor_correction + if (_sensor_correction_index[baro_index] < 0) { + for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) { + if ((baro.device_id > 0) && (corrections.baro_device_ids[correction_index] == baro.device_id)) { + _sensor_correction_index[baro_index] = correction_index; + } + } + } + + switch (_sensor_correction_index[baro_index]) { + case 0: + _offset[baro_index] = corrections.baro_offset_0; + _scale[baro_index] = corrections.baro_scale_0; + break; + + case 1: + _offset[baro_index] = corrections.baro_offset_1; + _scale[baro_index] = corrections.baro_scale_1; + break; + + case 2: + _offset[baro_index] = corrections.baro_offset_2; + _scale[baro_index] = corrections.baro_scale_2; + break; + + default: + _offset[baro_index] = 0.f; + _scale[baro_index] = 1.f; + } + } + } +} + +void VehicleAirData::ParametersUpdate() +{ + // Check if parameters have changed + if (_params_sub.updated()) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + } +} + +void VehicleAirData::Run() +{ + perf_begin(_cycle_perf); + + 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 (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 sensor_baro:%i", uorb_index); + } + } + + _advertised[uorb_index] = true; + + } else { + _last_data[uorb_index].timestamp = hrt_absolute_time(); + } + } + + } else { + updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]); + + if (updated[uorb_index]) { + if (_priority[uorb_index] == 0) { + // set initial priority + _priority[uorb_index] = _sensor_sub[uorb_index].get_priority(); + } + + // millibar to Pa + const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f; + + // pressure corrected with offset and scale (if available) + const float pressure_corrected = (raw_pressure_pascals - _offset[uorb_index]) * _scale[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(); + } + + _selected_sensor_sub_index = best_index; + _sensor_sub[_selected_sensor_sub_index].registerCallback(); + } + } + + if ((_selected_sensor_sub_index >= 0) && updated[_selected_sensor_sub_index]) { + ParametersUpdate(); + SensorCorrectionsUpdate(); + + 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; + + // Convert from millibar to Pa and apply temperature compensation + out.baro_pressure_pa = (100.0f * baro.pressure - _offset[_selected_sensor_sub_index]) * + _scale[_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 + // 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); + } + + // 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) { + // we switched due to a non-critical reason. No need to panic. + PX4_INFO("sensor_baro switch from #%i", failover_index); + } + + } else { + if (failover_index != -1) { + mavlink_log_emergency(&_mavlink_log_pub, "sensor_baro:#%i failed: %s%s%s%s%s!, reconfiguring priorities", + 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" : "")); + + // reduce priority of failed sensor to the minimum + _priority[failover_index] = ORB_PRIO_MIN; + } + } + } + + // reschedule timeout + ScheduleDelayed(100_ms); + + perf_end(_cycle_perf); +} + +void VehicleAirData::PrintStatus() +{ + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("selected barometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id, _selected_sensor_sub_index); + } + + perf_print_counter(_cycle_perf); + _voter.print(); +} diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp new file mode 100644 index 0000000000..1d5c052317 --- /dev/null +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VehicleAirData(); + ~VehicleAirData() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(); + void SensorCorrectionsUpdate(); + + static constexpr int MAX_SENSOR_COUNT = 3; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sens_baro_qnh + ) + + uORB::Publication _vehicle_air_data_pub{ORB_ID(vehicle_air_data)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; + + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { + {this, ORB_ID(sensor_baro), 0}, + {this, ORB_ID(sensor_baro), 1}, + {this, ORB_ID(sensor_baro), 2} + }; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + orb_advert_t _mavlink_log_pub{nullptr}; + + DataValidatorGroup _voter{1}; + unsigned _last_failover_count{0}; + + sensor_baro_s _last_data[MAX_SENSOR_COUNT] {}; + bool _advertised[MAX_SENSOR_COUNT] {}; + + float _offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f}; + float _scale[MAX_SENSOR_COUNT] {1.f, 1.f, 1.f}; + + int8_t _sensor_correction_index[MAX_SENSOR_COUNT] {-1, -1, -1}; + uint8_t _priority[MAX_SENSOR_COUNT] {}; + + int8_t _selected_sensor_sub_index{-1}; +}; diff --git a/src/modules/commander/baro_calibration.h b/src/modules/sensors/vehicle_air_data/params.c similarity index 83% rename from src/modules/commander/baro_calibration.h rename to src/modules/sensors/vehicle_air_data/params.c index b4c645efab..af00b31c91 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/sensors/vehicle_air_data/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -32,16 +32,14 @@ ****************************************************************************/ /** - * @file mag_calibration.h - * Barometer calibration routine + * QNH for barometer + * + * @min 500 + * @max 1500 + * @group Sensors + * @unit hPa + * + * @reboot_required true + * */ - -#ifndef BARO_CALIBRATION_H_ -#define BARO_CALIBRATION_H_ - -#include -#include - -int do_baro_calibration(orb_advert_t *mavlink_log_pub); - -#endif /* BARO_CALIBRATION_H_ */ +PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index a53ffef544..516b862898 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -63,11 +63,6 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en _corrections.accel_scale_2[i] = 1.0f; } - _corrections.baro_scale_0 = 1.0f; - _corrections.baro_scale_1 = 1.0f; - _corrections.baro_scale_2 = 1.0f; - - _baro.voter.set_timeout(300000); _mag.voter.set_timeout(300000); _mag.voter.set_equal_value_threshold(1000); @@ -94,7 +89,6 @@ void VotedSensorsUpdate::initializeSensors() initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX); initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX); initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX); - initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX); } void VotedSensorsUpdate::deinit() @@ -110,10 +104,6 @@ void VotedSensorsUpdate::deinit() for (int i = 0; i < _mag.subscription_count; i++) { orb_unsubscribe(_mag.subscription[i]); } - - for (int i = 0; i < _baro.subscription_count; i++) { - orb_unsubscribe(_baro.subscription[i]); - } } void VotedSensorsUpdate::parametersUpdate() @@ -715,101 +705,6 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) } } -void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) -{ - bool got_update = false; - float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 }; - float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 }; - - for (int uorb_index = 0; uorb_index < _baro.subscription_count; uorb_index++) { - bool baro_updated; - orb_check(_baro.subscription[uorb_index], &baro_updated); - - if (baro_updated) { - sensor_baro_s baro_report{}; - - int ret = orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report); - - if (ret != PX4_OK || baro_report.timestamp == 0) { - continue; //ignore invalid data - } - - // Convert from millibar to Pa - float corrected_pressure = 100.0f * baro_report.pressure; - - // apply temperature compensation - corrected_pressure = (corrected_pressure - *offsets[uorb_index]) * *scales[uorb_index]; - - // First publication with data - if (_baro.priority[uorb_index] == 0) { - int32_t priority = 0; - orb_priority(_baro.subscription[uorb_index], &priority); - _baro.priority[uorb_index] = (uint8_t)priority; - } - - _baro_device_id[uorb_index] = baro_report.device_id; - - got_update = true; - - float vect[3] = {baro_report.pressure, baro_report.temperature, 0.f}; - - _last_airdata[uorb_index].timestamp = baro_report.timestamp; - _last_airdata[uorb_index].baro_temp_celcius = baro_report.temperature; - _last_airdata[uorb_index].baro_pressure_pa = corrected_pressure; - - _baro.voter.put(uorb_index, baro_report.timestamp, vect, baro_report.error_count, _baro.priority[uorb_index]); - } - } - - if (got_update) { - int best_index; - _baro.voter.get_best(hrt_absolute_time(), &best_index); - - if (best_index >= 0) { - airdata = _last_airdata[best_index]; - - if (_baro.last_best_vote != best_index) { - _baro.last_best_vote = (uint8_t)best_index; - } - - if (_selection.baro_device_id != _baro_device_id[best_index]) { - _selection_changed = true; - _selection.baro_device_id = _baro_device_id[best_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 = _parameters.baro_qnh * 0.1f; - - /* measured pressure in kPa */ - const float p = airdata.baro_pressure_pa * 0.001f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - airdata.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)); - airdata.rho = pressure_to_density * airdata.baro_pressure_pa; - } - } -} - bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type) { if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { @@ -917,19 +812,15 @@ void VotedSensorsUpdate::printStatus() _accel.voter.print(); PX4_INFO("mag status:"); _mag.voter.print(); - PX4_INFO("baro status:"); - _baro.voter.print(); } -void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, - vehicle_magnetometer_s &magnetometer) +void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer) { _corrections_sub.update(&_corrections); accelPoll(raw); gyroPoll(raw); magPoll(magnetometer); - baroPoll(airdata); // publish sensor selection if changed if (_selection_changed) { @@ -946,7 +837,6 @@ void VotedSensorsUpdate::checkFailover() checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC); checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO); checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG); - checkFailover(_baro, "Baro", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE); } void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 4452691377..53d00a490e 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -44,7 +44,6 @@ #include #include #include -#include #include #include @@ -62,7 +61,6 @@ #include #include #include -#include #include #include @@ -112,7 +110,7 @@ public: /** * read new sensor data */ - void sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer); + void sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer); /** * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, @@ -194,14 +192,6 @@ private: */ void magPoll(vehicle_magnetometer_s &magnetometer); - /** - * Poll the barometer for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void baroPoll(vehicle_air_data_s &airdata); - /** * Check & handle failover of a sensor * @return true if a switch occured (could be for a non-critical reason) @@ -211,7 +201,6 @@ private: SensorData _accel {}; SensorData _gyro {}; SensorData _mag {}; - SensorData _baro {}; orb_advert_t _mavlink_log_pub{nullptr}; @@ -222,7 +211,6 @@ private: uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)}; sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ - vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ matrix::Dcmf _board_rotation {}; /**< rotation matrix for the orientation that the board is mounted */ @@ -238,7 +226,6 @@ private: float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */ uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */ - uint32_t _baro_device_id[SENSOR_COUNT_MAX] {}; /**< baro driver device id for each uorb instance */ uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */ uint32_t _mag_device_id[SENSOR_COUNT_MAX] {}; /**< mag driver device id for each uorb instance */ diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index b9f5b8bf7f..f64d3fdd46 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -137,6 +137,7 @@ public: uint8_t get_instance() const { return _instance; } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } + ORB_PRIO get_priority() { return advertised() ? _node->get_priority() : ORB_PRIO_UNINITIALIZED; } protected: diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index 68143d19d3..dc392528db 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -132,6 +132,7 @@ public: uint8_t get_instance() const { return _subscription.get_instance(); } orb_id_t get_topic() const { return _subscription.get_topic(); } + ORB_PRIO get_priority() { return _subscription.get_priority(); } /** * Set the interval in microseconds diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 5709f8d2cd..af28dc61f7 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -67,7 +67,8 @@ typedef const struct orb_metadata *orb_id_t; * Relevant for multi-topics / topic groups */ enum ORB_PRIO { - ORB_PRIO_MIN = 1, // leave 0 free for other purposes, eg. marking an uninitialized value + ORB_PRIO_UNINITIALIZED = 0, + ORB_PRIO_MIN = 1, ORB_PRIO_VERY_LOW = 25, ORB_PRIO_LOW = 50, ORB_PRIO_DEFAULT = 75, diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index ce8ddf19c7..0f3c31fcd0 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -142,7 +142,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertise if (existing_node != nullptr && (!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) { if (is_advertiser) { - existing_node->set_priority(priority); + existing_node->set_priority((ORB_PRIO)priority); /* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers * could get the same instance). */ diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 4040c6e24b..4aef593ab0 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -198,8 +198,8 @@ public: uint8_t get_instance() const { return _instance; } - int get_priority() const { return _priority; } - void set_priority(uint8_t priority) { _priority = priority; } + ORB_PRIO get_priority() const { return (ORB_PRIO)_priority; } + void set_priority(ORB_PRIO priority) { _priority = priority; } /** * Copies data and the corresponding generation