diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 500ccfe61f..f0c8546975 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -413,6 +413,30 @@ void Standard::fill_actuator_outputs() break; } + _torque_setpoint_0->timestamp = hrt_absolute_time(); + _torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _torque_setpoint_0->xyz[0] = mc_out[actuator_controls_s::INDEX_ROLL]; + _torque_setpoint_0->xyz[1] = mc_out[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_0->xyz[2] = mc_out[actuator_controls_s::INDEX_YAW]; + + _torque_setpoint_1->timestamp = hrt_absolute_time(); + _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL]; + _torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW]; + + _thrust_setpoint_0->timestamp = hrt_absolute_time(); + _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _thrust_setpoint_0->xyz[0] = 0.f; + _thrust_setpoint_0->xyz[1] = 0.f; + _thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE]; + + _thrust_setpoint_1->timestamp = hrt_absolute_time(); + _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _thrust_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_1->xyz[1] = 0.f; + _thrust_setpoint_1->xyz[2] = 0.f; + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index efc51b7a9a..d1278c7454 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -462,21 +462,57 @@ void Tiltrotor::fill_actuator_outputs() auto &mc_out = _actuators_out_0->control; auto &fw_out = _actuators_out_1->control; + _torque_setpoint_0->timestamp = hrt_absolute_time(); + _torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _torque_setpoint_0->xyz[0] = 0.f; + _torque_setpoint_0->xyz[1] = 0.f; + _torque_setpoint_0->xyz[2] = 0.f; + + _torque_setpoint_1->timestamp = hrt_absolute_time(); + _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _torque_setpoint_1->xyz[0] = 0.f; + _torque_setpoint_1->xyz[1] = 0.f; + _torque_setpoint_1->xyz[2] = 0.f; + + _thrust_setpoint_0->timestamp = hrt_absolute_time(); + _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _thrust_setpoint_0->xyz[0] = 0.f; + _thrust_setpoint_0->xyz[1] = 0.f; + _thrust_setpoint_0->xyz[2] = 0.f; + + _thrust_setpoint_1->timestamp = hrt_absolute_time(); + _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _thrust_setpoint_1->xyz[0] = 0.f; + _thrust_setpoint_1->xyz[1] = 0.f; + _thrust_setpoint_1->xyz[2] = 0.f; + // Multirotor output mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + _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; + if (_vtol_schedule.flight_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]; + // FW thrust is allocated on mc_thrust_sp[0] for tiltrotor with dynamic control allocation + _thrust_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + /* allow differential thrust if enabled */ if (_params->diff_thrust == 1) { mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; + _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; } } else { + 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; } // Fixed wing output @@ -491,6 +527,9 @@ void Tiltrotor::fill_actuator_outputs() fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; + _torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL]; + _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW]; } _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; 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 11f96514b9..99265c8d76 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -513,8 +513,11 @@ VtolAttitudeControl::Run() _vtol_type->fill_actuator_outputs(); _actuators_0_pub.publish(_actuators_out_0); _actuators_1_pub.publish(_actuators_out_1); - publishTorqueSetpoints(0); - publishThrustSetpoints(0); + + _vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0); + _vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1); + _vehicle_thrust_setpoint0_pub.publish(_thrust_setpoint_0); + _vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1); // Advertise/Publish vtol vehicle status _vtol_vehicle_status.timestamp = hrt_absolute_time(); @@ -524,39 +527,6 @@ VtolAttitudeControl::Run() perf_end(_loop_perf); } -void VtolAttitudeControl::publishTorqueSetpoints(const hrt_abstime ×tamp_sample) -{ - vehicle_torque_setpoint_s v_torque_sp = {}; - v_torque_sp.timestamp = hrt_absolute_time(); - v_torque_sp.timestamp_sample = timestamp_sample; - v_torque_sp.xyz[0] = _actuators_out_0.control[actuator_controls_s::INDEX_ROLL]; - v_torque_sp.xyz[1] = _actuators_out_0.control[actuator_controls_s::INDEX_PITCH]; - v_torque_sp.xyz[2] = _actuators_out_0.control[actuator_controls_s::INDEX_YAW]; - _vehicle_torque_setpoint0_pub.publish(v_torque_sp); - - v_torque_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_ROLL]; - v_torque_sp.xyz[1] = _actuators_out_1.control[actuator_controls_s::INDEX_PITCH]; - v_torque_sp.xyz[2] = _actuators_out_1.control[actuator_controls_s::INDEX_YAW]; - _vehicle_torque_setpoint1_pub.publish(v_torque_sp); -} - -void VtolAttitudeControl::publishThrustSetpoints(const hrt_abstime ×tamp_sample) -{ - vehicle_thrust_setpoint_s v_thrust_sp = {}; - v_thrust_sp.timestamp = hrt_absolute_time(); - v_thrust_sp.timestamp_sample = timestamp_sample; - // TODO: separate thrust - v_thrust_sp.xyz[0] = _actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE]; - v_thrust_sp.xyz[1] = 0.0f; - v_thrust_sp.xyz[2] = -_actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE]; - _vehicle_thrust_setpoint0_pub.publish(v_thrust_sp); - - v_thrust_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_THROTTLE]; - v_thrust_sp.xyz[1] = 0.0f; - v_thrust_sp.xyz[2] = 0.0f; - _vehicle_thrust_setpoint1_pub.publish(v_thrust_sp); -} - int VtolAttitudeControl::task_spawn(int argc, char *argv[]) { 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 a83dff2cc8..2bb060db5e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -142,12 +142,14 @@ public: struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} + struct vehicle_torque_setpoint_s *get_torque_setpoint_0() {return &_torque_setpoint_0;} + struct vehicle_torque_setpoint_s *get_torque_setpoint_1() {return &_torque_setpoint_1;} + struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() {return &_thrust_setpoint_0;} + struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() {return &_thrust_setpoint_1;} + struct Params *get_params() {return &_params;} private: - void publishTorqueSetpoints(const hrt_abstime ×tamp_sample); - void publishThrustSetpoints(const hrt_abstime ×tamp_sample); - void Run() override; uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)}; @@ -189,6 +191,11 @@ private: actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons) + vehicle_torque_setpoint_s _torque_setpoint_0{}; + vehicle_torque_setpoint_s _torque_setpoint_1{}; + vehicle_thrust_setpoint_s _thrust_setpoint_0{}; + vehicle_thrust_setpoint_s _thrust_setpoint_1{}; + airspeed_validated_s _airspeed_validated{}; // airspeed position_setpoint_triplet_s _pos_sp_triplet{}; tecs_status_s _tecs_status{}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 3cb501313e..3a8728ca82 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -65,6 +65,10 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _actuators_out_1 = _attc->get_actuators_out1(); _actuators_mc_in = _attc->get_actuators_mc_in(); _actuators_fw_in = _attc->get_actuators_fw_in(); + _torque_setpoint_0 = _attc->get_torque_setpoint_0(); + _torque_setpoint_1 = _attc->get_torque_setpoint_1(); + _thrust_setpoint_0 = _attc->get_thrust_setpoint_0(); + _thrust_setpoint_1 = _attc->get_thrust_setpoint_1(); _local_pos = _attc->get_local_pos(); _local_pos_sp = _attc->get_local_pos_sp(); _airspeed_validated = _attc->get_airspeed(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index fb34aac380..7d7bc43aa9 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -218,6 +218,11 @@ public: struct tecs_status_s *_tecs_status; struct vehicle_land_detected_s *_land_detected; + struct vehicle_torque_setpoint_s *_torque_setpoint_0; + struct vehicle_torque_setpoint_s *_torque_setpoint_1; + struct vehicle_thrust_setpoint_s *_thrust_setpoint_0; + struct vehicle_thrust_setpoint_s *_thrust_setpoint_1; + struct Params *_params; bool _flag_idle_mc = false; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"