From b1d772f594397e774e4ea074df16407d348b9d8f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 30 May 2019 13:48:31 -0400 Subject: [PATCH] mc_att_control move most orb subscriptions to uORB::Subscription --- src/modules/mc_att_control/mc_att_control.hpp | 37 ++- .../mc_att_control/mc_att_control_main.cpp | 211 ++---------------- 2 files changed, 38 insertions(+), 210 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 3185932db4b8..4bcdba156f5c 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -101,19 +102,10 @@ class MulticopterAttitudeControl : public ModuleBase /** * Check for parameter update and handle it. */ - void battery_status_poll(); void parameter_update_poll(); - void sensor_bias_poll(); - void vehicle_land_detected_poll(); - void sensor_correction_poll(); bool vehicle_attitude_poll(); - void vehicle_attitude_setpoint_poll(); - void vehicle_control_mode_poll(); - bool vehicle_manual_poll(); void vehicle_motor_limits_poll(); - bool vehicle_rates_setpoint_poll(); void vehicle_status_poll(); - void landing_gear_state_poll(); void publish_actuator_controls(); void publish_rates_setpoint(); @@ -150,20 +142,21 @@ class MulticopterAttitudeControl : public ModuleBase AttitudeControl _attitude_control; /**< class for attitude control calculations */ - int _v_att_sub{-1}; /**< vehicle attitude subscription */ - int _v_att_sp_sub{-1}; /**< vehicle attitude setpoint subscription */ - int _v_rates_sp_sub{-1}; /**< vehicle rates setpoint subscription */ - int _v_control_mode_sub{-1}; /**< vehicle control mode subscription */ - int _params_sub{-1}; /**< parameter updates subscription */ - int _manual_control_sp_sub{-1}; /**< manual control setpoint subscription */ - int _vehicle_status_sub{-1}; /**< vehicle status subscription */ - int _motor_limits_sub{-1}; /**< motor limits subscription */ - int _battery_status_sub{-1}; /**< battery status subscription */ + uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ + 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 _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 */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; + int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ - int _sensor_correction_sub{-1}; /**< sensor thermal correction subscription */ - int _sensor_bias_sub{-1}; /**< sensor in-run bias correction subscription */ - int _vehicle_land_detected_sub{-1}; /**< vehicle land detected subscription */ - int _landing_gear_sub{-1}; unsigned _gyro_count{1}; int _selected_gyro{0}; 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 2f708416b317..abcba1a7fd83 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -171,83 +171,20 @@ MulticopterAttitudeControl::parameters_updated() void MulticopterAttitudeControl::parameter_update_poll() { - bool updated; - /* Check if parameters have changed */ - orb_check(_params_sub, &updated); + parameter_update_s param_update; - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + if (_params_sub.update(¶m_update)) { updateParams(); parameters_updated(); } } -void -MulticopterAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check if vehicle control mode has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -bool -MulticopterAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - return true; - } - return false; -} - -void -MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); - } -} - -bool -MulticopterAttitudeControl::vehicle_rates_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); - return true; - } - return false; -} - void MulticopterAttitudeControl::vehicle_status_poll() { /* check if there is new status information */ - bool vehicle_status_updated; - orb_check(_vehicle_status_sub, &vehicle_status_updated); - - if (vehicle_status_updated) { - orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); - + if (_vehicle_status_sub.update(&_vehicle_status)) { /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (_actuators_id == nullptr) { if (_vehicle_status.is_vtol) { @@ -271,41 +208,20 @@ void MulticopterAttitudeControl::vehicle_motor_limits_poll() { /* check if there is a new message */ - bool updated; - orb_check(_motor_limits_sub, &updated); - - if (updated) { - multirotor_motor_limits_s motor_limits = {}; - orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits); + multirotor_motor_limits_s motor_limits{}; + if (_motor_limits_sub.update(&motor_limits)) { _saturation_status.value = motor_limits.saturation_status; } } -void -MulticopterAttitudeControl::battery_status_poll() -{ - /* check if there is a new message */ - bool updated; - orb_check(_battery_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status); - } -} - bool MulticopterAttitudeControl::vehicle_attitude_poll() { /* check if there is a new message */ - bool updated; - orb_check(_v_att_sub, &updated); - - if (updated) { - uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; - - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; + if (_v_att_sub.update(&_v_att)) { // Check for a heading reset if (prev_quat_reset_counter != _v_att.quat_reset_counter) { // we only extract the heading change from the delta quaternion @@ -316,60 +232,6 @@ MulticopterAttitudeControl::vehicle_attitude_poll() return false; } -void -MulticopterAttitudeControl::sensor_correction_poll() -{ - /* check if there is a new message */ - bool updated; - orb_check(_sensor_correction_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction); - } - - /* update the latest gyro selection */ - if (_sensor_correction.selected_gyro_instance < _gyro_count) { - _selected_gyro = _sensor_correction.selected_gyro_instance; - } -} - -void -MulticopterAttitudeControl::sensor_bias_poll() -{ - /* check if there is a new message */ - bool updated; - orb_check(_sensor_bias_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias); - } - -} - -void -MulticopterAttitudeControl::vehicle_land_detected_poll() -{ - /* check if there is a new message */ - bool updated; - orb_check(_vehicle_land_detected_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); - } - -} - -void -MulticopterAttitudeControl::landing_gear_state_poll() -{ - bool updated; - orb_check(_landing_gear_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(landing_gear), _landing_gear_sub, &_landing_gear); - } -} - float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { @@ -519,7 +381,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ void MulticopterAttitudeControl::control_attitude() { - vehicle_attitude_setpoint_poll(); + _v_att_sp_sub.update(&_v_att_sp); // reinitialize the setpoint while not armed to make sure no value from the last flight is still kept if (!_v_control_mode.flag_armed) { @@ -714,31 +576,12 @@ MulticopterAttitudeControl::publish_actuator_controls() void MulticopterAttitudeControl::run() { - - /* - * do subscriptions - */ - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); - _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); - _gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro)), 1, MAX_GYRO_COUNT); for (unsigned s = 0; s < _gyro_count; s++) { _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); } - _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); - _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); - _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _landing_gear_sub = orb_subscribe(ORB_ID(landing_gear)); - /* wakeup source: gyro data from sensor selected by the sensor app */ px4_pollfd_struct_t poll_fds = {}; poll_fds.events = POLLIN; @@ -754,7 +597,13 @@ MulticopterAttitudeControl::run() while (!should_exit()) { // check if the selected gyro has updated first - sensor_correction_poll(); + _sensor_correction_sub.update(&_sensor_correction); + + /* update the latest gyro selection */ + if (_sensor_correction.selected_gyro_instance < _gyro_count) { + _selected_gyro = _sensor_correction.selected_gyro_instance; + } + poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; /* wait for up to 100ms for data */ @@ -795,15 +644,16 @@ MulticopterAttitudeControl::run() } /* check for updates in other topics */ - vehicle_control_mode_poll(); + _v_control_mode_sub.update(&_v_control_mode); + _battery_status_sub.update(&_battery_status); + _sensor_bias_sub.update(&_sensor_bias); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + _landing_gear_sub.update(&_landing_gear); vehicle_status_poll(); vehicle_motor_limits_poll(); - battery_status_poll(); - sensor_bias_poll(); - vehicle_land_detected_poll(); - landing_gear_state_poll(); - const bool manual_control_updated = vehicle_manual_poll(); + const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); const bool attitude_updated = vehicle_attitude_poll(); + attitude_dt += dt; /* Check if we are in rattitude mode and the pilot is above the threshold on pitch @@ -856,7 +706,7 @@ MulticopterAttitudeControl::run() } else { /* attitude controller disabled, poll rates setpoint topic */ - if (vehicle_rates_setpoint_poll()) { + if (_v_rates_sp_sub.update(&_v_rates_sp)) { _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; @@ -905,24 +755,9 @@ MulticopterAttitudeControl::run() perf_end(_loop_perf); } - orb_unsubscribe(_v_att_sub); - orb_unsubscribe(_v_att_sp_sub); - orb_unsubscribe(_v_rates_sp_sub); - orb_unsubscribe(_v_control_mode_sub); - orb_unsubscribe(_params_sub); - orb_unsubscribe(_manual_control_sp_sub); - orb_unsubscribe(_vehicle_status_sub); - orb_unsubscribe(_motor_limits_sub); - orb_unsubscribe(_battery_status_sub); - for (unsigned s = 0; s < _gyro_count; s++) { orb_unsubscribe(_sensor_gyro_sub[s]); } - - orb_unsubscribe(_sensor_correction_sub); - orb_unsubscribe(_sensor_bias_sub); - orb_unsubscribe(_vehicle_land_detected_sub); - orb_unsubscribe(_landing_gear_sub); } int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])