Controling Yaw axis with "off-board" mode

[elhayra]

Hi,

I am trying to control all axes of quad-copter. While I was able to control x,y and z axes, I could't figure out how to control x axis. I tried many code combinations and also searched the internet several times, and ended up trying to use 2 different topics: one for controlling x,y,z , and the other for controlling yaw, but I still can't control yaw. This is my code so far:

//use mavros/setpoint_raw/local to control x,y,z
local_t_pos_pub = n.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 10);
//use mavros/setpoint_position/local to control YAW 
local_yaw_pub = n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
.
.
.
bool QNode::move(double m_x,double m_y,double m_alt,double m_yaw) {

    if (!master_connected) return false;

    mavros_msgs::PositionTarget targetMsg;
    targetMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
    targetMsg.header.frame_id = "aircraft";
    targetMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
                          mavros_msgs::PositionTarget::IGNORE_AFY |
                          mavros_msgs::PositionTarget::IGNORE_AFZ ;
                            //do not ignore yaw
                          //mavros_msgs::PositionTarget::IGNORE_YAW |
                            // mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
    targetMsg.header.stamp = ros::Time::now();
    targetMsg.position.x = m_x;
    targetMsg.position.y = m_y;
    targetMsg.position.z = m_alt;
    targetMsg.yaw = 30; //doesn't effect anything
    targetMsg.yaw_rate = 5; //doesn't effect anything
    local_t_pos_pub.publish(targetMsg);

    geometry_msgs::PoseStamped msg;
    msg.header.stamp=ros::Time::now();
    //msg.header.frame_id = "map";
    //msg.pose.position.x= m_x;
    //msg.pose.position.y= m_y;
    msg.pose.position.z=m_alt;
    //mavros wiki says that for this kind of msg, only YAW is extracted,
    //but next line doesn't effect anything:
    msg.pose.orientation= tf::createQuaternionMsgFromYaw(m_yaw);
    local_yaw_pub.publish(msg);
    std::cout << m_yaw << std::endl;
return true;
}

Since I am pretty much stuck, any help would be great.
Thanks a head!

[vooon]

http://mavlink.org/messages/common#SET_POSITION_TARGET_LOCAL_NED

You should also add velocity ignore bits, yaw is in radians, yaw_rate is radians/second.
Do not mix setpoint_raw topics with other setpoints. Just send mavros_msgs/PositionTarget

[elhayra]

Thank you for the fast replay.
I am now able to control yaw, but it doesn't seem to move as I expect it to.
I am feeding it with degrees which is then translated to rads inside my code.
When I send the msg I expect 1 of 2 things to happend:

1. Quad copter yawing X degrees to right/left
2. Quad copter yawing to X magnetic direction
   None of the above is happening. It seems like the quad is yawing to some random direction each time I send the msg. This is my updated code:

//publisher
_xyzPub = nodeHandle.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 10);
.
.
.
mavros_msgs::PositionTarget moveMsg;
moveMsg.coordinate_frame = mavros_msgs::PositionTarget::
                                                FRAME_BODY_OFFSET_NED;
moveMsg.type_mask =mavros_msgs::PositionTarget::IGNORE_AFX | 
                                     mavros_msgs::PositionTarget::IGNORE_AFY | 
                                     mavros_msgs::PositionTarget::IGNORE_AFZ |
                                     mavros_msgs::PositionTarget::IGNORE_VX  | 
                                     mavros_msgs::PositionTarget::IGNORE_VY  | 
                                     mavros_msgs::PositionTarget::IGNORE_VZ

moveMsg.header.stamp = ros::Time::now();
        moveMsg.position.x = x;
        moveMsg.position.y = y;
        moveMsg.position.z = z;
        moveMsg.yaw = yaw * 0.0174532925; //DEG to RAD
        moveMsg.yaw_rate = 1; 
        _xyzPub.publish(moveMsg);

Thanks again

[cs147]

I have the same problem!

[vooon]

What firmware used? Version?

I'm not sure that coordinate_frame really used, and fix to FRAME_LOCAL_NED.

[elhayra]

I am glad to see i'm not the only one. Maybe we are on to real bug here.
@vooon , when I am using FRAME_LOCAL_NED yaw is not working, and also the movements are relative to north-east coordinates, and not x-y as I want. So I must use coordinate_frame and set it to FRAME_BODY_OFFSET_NED to get movement on x-y.
I am using latest vertsion of MavProxy to test my code.

[cs147]

@vooon My last px4 update was from Sept 29. Do you know of any major recent changes that would affect the attitude control? I am not using MavProxy. I'm just using mavros/gazebo

This is the message documentation: http://docs.ros.org/jade/api/mavros_msgs/html/msg/PositionTarget.html
What frame do you think we should be using here?

Any tips?

[cs147]

@vooon I don't know if this is part of the problem, but: I am publishing on this topic: /mavros/setpoint_raw/local

/mavros/setpoint_raw/target_local shows that the commanded yaw_rate is -0.0 even though I set it to be >0 (I tried a variety of numbers and it seems that nothing works)
Additionally I command the type_mask to be 504 (ignoring all acceleration + velocity commands) but the type_mask in /mavros/setpoint_raw/target_local is 0
But the commanded position and yaw do show up as correct...

[vooon]

Mavros raw plugin really only support FRAME_LOCAL_NED, since frame conversion done for local ENU-NED.
Mavros always use ENU frame, even if constant is "_NED". That may confuse, but that done to match MAVLink enums (here LOCAL_ENU, which is not supported by known FCU's).

[vooon]

So next place is firmware source, as that place where come all loopback.

[cs147]

@vooon Should I be flipping the yaw I command to be ENU then? Or is that done in the firmware? I'm really confused, and I'm noticing that the yaw I command vs the yaw that the copter settles at seems to be offset by 90 degrees ...

[cs147]

@vooon I was looking here: http://docs.ros.org/indigo/api/mavros/html/mavros__uas_8h_source.html

00601         static inline double transform_frame_yaw_enu_ned(double yaw) {
00602                 return transform_frame_yaw(yaw);
00603         }
00604 
00608         static inline double transform_frame_yaw_ned_enu(double yaw) {
00609                 return transform_frame_yaw(yaw);
00610         }

Both of these call the same function.

Additionally, here: http://docs.ros.org/indigo/api/mavros/html/setpoint__raw_8cpp_source.html
yaw is transformed using those functions ...

[vooon]

ROS standard frame is ENU. I'm not sure now that ned-enu function is right.
But try Kinetic version first. Indigo frozen.

[cs147]

@vooon I do need to use indigo (for compatibility with other packages). Is this going to be a problem?

[cs147]

@vooon I checked the MAVLink Inspector widget as I was running the node.

The commanded yaw from mavros is identical to the commanded yaw in MAVLink. So where does the conversion occur? Did you look into this / is there a problem with either how I command the yaw angle or how the ENU to NED conversion is done in firmware?

cc: @eric1221bday

[AlexisTM]

@csorice147 You can install version 0.18.5 on Indigo as well, worked for me (using catkin_make_isolated because MAVLink is not a catkin package and following this guide)

[truhoang]

@elhayr1 have you successfully send yaw rate commands using your method in comment?

[elhayrobotican]

@csorice147, apparently code was fine. The drone I used in gazebo simulation didn't support yaw movement

[AlexisTM]

You can try to use the iris one, it supports yaw movement :D

[DalderupMaurice]

@elhayrobotican wich drone did you use?
Also, I was able to perform a yaw rotation once through publishing a command to mavros/setpoint_position/local. The next 100 tries however, did not work.. I am pretty much clueless what went wrong / different

[TSC21]

@elhayr1 and @cs147 is this solved for you both?

[TSC21]

Closing this as there's no feedback. Please reopen if needed.

[mandelyoni]

Is there a mavlink command to control yaw rate?
Thanks

[AlexisTM]

@mandelyoni Yes you can control yawrate; but you have to control 4 components of the copter: X, Y, Z and yaw, if you do not; what should your copter do?

[mandelyoni]

I would like to control all the angle rates and the thrust. I close the position loops by myself.
Currently I can't find a command that works for the yaw rate.
Any idea?

[TSC21]

@mandelyoni if you want to control the rates and the thrust you can either use setpoint_attitude by sending the rates on a geometry_msgs/TwistStamped message to cmd_vel topic and the thrust in mavros_msgs/Thrust message to thethrust topic. The topics would be sync on the setpoint_attitude plugin and the setpoint data is sent to the FCU in SET_ATTITUDE_TARGET messages.
The other option is to use setpoint_raw by sending mavros_msgs/AttitudeTarget messages to attitude topic. But you will have to set the proper bitmask on the msg, while the setpoint_attitude plugin already sets the bitmask to control the rates.

[mandelyoni]

Hi,

Did you try this on a drone? With Mavros or Mavlink?
it seems that the PX4 does not respond to the yaw rate command both through Mavros and Mavlink.

Thanks, Yoni.

[TSC21]

https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L1319-L1339 If you are doing things right, this would work.

[mandelyoni]

Dear TSC21,

I did everything according to handle_message_set_attitude_target.
While commanding the pitch rate and roll rate, everything seems ok (the drone flies to the required direction). But when I command the yaw rate, I receive no response.
Please advise.

Thanks, yoni.

[AlexisTM]

@mandelyoni The following is the code relative to attitude_target on the PX4.

https://github.com/PX4/Firmware/blob/25528a5ae0cc9cbada7c2daa266049f293fe05b0/src/modules/mavlink/mavlink_receiver.cpp#L1218-L1344

[mandelyoni]

Hi,
While commanding the pitch rate and roll rate, everything seems ok (the drone flies to the required direction), when I command the yaw rate, I receive no response. I tried both through Mavlink and
mavros.
Any idea?

[TSC21]

@mandelyoni please stop spamming the issues lists. You are not getting your problem solved faster that way.

[juanmed]

Hello, I am also trying to achieve control of the IRIS drone, by setting body rate and thrust commands through the /mavros/setpoint_raw/attitude topic, in gazebo simulation using PX4 SITL and MAVROS. So far I have not been successful and the drone flies but crashes immediately.

After looking at the documentation of the AttitudeTarget message, I understood the mask I should set in the type_mask field is 128 in order to ignore attitude. Could anyone please confirm this is correct?

So far I have not been successful, although I can control the IRIS drone in RotorS simulator by using my own low level controller (rotor speeds) and the same high level controller (thrust and body rates) I am trying to use with PX4+MAVROS.

Thanks for any answers!

[AlexisTM]

@juanmed What is the input for the other axis? In offboard mode, you are supposed to send the full setpoint.

Instead, use /mavros/setpoint_raw/local, this will allow you to give the following controls: position, velocity, acceleration for X,Y,Z and position and velocity for yaw.
I would propose you to use Vx, Vy, Vz, Vyaw.

Also, please do such tests in simulation first. That will avoid crashes.

[juanmed]

@AlexisTM Thank you for your reply. Sorry but what do you mean by 'the other axis' and 'the full setpoint'? It seems I am missing something.

I am sending AttitudeTarget messages to the /mavros/setpoint_raw/attitude topic because I am using my own position controller and a trajectory generator. I do not want to set a goal position and let the drone fly by itself there but to actually fly through a path computed by myself.

And I am doing all this in simulation in gazebo using PX4 SITL. Thanks for the advice.

[Jaeyoung-Lim]

@juanmed Hi, does this mean that you have issues running mavros_controllers ? The questions you are asking seems to be implemented in geometric_controller. I ask this as I am aware that you were using the package.

[juanmed]

@Jaeyoung-Lim Thanks for your reply. mavros_controllers works perfectly and has been a very good reference. It is my implementation, in which I use different controllers, that is not working as smoothly.