Thrust to throttle in /mavros/setpoint_raw/attitude

Jaeyoung-Lim · April 3, 2019, 7:32am

@robin-ecn You can look into how the mixer works : https://dev.px4.io/en/concept/mixing.html

The actuator control message goes through the mixer as you can see here: http://docs.ros.org/api/mavros_msgs/html/msg/ActuatorControl.html

robin-ecn · April 5, 2019, 8:15am

Hi, Jaeyong.

In order to control each motor directly, I have read the webpages that you recommand and thank for that.

Now my idea is I receive the pose information from mocap in my conputor and I compute the required force and torques to track a trajectory. Then, I can mapping the force and torques into angular velocity for each motor. Then I just need to send those commands to /actuator_control.

I still have two questions in terms of mixer for quadrotors.

(1) I need to write a new simple mixer using tag M instead of R?

(2) I find the quad+x.main.mix to understand how the mixer maps the roll, pitch, yaw and thrust into the actuator commands. But there is only one line

R: 4+ 10000 10000 10000 0

I can not understand how this is transformed to the actuator_control.

Could you give me some help? Thanks

Jaeyoung-Lim · April 5, 2019, 8:19am

@robin-ecn It is because it is a multirotor mixer, geometry of 4+. It is explained in the document

robin-ecn · April 5, 2019, 8:21am

@Jaeyoung-Lim yes, is it possible to find out how those roll, pitch, yaw and thrust commandes are transformed into actuator commandes, like PWM?

This is because I try to understand how this works before writing my mixer file. Maybe it is not necessary?

Jaeyoung-Lim · April 5, 2019, 8:30am

@robin-ecn So the purpose of a mixer file is to use actuator commands without knowing how exactly the geometry of the drone is. If you want to look into the multirotor mixer you can look here

The attitude controller simply commands angular moments and thrust and this translates to the actuator inputs according to the geometry of the mixer. This enables same attitude controller be applied to various airframes.

pedro27 · March 18, 2020, 5:18pm

Hello everyone,

I am also trying to send thrust and attitude values to the drone with setpoint_raw but I end up with a problem which I think is related to the mode. The problem is that when I arm the drone and then publish a setpoint_raw message, it activates a failsafe saying: no RC and no Offboard. I am currently in the Offboard mode and I wanted to ask you which mode I should use to send thrust commands and not position commands. Thank you in advance.

Jaeyoung-Lim · March 19, 2020, 2:33pm

@pedro27 You need to be sending setpoints before you arm the drone and before you enter offboard mode

pedro27 · March 19, 2020, 5:13pm

@Jaeyoung-Lim that is actually what I did. Then once the drone was armed, I stoped sending position setpoints and tried to send only thrust and attitude commands and that’s when I got the Failsafe activated.

Jaeyoung-Lim · March 19, 2020, 9:17pm

@pedro27 why did you need to switch? It should work if you stream the attitude setpoints at a fixed rate

pedro27 · March 20, 2020, 1:12pm

@Jaeyoung-Lim You mean that before arming and entering offboard mode I should send attitude setpoints? No position setpoints? Because it looks like in order to arm and takeoff it needs a position setpoint but I might be wrong.

Jaeyoung-Lim · March 20, 2020, 1:53pm

@pedro27 It should also work if you send attitude setpoints and no position setpoints. It is more important you send valid setpoints before the timeout

pedro27 · March 20, 2020, 2:49pm

My code is the following:

#include <ros/ros.h>

#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/Thrust.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <gazebo_msgs/ModelStates.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}

int main(int argc, char **argv)
{
ros::init(argc, argv, “offb_node”);
ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("mavros/set_mode");



ros::Publisher thrust_pub = nh.advertise<mavros_msgs::AttitudeTarget>
        ("mavros/setpoint_raw/target_attitude",10);

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(100.0);

// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}

mavros_msgs::AttitudeTarget attitude_wanted;

double mag = sqrt(0.08*0.08 + 1);

attitude_wanted.header.stamp=ros::Time::now();
attitude_wanted.orientation.x = 0;
attitude_wanted.orientation.y = 0.08/mag;
attitude_wanted.orientation.z = 0;
attitude_wanted.orientation.w = 1/mag;

attitude_wanted.thrust = 0.3;

//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
thrust_pub.publish(attitude_wanted);
ros::spinOnce();
rate.sleep();
}

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = “OFFBOARD”;

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

ros::Time last_request = ros::Time::now();

while(ros::ok()){
if( current_state.mode != “OFFBOARD” &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO(“Offboard enabled”);
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO(“Vehicle armed”);
}
last_request = ros::Time::now();
}
}

thrust_pub.publish(attitude_wanted);
ros::spinOnce();
rate.sleep();

}

return 0;

}

It is pretty simple but I now keep getting:

I did as you said @Jaeyoung-Lim and I don’t know where it blocks. Thank you a lot for your help!

Jaeyoung-Lim · March 20, 2020, 5:11pm

You are not sending the setpoints while you are sending the offboard requests

pedro27 · March 22, 2020, 11:54am

@Jaeyoung-Lim

So after which Line should I add:

thrust_pub.publish(attitude_wanted);
ros::spinOnce();
rate.sleep();

?

Jaeyoung-Lim · March 23, 2020, 7:57am

@pedro27 You can compare your code with https://github.com/Jaeyoung-Lim/mavros_controllers/blob/master/geometric_controller/src/geometric_controller.cpp#L218

pedro27 · March 23, 2020, 2:36pm

Thank you @Jaeyoung-Lim , I actually found my mistake which was so stupid… Indeed I pulished on the topic that mavros is publishing, not the one he is subscring to. So I changed

ros::Publisher thrust_pub = nh.advertise<mavros_msgs::AttitudeTarget>
        ("mavros/setpoint_raw/target_attitude",10);

to

ros::Publisher thrust_pub = nh.advertise<mavros_msgs::AttitudeTarget>
        ("mavros/setpoint_raw/target",10);

Jaeyoung-Lim · March 23, 2020, 2:53pm

@pedro27 There is no topic that mavros subscribes to at mavros/setpoint_raw/target it should be ~setpoint_raw/attitude

You can look at the documentation here

pedro27 · March 23, 2020, 5:02pm

Oops yes sorry I ment mavros/setpoint_raw/attitude . My bad.

Vishwas_Iyer · April 27, 2020, 11:40am

Hi Robin,

I am also trying to calculate the required force and torque for a drone at various maneuvers. You have mentioned in this comment that you have calculated the required force and torques to track a trajectory. May I know how you did that? I also want to directly control the PWM signals and I am stuck at calculating the desired forces.

ZhishuoLi · March 15, 2023, 11:14am

Hi,
Do you find out the relationship between Thrust to throttle in /mavros/setpoint_raw/attitude?
I am going to use the thrust and body_rate in the /mavros/setpoint_raw/attitude to control my UAV, but it is not satisfactory. May I know how you did that? Wish your reply, thanks!