diff --git a/msg/ActuatorControls.msg b/msg/ActuatorControls.msg index 5a183a2cdc..89580b22e7 100644 --- a/msg/ActuatorControls.msg +++ b/msg/ActuatorControls.msg @@ -11,7 +11,6 @@ uint8 INDEX_AIRBRAKES = 6 uint8 INDEX_LANDING_GEAR = 7 uint8 INDEX_GIMBAL_SHUTTER = 3 uint8 INDEX_CAMERA_ZOOM = 4 -uint8 INDEX_COLLECTIVE_TILT = 8 uint8 GROUP_INDEX_ATTITUDE = 0 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 437c5f86ad..bbb1f159c0 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -178,6 +178,7 @@ set(msg_files TaskStackInfo.msg TecsStatus.msg TelemetryStatus.msg + TiltrotorExtraControls.msg TimesyncStatus.msg TrajectoryBezier.msg TrajectorySetpoint.msg diff --git a/msg/TiltrotorExtraControls.msg b/msg/TiltrotorExtraControls.msg new file mode 100644 index 0000000000..20af316cec --- /dev/null +++ b/msg/TiltrotorExtraControls.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32 collective_tilt_normalized_setpoint # Collective tilt angle of motors of tiltrotor, 0: vertical, 1: horizontal [0, 1] +float32 collective_thrust_normalized_setpoint # Collective thrust setpoint [0, 1] diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 17b881dca9..3210b48bb8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -43,7 +43,6 @@ #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" -#include #include class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness @@ -81,6 +80,4 @@ protected: uint32_t _stopped_motors{}; ///< currently stopped motors int _first_control_surface_idx{0}; ///< applies to matrix 1 - - uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index bd9e1db0fc..13faf51e6c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -110,10 +110,11 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector #include #include @@ -107,4 +108,6 @@ protected: }; YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; + + uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; }; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e46c7c8dd0..c40464400a 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -105,6 +105,7 @@ void LoggedTopics::add_default_topics() add_topic("system_power", 500); add_optional_topic("takeoff_status", 1000); add_optional_topic("tecs_status", 200); + add_optional_topic("tiltrotor_extra_controls", 100); add_topic("trajectory_setpoint", 200); add_topic("transponder_report"); add_topic("vehicle_acceleration", 50); diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index f561327496..7528d85297 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -428,16 +428,16 @@ void Tiltrotor::fill_actuator_outputs() _torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; _torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + // Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC), + // pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they + // can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated + // by the allocator directly. + float collective_thrust_normalized_setpoint = 0.f; + if (_vtol_mode == vtol_mode::FW_MODE) { - // for the legacy mixing system pubish FW throttle on the MC output - mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; - - // Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC), - // pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they - // can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated - // by the allocator directly. _thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + collective_thrust_normalized_setpoint = fw_in[actuator_controls_s::INDEX_THROTTLE]; /* allow differential thrust if enabled */ if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { @@ -446,10 +446,8 @@ void Tiltrotor::fill_actuator_outputs() } } else { - - // see comment above for passing magnitude of thrust, not 3D thrust - mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; _thrust_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; + collective_thrust_normalized_setpoint = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; } // Landing gear @@ -461,8 +459,6 @@ void Tiltrotor::fill_actuator_outputs() } // Fixed wing output - fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control; - if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) { fw_out[actuator_controls_s::INDEX_ROLL] = 0; fw_out[actuator_controls_s::INDEX_PITCH] = 0; @@ -485,6 +481,13 @@ void Tiltrotor::fill_actuator_outputs() _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); + + // publish tiltrotor extra controls + tiltrotor_extra_controls_s tiltrotor_extra_controls = {}; + tiltrotor_extra_controls.collective_tilt_normalized_setpoint = _tilt_control; + tiltrotor_extra_controls.collective_thrust_normalized_setpoint = collective_thrust_normalized_setpoint; + tiltrotor_extra_controls.timestamp = hrt_absolute_time(); + _tiltrotor_extra_controls_pub.publish(tiltrotor_extra_controls); } void Tiltrotor::blendThrottleAfterFrontTransition(float scale) diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index e08796c846..369e335684 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -44,6 +44,9 @@ #include #include +#include +#include + class Tiltrotor : public VtolType { @@ -77,6 +80,8 @@ private: vtol_mode _vtol_mode{vtol_mode::MC_MODE}; /**< vtol flight mode, defined by enum vtol_mode */ + uORB::Publication _tiltrotor_extra_controls_pub{ORB_ID(tiltrotor_extra_controls)}; + float _tilt_control{0.0f}; /**< actuator value for the tilt servo */ void parameters_update() override;