Px4-autopilot: Setpoint raw issues Mavros ROS

[schmittlema]

This is based off of issue (https://github.com/PX4/Firmware/issues/5907) which is closed but I still can't get to work.

I have implemented with setpoint raw and multiple type masks (below) with no success. I get the quad rolling around mostly. Setting just position or velocity works fine (given the right typemask) but not both. I am trying to auto hold a altitude while moving around via x and y velocities. In all cases I set the velocity and positions, even the ignored ones (not that it made a difference).
coordinate frame is set to 1 if that is important.

Ignore x,y position: 0b0000101111000011
Ignore x,y position and z velocity: 0b0000101111100011 <-- this will rocket straight up if (despite being ignored) I set position.x, position.y, velocity.z = 0. When I commented them out, it rolled around.

Is there something wrong with my typemask? Thanks!

[TobiMiller]

Here my message generation in ros with python.
mask = 4035 # xy vel + z pos
mask = 3011 # xy vel + z pos + yaw
mask = 4088 # xyz pos
mask = 3064 # xyz pos + yaw

    raw_msg = PositionTarget()

    raw_msg.header.frame_id = "home"
    raw_msg.header.stamp = rospy.Time.now()
    raw_msg.coordinate_frame = 1
    raw_msg.type_mask  = mask
    raw_msg.position.x = x
    raw_msg.position.y = y
    raw_msg.position.z = z
    raw_msg.velocity.x = vx
    raw_msg.velocity.y = vy
    raw_msg.yaw        = yaw
    pubRaw.publish(raw_msg)

[schmittlema]

It is still rolling around. Am I missing something here? 4035 seems to allow z velocity control. And I don't fully understand the header/coordinate_frame stuff. Does the headerstamp need to be updated regularly?

        rpos = PositionTarget()
        rpos.type_mask = 4035 #0b0000101111100011
        rpos.header.frame_id = "home"
        rpos.header.stamp = rospy.Time.now()
        rpos.coordinate_frame = 1 
        rpos.position.z = 5 
        rpos.position.y = 0 
        rpos.position.x = 0 

        rpos.velocity.x = 2 
        rpos.velocity.y = 2

        self._takeoff(rpos) #This function connects to mavros and sets to offboard mode

        rate = rospy.Rate(10)
        print "Main Running"
        while not rospy.is_shutdown():
            rpos.header.stamp = rospy.Time.now()
            self.raw_pos.publish(rpos)

[TobiMiller]

try not to use the auto takeoff command and go directly into offboard mode. does anything change the behaviour? i think i had the same problem with going into offboard after an autotakeoff service call. i think it is a bug in firmware.

[schmittlema]

Yeah it already goes straight into offboard. I was never using auto-takeoff

[TobiMiller]

pls send the whole script. i will have a look. http://pastebin.com

[schmittlema]

The main code that calls these functions:
https://pastebin.com/uJmze5H7
The environment code:
https://pastebin.com/5zjWnUwy

Thank you!

[TobiMiller]

i would say you should create just a simple python script which publishes your message and you try qgroundcontrol to set the offboard mode. if this works you can go further. for me your code looks too bloated. beside some indent issues there is a lot of stuff you dont need in your code.

[schmittlema]

Ok. I made a simplified version for debugging this problem. Hopefully we can find my mistake and it isn't a bug:
https://pastebin.com/stHYfQRT

[TobiMiller]

https://pastebin.com/33sub8Wi

run this script. open qgroundcontrol. set offboard mode. arm. if this works your problem is somewhere else

[schmittlema]

I have found another way using a PID controller that worked. https://github.com/lukvacek/docking . Thanks for your help though!

[magiccjae]

@TobiMiller
Have you tested that your mask 4035(X, Y - velocity, Z - position) is working? Is there any particular reason that you allow Z-velocity as well when you only want to command Z-position? (i.e. why the mask is not 4067?)

[TSC21]

@schmittlema updates?

[schmittlema]

@TSC21 I haven't really worked on this since I found a PID controller that would do the trick. But I tested some stuff today and it still didn't work. This is all tested in Gazebo simulator
4035 - rolled around
4067 - never takes off
4088 - holds a position properly

here is the code:
https://pastebin.com/0Qig6SRf

[TSC21]

You cannot control velocity and position at the same time as the Firmware doesn't allow it. Or you control position, or you control velocity, or you control attitude. The only exception is with yaw, which you can control the angle or the rate combined with position or velocity control.

[schmittlema]

So there is no way to hold altitude and control xy velocity, or attitude? This would be a great feature to have for future versions of mavros/px4.

[TSC21]

So there is no way to hold altitude and control xy velocity, or attitude? This would be a great feature to have for future versions of mavros/px4.

MAVROS already allows it through setpoint_raw plugin. But not in the Firmware side. If you have an idea how to and have time to implement it, feel free to do a PR to PX4 Firmware with that. You will have to modify the mc_pos_control and the mc_att_control which I think it won't be so trivial.

[schmittlema]

ok. So then my problem that setpoint_raw isn't working for holding z and controlling xy velocity is still a valid issue?

[TSC21]

There is no problem. There is lack of hybrid control mode support on the Firmware side.

[schmittlema]

ohhhh ok so mavros is able to do it but px4 firmware can't support that yet. Got it, sorry for the confusion.

[TobiMiller]

@TSC21 You are wrong. The bitmask for xy velocity + z pos is implemented ;)
https://github.com/PX4/Firmware/commit/7be71459f5c2b9a206dc21bb7990d7d0f69a2968

[TSC21]

Oh my bad. Didn't saw that update. Thanks for the warning.

[TSC21]

@schmittlema, bitmasks from my analysis:
control vx, vy, z, y -> 3043 (0xBE3)
control vx, vy, z -> 4067 (0xFE3)
control vx, vy, z, y_rate -> 2019 (0x7E3)

control vx, vy, vz, y -> 3015 (0xBC7)

4035 -> control vx, vy, vz and z -> I suppose that's not what you want and that leads to the strange behavior you are seeing.
4088 -> control x, y, z -> so pure position control without yaw.

So any of the first for should work for you (the 4th also but it's pure velocity control).

About your code:

start_pos = PoseStamped()
start_pos.pose.position.x = 0
start_pos.pose.position.y = 0
start_pos.pose.position.z = 10

You init this and publish it to setpoint_position - what I advise you is to publish this to setpoint_raw as you do below so to avoid concurrency problems between modes.
Also:

target.position.x = 0
target.position.y = 0
...
target.yaw = 0

if you are ignoring those bits, you don't have to send this.

[schmittlema]

So i made your edits and still experience no movement with 4067 (my main setup i am trying to run).
https://pastebin.com/RU92M2NG
Suggestions?

[TSC21]

Do you have the console log from mavros?

[schmittlema]

Sorry but how do I get the console log from mavros?

[TSC21]

The output of the roslaunch mavros. Just copy paste output on the terminal after you run your control script

[schmittlema]

ok thanks. https://pastebin.com/QxMij8tA

[TSC21]

[ INFO] [1501268070.024584268, 4.578000000]: FCU: [cmd] ARMED by arm/disarm component command

Is somewhat disarming when it gets to the main loop.

[schmittlema]

hmm so do you think it isn't accepting the bitmask so it disarms? Also, could I possibly be working with a outdated version of px4?

[TobiMiller]

@schmittlema I think the community can only help you when you use the master

[TSC21]

What's the hash of the latest commit on your repo? Did you pull the latest master into it?

[schmittlema]

ok I think i found the problem. I am working off of a forked version that was forked pre https://github.com/PX4/Firmware/commit/7be71459f5c2b9a206dc21bb7990d7d0f69a2968 so it doesn't have that bitmask implementation.
I will work on updating my fork, and if i still have issues I will reopen. Sorry for the inconvenience i should have checked this earlier, and thank you all for your help.

[siddhu45]

Hi, I am trying to send commands using the setpoint raw commands and making a sort of hybrid controller. I want to send vertical velocity (Vz) to topic /mavros/setpoint_raw/local and attitude angles to /mavros/setpoint_raw/attitude (I am masking relevant values). I tried to send individually the velocity commands (works) and attitude commands with thrust (works), but I want to combine these two topics if possible and send attitude orientation with the vertical velocity, instead of thrust (just like the Parrot Bebop accepts these commands through MAVROS - attitude angles + vertical velocity). Is it possible on Pixhawk firmware to do this?? Thanks

[siddhu45]

Any insight @TSC21 @schmittlema ?

[schmittlema]

no it is not possible. You can only mask one or the other at a time. I had the same issue trying to send position and velocity commands. I ended up making a PID controller for the attitude commands so I could do both.

[siddhu45]

Hi, thanks for your reply. So, you are using the attitude setpoints topic now, is it?
Because I tried to send the attitude angles and vertical velocity, I won't say it works, but it gives some strange behaviour. Sometimes it doesn't hover and other times it constantly starts going up, and if I give a command in x and y, then it follows but doesn't seem to work as we would expect. So, I am guessing there can be some tweaks on the firmware side which could make it work because it's not like it does not work at all.

[schmittlema]

Yeah tweaking it could work. Making the PID loops seemed easier to me, but tweaking firmware would be a better long term solution.

[siddhu45]

Ok, great! Anyone from firmware side want to comment?

[ytw1996]

 hello,I just met a problem about using "setpoint_raw/local" to control my quad.I want to use ROS node to contorl the quad in its body frame but not the earth frame.However I notice that Mavros has already translate the body frame into the earth frame.
I did some search and find that the MAVROS topic"setpoint_raw/local"might control the quad in body frame.So I tried and finded that, by using this topic,the quad can takeoff with the setting of the position z.In this progress,the quad can keep the Yaw to the direction while it boot up.It seems workng.Then I failed because I find when I set the position x ,the quad cannt move along with the x axis in its body frame,but in the earth frame
 Fo you have any advice?

[mandelyoni]

Dear Sir,

We're running in Offboard mode and sending a set_attitude_target message through Mavlink.

It seems that the roll and pitch rate commands work, ok but the yaw rate doesn't work at all. This is the code from the Mavlink_receiver.cpp :

MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) {
...
....
....
rates_sp.roll = set_attitude_target.body_roll_rate;
rates_sp.pitch = set_attitude_target.body_pitch_rate;
rates_sp.yaw = set_attitude_target.body_yaw_rate;

}

Please advise.

Thanks,

[siddhu45]

@ytw1996 it depends what type of commands you want to send to the quad. The mavros follows the generic conventions and converts from ENU to the destination frames automatically. You need not worry about any transformation. Just follow the normal conventions and you should be able to command. In my opinion, if you are sending position commands, you should command in global (or Earth) frame and the mavros will do the required transformation. Same follows for other types of command.

@mandelyoni By doesn't work, what do you mean? what is the response of the UAV? Check if your UAV has gains set for yaw control. Try giving some feedforward gain for yaw.

Hope this helps!
Cheers

[ytw1996]

@siddhu45 yes, it actually worked in the Earth frame by sending position commands. Since my project is aimed at indoors obstacle avoidance ,I need to control the head of quad to a specific angle by sending the Yaw command, and I use a rotation matrix to transform the 3-d position in body frame to Earth frame.In this way, I can control the quad in body frame.