diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 64e0da5ffaba..7a002bdd3049 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -247,6 +247,7 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]): ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), ('mc_att_control', r'mc_att_control_main\.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$'), diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index e5062095e685..c685dfb35224 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -33,6 +33,8 @@ #include "FixedwingAttitudeControl.hpp" +#include + using namespace time_literals; /** @@ -120,9 +122,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN"); _parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE"); - // initialize to invalid VTOL type - _parameters.vtol_type = -1; - /* fetch initial parameter values */ parameters_update(); @@ -242,10 +241,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres); - if (_vehicle_status.is_vtol) { - param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); - } - param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en); param_get(_parameter_handles.airspeed_mode, &tmp); @@ -292,13 +287,25 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } + + if (_vehicle_status.is_vtol) { + const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode; + const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; + + if (is_hovering || is_tailsitter_transition) { + _vcontrol_mode.flag_control_attitude_enabled = false; + _vcontrol_mode.flag_control_manual_enabled = false; + } + } } void FixedwingAttitudeControl::vehicle_manual_poll() { + const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode; + const bool is_fixed_wing = !_vehicle_status.is_rotary_wing; - if (_vcontrol_mode.flag_control_manual_enabled && !_vehicle_status.is_rotary_wing) { + if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) { @@ -394,7 +401,7 @@ FixedwingAttitudeControl::vehicle_rates_setpoint_poll() if (updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp); - if (_parameters.vtol_type == vtol_type::TAILSITTER) { + if (_is_tailsitter) { float tmp = _rates_sp.roll; _rates_sp.roll = -_rates_sp.yaw; _rates_sp.yaw = tmp; @@ -424,25 +431,17 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); - // if VTOL and not in fixed wing mode we should only control body-rates which are published - // by the multicoper attitude controller. Therefore, modify the control mode to achieve rate - // control only - if (_vehicle_status.is_vtol) { - if (_vehicle_status.is_rotary_wing) { - _vcontrol_mode.flag_control_attitude_enabled = false; - _vcontrol_mode.flag_control_manual_enabled = false; - } - } - /* 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); - _parameter_handles.vtol_type = param_find("VT_TYPE"); + int32_t vt_type = -1; - parameters_update(); + if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { + _is_tailsitter = (vt_type == vtol_type::TAILSITTER); + } } else { _actuators_id = ORB_ID(actuator_controls_0); @@ -566,7 +565,7 @@ void FixedwingAttitudeControl::run() /* get current rotation matrix and euler angles from control state quaternions */ matrix::Dcmf R = matrix::Quatf(_att.q); - if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { + if (_is_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to @@ -609,7 +608,7 @@ void FixedwingAttitudeControl::run() _att.yawspeed = helper; } - matrix::Eulerf euler_angles(R); + const matrix::Eulerf euler_angles(R); vehicle_attitude_setpoint_poll(); vehicle_control_mode_poll(); @@ -623,7 +622,7 @@ void FixedwingAttitudeControl::run() _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; /* lock integrator until control is started */ - bool lock_integrator = !(_vcontrol_mode.flag_control_rates_enabled && !_vehicle_status.is_rotary_wing); + bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled || _vehicle_status.is_rotary_wing; /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index abf960073beb..53fe29f03d5c 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -60,7 +60,6 @@ #include #include #include -#include using matrix::Eulerf; using matrix::Quatf; @@ -140,6 +139,8 @@ class FixedwingAttitudeControl final : public ModuleBase #include #include +#include #include @@ -174,6 +175,7 @@ class MulticopterAttitudeControl : public ModuleBase orb_advert_t _landing_gear_pub{nullptr}; orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ + orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ @@ -277,6 +279,8 @@ class MulticopterAttitudeControl : public ModuleBase (ParamInt) _param_mc_airmode ) + bool _is_tailsitter{false}; + matrix::Vector3f _rate_p; /**< P gain for angular rate error */ matrix::Vector3f _rate_i; /**< I gain for angular rate error */ matrix::Vector3f _rate_int_lim; /**< integrator state limit for rate loop */ 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 d55b468affcf..b12bdaed6e83 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -252,9 +252,16 @@ MulticopterAttitudeControl::vehicle_status_poll() if (_actuators_id == nullptr) { if (_vehicle_status.is_vtol) { _actuators_id = ORB_ID(actuator_controls_virtual_mc); + _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 = (vt_type == vtol_type::TAILSITTER); + } } else { _actuators_id = ORB_ID(actuator_controls_0); + _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); } } } @@ -407,7 +414,6 @@ void MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp) { vehicle_attitude_setpoint_s attitude_setpoint{}; - landing_gear_s landing_gear{}; const float yaw = Eulerf(Quatf(_v_att.q)).psi(); /* reset yaw setpoint to current position if needed */ @@ -495,12 +501,12 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ attitude_setpoint.q_d_valid = true; 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); + } _landing_gear.landing_gear = get_landing_gear_state(); - - attitude_setpoint.timestamp = landing_gear.timestamp = hrt_absolute_time(); - orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); - _landing_gear.timestamp = hrt_absolute_time(); orb_publish_auto(ORB_ID(landing_gear), &_landing_gear_pub, &_landing_gear, nullptr, ORB_PRIO_DEFAULT); } @@ -805,7 +811,15 @@ MulticopterAttitudeControl::run() bool attitude_setpoint_generated = false; - if (_v_control_mode.flag_control_attitude_enabled && _vehicle_status.is_rotary_wing) { + const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode; + + // vehicle is a tailsitter in transition mode + const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; + + bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); + + + if (run_att_ctrl) { if (attitude_updated) { // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode if (_v_control_mode.flag_control_manual_enabled && @@ -822,7 +836,7 @@ MulticopterAttitudeControl::run() } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled && _vehicle_status.is_rotary_wing) { + if (_v_control_mode.flag_control_manual_enabled && is_hovering) { if (manual_control_updated) { /* manual rates control - ACRO mode */ Vector3f man_rate_sp( @@ -856,9 +870,12 @@ MulticopterAttitudeControl::run() } if (attitude_updated) { + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || _vehicle_land_detected.landed || - (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing); // VTOL in FW mode + (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode); + attitude_dt = 0.f; } 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 ada9969d25c3..5e7fd840cf26 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -752,7 +752,7 @@ VtolAttitudeControl::start() _control_task = px4_task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, SCHED_PRIORITY_ATTITUDE_CONTROL + 1, - 1230, + 1320, (px4_main_t)&VtolAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 988b73a0e95f..2b870e212151 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -82,6 +82,7 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0); * @min 0 * @max 2 * @decimal 0 + * @reboot_required true * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 53aabcdb85a0..274c8caf93c7 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -370,7 +370,9 @@ bool VtolType::is_motor_off_channel(const int channel) int tmp; int channels = _params->fw_motors_off; - for (int i = 0; i < _params->vtol_motor_count; ++i) { + static constexpr int num_outputs_max = 8; + + for (int i = 0; i < num_outputs_max; ++i) { tmp = channels % 10; if (tmp == 0) {