yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00: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
Download Patch File Download Diff File Expand all files Collapse all files

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();
}