From c8e59c4e39c9f46633c07ea2d16d9d03706caf69 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 28 Jul 2019 11:55:52 -0400 Subject: [PATCH] parameter_update use uORB::Subscription consistently --- src/drivers/heater/heater.cpp | 8 ++++-- src/drivers/heater/heater.h | 2 +- src/drivers/lights/rgbled/rgbled.cpp | 9 ++++-- .../rgbled_ncp5623c/rgbled_ncp5623c.cpp | 9 ++++-- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 21 ++++++-------- src/drivers/pwm_out_sim/PWMSim.cpp | 17 +++++------ src/drivers/px4fmu/fmu.cpp | 14 ++++++---- src/drivers/px4io/px4io.cpp | 12 +++++--- .../snapdragon_pwm_out/snapdragon_pwm_out.cpp | 17 ++++++----- src/drivers/tap_esc/tap_esc.cpp | 20 ++++++------- src/drivers/uavcan/uavcan_main.cpp | 17 ++++++----- src/examples/bottle_drop/bottle_drop.cpp | 17 +++++------ src/examples/fixedwing_control/main.cpp | 26 ++++++++--------- src/examples/rover_steering_control/main.cpp | 28 ++++++++----------- .../attitude_estimator_q_main.cpp | 11 +++++--- src/modules/commander/Commander.cpp | 12 ++++---- src/modules/commander/Commander.hpp | 2 ++ src/modules/ekf2/ekf2_main.cpp | 13 +++++---- .../FixedwingAttitudeControl.cpp | 13 +++++---- .../FixedwingAttitudeControl.hpp | 2 +- .../FixedwingPositionControl.cpp | 14 ++++------ .../FixedwingPositionControl.hpp | 2 +- .../land_detector/FixedwingLandDetector.cpp | 8 ++++-- .../land_detector/FixedwingLandDetector.h | 2 +- src/modules/land_detector/LandDetector.cpp | 9 ++++-- src/modules/land_detector/LandDetector.h | 2 +- src/modules/logger/logger.cpp | 6 ++-- src/modules/mavlink/mavlink_main.cpp | 14 ++++++---- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++------- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/mc_att_control/mc_att_control.hpp | 2 +- .../mc_att_control/mc_att_control_main.cpp | 10 ++++--- .../mc_pos_control/mc_pos_control_main.cpp | 11 +++++--- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 12 ++++---- .../RoverPositionControl.cpp | 23 ++++++--------- .../RoverPositionControl.hpp | 5 ++-- src/modules/sensors/sensors.cpp | 13 +++++---- src/modules/sih/sih.cpp | 14 ++++------ src/modules/sih/sih.hpp | 3 +- src/modules/simulator/simulator.cpp | 16 ++++------- src/modules/simulator/simulator.h | 8 ++---- src/modules/vmount/vmount.cpp | 19 ++++++------- .../vtol_att_control_main.cpp | 12 ++++---- .../vtol_att_control/vtol_att_control_main.h | 2 +- src/templates/module/module.cpp | 24 ++++++---------- src/templates/module/module.h | 8 +++++- 47 files changed, 274 insertions(+), 259 deletions(-) diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 7186928b33..48cb7a2cd1 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -292,9 +292,13 @@ float Heater::temperature_setpoint(char *argv[]) void Heater::update_params(const bool force) { - parameter_update_s param_update; + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (_params_sub.update(¶m_update) || force) { + // update parameters from storage ModuleParams::updateParams(); } } diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index e492d570f6..1c8fb66e62 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -174,7 +174,7 @@ private: float _integrator_value = 0.0f; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; float _proportional_value = 0.0f; diff --git a/src/drivers/lights/rgbled/rgbled.cpp b/src/drivers/lights/rgbled/rgbled.cpp index a15a584326..49cf847f16 100644 --- a/src/drivers/lights/rgbled/rgbled.cpp +++ b/src/drivers/lights/rgbled/rgbled.cpp @@ -80,7 +80,8 @@ private: volatile bool _running{false}; volatile bool _should_run{true}; bool _leds_enabled{true}; - uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; LedController _led_controller; @@ -202,11 +203,13 @@ RGBLED::Run() return; } - if (_param_sub.updated()) { + // check for parameter updates + if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; - _param_sub.copy(&pupdate); + _parameter_update_sub.copy(&pupdate); + // update parameters from storage update_params(); // Immediately update to change brightness diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp index a794902ed5..b77211e35f 100755 --- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp +++ b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp @@ -80,7 +80,8 @@ private: volatile bool _running{false}; volatile bool _should_run{true}; bool _leds_enabled{true}; - uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; LedController _led_controller; @@ -166,11 +167,13 @@ RGBLED_NPC5623C::Run() return; } - if (_param_sub.updated()) { + // check for parameter updates + if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; - _param_sub.copy(&pupdate); + _parameter_update_sub.copy(&pupdate); + // update parameters from storage update_params(); // Immediately update to change brightness diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 2e6db9ce33..340c4540d1 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include #include @@ -260,7 +260,7 @@ void task_main(int argc, char *argv[]) Mixer::Airmode airmode = Mixer::Airmode::disabled; update_params(airmode); - int params_sub = orb_subscribe(ORB_ID(parameter_update)); + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; int rc_channels_sub = -1; @@ -418,16 +418,15 @@ void task_main(int argc, char *argv[]) _task_should_exit = true; } - /* check for parameter updates */ - bool param_updated = false; - orb_check(params_sub, ¶m_updated); + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - if (param_updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), params_sub, &update); + // update parameters from storage update_params(airmode); } - } delete pwm_out; @@ -447,10 +446,6 @@ void task_main(int argc, char *argv[]) orb_unsubscribe(rc_channels_sub); } - if (params_sub != -1) { - orb_unsubscribe(params_sub); - } - perf_free(_perf_control_latency); _is_running = false; diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 4f5c0934f0..94809cea10 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -36,6 +36,8 @@ #include #include +#include +#include #include PWMSim::PWMSim() : @@ -162,7 +164,7 @@ PWMSim::run() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); update_params(); - int params_sub = orb_subscribe(ORB_ID(parameter_update)); + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; /* loop until killed */ while (!should_exit()) { @@ -323,13 +325,13 @@ PWMSim::run() } } - /* check for parameter updates */ - bool param_updated = false; - orb_check(params_sub, ¶m_updated); + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - if (param_updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), params_sub, &update); + // update parameters from storage update_params(); } } @@ -341,7 +343,6 @@ PWMSim::run() } orb_unsubscribe(_armed_sub); - orb_unsubscribe(params_sub); } int diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 884530668d..89246ee2e8 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -177,7 +177,7 @@ private: unsigned _current_update_rate{0}; - uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; unsigned _num_outputs{0}; @@ -767,7 +767,13 @@ PX4FMU::Run() update_pwm_out_state(pwm_on); } - if (_param_sub.updated()) { + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage update_params(); } @@ -783,16 +789,12 @@ PX4FMU::Run() void PX4FMU::update_params() { - parameter_update_s pupdate; - _param_sub.update(&pupdate); - update_pwm_rev_mask(); update_pwm_trims(); updateParams(); } - int PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e61f291f58..8270572e36 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -256,7 +256,7 @@ private: uORB::Subscription _t_actuator_controls_3{ORB_ID(actuator_controls_3)};; ///< actuator controls group 3 topic uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic uORB::Subscription _t_vehicle_control_mode{ORB_ID(vehicle_control_mode)}; ///< vehicle control mode topic - uORB::Subscription _t_param{ORB_ID(parameter_update)}; ///< parameter update topic + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< parameter update topic uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic bool _param_update_force; ///< force a parameter update @@ -997,10 +997,14 @@ PX4IO::task_main() * * XXX this may be a bit spammy */ - if (_t_param.updated() || _param_update_force) { - _param_update_force = false; + + // check for parameter updates + if (_parameter_update_sub.updated() || _param_update_force) { + // clear update parameter_update_s pupdate; - _t_param.copy(&pupdate); + _parameter_update_sub.copy(&pupdate); + + _param_update_force = false; if (!_rc_handling_disabled) { /* re-upload RC input config as it may have changed */ diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp index d642a2c5fd..a216c0bb21 100644 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp @@ -40,7 +40,7 @@ #include // NAN #include -#include +#include #include #include #include @@ -357,7 +357,7 @@ void task_main(int argc, char *argv[]) Mixer::Airmode airmode = Mixer::Airmode::disabled; update_params(airmode); - int params_sub = orb_subscribe(ORB_ID(parameter_update)); + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; // Start disarmed _armed.armed = false; @@ -471,13 +471,13 @@ void task_main(int argc, char *argv[]) } } - /* check for parameter updates */ - bool param_updated = false; - orb_check(params_sub, ¶m_updated); + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - if (param_updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), params_sub, &update); + // update parameters from storage update_params(airmode); } } @@ -491,7 +491,6 @@ void task_main(int argc, char *argv[]) } orb_unsubscribe(_armed_sub); - orb_unsubscribe(params_sub); perf_free(_perf_control_latency); diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index c4fee63e43..27970cc02b 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include #include @@ -114,7 +114,9 @@ private: bool _is_armed = false; int _armed_sub = -1; int _test_motor_sub = -1; - int _params_sub = -1; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + orb_advert_t _outputs_pub = nullptr; actuator_outputs_s _outputs = {}; actuator_armed_s _armed = {}; @@ -191,7 +193,6 @@ TAP_ESC::~TAP_ESC() orb_unsubscribe(_armed_sub); orb_unsubscribe(_test_motor_sub); - orb_unsubscribe(_params_sub); orb_unadvertise(_outputs_pub); orb_unadvertise(_esc_feedback_pub); @@ -336,7 +337,6 @@ int TAP_ESC::init() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); return ret; } @@ -609,13 +609,13 @@ void TAP_ESC::cycle() } - /* check for parameter updates */ - bool param_updated = false; - orb_check(_params_sub, ¶m_updated); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (param_updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); + // update parameters from storage updateParams(); } } diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index fad2e46832..42320559ae 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -55,6 +55,7 @@ #include #include +#include #include #include @@ -810,17 +811,17 @@ int UavcanNode::run() update_params(); - int params_sub = orb_subscribe(ORB_ID(parameter_update)); + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; while (!_task_should_exit) { - /* check for parameter updates */ - bool param_updated = false; - orb_check(params_sub, ¶m_updated); + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - if (param_updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), params_sub, &update); + // update parameters from storage update_params(); } @@ -995,8 +996,6 @@ int UavcanNode::run() } } - orb_unsubscribe(params_sub); - (void)::close(busevent_fd); teardown(); diff --git a/src/examples/bottle_drop/bottle_drop.cpp b/src/examples/bottle_drop/bottle_drop.cpp index 5b32587e44..d7be5df8d4 100644 --- a/src/examples/bottle_drop/bottle_drop.cpp +++ b/src/examples/bottle_drop/bottle_drop.cpp @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -411,9 +411,7 @@ BottleDrop::task_main() int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct parameter_update_s update; - memset(&update, 0, sizeof(update)); - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; struct mission_item_s flight_vector_s {}; struct mission_item_s flight_vector_e {}; @@ -492,12 +490,11 @@ BottleDrop::task_main() orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } - // Get parameter updates - orb_check(parameter_update_sub, &updated); - - if (updated) { - // copy global position - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); // update all parameters param_get(param_gproperties, &z_0); diff --git a/src/examples/fixedwing_control/main.cpp b/src/examples/fixedwing_control/main.cpp index 8589c26629..aa4258639f 100644 --- a/src/examples/fixedwing_control/main.cpp +++ b/src/examples/fixedwing_control/main.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -326,15 +327,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); + + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; /* Setup of loop */ - struct pollfd fds[2] = {}; - fds[0].fd = param_sub; + struct pollfd fds[1] {}; + fds[0].fd = att_sub; fds[0].events = POLLIN; - fds[1].fd = att_sub; - fds[1].events = POLLIN; while (!thread_should_exit) { @@ -348,7 +348,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) * This design pattern makes the controller also agnostic of the attitude * update speed - it runs as fast as the attitude updates with minimal latency. */ - int ret = poll(fds, 2, 500); + int ret = poll(fds, 1, 500); if (ret < 0) { /* @@ -361,18 +361,18 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* no return value = nothing changed for 500 ms, ignore */ } else { - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag (uORB API requirement) */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - /* if a param update occured, re-read our parameters */ + // if a param update occured, re-read our parameters parameters_update(&ph, &p); } /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { + if (fds[0].revents & POLLIN) { /* Check if there is a new position measurement or position setpoint */ diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 723a371b05..3731bc38fe 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -271,20 +271,16 @@ int rover_steering_control_thread_main(int argc, char *argv[]) int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; /* Setup of loop */ - struct pollfd fds[2]; + struct pollfd fds[1] {}; - fds[0].fd = param_sub; + fds[0].fd = att_sub; fds[0].events = POLLIN; - fds[1].fd = att_sub; - - fds[1].events = POLLIN; - while (!thread_should_exit) { /* @@ -297,7 +293,7 @@ int rover_steering_control_thread_main(int argc, char *argv[]) * This design pattern makes the controller also agnostic of the attitude * update speed - it runs as fast as the attitude updates with minimal latency. */ - int ret = poll(fds, 2, 500); + int ret = poll(fds, 1, 500); if (ret < 0) { /* @@ -310,18 +306,18 @@ int rover_steering_control_thread_main(int argc, char *argv[]) /* no return value = nothing changed for 500 ms, ignore */ } else { - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag (uORB API requirement) */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - /* if a param update occured, re-read our parameters */ + // if a param update occured, re-read our parameters parameters_update(&ph, &pp); } /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { + if (fds[0].revents & POLLIN) { /* Check if there is a new position measurement or position setpoint */ diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 326e36e374..92164319f3 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -109,7 +109,7 @@ private: int _sensors_sub = -1; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _vision_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)}; @@ -437,10 +437,13 @@ void AttitudeEstimatorQ::task_main() void AttitudeEstimatorQ::update_parameters(bool force) { - if (_params_sub.updated()) { - parameter_update_s param_update; - _params_sub.copy(¶m_update); + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + // update parameters param_get(_params_handles.w_acc, &_w_accel); param_get(_params_handles.w_mag, &_w_mag); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a9d6268a86..12d994ee1a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -87,7 +87,6 @@ #include #include #include -#include #include #include #include @@ -1297,7 +1296,6 @@ Commander::run() uORB::Subscription cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription land_detector_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription param_changed_sub{ORB_ID(parameter_update)}; uORB::Subscription safety_sub{ORB_ID(safety)}; uORB::Subscription sp_man_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription subsys_sub{ORB_ID(subsystem_info)}; @@ -1418,14 +1416,14 @@ Commander::run() transition_result_t arming_ret = TRANSITION_NOT_CHANGED; /* update parameters */ - bool params_updated = param_changed_sub.updated(); + bool params_updated = _parameter_update_sub.updated(); if (params_updated || param_init_forced) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); - /* parameters changed */ - parameter_update_s param_changed; - param_changed_sub.copy(¶m_changed); - + // update parameters from storage updateParams(); /* update parameters */ diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 90a14ffaad..a5fd4dcfd4 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -274,6 +275,7 @@ private: bool _print_avoidance_msg_once{false}; // Subscriptions + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index f0979f2197..f36f14ce2c 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -265,7 +265,7 @@ private: uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; @@ -739,10 +739,13 @@ void Ekf2::run() perf_begin(_perf_update_data); - if (_params_sub.updated()) { - // read from param to clear updated flag - parameter_update_s update; - _params_sub.copy(&update); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage updateParams(); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c539d989f9..8b25362a9f 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -454,15 +454,16 @@ void FixedwingAttitudeControl::Run() if (_att_sub.update(&_att)) { - /* only update parameters if they changed */ - bool params_updated = _params_sub.updated(); + // only update parameters if they changed + bool params_updated = _parameter_update_sub.updated(); + // check for parameter updates if (params_updated) { - /* read from param to clear updated flag */ - parameter_update_s update; - _params_sub.copy(&update); + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - /* update parameters from storage */ + // update parameters from storage parameters_update(); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 651c206a67..e33369cf5b 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -101,7 +101,7 @@ private: uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */ uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 042d561d6f..1c53832d6a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1662,15 +1662,13 @@ FixedwingPositionControl::Run() /* only run controller if position changed */ if (_global_pos_sub.update(&_global_pos)) { - /* only update parameters if they changed */ - bool params_updated = _params_sub.updated(); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (params_updated) { - /* read from param to clear updated flag */ - parameter_update_s update; - _params_sub.copy(&update); - - /* update parameters from storage */ + // update parameters from storage parameters_update(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index c5364e2c18..2cec57b5ed 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -156,7 +156,7 @@ private: uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription */ uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _manual_control_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates */ - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates */ uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; ///< vehicle attitude subscription */ diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 64b99025fd..f190b5f335 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -62,9 +62,13 @@ void FixedwingLandDetector::_update_topics() void FixedwingLandDetector::_update_params() { - parameter_update_s param_update; + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (_param_update_sub.update(¶m_update)) { + // update parameters from storage _update_params(); } } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 020a769e20..843766e795 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -73,7 +73,7 @@ private: static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; - uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 9a031aa576..e6e2a7d2ea 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -151,10 +151,15 @@ void LandDetector::Run() void LandDetector::_check_params(const bool force) { - parameter_update_s param_update; + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (_param_update_sub.update(¶m_update) || force) { + // update parameters from storage _update_params(); + _update_total_flight_time(); } } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 41dc9df878..a971e3328d 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -174,7 +174,7 @@ private: perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "land_detector_cycle")}; uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; - uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; DEFINE_PARAMETERS_CUSTOM_PARENT( ModuleParams, diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 8bd3f210c2..c40e221f20 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -976,9 +976,11 @@ void Logger::run() /* Check if parameters have changed */ if (!_should_stop_file_log) { // do not record param changes after disarming - parameter_update_s param_update; + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - if (parameter_update_sub.update(¶m_update)) { write_changed_parameters(LogType::Full); } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 026e9a9d6d..a58ad4d2ed 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2141,9 +2141,9 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_init(&_message_buffer_mutex, nullptr); } + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; + MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command), 0, true); - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); - uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); uint64_t status_time = 0; MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack), 0, true); @@ -2239,9 +2239,13 @@ Mavlink::task_main(int argc, char *argv[]) update_rate_mult(); - parameter_update_s param_update; + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - if (param_sub->update(¶m_time, ¶m_update)) { + // update parameters from storage mavlink_update_parameters(); #if defined(CONFIG_NET) @@ -2250,7 +2254,7 @@ Mavlink::task_main(int argc, char *argv[]) _src_addr_initialized = false; } -#endif +#endif // CONFIG_NET } check_radio_config(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 39c5fc83a9..b8d83625e7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2569,9 +2569,15 @@ MavlinkReceiver::Run() hrt_abstime last_send_update = 0; while (!_mavlink->_task_should_exit) { - // Check for updated parameters. - if (_param_update_sub.updated()) { - update_params(); + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); } if (poll(&fds[0], 1, timeout) > 0) { @@ -2718,11 +2724,3 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent) pthread_attr_destroy(&receiveloop_attr); } - -void -MavlinkReceiver::update_params() -{ - parameter_update_s param_update; - _param_update_sub.update(¶m_update); - updateParams(); -} diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ddfdda261d..1b9051724a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -259,7 +259,7 @@ private: // ORB subscriptions uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 200c7d92e6..10fb522d63 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -146,7 +146,7 @@ private: uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3464b3254d..fa265e9683 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -137,11 +137,13 @@ MulticopterAttitudeControl::parameters_updated() void MulticopterAttitudeControl::parameter_update_poll() { - /* Check if parameters have changed */ - parameter_update_s param_update; + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (_params_sub.update(¶m_update)) { - updateParams(); + // update parameters from storage parameters_updated(); } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 96a534b587..7ae1fcbbd6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -123,7 +123,7 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _control_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 _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude */ uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */ @@ -312,10 +312,13 @@ MulticopterPositionControl::warn_rate_limited(const char *string) int MulticopterPositionControl::parameters_update(bool force) { - parameter_update_s param_upd; - bool updated = _params_sub.update(¶m_upd); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - if (updated || force) { + // update parameters from storage ModuleParams::updateParams(); SuperBlock::updateParams(); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b0549c8a28..501746231d 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -317,7 +317,7 @@ private: uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */ uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ - uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; /**< param update subscription */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< param update subscription */ uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */ uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */ uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3f9b6336eb..32c096bfb9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -119,9 +119,6 @@ Navigator::~Navigator() void Navigator::params_update() { - parameter_update_s param_update; - _param_update_sub.update(¶m_update); - updateParams(); if (_handle_back_trans_dec_mss != PARAM_INVALID) { @@ -202,8 +199,13 @@ Navigator::run() } } - /* parameters updated */ - if (_param_update_sub.updated()) { + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage params_update(); } diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 5f100e9703..ab6c2fb6d9 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -70,18 +70,15 @@ RoverPositionControl::~RoverPositionControl() perf_free(_loop_perf); } -void RoverPositionControl::parameters_update(int parameter_update_sub, bool force) +void RoverPositionControl::parameters_update(bool force) { - bool updated; - struct parameter_update_s param_upd; + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - orb_check(parameter_update_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_upd); - } - - if (force || updated) { + // update parameters from storage updateParams(); _gnd_control.set_l1_damping(_param_l1_damping.get()); @@ -275,7 +272,6 @@ RoverPositionControl::run() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -287,7 +283,7 @@ RoverPositionControl::run() orb_set_interval(_global_pos_sub, 20); orb_set_interval(_local_pos_sub, 20); - parameters_update(_params_sub, true); + parameters_update(true); /* wakeup source(s) */ px4_pollfd_struct_t fds[3]; @@ -318,7 +314,7 @@ RoverPositionControl::run() _vehicle_acceleration_sub.update(); /* update parameters from storage */ - parameters_update(_params_sub); + parameters_update(); bool manual_mode = _control_mode.flag_control_manual_enabled; @@ -430,7 +426,6 @@ RoverPositionControl::run() orb_unsubscribe(_global_pos_sub); orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_manual_control_sub); - orb_unsubscribe(_params_sub); orb_unsubscribe(_pos_sp_triplet_sub); orb_unsubscribe(_vehicle_attitude_sub); orb_unsubscribe(_sensor_combined_sub); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index c10a3cc681..9b75824014 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -105,11 +105,12 @@ private: int _global_pos_sub{-1}; int _local_pos_sub{-1}; int _manual_control_sub{-1}; /**< notification of manual control updates */ - int _params_sub{-1}; /**< notification of parameter updates */ int _pos_sp_triplet_sub{-1}; int _vehicle_attitude_sub{-1}; int _sensor_combined_sub{-1}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + manual_control_setpoint_s _manual{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ vehicle_control_mode_s _control_mode{}; /**< control mode */ @@ -167,7 +168,7 @@ private: /** * Update our local parameter cache. */ - void parameters_update(int parameter_update_sub, bool force = false); + void parameters_update(bool force = false); void manual_control_setpoint_poll(); void position_setpoint_triplet_poll(); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ca6f16f7ad..448fff2580 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -168,7 +168,7 @@ 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 _params_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ + 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::Publication _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */ @@ -364,12 +364,13 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw) void Sensors::parameter_update_poll(bool forced) { - /* Check if any parameter has changed */ - parameter_update_s update; - - if (_params_sub.update(&update) || forced) { - /* read from param to clear updated flag */ + // check for parameter updates + if (_parameter_update_sub.updated() || forced) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + // update parameters from storage parameters_update(); updateParams(); diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index ca93824212..c15ff93d3f 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -110,7 +110,6 @@ void Sih::run() _actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs)); // initialize parameters - _parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); parameters_update_poll(); init_variables(); @@ -143,7 +142,6 @@ void Sih::run() hrt_cancel(&_timer_call); // close the periodic timer interruption px4_sem_destroy(&_data_semaphore); orb_unsubscribe(_actuator_out_sub); - orb_unsubscribe(_parameter_update_sub); } // timer_callback() is used as a real time callback to post the semaphore @@ -186,13 +184,13 @@ void Sih::inner_loop() void Sih::parameters_update_poll() { - bool updated = false; - struct parameter_update_s param_upd; + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - orb_check(_parameter_update_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameter_update_sub, ¶m_upd); + // update parameters from storage updateParams(); parameters_updated(); } diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 64553e6584..e0e510c256 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include // to publish groundtruth #include // to publish groundtruth @@ -119,7 +120,7 @@ private: vehicle_global_position_s _gpos_gt{}; orb_advert_t _gpos_gt_pub{nullptr}; - int _parameter_update_sub {-1}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; int _actuator_out_sub {-1}; // hard constants diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index a7fde33cd6..2872f9cee5 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -77,17 +77,13 @@ void Simulator::write_gps_data(void *buf) void Simulator::parameters_update(bool force) { - bool updated; - struct parameter_update_s param_upd; + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - orb_check(_param_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _param_sub, ¶m_upd); - } - - if (updated || force) { - // update C++ param system + // update parameters from storage updateParams(); } } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 2518e3ba45..7161212789 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -181,8 +182,6 @@ private: _gps.writeData(&gps_data); - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - _battery_status.timestamp = hrt_absolute_time(); _px4_accel.set_sample_rate(250); @@ -191,9 +190,6 @@ private: ~Simulator() { - // Unsubscribe from uORB topics. - orb_unsubscribe(_param_sub); - // free perf counters perf_free(_perf_gps); perf_free(_perf_sim_delay); @@ -230,7 +226,7 @@ private: orb_advert_t _irlock_report_pub{nullptr}; orb_advert_t _visual_odometry_pub{nullptr}; - int _param_sub{-1}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; unsigned int _port{14560}; diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index 00a087adaf..0fe76ad8b6 100644 --- a/src/modules/vmount/vmount.cpp +++ b/src/modules/vmount/vmount.cpp @@ -61,7 +61,7 @@ #include "output_rc.h" #include "output_mavlink.h" -#include +#include #include #include @@ -216,7 +216,7 @@ static int vmount_thread_main(int argc, char *argv[]) return -1; } - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; thread_running = true; ControlData *control_data = nullptr; g_thread_data = &thread_data; @@ -370,13 +370,14 @@ static int vmount_thread_main(int argc, char *argv[]) break; } + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); - //check for parameter changes - bool updated; - - if (orb_check(parameter_update_sub, &updated) == 0 && updated) { - parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); + // update parameters from storage + bool updated = false; update_params(param_handles, params, updated); if (updated) { @@ -402,8 +403,6 @@ static int vmount_thread_main(int argc, char *argv[]) g_thread_data = nullptr; - orb_unsubscribe(parameter_update_sub); - for (int i = 0; i < input_objs_len_max; ++i) { if (thread_data.input_objs[i]) { delete (thread_data.input_objs[i]); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 85ff2a6a52..49683cccf2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -333,13 +333,13 @@ VtolAttitudeControl::Run() } if (should_run) { - /* only update parameters if they changed */ - if (_params_sub.updated()) { - /* read from param to clear updated flag */ - parameter_update_s update; - _params_sub.copy(&update); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - /* update parameters from storage */ + // update parameters from storage parameters_update(); } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 359fdccdab..2e9a81acaf 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -143,7 +143,7 @@ private: uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)}; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; //parameter updates subscription + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; //vehicle attitude subscription diff --git a/src/templates/module/module.cpp b/src/templates/module/module.cpp index d1e4c7fb12..6780cc6c4e 100644 --- a/src/templates/module/module.cpp +++ b/src/templates/module/module.cpp @@ -145,8 +145,7 @@ void Module::run() fds[0].events = POLLIN; // initialize parameters - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - parameters_update(parameter_update_sub, true); + parameters_update(true); while (!should_exit()) { @@ -170,26 +169,21 @@ void Module::run() } - - parameters_update(parameter_update_sub); + parameters_update(); } orb_unsubscribe(sensor_combined_sub); - orb_unsubscribe(parameter_update_sub); } -void Module::parameters_update(int parameter_update_sub, bool force) +void Module::parameters_update(bool force) { - bool updated; - struct parameter_update_s param_upd; + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); - orb_check(parameter_update_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_upd); - } - - if (force || updated) { + // update parameters from storage updateParams(); } } diff --git a/src/templates/module/module.h b/src/templates/module/module.h index a7e9fdefd8..772d59bd68 100644 --- a/src/templates/module/module.h +++ b/src/templates/module/module.h @@ -35,6 +35,8 @@ #include #include +#include +#include extern "C" __EXPORT int module_main(int argc, char *argv[]); @@ -71,12 +73,16 @@ private: * @param parameter_update_sub uorb subscription to parameter_update * @param force for a parameter update */ - void parameters_update(int parameter_update_sub, bool force = false); + void parameters_update(bool force = false); DEFINE_PARAMETERS( (ParamInt) _param_sys_autostart, /**< example parameter */ (ParamInt) _param_sys_autoconfig /**< another parameter */ ) + + // Subscriptions + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + };