diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 91dc5c339d..b48337f3d6 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -496,6 +496,20 @@ RoverPositionControl::Run() // timestamp and publish controls _act_controls.timestamp = hrt_absolute_time(); _actuator_controls_pub.publish(_act_controls); + + vehicle_thrust_setpoint_s v_thrust_sp{}; + v_thrust_sp.timestamp = hrt_absolute_time(); + v_thrust_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_THROTTLE]; + v_thrust_sp.xyz[1] = 0.0f; + v_thrust_sp.xyz[2] = 0.0f; + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + + vehicle_torque_setpoint_s v_torque_sp{}; + v_torque_sp.timestamp = hrt_absolute_time(); + v_torque_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_ROLL]; + v_torque_sp.xyz[1] = _act_controls.control[actuator_controls_s::INDEX_PITCH]; + v_torque_sp.xyz[2] = _act_controls.control[actuator_controls_s::INDEX_YAW]; + _vehicle_torque_setpoint_pub.publish(v_torque_sp); } } } diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 5fc1593df9..7bfe8a6e85 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -73,6 +73,8 @@ #include #include #include +#include +#include using matrix::Dcmf; @@ -126,6 +128,8 @@ private: actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; vehicle_local_position_setpoint_s _trajectory_setpoint{}; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};