From 0cc250194d36347f8fe6e6d143a0040e8afc7e54 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 30 Nov 2019 12:58:36 -0500 Subject: [PATCH] VTOL: explicitly start all FW & MC controllers in VTOL mode --- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 17 ++-- Tools/uorb_graph/create.py | 9 -- posix-configs/SITL/init/test/test_shutdown | 10 +-- .../FixedwingAttitudeControl.cpp | 71 ++++++---------- .../FixedwingAttitudeControl.hpp | 15 ++-- .../FixedwingPositionControl.cpp | 83 ++++++++----------- .../FixedwingPositionControl.hpp | 11 +-- src/modules/mc_att_control/mc_att_control.hpp | 12 +-- .../mc_att_control/mc_att_control_main.cpp | 51 +++++------- .../mc_pos_control/mc_pos_control_main.cpp | 53 +++++------- .../MulticopterRateControl.cpp | 46 ++++------ .../MulticopterRateControl.hpp | 11 +-- 12 files changed, 149 insertions(+), 240 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 28c734e860..c5ee5b2e20 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -15,16 +15,17 @@ ekf2 start # End Estimator group selection # ############################################################################### - -vtol_att_control start -mc_rate_control start -mc_att_control start -mc_pos_control start -fw_att_control start -fw_pos_control_l1 start airspeed_selector start -# +vtol_att_control start + +mc_rate_control start vtol +mc_att_control start vtol +mc_pos_control start vtol + +fw_att_control start vtol +fw_pos_control_l1 start vtol + # Start Land Detector # Multicopter for now until we have something for VTOL # diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index cc9a80ffc0..9f73c828b0 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -239,15 +239,6 @@ class Graph(object): special_cases_pub = [ ('replay', r'Replay\.cpp$', None, r'^sub\.orb_meta$'), - ('fw_pos_control_l1', r'FixedwingPositionControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), - - ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), - - ('mc_rate_control', r'MulticopterRateControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), - ('mc_att_control', r'mc_att_control_main\.cpp$', r'\_attitude_sp_id=([^,)]+)', r'^_attitude_sp_id$'), - - ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), - ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), ('uavcan', r'sensors/.*\.cpp$', r'\bUavcanCDevSensorBridgeBase\([^{]*DEVICE_PATH,([^,)]+)', r'^_orb_topic$'), ] diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 2f6d527a0c..908f0cfdd2 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -26,11 +26,11 @@ land_detector start vtol navigator start ekf2 start vtol_att_control start -mc_pos_control start -mc_att_control start -mc_rate_control start -fw_pos_control_l1 start -fw_att_control start +mc_pos_control start vtol +mc_att_control start vtol +mc_rate_control start vtol +fw_pos_control_l1 start vtol +fw_att_control start vtol airspeed_selector start #mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 5fcd970d64..e1657d160a 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -40,13 +40,21 @@ using math::constrain; using math::gradual; using math::radians; -FixedwingAttitudeControl::FixedwingAttitudeControl() : +FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + _actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), + _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { // check if VTOL first - vehicle_status_poll(); + if (vtol) { + int32_t vt_type = -1; + + if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { + _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + } + } /* fetch initial parameter values */ parameters_update(); @@ -165,14 +173,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() _att_sp.timestamp = hrt_absolute_time(); - if (_attitude_sp_pub != nullptr) { - /* publish the attitude rates setpoint */ - orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - - } else if (_attitude_setpoint_id) { - /* advertise the attitude rates setpoint */ - _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); - } + _attitude_sp_pub.publish(_att_sp); } else if (_vcontrol_mode.flag_control_rates_enabled && !_vcontrol_mode.flag_control_attitude_enabled) { @@ -220,30 +221,6 @@ FixedwingAttitudeControl::vehicle_rates_setpoint_poll() } } -void -FixedwingAttitudeControl::vehicle_status_poll() -{ - if (_vehicle_status_sub.update(&_vehicle_status)) { - /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (!_actuators_id) { - if (_vehicle_status.is_vtol) { - _actuators_id = ORB_ID(actuator_controls_virtual_fw); - _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); - - int32_t vt_type = -1; - - if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { - _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); - } - - } else { - _actuators_id = ORB_ID(actuator_controls_0); - _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); - } - } - } -} - void FixedwingAttitudeControl::vehicle_land_detected_poll() { @@ -378,7 +355,10 @@ void FixedwingAttitudeControl::Run() const matrix::Eulerf euler_angles(R); vehicle_attitude_setpoint_poll(); - vehicle_status_poll(); // this poll has to be before the control_mode_poll, otherwise rate sp are not published during whole transition + + // vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition + _vehicle_status_sub.update(&_vehicle_status); + vehicle_control_mode_poll(); vehicle_manual_poll(); _global_pos_sub.update(&_global_pos); @@ -646,14 +626,8 @@ void FixedwingAttitudeControl::Run() if (_vcontrol_mode.flag_control_rates_enabled || _vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_manual_enabled) { - /* publish the actuator controls */ - if (_actuators_0_pub != nullptr) { - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - - } else if (_actuators_id) { - _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); - } + _actuators_0_pub.publish(_actuators); _actuators_2_pub.publish(_actuators_airframe); } } @@ -728,7 +702,15 @@ void FixedwingAttitudeControl::control_flaps(const float dt) int FixedwingAttitudeControl::task_spawn(int argc, char *argv[]) { - FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(); + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(vtol); if (instance) { _object.store(instance); @@ -774,8 +756,9 @@ fw_att_control is the fixed wing attitude controller. )DESCR_STR"); - PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_NAME("fw_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 660d5c4d78..a9d1e4e7fe 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -76,7 +76,7 @@ class FixedwingAttitudeControl final : public ModuleBase _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Publication _actuators_0_pub; uORB::Publication _actuators_2_pub{ORB_ID(actuator_controls_2)}; /**< actuator control group 1 setpoint (Airframe) */ - uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ - uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< rate controller status publication */ - - orb_id_t _attitude_setpoint_id{nullptr}; - orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */ - - orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ - orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */ + uORB::Publication _attitude_sp_pub; + uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; actuator_controls_s _actuators {}; /**< actuator control inputs */ actuator_controls_s _actuators_airframe {}; /**< actuator control inputs */ @@ -233,7 +229,6 @@ private: void vehicle_manual_poll(); void vehicle_attitude_setpoint_poll(); void vehicle_rates_setpoint_poll(); - void vehicle_status_poll(); void vehicle_land_detected_poll(); float get_airspeed_and_update_scaling(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c8c4feee65..57a139f5a8 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -33,13 +33,29 @@ #include "FixedwingPositionControl.hpp" -FixedwingPositionControl::FixedwingPositionControl() : +#include + +FixedwingPositionControl::FixedwingPositionControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), - _loop_perf(perf_alloc(PC_ELAPSED, "fw_pos_control_l1: cycle")), + _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _launchDetector(this), _runway_takeoff(this) { + if (vtol) { + _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); + + // VTOL parameter VTOL_TYPE + int32_t vt_type = -1; + param_get(param_find("VT_TYPE"), &vt_type); + + _vtol_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + + } else { + _parameter_handles.airspeed_trans = PARAM_INVALID; + } + // limit to 50 Hz _global_pos_sub.set_interval_ms(20); @@ -98,13 +114,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); - // if vehicle is vtol these handles will be set when we get the vehicle status - _parameter_handles.airspeed_trans = PARAM_INVALID; - _parameter_handles.vtol_type = PARAM_INVALID; - - // initialize to invalid vtol type - _parameters.vtol_type = -1; - /* fetch initial parameter values */ parameters_update(); } @@ -166,11 +175,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_throtTC_scale, &(_parameters.land_throtTC_scale)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); - // VTOL parameter VTOL_TYPE - if (_parameter_handles.vtol_type != PARAM_INVALID) { - param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); - } - // VTOL parameter VT_ARSP_TRANS if (_parameter_handles.airspeed_trans != PARAM_INVALID) { param_get(_parameter_handles.airspeed_trans, &_parameters.airspeed_trans); @@ -333,27 +337,6 @@ FixedwingPositionControl::vehicle_command_poll() } } -void -FixedwingPositionControl::vehicle_status_poll() -{ - if (_vehicle_status_sub.update(&_vehicle_status)) { - /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_attitude_setpoint_id == nullptr) { - if (_vehicle_status.is_vtol) { - _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); - - _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); - _parameter_handles.vtol_type = param_find("VT_TYPE"); - - parameters_update(); - - } else { - _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); - } - } - } -} - void FixedwingPositionControl::airspeed_poll() { @@ -399,12 +382,12 @@ FixedwingPositionControl::vehicle_attitude_poll() // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight - if (static_cast(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + if (_vtol_tailsitter) { Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); _R_nb = _R_nb * R_offset; } - Eulerf euler_angles(_R_nb); + const Eulerf euler_angles(_R_nb); _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); @@ -1708,7 +1691,7 @@ FixedwingPositionControl::Run() vehicle_command_poll(); vehicle_control_mode_poll(); _vehicle_land_detected_sub.update(&_vehicle_land_detected); - vehicle_status_poll(); + _vehicle_status_sub.update(&_vehicle_status); _vehicle_acceleration_sub.update(); _vehicle_rates_sub.update(); @@ -1753,15 +1736,7 @@ FixedwingPositionControl::Run() _control_mode.flag_control_acceleration_enabled || _control_mode.flag_control_altitude_enabled) { - /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - - } else if (_attitude_setpoint_id != nullptr) { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); - } + _attitude_sp_pub.publish(_att_sp); // only publish status in full FW mode if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING @@ -1898,7 +1873,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame - if (static_cast(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + if (_vtol_tailsitter) { float tmp = accel_body(0); accel_body(0) = -accel_body(2); accel_body(2) = tmp; @@ -1944,7 +1919,15 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee int FixedwingPositionControl::task_spawn(int argc, char *argv[]) { - FixedwingPositionControl *instance = new FixedwingPositionControl(); + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + FixedwingPositionControl *instance = new FixedwingPositionControl(vtol); if (instance) { _object.store(instance); @@ -1990,9 +1973,9 @@ fw_pos_control_l1 is the fixed wing position controller. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 7c571be0f1..585d4534de 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -129,7 +129,7 @@ class FixedwingPositionControl final : public ModuleBase _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; - orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint - orb_id_t _attitude_setpoint_id{nullptr}; - + uORB::Publication _attitude_sp_pub; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication @@ -253,6 +251,8 @@ private: float _asp_after_transition{0.0f}; bool _was_in_transition{false}; + bool _vtol_tailsitter{false}; + // estimator reset counters uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state @@ -307,7 +307,6 @@ private: // VTOL float airspeed_trans; - int32_t vtol_type; } _parameters{}; ///< local copies of interesting parameters struct { @@ -370,8 +369,6 @@ private: param_t land_airspeed_scale; param_t land_throtTC_scale; param_t loiter_radius; - - param_t vtol_type; } _parameter_handles {}; ///< handles for interesting parameters DEFINE_PARAMETERS( diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 81ad00bdda..12f582d8b1 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -66,9 +66,8 @@ class MulticopterAttitudeControl : public ModuleBase public px4::WorkItem { public: - MulticopterAttitudeControl(); - - virtual ~MulticopterAttitudeControl(); + MulticopterAttitudeControl(bool vtol = false); + ~MulticopterAttitudeControl() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -93,8 +92,6 @@ private: */ void parameters_updated(); - void vehicle_status_poll(); - void publish_rates_setpoint(); float throttle_curve(float throttle_stick_input); @@ -122,10 +119,7 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ - - orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; - - orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ + uORB::Publication _vehicle_attitude_setpoint_pub; struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp {}; /**< vehicle attitude setpoint */ 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 635e01d495..9d2d9acb79 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -51,11 +51,20 @@ using namespace matrix; -MulticopterAttitudeControl::MulticopterAttitudeControl() : +MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { + if (vtol) { + int32_t vt_type = -1; + + if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { + _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + } + } + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; /* initialize quaternions in messages to be valid */ @@ -95,29 +104,6 @@ MulticopterAttitudeControl::parameters_updated() _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); } -void -MulticopterAttitudeControl::vehicle_status_poll() -{ - /* check if there is new status information */ - if (_vehicle_status_sub.update(&_vehicle_status)) { - /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_attitude_sp_id == nullptr) { - if (_vehicle_status.is_vtol) { - _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint); - - int32_t vt_type = -1; - - if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { - _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); - } - - } else { - _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); - } - } - } -} - float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { @@ -233,9 +219,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); attitude_setpoint.timestamp = hrt_absolute_time(); - if (_attitude_sp_id != nullptr) { - orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); - } + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); } /** @@ -308,7 +292,7 @@ MulticopterAttitudeControl::Run() _manual_control_sp_sub.update(&_manual_control_sp); _v_control_mode_sub.update(&_v_control_mode); _vehicle_land_detected_sub.update(&_vehicle_land_detected); - vehicle_status_poll(); + _vehicle_status_sub.update(&_vehicle_status); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't @@ -365,7 +349,15 @@ MulticopterAttitudeControl::Run() int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) { - MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(); + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol); if (instance) { _object.store(instance); @@ -425,6 +417,7 @@ https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth PRINT_MODULE_USAGE_NAME("mc_att_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; 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 bdeb594612..c218e36f59 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -83,8 +83,7 @@ class MulticopterPositionControl : public ModuleBase public ModuleParams, public px4::WorkItem { public: - MulticopterPositionControl(); - + MulticopterPositionControl(bool vtol = false); ~MulticopterPositionControl() override; /** @see ModuleBase */ @@ -106,8 +105,7 @@ public: private: Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ - orb_id_t _attitude_setpoint_id{nullptr}; ///< orb metadata to publish attitude setpoint dependent if VTOL or not - orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; ///< attitude setpoint publication handle + uORB::Publication _vehicle_attitude_setpoint_pub; uORB::PublicationQueued _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */ orb_advert_t _mavlink_log_pub{nullptr}; @@ -236,11 +234,6 @@ private: */ void warn_rate_limited(const char *str); - /** - * Publish attitude. - */ - void publish_attitude(); - /** * Adjust the setpoint during landing. * Thrust is adjusted to support the land-detector during detection. @@ -280,15 +273,21 @@ private: void send_vehicle_cmd_do(uint8_t nav_state); }; -MulticopterPositionControl::MulticopterPositionControl() : +MulticopterPositionControl::MulticopterPositionControl(bool vtol) : SuperBlock(nullptr, "MPC"), ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), _cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")) { + if (vtol) { + // if vehicle is a VTOL we want to enable weathervane capabilities + _wv_controller = new WeatherVane(); + } + // fetch initial parameter values parameters_update(true); @@ -394,24 +393,7 @@ MulticopterPositionControl::parameters_update(bool force) void MulticopterPositionControl::poll_subscriptions() { - if (_vehicle_status_sub.update(&_vehicle_status)) { - - // set correct uORB ID, depending on if vehicle is VTOL or not - if (!_attitude_setpoint_id) { - if (_vehicle_status.is_vtol) { - _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); - - } else { - _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); - } - } - - // if vehicle is a VTOL we want to enable weathervane capabilities - if (_wv_controller == nullptr && _vehicle_status.is_vtol) { - _wv_controller = new WeatherVane(); - } - } - + _vehicle_status_sub.update(&_vehicle_status); _vehicle_land_detected_sub.update(&_vehicle_land_detected); _control_mode_sub.update(&_control_mode); _home_pos_sub.update(&_home_pos); @@ -702,9 +684,7 @@ MulticopterPositionControl::Run() // Not publishing when not running a flight task // in stabilized mode attitude setpoints get ignored // in offboard with attitude setpoints they come from MAVLink directly - if (_attitude_setpoint_id != nullptr) { - orb_publish_auto(_attitude_setpoint_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); - } + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); _wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z(); @@ -1079,7 +1059,15 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state) int MulticopterPositionControl::task_spawn(int argc, char *argv[]) { - MulticopterPositionControl *instance = new MulticopterPositionControl(); + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + MulticopterPositionControl *instance = new MulticopterPositionControl(vtol); if (instance) { _object.store(instance); @@ -1124,6 +1112,7 @@ logging. PRINT_MODULE_USAGE_NAME("mc_pos_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 1d1e1b5d08..79c48af81a 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -42,9 +42,10 @@ using namespace matrix; using namespace time_literals; using math::radians; -MulticopterRateControl::MulticopterRateControl() : +MulticopterRateControl::MulticopterRateControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; @@ -97,23 +98,6 @@ MulticopterRateControl::parameters_updated() _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY); } -void -MulticopterRateControl::vehicle_status_poll() -{ - /* check if there is new status information */ - 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) { - _actuators_id = ORB_ID(actuator_controls_virtual_mc); - - } else { - _actuators_id = ORB_ID(actuator_controls_0); - } - } - } -} - float MulticopterRateControl::get_landing_gear_state() { @@ -183,7 +167,7 @@ MulticopterRateControl::Run() } } - vehicle_status_poll(); + _vehicle_status_sub.update(&_vehicle_status); const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); @@ -326,14 +310,14 @@ MulticopterRateControl::Run() } actuators.timestamp = hrt_absolute_time(); - orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT); + _actuators_0_pub.publish(actuators); } else if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { // publish actuator controls actuator_controls_s actuators{}; actuators.timestamp = hrt_absolute_time(); - orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT); + _actuators_0_pub.publish(actuators); } } } @@ -343,7 +327,15 @@ MulticopterRateControl::Run() int MulticopterRateControl::task_spawn(int argc, char *argv[]) { - MulticopterRateControl *instance = new MulticopterRateControl(); + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + MulticopterRateControl *instance = new MulticopterRateControl(vtol); if (instance) { _object.store(instance); @@ -394,19 +386,15 @@ The controller has a PID loop for angular rate error. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller"); + PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -/** - * Multicopter rate control app start / stop handling function - */ -extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]); - -int mc_rate_control_main(int argc, char *argv[]) +extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]) { return MulticopterRateControl::main(argc, argv); } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 7bf82c4ac9..c8aff461dc 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -63,9 +63,8 @@ class MulticopterRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - MulticopterRateControl(); - - virtual ~MulticopterRateControl(); + MulticopterRateControl(bool vtol = false); + ~MulticopterRateControl() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -90,8 +89,6 @@ private: */ void parameters_updated(); - void vehicle_status_poll(); - /** * Get the landing gear state based on the manual control switch position * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN @@ -112,13 +109,11 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + uORB::Publication _actuators_0_pub; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ - orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ - landing_gear_s _landing_gear{}; manual_control_setpoint_s _manual_control_sp{}; vehicle_control_mode_s _v_control_mode{};