diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 17074ca8a5..f79b64e26b 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -236,6 +236,9 @@ MulticopterRateControl::Run() actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear; actuators.timestamp_sample = angular_velocity.timestamp_sample; + publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample); + publishThrustSetpoint(angular_velocity.timestamp_sample); + // scale effort by battery status if enabled if (_param_mc_bat_scale_en.get()) { if (_battery_status_sub.updated()) { @@ -271,6 +274,30 @@ MulticopterRateControl::Run() perf_end(_loop_perf); } +void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, 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] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; + v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; + v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; + + _vehicle_torque_setpoint_pub.publish(v_torque_sp); +} + +void MulticopterRateControl::publishThrustSetpoint(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; + v_thrust_sp.xyz[0] = 0.0f; + v_thrust_sp.xyz[1] = 0.0f; + v_thrust_sp.xyz[2] = PX4_ISFINITE(_thrust_sp) ? -_thrust_sp : 0.0f; // Z is Down + + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); +} + void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt) { for (int i = 0; i < 4; i++) { diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 410e5dc1f4..6f8ef98288 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -60,6 +60,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -90,6 +92,9 @@ private: void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt); + void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime ×tamp_sample); + void publishThrustSetpoint(const hrt_abstime ×tamp_sample); + RateControl _rate_control; ///< class for rate control calculations uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; @@ -110,6 +115,8 @@ private: uORB::Publication _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)}; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; vehicle_control_mode_s _v_control_mode{}; vehicle_status_s _vehicle_status{};