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

move initial sensor priority to parameters and purge ORB_PRIORITY

Browse Source 
- CAL_ACCx_EN -> CAL_ACCx_PRIO
 - CAL_GYROx_EN -> CAL_GYROx_PRIO
 - CAL_MAGx_EN -> CAL_MAGx_PRIO
This commit is contained in:
Daniel Agar
2020-08-16 22:21:04 -04:00
parent 971b1e4b4d
commit 27f23ac290
124 changed files with 463 additions and 509 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+4 -19
View File
@@ -263,8 +263,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED;
int32_t device_id_primary = 0;
unsigned active_sensors = 0;
for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) {
@@ -274,14 +272,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
calibrations[cur_accel].set_device_id(accel_sub.get().device_id);
active_sensors++;
// Get priority
const ORB_PRIO prio = accel_sub.get_priority();
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = accel_sub.get().device_id;
}
} else {
calibrations[cur_accel].Reset();
}
@@ -365,7 +355,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
calibrations[i].ParametersSave();
}
param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary);
param_notify_changes();
/* if there is a any preflight-check system response, let the barrage of messages through */
@@ -384,7 +373,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
#if !defined(CONSTRAINED_FLASH)
PX4_INFO("Accelerometer quick calibration");
int32_t device_id_primary = 0;
bool success = false;
// sensor thermal corrections (optional)
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
@@ -492,20 +481,16 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
} else {
calibration.set_offset(offset);
success = true;
}
calibration.ParametersSave();
if (device_id_primary == 0) {
device_id_primary = arp.device_id;
}
calibration.PrintStatus();
}
}
if (device_id_primary != 0) {
param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary);
if (success) {
param_notify_changes();
return PX4_OK;
}
src/modules/commander/gyro_calibration.cpp
-13
View File
@@ -212,9 +212,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
gyro_worker_data_t worker_data{};
worker_data.mavlink_log_pub = mavlink_log_pub;
ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED;
int32_t device_id_primary = 0;
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro));
@@ -232,14 +229,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) {
worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id);
// Get priority
const ORB_PRIO prio = gyro_sub.get_priority();
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = gyro_sub.get().device_id;
}
}
// reset calibration index to match uORB numbering
@@ -317,8 +306,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
calibration.ParametersSave();
}
param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary);
if (failed) {
calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params");
res = PX4_ERROR;
src/modules/mavlink/mavlink_receiver.h
+6 -6
View File
@@ -262,12 +262,12 @@ private:
uORB::Publication<vehicle_trajectory_waypoint_s> _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
// ORB publications (multi)
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping)};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};
// ORB publications (queue length > 1)
uORB::PublicationQueued<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
src/modules/mc_rate_control/MulticopterRateControl.hpp
+1 -1
View File
@@ -107,7 +107,7 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
src/modules/rc_update/rc_update.h
+1 -1
View File
@@ -157,7 +157,7 @@ private:
uORB::Publication<rc_channels_s> _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */
uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; /**< manual control signal topic */
rc_channels_s _rc {}; /**< r/c channel data */
src/modules/replay/Replay.cpp
+1 -1
View File
@@ -955,7 +955,7 @@ Replay::publishTopic(Subscription &sub, void *data)
if (advertised) {
int instance;
sub.orb_advert = orb_advertise_multi(sub.orb_meta, data, &instance, ORB_PRIO_DEFAULT);
sub.orb_advert = orb_advertise_multi(sub.orb_meta, data, &instance);
published = true;
}
}
src/modules/sensors/sensor_params_acc0.c
+10 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 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
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
/**
* Accelerometer 0 enabled
* Accelerometer 0 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC0_EN, 1);
PARAM_DEFINE_INT32(CAL_ACC0_PRIO, 50);
/**
* Accelerometer X-axis offset
src/modules/sensors/sensor_params_acc1.c
+10 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 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
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
/**
* Accelerometer 1 enabled
* Accelerometer 1 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC1_EN, 1);
PARAM_DEFINE_INT32(CAL_ACC1_PRIO, 50);
/**
* Accelerometer X-axis offset
src/modules/sensors/sensor_params_acc2.c
+10 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 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
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
/**
* Accelerometer 2 enabled
* Accelerometer 2 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC2_EN, 1);
PARAM_DEFINE_INT32(CAL_ACC2_PRIO, 50);
/**
* Accelerometer X-axis offset
src/modules/sensors/sensor_params_accel.c
-40
View File
@@ -1,40 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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.
*
****************************************************************************/
/**
* Primary accel ID
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0);
src/modules/sensors/sensor_params_gyro.c
-40
View File
@@ -1,40 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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.
*
****************************************************************************/
/**
* Primary gyro ID
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
src/modules/sensors/sensor_params_gyro0.c
+10 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 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
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
/**
* Gyro 0 enabled
* Gyro 0 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO0_EN, 1);
PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, 50);
/**
* Gyro X-axis offset
src/modules/sensors/sensor_params_gyro1.c
+10 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 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
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
/**
* Gyro 1 enabled
* Gyro 1 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO1_EN, 1);
PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, 50);
/**
* Gyro X-axis offset
src/modules/sensors/sensor_params_gyro2.c
+10 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 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
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
/**
* Gyro 2 enabled
* Gyro 2 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO2_EN, 1);
PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, 50);
/**
* Gyro X-axis offset
src/modules/sensors/sensor_params_mag.c
+1 -9
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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
@@ -31,14 +31,6 @@
*
****************************************************************************/
/**
* Primary mag ID
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
/**
* Bitfield selecting mag sides for calibration
*
src/modules/sensors/sensor_params_mag0.c
+9 -3
View File
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
/**
* Mag 0 enabled
* Mag 0 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG0_EN, 1);
PARAM_DEFINE_INT32(CAL_MAG0_PRIO, 50);
/**
* Rotation of magnetometer 0 relative to airframe.
src/modules/sensors/sensor_params_mag1.c
+9 -3
View File
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
/**
* Mag 1 enabled
* Mag 1 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG1_EN, 1);
PARAM_DEFINE_INT32(CAL_MAG1_PRIO, 50);
/**
* Rotation of magnetometer 1 relative to airframe.
src/modules/sensors/sensor_params_mag2.c
+9 -3
View File
@@ -40,13 +40,19 @@
PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
/**
* Mag 2 enabled
* Mag 2 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG2_EN, 1);
PARAM_DEFINE_INT32(CAL_MAG2_PRIO, 50);
/**
* Rotation of magnetometer 2 relative to airframe.
src/modules/sensors/sensor_params_mag3.c
+10 -4
View File
@@ -40,16 +40,22 @@
PARAM_DEFINE_INT32(CAL_MAG3_ID, 0);
/**
* Mag 3 enabled
* Mag 3 priority.
*
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @boolean
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG3_EN, 1);
PARAM_DEFINE_INT32(CAL_MAG3_PRIO, 50);
/**
* Rotation of magnetometer 2 relative to airframe.
* Rotation of magnetometer 3 relative to airframe.
*
* An internal magnetometer will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
src/modules/sensors/sensors.cpp
+1 -2
View File
@@ -340,8 +340,7 @@ void Sensors::diff_pres_poll()
/* 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,
ORB_PRIO_HIGH);
_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
+1 -6
View File
@@ -156,11 +156,6 @@ void VehicleAirData::Run()
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;
@@ -282,7 +277,7 @@ void VehicleAirData::Run()
}
// reduce priority of failed sensor to the minimum
_priority[failover_index] = ORB_PRIO_MIN;
_priority[failover_index] = 1;
}
}
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+19 -1
View File
@@ -122,6 +122,24 @@ void VehicleMagnetometer::ParametersUpdate(bool force)
}
_mag_comp_type = mag_comp_typ;
// update mag priority (CAL_MAGx_PRIO)
for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
const int32_t priority_old = _calibration[mag].priority();
_calibration[mag].ParametersUpdate();
const int32_t priority_new = _calibration[mag].priority();
if (priority_old != priority_new) {
if (_priority[mag] == priority_old) {
_priority[mag] = priority_new;
} else {
// change relative priority to incorporate any sensor faults
int priority_change = priority_new - priority_old;
_priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100);
}
}
}
}
}
@@ -207,7 +225,7 @@ void VehicleMagnetometer::Run()
if (_calibration[uorb_index].device_id() != report.device_id) {
_calibration[uorb_index].set_external(report.is_external);
_calibration[uorb_index].set_device_id(report.device_id);
_priority[uorb_index] = _sensor_sub[uorb_index].get_priority();
_priority[uorb_index] = _calibration[uorb_index].priority();
}
if (_calibration[uorb_index].enabled()) {
src/modules/sensors/voted_sensors_update.cpp
+42 -30
View File
@@ -88,28 +88,46 @@ void VotedSensorsUpdate::parametersUpdate()
if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) {
if (_accel.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
// find corresponding sensor_accel publication
for (uint8_t i = 0; i < ACCEL_COUNT_MAX; i++) {
uORB::SubscriptionData<sensor_accel_s> sensor_accel{ORB_ID(sensor_accel), i};
sensor_accel.update();
// find corresponding configured accel priority
int8_t accel_cal_index = calibration::FindCalibrationIndex("ACC", imu.get().accel_device_id);
if (imu.get().accel_device_id == sensor_accel.get().device_id) {
_accel.priority[uorb_index] = sensor_accel.get_priority();
break;
if (accel_cal_index >= 0) {
// found matching CAL_ACCx_PRIO
int32_t accel_priority_old = _accel.priority_configured[uorb_index];
_accel.priority_configured[uorb_index] = calibration::GetCalibrationParam("ACC", "PRIO", accel_cal_index);
if (accel_priority_old != _accel.priority_configured[uorb_index]) {
if (_accel.priority_configured[uorb_index] == 0) {
// disabled
_accel.priority[uorb_index] = 0;
} else {
// change relative priority to incorporate any sensor faults
int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old;
_accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, 1, 100);
}
}
}
if (_gyro.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
// find corresponding sensor_gyro publication
for (uint8_t i = 0; i < GYRO_COUNT_MAX; i++) {
uORB::SubscriptionData<sensor_accel_s> sensor_gyro{ORB_ID(sensor_gyro), i};
sensor_gyro.update();
// find corresponding configured gyro priority
int8_t gyro_cal_index = calibration::FindCalibrationIndex("GYRO", imu.get().gyro_device_id);
if (imu.get().gyro_device_id == sensor_gyro.get().device_id) {
_gyro.priority[uorb_index] = sensor_gyro.get_priority();
break;
if (gyro_cal_index >= 0) {
// found matching CAL_GYROx_PRIO
int32_t gyro_priority_old = _gyro.priority_configured[uorb_index];
_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParam("GYRO", "PRIO", gyro_cal_index);
if (gyro_priority_old != _gyro.priority_configured[uorb_index]) {
if (_gyro.priority_configured[uorb_index] == 0) {
// disabled
_gyro.priority[uorb_index] = 0;
} else {
// change relative priority to incorporate any sensor faults
int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old;
_gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, 1, 100);
}
}
}
@@ -122,21 +140,13 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
for (int uorb_index = 0; uorb_index < 3; uorb_index++) {
vehicle_imu_s imu_report;
if (_accel.enabled[uorb_index] && _gyro.enabled[uorb_index] && _vehicle_imu_sub[uorb_index].update(&imu_report)) {
if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0)
&& _vehicle_imu_sub[uorb_index].update(&imu_report)) {
// copy corresponding vehicle_imu_status for accel & gyro error counts
vehicle_imu_status_s imu_status{};
_vehicle_imu_status_sub[uorb_index].copy(&imu_status);
// First publication with data
if (_accel.priority[uorb_index] == 0) {
_accel.priority[uorb_index] = _accel.subscription[uorb_index].get_priority();
}
if (_gyro.priority[uorb_index] == 0) {
_gyro.priority[uorb_index] = _gyro.subscription[uorb_index].get_priority();
}
_accel_device_id[uorb_index] = imu_report.accel_device_id;
_gyro_device_id[uorb_index] = imu_report.gyro_device_id;
@@ -250,12 +260,12 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
}
// reduce priority of failed sensor to the minimum
sensor.priority[failover_index] = ORB_PRIO_MIN;
sensor.priority[failover_index] = 1;
int ctr_valid = 0;
for (uint8_t i = 0; i < sensor.subscription_count; i++) {
if (sensor.enabled[i] && (sensor.priority[i] > ORB_PRIO_MIN)) {
if (sensor.priority[i] > 1) {
ctr_valid++;
}
}
@@ -300,6 +310,8 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor
if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) {
sensor_data.advertised[i] = true;
sensor_data.priority[i] = DEFAULT_PRIORITY;
sensor_data.priority_configured[i] = DEFAULT_PRIORITY;
if (i > 0) {
/* the first always exists, but for each further sensor, add a new validator */
@@ -366,7 +378,7 @@ void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt)
for (int sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) {
// check that the sensor we are checking against is not the same as the primary
if (_accel.enabled[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) {
if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) {
float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary
@@ -415,7 +427,7 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt)
for (int sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) {
// check that the sensor we are checking against is not the same as the primary
if (_gyro.enabled[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) {
if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) {
float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary
src/modules/sensors/voted_sensors_update.h
+5 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-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
@@ -126,6 +126,8 @@ private:
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX);
static constexpr uint8_t DEFAULT_PRIORITY = 50;
struct SensorData {
SensorData() = delete;
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {}
@@ -133,10 +135,10 @@ private:
uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
DataValidatorGroup voter{1};
unsigned int last_failover_count{0};
ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
int32_t priority[SENSOR_COUNT_MAX] {};
int32_t priority_configured[SENSOR_COUNT_MAX] {};
uint8_t last_best_vote{0}; /**< index of the latest best vote */
uint8_t subscription_count{0};
bool enabled[SENSOR_COUNT_MAX] {true, true, true};
bool advertised[SENSOR_COUNT_MAX] {false, false, false};
};
src/modules/sih/sih.hpp
+4 -4
View File
@@ -98,10 +98,10 @@ private:
void parameters_updated();
// simulated sensor instances
PX4Accelerometer _px4_accel{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{ 197388, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
PX4Accelerometer _px4_accel{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
// to publish the gps position
vehicle_gps_position_s _vehicle_gps_pos{};
src/modules/simulator/simulator.h
+4 -4
View File
@@ -154,10 +154,10 @@ private:
static Simulator *_instance;
// simulated sensor instances
PX4Accelerometer _px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{6620172, ORB_PRIO_DEFAULT}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
PX4Accelerometer _px4_accel{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1311244}; // 2294028: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
perf_counter_t _perf_sim_delay{perf_alloc(PC_ELAPSED, MODULE_NAME": network delay")};
perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")};
src/modules/uORB/PublicationMulti.hpp
+7 -18
View File
@@ -59,23 +59,20 @@ public:
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub/sub, 0 means don't publish as multi
*/
PublicationMulti(ORB_ID id, ORB_PRIO priority = ORB_PRIO_DEFAULT) :
PublicationBase(id),
_priority(priority)
PublicationMulti(ORB_ID id) :
PublicationBase(id)
{}
PublicationMulti(const orb_metadata *meta, ORB_PRIO priority = ORB_PRIO_DEFAULT) :
PublicationBase(static_cast<ORB_ID>(meta->o_id)),
_priority(priority)
PublicationMulti(const orb_metadata *meta) :
PublicationBase(static_cast<ORB_ID>(meta->o_id))
{}
bool advertise()
{
if (!advertised()) {
int instance = 0;
_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, _priority, QSIZE);
_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE);
}
return advertised();
@@ -93,9 +90,6 @@ public:
return (orb_publish(get_topic(), _handle, &data) == PX4_OK);
}
protected:
const ORB_PRIO _priority;
};
/**
@@ -109,14 +103,9 @@ public:
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub
*/
PublicationMultiData(ORB_ID id, ORB_PRIO priority = ORB_PRIO_DEFAULT) :
PublicationMulti<T>(id, priority)
{}
PublicationMultiData(const orb_metadata *meta, ORB_PRIO priority = ORB_PRIO_DEFAULT) :
PublicationMulti<T>(meta, priority)
{}
PublicationMultiData(ORB_ID id) : PublicationMulti<T>(id) {}
PublicationMultiData(const orb_metadata *meta) : PublicationMulti<T>(meta) {}
T &get() { return _data; }
void set(const T &data) { _data = data; }
src/modules/uORB/Subscription.hpp
-1
View File
@@ -129,7 +129,6 @@ public:
uint8_t get_instance() const { return _instance; }
unsigned get_last_generation() const { return _last_generation; }
ORB_PRIO get_priority() { return advertised() ? _node->get_priority() : ORB_PRIO_UNINITIALIZED; }
orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
protected:
src/modules/uORB/SubscriptionInterval.hpp
-1
View File
@@ -137,7 +137,6 @@ public:
uint8_t get_instance() const { return _subscription.get_instance(); }
uint32_t get_interval_us() const { return _interval_us; }
unsigned get_last_generation() const { return _subscription.get_last_generation(); }
ORB_PRIO get_priority() { return _subscription.get_priority(); }
orb_id_t get_topic() const { return _subscription.get_topic(); }
/**
src/modules/uORB/uORB.cpp
+4 -9
View File
@@ -50,15 +50,15 @@ orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *da
return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
}
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, ORB_PRIO priority)
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
{
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority);
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance);
}
orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
ORB_PRIO priority, unsigned int queue_size)
unsigned int queue_size)
{
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority, queue_size);
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size);
}
int orb_unadvertise(orb_advert_t handle)
@@ -112,11 +112,6 @@ int orb_group_count(const struct orb_metadata *meta)
return instance;
}
int orb_priority(int handle, enum ORB_PRIO *priority)
{
return uORB::Manager::get_instance()->orb_priority(handle, priority);
}
int orb_set_interval(int handle, unsigned interval)
{
return uORB::Manager::get_instance()->orb_set_interval(handle, interval);
src/modules/uORB/uORB.h
+4 -25
View File
@@ -62,21 +62,6 @@ typedef const struct orb_metadata *orb_id_t;
*/
#define ORB_MULTI_MAX_INSTANCES 4 // This must be < 10 (because it's the last char of the node path)
/**
* Topic priority.
* Relevant for multi-topics / topic groups
*/
enum ORB_PRIO {
ORB_PRIO_UNINITIALIZED = 0,
ORB_PRIO_MIN = 1,
ORB_PRIO_VERY_LOW = 25,
ORB_PRIO_LOW = 50,
ORB_PRIO_DEFAULT = 75,
ORB_PRIO_HIGH = 100,
ORB_PRIO_VERY_HIGH = 125,
ORB_PRIO_MAX = 255
};
/**
* Generates a pointer to the uORB metadata structure for
* a given topic.
@@ -151,14 +136,13 @@ extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const v
/**
* @see uORB::Manager::orb_advertise_multi()
*/
extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
enum ORB_PRIO priority) __EXPORT;
extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT;
/**
* @see uORB::Manager::orb_advertise_multi()
*/
extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
enum ORB_PRIO priority, unsigned int queue_size) __EXPORT;
unsigned int queue_size) __EXPORT;
/**
* @see uORB::Manager::orb_unadvertise()
@@ -179,10 +163,10 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con
* @see uORB::Manager::orb_advertise_multi() for meaning of the individual parameters
*/
static inline int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data,
int *instance, enum ORB_PRIO priority)
int *instance)
{
if (!*handle) {
*handle = orb_advertise_multi(meta, data, instance, priority);
*handle = orb_advertise_multi(meta, data, instance);
if (*handle) {
return 0;
@@ -233,11 +217,6 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
*/
extern int orb_group_count(const struct orb_metadata *meta) __EXPORT;
/**
* @see uORB::Manager::orb_priority()
*/
extern int orb_priority(int handle, enum ORB_PRIO *priority) __EXPORT;
/**
* @see uORB::Manager::orb_set_interval()
*/
src/modules/uORB/uORBCommon.hpp
-1
View File
@@ -47,7 +47,6 @@ static constexpr unsigned orb_maxpath = 64;
struct orb_advertdata {
const struct orb_metadata *meta;
int *instance;
ORB_PRIO priority;
};
}
src/modules/uORB/uORBDeviceMaster.cpp
+3 -4
View File
@@ -59,7 +59,7 @@ uORB::DeviceMaster::~DeviceMaster()
px4_sem_destroy(&_lock);
}
int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, ORB_PRIO priority)
int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance)
{
int ret = PX4_ERROR;
@@ -107,7 +107,7 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver
}
/* construct the new node, passing the ownership of path to it */
uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority);
uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath);
/* if we didn't get a device, that's bad, free the path too */
if (node == nullptr) {
@@ -142,7 +142,6 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver
if (existing_node != nullptr &&
(!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) {
if (is_advertiser) {
existing_node->set_priority((ORB_PRIO)priority);
/* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers
* could get the same instance).
*/
@@ -194,7 +193,7 @@ void uORB::DeviceMaster::printStatistics()
return;
}
PX4_INFO_RAW("%-*s INST #SUB #Q SIZE PRIO PATH\n", (int)max_topic_name_length - 2, "TOPIC NAME");
PX4_INFO_RAW("%-*s INST #SUB #Q SIZE PATH\n", (int)max_topic_name_length - 2, "TOPIC NAME");
cur_node = first_node;
src/modules/uORB/uORBDeviceMaster.hpp
+1 -1
View File
@@ -65,7 +65,7 @@ class uORB::DeviceMaster
{
public:
int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, ORB_PRIO priority);
int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance);
/**
* Public interface for getDeviceNodeLocked(). Takes care of synchronization.
src/modules/uORB/uORBDeviceNode.cpp
+5 -11
View File
@@ -45,10 +45,9 @@
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path,
ORB_PRIO priority, uint8_t queue_size) :
uint8_t queue_size) :
CDev(path),
_meta(meta),
_priority(priority),
_instance(instance),
_queue_size(queue_size)
{
@@ -251,10 +250,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
*(uintptr_t *)arg = (uintptr_t)this;
return PX4_OK;
case ORBIOCGPRIORITY:
*(int *)arg = get_priority();
return PX4_OK;
case ORBIOCSETQUEUESIZE: {
lock();
int ret = update_queue_size(arg);
@@ -351,7 +346,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle)
}
#ifdef ORB_COMMUNICATOR
int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, ORB_PRIO priority)
int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
{
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
@@ -364,7 +359,7 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, ORB_PRIO pr
/*
//TODO: Check if we need this since we only unadvertise when things all shutdown and it doesn't actually remove the device
int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta, ORB_PRIO priority)
int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta)
{
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && meta != nullptr) {
@@ -401,14 +396,13 @@ uORB::DeviceNode::print_statistics(int max_topic_length)
lock();
const uint8_t instance = get_instance();
const uint8_t priority = get_priority();
const int8_t sub_count = subscriber_count();
const uint8_t queue_size = get_queue_size();
unlock();
PX4_INFO_RAW("%-*s %2i %4i %2i %4i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
queue_size, get_meta()->o_size, priority, get_devname());
PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
queue_size, get_meta()->o_size, get_devname());
return true;
}
src/modules/uORB/uORBDeviceNode.hpp
+3 -8
View File
@@ -56,8 +56,7 @@ class SubscriptionCallback;
class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
{
public:
DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, ORB_PRIO priority,
uint8_t queue_size = 1);
DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1);
virtual ~DeviceNode();
// no copy, assignment, move, move assignment
@@ -119,8 +118,8 @@ public:
static int unadvertise(orb_advert_t handle);
#ifdef ORB_COMMUNICATOR
static int16_t topic_advertised(const orb_metadata *meta, ORB_PRIO priority);
//static int16_t topic_unadvertised(const orb_metadata *meta, ORB_PRIO priority);
static int16_t topic_advertised(const orb_metadata *meta);
//static int16_t topic_unadvertised(const orb_metadata *meta);
/**
* processes a request for add subscription from remote
@@ -199,9 +198,6 @@ public:
uint8_t get_instance() const { return _instance; }
ORB_PRIO get_priority() const { return (ORB_PRIO)_priority; }
void set_priority(ORB_PRIO priority) { _priority = priority; }
/**
* Copies data and the corresponding generation
* from a node to the buffer provided.
@@ -235,7 +231,6 @@ private:
px4::atomic<unsigned> _generation{0}; /**< object generation count */
List<uORB::SubscriptionCallback *> _callbacks;
ORB_PRIO _priority; /**< priority of the topic */
const uint8_t _instance; /**< orb multi instance identifier */
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
uint8_t _queue_size; /**< maximum number of elements in the queue */
src/modules/uORB/uORBManager.cpp
+5 -10
View File
@@ -168,7 +168,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
}
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
ORB_PRIO priority, unsigned int queue_size)
unsigned int queue_size)
{
#ifdef ORB_USE_PUBLISHER_RULES
@@ -195,7 +195,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
#endif /* ORB_USE_PUBLISHER_RULES */
/* open the node as an advertiser */
int fd = node_open(meta, true, instance, priority);
int fd = node_open(meta, true, instance);
if (fd == PX4_ERROR) {
PX4_ERR("%s advertise failed (%i)", meta->o_name, errno);
@@ -224,7 +224,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
#ifdef ORB_COMMUNICATOR
// For remote systems call over and inform them
uORB::DeviceNode::topic_advertised(meta, priority);
uORB::DeviceNode::topic_advertised(meta);
#endif /* ORB_COMMUNICATOR */
/* the advertiser may perform an initial publish to initialise the object */
@@ -307,11 +307,6 @@ int uORB::Manager::orb_check(int handle, bool *updated)
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
}
int uORB::Manager::orb_priority(int handle, enum ORB_PRIO *priority)
{
return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
}
int uORB::Manager::orb_set_interval(int handle, unsigned interval)
{
return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
@@ -324,7 +319,7 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
return ret;
}
int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance, ORB_PRIO priority)
int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
{
char path[orb_maxpath];
int fd = -1;
@@ -365,7 +360,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
ret = PX4_ERROR;
if (get_device_master()) {
ret = _device_master->advertise(meta, advertiser, instance, priority);
ret = _device_master->advertise(meta, advertiser, instance);
}
/* it's OK if it already exists */
src/modules/uORB/uORBManager.hpp
+4 -22
View File
@@ -106,8 +106,7 @@ public:
* Any number of advertisers may publish to a topic; publications are atomic
* but co-ordination between publishers is not provided by the ORB.
*
* Internally this will call orb_advertise_multi with an instance of 0 and
* default priority.
* Internally this will call orb_advertise_multi with an instance of 0.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
@@ -124,7 +123,7 @@ public:
*/
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
{
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT, queue_size);
return orb_advertise_multi(meta, data, nullptr, queue_size);
}
/**
@@ -149,10 +148,6 @@ public:
* @param instance Pointer to an integer which will yield the instance ID (0-based)
* of the publication. This is an output parameter and will be set to the newly
* created instance, ie. 0 for the first advertiser, 1 for the next and so on.
* @param priority The priority of the instance. If a subscriber subscribes multiple
* instances, the priority allows the subscriber to prioritize the best
* data source as long as its available. The subscriber is responsible to check
* and handle different priorities (@see orb_priority()).
* @param queue_size Maximum number of buffered elements. If this is 1, no queuing is
* used.
* @return PX4_ERROR on error, otherwise returns a handle
@@ -161,7 +156,7 @@ public:
* ORB_DEFINE with no corresponding ORB_DECLARE)
* this function will return -1 and set errno to ENOENT.
*/
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, ORB_PRIO priority,
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
unsigned int queue_size = 1);
/**
@@ -308,18 +303,6 @@ public:
*/
int orb_exists(const struct orb_metadata *meta, int instance);
/**
* Return the priority of the topic
*
* @param handle A handle returned from orb_subscribe.
* @param priority Returns the priority of this topic. This is only relevant for
* topics which are published by multiple publishers (e.g. mag0, mag1, etc.)
* and allows a subscriber to pick the topic with the highest priority,
* independent of the startup order of the associated publishers.
* @return OK on success, PX4_ERROR otherwise with errno set accordingly.
*/
int orb_priority(int handle, enum ORB_PRIO *priority);
/**
* Set the minimum interval between which updates are seen for a subscription.
*
@@ -383,8 +366,7 @@ private: // class methods
* Handles creation of the object and the initial publication for
* advertisers.
*/
int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr,
ORB_PRIO priority = ORB_PRIO_DEFAULT);
int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr);
private: // data members
static Manager *_Instance;
src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+6 -27
View File
@@ -229,7 +229,7 @@ int uORBTest::UnitTest::test_unadvertise()
orb_test_s t{};
for (int i = 0; i < 4; ++i) {
_pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX);
_pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i]);
if (instance_test[i] != i) {
return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]);
@@ -335,12 +335,12 @@ int uORBTest::UnitTest::test_multi()
orb_test_s u{};
int instance0;
_pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
_pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0);
test_note("advertised");
int instance1;
_pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
_pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1);
if (instance0 != 0) {
return test_fail("mult. id0: %d", instance0);
@@ -385,25 +385,6 @@ int uORBTest::UnitTest::test_multi()
return test_fail("sub #1 val. mismatch: %d", u.val);
}
/* test priorities */
ORB_PRIO prio;
if (PX4_OK != orb_priority(sfd0, &prio)) {
return test_fail("prio #0");
}
if (prio != ORB_PRIO_MAX) {
return test_fail("prio: %d", prio);
}
if (PX4_OK != orb_priority(sfd1, &prio)) {
return test_fail("prio #1");
}
if (prio != ORB_PRIO_MIN) {
return test_fail("prio: %d", prio);
}
if (PX4_OK != latency_test<orb_test_s>(ORB_ID(orb_test), false)) {
return test_fail("latency test failed");
}
@@ -431,7 +412,7 @@ int uORBTest::UnitTest::pub_test_multi2_main()
orb_advert_t &pub = orb_pub[i];
int idx = i;
// PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time());
pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT);
pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx);
if (idx != i) {
_thread_should_exit = true;
@@ -550,12 +531,10 @@ int uORBTest::UnitTest::test_multi_reversed()
t.val = 0;
int instance2;
_pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX);
_pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2);
int instance3;
_pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN);
_pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3);
test_note("advertised");