fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors partially move to new uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar 2019-06-04 14:40:16 -04:00
parent 55c9786157
commit 528d2f61a0
3 changed files with 28 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,14 +39,8 @@
#include "rc_update.h"
#include <string.h>
#include <float.h>
#include <errno.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/input_rc.h>
using namespace sensors;
@ -57,32 +51,6 @@ RCUpdate::RCUpdate(const Parameters &parameters)
_filter_yaw(50.0f, 10.f),
_filter_throttle(50.0f, 10.f)
{
memset(&_rc, 0, sizeof(_rc));
memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
memset(&_param_rc_values, 0, sizeof(_param_rc_values));
}
int RCUpdate::init()
{
_rc_sub = orb_subscribe(ORB_ID(input_rc));
if (_rc_sub < 0) {
return -errno;
}
_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
if (_rc_parameter_map_sub < 0) {
return -errno;
}
return 0;
}
void RCUpdate::deinit()
{
orb_unsubscribe(_rc_sub);
orb_unsubscribe(_rc_parameter_map_sub);
}
void RCUpdate::update_rc_functions()
@ -134,11 +102,8 @@ void RCUpdate::update_rc_functions()
void
RCUpdate::rc_parameter_map_poll(ParameterHandles &parameter_handles, bool forced)
{
bool map_updated;
orb_check(_rc_parameter_map_sub, &map_updated);
if (map_updated) {
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
if (_rc_parameter_map_sub.updated()) {
_rc_parameter_map_sub.copy(&_rc_parameter_map);
/* update parameter handles to which the RC channels are mapped */
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
@ -253,14 +218,10 @@ RCUpdate::set_params_from_rc(const ParameterHandles &parameter_handles)
void
RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
{
bool rc_updated;
orb_check(_rc_sub, &rc_updated);
if (rc_updated) {
if (_rc_sub.updated()) {
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
struct input_rc_s rc_input;
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
input_rc_s rc_input{};
_rc_sub.copy(&rc_input);
/* detect RC signal loss */
bool signal_lost;

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

@ -44,7 +44,9 @@
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/rc_parameter_map.h>
namespace sensors
{
@ -63,17 +65,6 @@ public:
*/
RCUpdate(const Parameters &parameters);
/**
* initialize subscriptions etc.
* @return 0 on success, <0 otherwise
*/
int init();
/**
* deinitialize the object (we cannot use the destructor because it is called on the wrong thread)
*/
void deinit();
/**
* Check for changes in rc_parameter_map
*/
@ -111,17 +102,17 @@ private:
void set_params_from_rc(const ParameterHandles &parameter_handles);
int _rc_sub = -1; /**< raw rc channels data subscription */
int _rc_parameter_map_sub = -1; /**< rc parameter map subscription */
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 */
struct rc_channels_s _rc; /**< r/c channel data */
rc_channels_s _rc {}; /**< r/c channel data */
struct rc_parameter_map_s _rc_parameter_map;
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
rc_parameter_map_s _rc_parameter_map {};
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {}; /**< parameter values for RC control */
const Parameters &_parameters;

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

@ -163,10 +163,10 @@ private:
const bool _hil_enabled; /**< if true, HIL is active */
bool _armed{false}; /**< arming status of the vehicle */
int _actuator_ctrl_0_sub{-1}; /**< attitude controls sub */
int _diff_pres_sub{-1}; /**< raw differential pressure subscription */
int _vcontrol_mode_sub{-1}; /**< vehicle control mode subscription */
int _params_sub{-1}; /**< notification of parameter updates */
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 */
orb_advert_t _sensor_pub{nullptr}; /**< combined sensor data topic */
orb_advert_t _airdata_pub{nullptr}; /**< combined sensor data topic */
@ -220,11 +220,6 @@ private:
*/
void diff_pres_poll(const vehicle_air_data_s &airdata);
/**
* Check for changes in vehicle control mode.
*/
void vehicle_control_mode_poll();
/**
* Check for changes in parameters.
*/
@ -297,16 +292,9 @@ Sensors::adc_init()
void
Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
{
bool updated;
orb_check(_diff_pres_sub, &updated);
differential_pressure_s diff_pres{};
if (updated) {
differential_pressure_s diff_pres;
int ret = orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &diff_pres);
if (ret != PX4_OK) {
return;
}
if (_diff_pres_sub.update(&diff_pres)) {
float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature :
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
@ -360,33 +348,14 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
}
}
void
Sensors::vehicle_control_mode_poll()
{
struct vehicle_control_mode_s vcontrol_mode;
bool vcontrol_mode_updated;
orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
if (vcontrol_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
_armed = vcontrol_mode.flag_armed;
}
}
void
Sensors::parameter_update_poll(bool forced)
{
bool param_updated = false;
/* Check if any parameter has changed */
orb_check(_params_sub, &param_updated);
parameter_update_s update;
if (param_updated || forced) {
if (_params_sub.update(&update) || forced) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
parameters_update();
updateParams();
@ -557,10 +526,10 @@ Sensors::adc_poll()
connected &= valid_chan[b];
}
actuator_controls_s ctrl;
orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
actuator_controls_s ctrl{};
_actuator_ctrl_0_sub.copy(&ctrl);
battery_status_s battery_status;
battery_status_s battery_status{};
_battery[b].updateBatteryStatus(t, bat_voltage_v[b], bat_current_a[b],
connected, selected_source == b, b,
ctrl.control[actuator_controls_s::INDEX_THROTTLE],
@ -592,20 +561,11 @@ Sensors::run()
vehicle_air_data_s airdata = {};
vehicle_magnetometer_s magnetometer = {};
_rc_update.init();
_voted_sensors_update.init(raw);
/* (re)load params and calibration */
parameter_update_poll(true);
/*
* do subscriptions
*/
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
/* get a set of initial values */
_voted_sensors_update.sensors_poll(raw, airdata, magnetometer);
@ -649,7 +609,11 @@ Sensors::run()
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
if (_vcontrol_mode_sub.updated()) {
vehicle_control_mode_s vcontrol_mode{};
_vcontrol_mode_sub.copy(&vcontrol_mode);
_armed = vcontrol_mode.flag_armed;
}
/* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro
* a mandatory sensor) */
@ -714,11 +678,6 @@ Sensors::run()
perf_end(_loop_perf);
}
orb_unsubscribe(_diff_pres_sub);
orb_unsubscribe(_vcontrol_mode_sub);
orb_unsubscribe(_params_sub);
orb_unsubscribe(_actuator_ctrl_0_sub);
if (_sensor_pub) {
orb_unadvertise(_sensor_pub);
}
@ -731,7 +690,6 @@ Sensors::run()
orb_unadvertise(_magnetometer_pub);
}
_rc_update.deinit();
_voted_sensors_update.deinit();
}