Holding altitude while controlling quadcopter with velocity commands

[L4ncelot]

Hi,

I have an algorithm which gives me velocity commands (ROS geometry_msgs/Twist type of message).

Now what I want to achieve with this is to control my quadcopter using those velocity commands with MAVROS package. The problem is that the algorithm expects quadcopter to hold it's altitude. So basically I want to hold given altitude and also control somehow linear velocity to fly somewhere in given altitude.

I've tried using MAVROS /mavros/setpoint_raw/local topic with mavros_msgs/PositionTarget type of message to do this. But this only works if I'm sending either position or velocity, but not both at the same time.

Is there some kind of altitude hold control instead of classic position control which doesn't work with velocity commands at the same time? I don't really want to write my own altitude controller :). I'm using the latest version of px4 firmware.

Thank you for any advice.

Edit: Here I can see setting the altitude hold in position's z-coordinate. This works, but the velocity values are doing nothing in this case no matter what even if this bool operation is false.

This is how I set the type_mask parameter:

    mavros_msgs::PositionTarget position_target;
    position_target.type_mask =
            mavros_msgs::PositionTarget::IGNORE_AFX |
            mavros_msgs::PositionTarget::IGNORE_AFY |
            mavros_msgs::PositionTarget::IGNORE_AFZ |
            mavros_msgs::PositionTarget::IGNORE_PX |
            mavros_msgs::PositionTarget::IGNORE_PY |
            mavros_msgs::PositionTarget::IGNORE_YAW |
            mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

[julianoes]

@L4ncelot Ha, I was running into exactly this as well, I think.

I've implemented in #5868 that altitude is held if the z velocity setpoint is 0 and position is held if the x and y velocity setpoint is 0. Can you try with my PR and maybe leave a comment there if something doesn't work for you?

[L4ncelot]

I tried your PR but it doesn't seem to work. Even if I only specify the z-coordinate and everything else leave to 0, the quadcopter can't reach the position. It starts to climb but it tilts forth and back no matter if it already reached it's destination or not. And then the quadcopter flies away and acts strangely :).

Changing then the velocity x,y coordinates seems to do nothing.

Am I doing it the right way? I mean, is there any difference if I use perhaps sp_position and sp_velocity topics?

[julianoes]

You'll have to share the code that you use to send setpoints, otherwise it's hard for me to judge.

[L4ncelot]

Well, I have a lot of things there which could be misleading to you. But I've commented everything which is not related to publishing position and it boils down to this code basically:

ros::Publisher pub_local_position_raw = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 1);

mavros_msgs::PositionTarget position_target;
    position_target.type_mask =
            mavros_msgs::PositionTarget::IGNORE_AFX |
            mavros_msgs::PositionTarget::IGNORE_AFY |
            mavros_msgs::PositionTarget::IGNORE_AFZ |
            mavros_msgs::PositionTarget::IGNORE_PX |
            mavros_msgs::PositionTarget::IGNORE_PY |
            mavros_msgs::PositionTarget::IGNORE_YAW |
            mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
position_target.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
position_target.position.z = 15.0;

ros::Rate rate(20.0);
ros::Time last_request = ros::Time::now();
while (ros::ok()) {
        ros::Duration time_difference = ros::Time::now() - last_request;
        if (current_state.mode != "OFFBOARD" &&
            time_difference > ros::Duration(2.0)) {
            setOffboardMode(srv_set_mode_client, set_mode_offboard);
            last_request = ros::Time::now();
        } else {
            if (!current_state.armed &&
                time_difference > ros::Duration(2.0)) {
                armVehicle(srv_arming_client, arm_cmd);
                last_request = ros::Time::now();
            }
        }

        pub_local_position_raw.publish(position_target);
        ros::spinOnce();
        rate.sleep();
}

Note that even if I set z-coordinate of position to 0 with all other values also at 0, quadcopter keeps behaving strangely as I described above. I can provide full code after some refactoring for you to read it easier, but after all comments it really does only the thing listed in code above so I think it's not necessary. In master branch the position control with this code works, but not with velocity commands.

[julianoes]

Aha, but you are not setting the velocities? I would add: mavros_msgs::PositionTarget::IGNORE_PZ | and then set all 3 velocities.

[L4ncelot]

Nono, I've set velocities and position first like this:

position_target.position.z = 15.0;
position_target.velocity.x = 1.0;
position_target.velocity.y = 0.0;
position_target.velocity.z = 0.0;

But then I encountered the strange flying behavior so I was curious if the flying model is messed up even for position control, which wasn't on master branch. So I commented velocities and tried if it works for z position control. As I said earlier, on master branch if I try to control quadcopter using either position or velocities, it works fine. But using both at the same time doesn't work.

[julianoes]

Oh, understood. Well I have not tried that. It would probably need some work in the position controller.

[L4ncelot]

Ok, I'll try something else then. Thank you for advice.

[BRNKR]

it is implemented and can be closed. you need to use setpoint raw and the right type_mask

[schmittlema]

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 positionx, poisition.y,velocity.z = 0. When I commented them out, it rolled around.

Is there something wrong with my typemask? Thanks!