From c0e735c88a9d6ff3785eafc55c714859635c917c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 3 Jun 2019 22:47:26 -0400 Subject: [PATCH] vtol_att_control move to new uORB::Subscription --- .../vtol_att_control_main.cpp | 223 ++---------------- .../vtol_att_control/vtol_att_control_main.h | 44 ++-- 2 files changed, 34 insertions(+), 233 deletions(-) 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 8a56cdc3d9..7aae48165e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -140,36 +140,6 @@ VtolAttitudeControl::~VtolAttitudeControl() VTOL_att_control::g_control = nullptr; } -/** -* Check for changes in vehicle control mode. -*/ -void VtolAttitudeControl::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); - } -} - -/** -* Check for changes in manual inputs. -*/ -void VtolAttitudeControl::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); - } -} - /** * Check for inputs from mc attitude controller. */ @@ -196,158 +166,18 @@ void VtolAttitudeControl::actuator_controls_fw_poll() } } -/** -* Check for airspeed updates. -*/ -void -VtolAttitudeControl::vehicle_airspeed_poll() -{ - bool updated; - orb_check(_airspeed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - } -} - -/** -* Check for attitude update. -*/ -void -VtolAttitudeControl::vehicle_attitude_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - } -} - -/** -* Check for sensor updates. -*/ -void -VtolAttitudeControl::vehicle_local_pos_poll() -{ - bool updated; - /* Check if parameters have changed */ - orb_check(_local_pos_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); - } - -} - -/** -* Check for setpoint updates. -*/ -void -VtolAttitudeControl::vehicle_local_pos_sp_poll() -{ - bool updated; - /* Check if parameters have changed */ - orb_check(_local_pos_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); - } - -} - -/** -* Check for position setpoint updates. -*/ -void -VtolAttitudeControl::pos_sp_triplet_poll() -{ - bool updated; - /* Check if parameters have changed */ - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - -} - -/** -* Check for mc virtual attitude setpoint updates. -*/ -void -VtolAttitudeControl::mc_virtual_att_sp_poll() -{ - bool updated; - - orb_check(_mc_virtual_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub, &_mc_virtual_att_sp); - } -} - -/** -* Check for fw virtual attitude setpoint updates. -*/ -void -VtolAttitudeControl::fw_virtual_att_sp_poll() -{ - bool updated; - - orb_check(_fw_virtual_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub, &_fw_virtual_att_sp); - } -} - /** * Check for command updates. */ void VtolAttitudeControl::vehicle_cmd_poll() { - bool updated; - orb_check(_vehicle_cmd_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &_vehicle_cmd); + if (_vehicle_cmd_sub.updated()) { + _vehicle_cmd_sub.copy(&_vehicle_cmd); handle_command(); } } -/** -* Check for TECS status updates. -*/ -void -VtolAttitudeControl::tecs_status_poll() -{ - bool updated; - - orb_check(_tecs_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(tecs_status), _tecs_status_sub, &_tecs_status); - } -} - -/** -* Check for land detector updates. -*/ -void -VtolAttitudeControl::land_detected_poll() -{ - bool updated; - - orb_check(_land_detected_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected); - } -} - /** * Check received command */ @@ -526,23 +356,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) void VtolAttitudeControl::task_main() { - fflush(stdout); - /* do subscriptions */ - _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); - _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _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)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - _tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); - _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -557,13 +371,10 @@ void VtolAttitudeControl::task_main() while (!_task_should_exit) { /* only update parameters if they changed */ - bool params_updated = false; - orb_check(_params_sub, ¶ms_updated); - - if (params_updated) { + if (_params_sub.updated()) { /* read from param to clear updated flag */ parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); + _params_sub.copy(&update); /* update parameters from storage */ parameters_update(); @@ -598,16 +409,16 @@ void VtolAttitudeControl::task_main() continue; } - vehicle_control_mode_poll(); - vehicle_manual_poll(); - vehicle_attitude_poll(); - vehicle_local_pos_poll(); - vehicle_local_pos_sp_poll(); - pos_sp_triplet_poll(); - vehicle_airspeed_poll(); + _v_control_mode_sub.update(&_v_control_mode); + _manual_control_sp_sub.update(&_manual_control_sp); + _v_att_sub.update(&_v_att); + _local_pos_sub.update(&_local_pos); + _local_pos_sp_sub.update(&_local_pos_sp); + _pos_sp_triplet_sub.update(&_pos_sp_triplet); + _airspeed_sub.update(&_airspeed); + _tecs_status_sub.update(&_tecs_status); + _land_detected_sub.update(&_land_detected); vehicle_cmd_poll(); - tecs_status_poll(); - land_detected_poll(); actuator_controls_fw_poll(); actuator_controls_mc_poll(); @@ -633,7 +444,7 @@ void VtolAttitudeControl::task_main() // check in which mode we are in and call mode specific functions if (_vtol_type->get_mode() == mode::ROTARY_WING) { - mc_virtual_att_sp_poll(); + _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; @@ -645,7 +456,7 @@ void VtolAttitudeControl::task_main() } else if (_vtol_type->get_mode() == mode::FIXED_WING) { - fw_virtual_att_sp_poll(); + _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; @@ -656,8 +467,8 @@ void VtolAttitudeControl::task_main() } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC || _vtol_type->get_mode() == mode::TRANSITION_TO_FW) { - mc_virtual_att_sp_poll(); - fw_virtual_att_sp_poll(); + _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); + _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; 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 b902338148..f7ee0b47b0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -54,7 +54,6 @@ #include #include -#include #include #include #include @@ -62,6 +61,7 @@ #include #include +#include #include #include #include @@ -123,21 +123,22 @@ private: int _control_task{-1}; //task handle for VTOL attitude controller /* handlers for subscriptions */ - int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs - int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs - int _airspeed_sub{-1}; // airspeed subscription - int _fw_virtual_att_sp_sub{-1}; - int _land_detected_sub{-1}; - int _local_pos_sp_sub{-1}; // setpoint subscription - int _local_pos_sub{-1}; // sensor subscription - int _manual_control_sp_sub{-1}; //manual control setpoint subscription - int _mc_virtual_att_sp_sub{-1}; - int _params_sub{-1}; //parameter updates subscription - int _pos_sp_triplet_sub{-1}; // local position setpoint subscription - int _tecs_status_sub{-1}; - int _v_att_sub{-1}; //vehicle attitude subscription - int _v_control_mode_sub{-1}; //vehicle control mode subscription - int _vehicle_cmd_sub{-1}; + int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs + int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs + + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; // airspeed subscription + uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; + uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription + 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 _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 + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription + uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; //handlers for publishers orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust) @@ -212,20 +213,9 @@ private: void task_main(); //main task static int task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - void land_detected_poll(); - void tecs_status_poll(); - void vehicle_attitude_poll(); //Check for attitude updates. void vehicle_cmd_poll(); - void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - void fw_virtual_att_sp_poll(); - void mc_virtual_att_sp_poll(); - void pos_sp_triplet_poll(); // Check for changes in position setpoint values - void vehicle_airspeed_poll(); // Check for changes in airspeed - void vehicle_local_pos_poll(); // Check for changes in sensor values - void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values int parameters_update(); //Update local parameter cache