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

sensors: update orb_publish to uORB::Publication<>

Browse Source
This commit is contained in:
Daniel Agar
2019-09-02 20:44:03 -04:00
committed by GitHub
parent c8f77d438d
commit 20e6adc4ca
5 changed files with 33 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/sensors/rc_update.cpp
View File

@@ -39,9 +39,6 @@
#include "rc_update.h"
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
using namespace sensors;
RCUpdate::RCUpdate(const Parameters &parameters)
@@ -316,14 +313,13 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
_rc.frame_drop_count = rc_input.rc_lost_frame_count;
/* publish rc_channels topic even if signal is invalid, for debug */
int instance;
orb_publish_auto(ORB_ID(rc_channels), &_rc_pub, &_rc, &instance, ORB_PRIO_DEFAULT);
_rc_pub.publish(_rc);
/* only publish manual control if the signal is still present and was present once */
if (!signal_lost && rc_input.timestamp_last_signal > 0) {
/* initialize manual setpoint */
struct manual_control_setpoint_s manual = {};
manual_control_setpoint_s manual{};
/* set mode slot to unassigned */
manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;
/* set the timestamp to the last signal time */
@@ -405,11 +401,10 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
_parameters.rc_man_th, _parameters.rc_man_inv);
/* publish manual_control_setpoint topic */
orb_publish_auto(ORB_ID(manual_control_setpoint), &_manual_control_pub, &manual, &instance,
ORB_PRIO_HIGH);
_manual_control_pub.publish(manual);
/* copy from mapped manual control to control group 3 */
struct actuator_controls_s actuator_group_3 = {};
actuator_controls_s actuator_group_3{};
actuator_group_3.timestamp = rc_input.timestamp_last_signal;
@@ -423,8 +418,7 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
actuator_group_3.control[7] = manual.aux3;
/* publish actuator_controls_3 topic */
orb_publish_auto(ORB_ID(actuator_controls_3), &_actuator_group_3_pub, &actuator_group_3, &instance,
ORB_PRIO_DEFAULT);
_actuator_group_3_pub.publish(actuator_group_3);
/* Update parameters from RC Channels (tuning with RC) if activated */
if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1e6) {

11
src/modules/sensors/rc_update.h
View File

@@ -44,7 +44,11 @@
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/rc_parameter_map.h>
@@ -105,9 +109,10 @@ private:
uORB::Subscription _rc_sub{ORB_ID(input_rc)}; /**< raw rc channels data subscription */
uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)}; /**< rc parameter map subscription */
orb_advert_t _rc_pub = nullptr; /**< raw r/c control topic */
orb_advert_t _manual_control_pub = nullptr; /**< manual control signal topic */
orb_advert_t _actuator_group_3_pub = nullptr;/**< manual control as actuator topic */
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_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */
rc_channels_s _rc {}; /**< r/c channel data */

46
src/modules/sensors/sensors.cpp
View File

@@ -168,12 +168,14 @@ private:
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
orb_advert_t _sensor_pub{nullptr}; /**< combined sensor data topic */
orb_advert_t _airdata_pub{nullptr}; /**< combined sensor data topic */
orb_advert_t _magnetometer_pub{nullptr}; /**< combined sensor data topic */
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */
uORB::Publication<sensor_preflight_s> _sensor_preflight{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */
uORB::Publication<vehicle_air_data_s> _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */
uORB::Publication<vehicle_magnetometer_s> _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */
#if BOARD_NUMBER_BRICKS > 0
orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */
@@ -185,17 +187,13 @@ private:
int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */
#endif /* BOARD_NUMBER_BRICKS > 1 */
orb_advert_t _airspeed_pub{nullptr}; /**< airspeed */
orb_advert_t _sensor_preflight{nullptr}; /**< sensor preflight topic */
perf_counter_t _loop_perf; /**< loop performance counter */
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
differential_pressure_s _diff_pres {};
orb_advert_t _diff_pres_pub{nullptr}; /**< differential_pressure */
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
Parameters _parameters{}; /**< local copies of interesting parameters */
@@ -358,8 +356,7 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
airspeed.air_temperature_celsius = air_temperature_celsius;
if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) {
int instance;
orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &airspeed, &instance, ORB_PRIO_DEFAULT);
_airspeed_pub.publish(airspeed);
}
}
}
@@ -475,8 +472,7 @@ Sensors::adc_poll()
(diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
int instance;
orb_publish_auto(ORB_ID(differential_pressure), &_diff_pres_pub, &_diff_pres, &instance, ORB_PRIO_DEFAULT);
_diff_pres_pub.publish(_diff_pres);
}
} else
@@ -595,8 +591,6 @@ Sensors::run()
_rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */);
_sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt);
/* wakeup source */
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;
@@ -653,15 +647,14 @@ Sensors::run()
_voted_sensors_update.setRelativeTimestamps(raw);
int instance;
orb_publish_auto(ORB_ID(sensor_combined), &_sensor_pub, &raw, &instance, ORB_PRIO_DEFAULT);
_sensor_pub.publish(raw);
if (airdata.timestamp != airdata_prev_timestamp) {
orb_publish_auto(ORB_ID(vehicle_air_data), &_airdata_pub, &airdata, &instance, ORB_PRIO_DEFAULT);
_airdata_pub.publish(airdata);
}
if (magnetometer.timestamp != magnetometer_prev_timestamp) {
orb_publish_auto(ORB_ID(vehicle_magnetometer), &_magnetometer_pub, &magnetometer, &instance, ORB_PRIO_DEFAULT);
_magnetometer_pub.publish(magnetometer);
}
_voted_sensors_update.checkFailover();
@@ -674,7 +667,8 @@ Sensors::run()
_voted_sensors_update.calcAccelInconsistency(preflt);
_voted_sensors_update.calcGyroInconsistency(preflt);
_voted_sensors_update.calcMagInconsistency(preflt);
orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt);
_sensor_preflight.publish(preflt);
}
}
@@ -700,18 +694,6 @@ Sensors::run()
perf_end(_loop_perf);
}
if (_sensor_pub) {
orb_unadvertise(_sensor_pub);
}
if (_airdata_pub) {
orb_unadvertise(_airdata_pub);
}
if (_magnetometer_pub) {
orb_unadvertise(_magnetometer_pub);
}
_voted_sensors_update.deinit();
}

14
src/modules/sensors/voted_sensors_update.cpp
View File

@@ -1066,12 +1066,7 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
if (_sensor_correction_pub == nullptr) {
_sensor_correction_pub = orb_advertise(ORB_ID(sensor_correction), &_corrections);
} else {
orb_publish(ORB_ID(sensor_correction), _sensor_correction_pub, &_corrections);
}
_sensor_correction_pub.publish(_corrections);
_corrections_changed = false;
}
@@ -1080,12 +1075,7 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s
if (_selection_changed) {
_selection.timestamp = hrt_absolute_time();
if (_sensor_selection_pub == nullptr) {
_sensor_selection_pub = orb_advertise(ORB_ID(sensor_selection), &_selection);
} else {
orb_publish(ORB_ID(sensor_selection), _sensor_selection_pub, &_selection);
}
_sensor_selection_pub.publish(_selection);
_selection_changed = false;
}

6
src/modules/sensors/voted_sensors_update.h
View File

@@ -52,6 +52,7 @@
#include <lib/ecl/validation/data_validator.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
@@ -247,8 +248,9 @@ private:
SensorData _baro {};
orb_advert_t _mavlink_log_pub{nullptr};
orb_advert_t _sensor_correction_pub{nullptr}; /**< handle to the sensor correction uORB topic */
orb_advert_t _sensor_selection_pub{nullptr}; /**< handle to the sensor selection uORB topic */
uORB::Publication<sensor_correction_s> _sensor_correction_pub{ORB_ID(sensor_correction)}; /**< handle to the sensor correction uORB topic */
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
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 */