Controlling the actuators of quadcopter with torque and thrust inputs

I want to control the actuators directly using torque and thrust inputs in SITL. I have tried using:

  • ‘/mavros/setpoint_raw/attitude’ for this but the quadcopter is not flying as expected. I wrote a script which sets it in offboard mode and continuously sends the position setpoint(msg type: mavros_msgs/PoseStamped) as 5m and another message movement_cmd(msg type: mavros_msgs/AttitudeTarget) with values:

movement_cmd.header.frame_id = ‘base_link’
movement_cmd.body_rate.z = 0.1
movement_cmd.body_rate.x = 0.5
movement_cmd.body_rate.y = 0.8
movement_cmd.type_mask = 7

I wrote the code which publishes movement_cmd msg only till the drone reaches the position setpoint of 5m. But the drone appears to fly up and down repeatedly for a while and then reach its setpoint. So here, I’m not able to observe the application of ‘mavros_msgs/AttitudeTarget’ as expected.

  • I also went through few discussions in which control allocation matrices are being used to send the torque and thrust commands to control the motors directly but I’m not sure how to approach this method.

Can anyone suggest me how to proceed with this issue?