Px4-autopilot: Offboard attitude control completely broken/unuseable

[anuppari]

I posted a similar issue in #6830 but got no response. I have now confirmed that it is not just in simulation, but also when the firmware is run on actual hardware.

There are large delays between when an attitude setpoint is set and when the px4 begins tracking the setpoint. See the images below. Note that I am comparing the setpoints the px4 itself is sending back over mavlink (via the setpoint loopback topic in mavros) to the actual attitude via a mocap system. This rules out any issue in ROS/mavros/mavlink/telemetry communication between companion computer and px4. Also, the mocap has a low latency connection to the plot computer, whereas mavros is over wireless, so the actual delay is even larger than what it noted below.

Observe the time period 1283-1285 seconds, where the x coordinate of the orientation (gold) steadily decreases, but the actual copter (blue) does not respond for 2 seconds! And throughout the time period pictured, there is a 0.25-0.5 second delay between commands and response.

Again notice the ~0.3 second delay in tracking. Also notice the weirdness at 1314-1315 seconds. The copter begins tracking, then pauses for 0.5 s, and then jumps to tracking again!?! Same thing at 1318-1320s.

Please help!

[lovettchris]

Yeah, I'm seeing pretty poor alt hold in offboard mode too, love to know how to fix it...

[anuppari]

So after looking through the mavros code, it looks like when setting attitude/thrust/attitude rates on their corresponding topics (~setpoint_attitude/attitude, ~setpoint_attitude/att_throttle, ~setpoint_attitude/cmd_vel), mavros sets the bitmask in the SET_ATTITUDE_TARGET mavlink message to ignore the others. So for example, publishing to ~setpoint_attitude/attitude, mavros masks out the throttle and attitude rates. However, when doing e.g., position control, one would need to set all three of attitude, thrust and rates, so the mavros is sending 3 messages for every setpoint, each with 2 fields masked out. The px4 doesn't seem to like this (it seems the px4 interprets the mask as setting different submodes within attitude control mode, rather than just using the masks to update the correct internal setpoints, though I haven't dug through px4 code enough to confirm).

The fix is to use the setpoint_raw/attitude topic that mavros subscribes to to publish all 3 (orientation, throttle, rates) in one message. Unfortunately, this is not a standard ROS message like pose, double and twist when using the setpoint_attitude topics, but if you're writing your own node to publish then it doesn't matter, and this way seems to work in both simulation and hardware.

[lovettchris]

Ah, indeed, it would not. Just be aware that PX4 only supports a subset of all possible bitmask combinations. (~/src/modules/mavlink/mavlink_receiver.cpp, lines 1200 through 1236). I've gotten good results with attitude control by sending only this:

void MavLinkVehicleImpl::moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust)
{
    checkOffboard();

    MavLinkSetAttitudeTarget msg;
    msg.time_boot_ms = getTimeStamp();
    msg.target_system = getTargetSystemId();
    msg.target_component = getTargetComponentId();

    float radRoll = roll * static_cast<float>(M_PI) / 180.0f;
    float radPitch = pitch * static_cast<float>(M_PI) / 180.0f;
    float radYaw = yaw * static_cast<float>(M_PI) / 180.0f;
    mavlink_euler_to_quaternion(radRoll, radPitch, radYaw, msg.q);

    // thrust must be between -1 and 1.
    thrust = static_cast<float>(fmax(-1.0f, fmin(1.0f, thrust)));
    msg.thrust = thrust;    
    msg.body_roll_rate = rollRate * static_cast<float>(M_PI) / 180.0f;
    msg.body_pitch_rate = pitchRate * static_cast<float>(M_PI) / 180.0f;
    msg.body_yaw_rate = yawRate * static_cast<float>(M_PI) / 180.0f;

    const uint8_t kIgnoreBodyRollRateBit = 1; //  mask bit 1: body roll rate,
    const uint8_t kIgnoreBodyPitchRateBit = 2; // bit 2: body pitch rate, 
    const uint8_t kIgnoreBodyYawRateBit = 4; // bit 3: body yaw rate. 
    const uint8_t kIgnoreBit4Reserved = 8; //  bit 4-bit 6: reserved
    const uint8_t kIgnoreBit5Reserved = 0x10;
    const uint8_t kIgnoreBit6Reserved = 0x20;
    const uint8_t kIgnoreThrottleBit = 0x40; // bit 7: throttle, 
    const uint8_t kIgnoreAttitudeBit = 0x80; // bit 8: attitude

    msg.type_mask = 0;
    if (rollRate == 0) {
        msg.type_mask |= kIgnoreBodyRollRateBit;
    }
    if (pitchRate == 0) {
        msg.type_mask |= kIgnoreBodyPitchRateBit;
    }
    if (yawRate == 0) {
        msg.type_mask |= kIgnoreBodyYawRateBit;
    }
    writeMessage(msg);
}

[dagar]

@anuppari is there anything that needs to be done here? Better documentation, or better offboard handling?

[anuppari]

I think it's both. PX4 should definitely handle offboard better in this kind of scenario where two attitude setpoint messages are sent, each with different fields masked out. But mavros (or px4) should definitely have better documentation that if one is trying to do complete offboard control, and therefore would necessarily need to set both attitude and thrust setpoints, they should use setpoint_raw messages, at least until this issue is fixed.

[realdealneil]

I would love to have a simple way to implement coordinated flight on the px4 stack. This was relatively simple to implement on APM (by changing firmware source code), but I'm finding it tricky in the least on px4. The control I'd like to have is to be able to command roll angle, pitch angle, and yaw rate, but allow thrust to automatically be determined to hold altitude. I have a path forward using something similar to what @lovettchris implemented above, but this just feels way harder than it should be.

[dagar]

@realdealneil we can add and maintain proper support for that. Where are you getting stuck?

[lovettchris]

What you are asking for is related to pull request #5816, where we added altitude hold control to the "moveByVelocity" case. But this case was easy because mc_pos_control pretty much already supported this case, it was jut not exposed through to mavlink, so the entire change really just boiled down to this one line in that file:

    _pos_sp(2) = _pos_sp_triplet.current.z; 

Doing this and providing attitude control seems harder because there's all kinds of weird edge cases, like obviously you can't hold altitude if you roll 90 degrees and so on... perhaps if you added a mode that interpreted "attitude" as more of a hint than a direct command then you could override the requested attitude in order to guarantee the alt hold... but that seems like deeper changes to mc_att_control. I agree it would be cool though. It would provide a way of doing more graceful flying without having to worry about crashing into the ground...

[realdealneil]

Is there a mavlink packet that directly supports the "moveByVelocity" case in the body frame? I'm trying to do my development in mavros and don't want to change PX4 firmware unless I can't achieve my objective without doing so.

I've so far tried /mavros/setpoint_velocity/cmd_vel (which looks like it uses "set_position_target_local_ned", but the inputs are in ENU. This doesn't support setting roll and pitch angles (which I would prefer so that I can attempt to cancel side-slip with roll angle).

Next, I tried using /mavros/setpoint_raw/local (which uses set_position_target_local_ned as well). I tried using the "FRAME_BODY_NED" coordinate frame, but it flew me off in weird directions that did not correlate to the body frame in any way I could figure out. I could use an NED frame (an inertial frame), but I was hoping that I could command velocities directly in the body frame to keep things simple.

Now, I'm going to try /mavros/setpoint_raw/attitude and see if that accomplishes what I'm after. I don't like having to convert roll/pitch/yaw into a quaternion in order to pass it in to the controller, and I don't like having to command the full quaternion (I'd prefer to pass in a roll angle, a pitch angle, and a yaw rate). I'd also like to be have the PX4 handle altitude control independent of my attitude/rate commands. Sorry if this sounds like griping. I think the mavros system and PX4 stack are excellent systems. If I'm missing a way of implementing simple body-frame-centric control, I'd love to know what I'm missing.

[realdealneil]

I was able to use the /mavros/setpoint_raw/attitude packet to achieve my purposes. I had to write an altitude/altitude rate controller, but that is working well now. Thanks for the advice. This thread can be marked completed.

[mandelyoni]

Dear @realdealneil,

How can I use mavros to send attitude and thrust commands (roll, pitch, yaw, thrust) to a PX4 stack?
Must I use OFFBOARD mode?
I can't find a detailed and correct answer.

Thank you, Yoni.

[realdealneil]

That's what I ended up doing. There may be other ways, but that's the
only way I've gotten it to work.

On Jan 26, 2018 23:02, "mandelyoni" [e-mail] wrote:

Dear @realdealneil https://github.com/realdealneil,

How can I use mavros to send attitude and thrust commands (roll, pitch,
yaw, thrust) to a PX4 stack?
Must I use OFFBOARD mode?
I can't find a detailed and correct answer.

Thank you, Yoni.

—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
https://github.com/PX4/Firmware/issues/6873#issuecomment-360963099, or mute
the thread
https://github.com/notifications/unsubscribe-auth/AGoEKcfgpVQy44t9r9csdcgkdYN1FgO4ks5tOrvsgaJpZM4MkcwO
.

[mandelyoni]

Were you able to send roll, pitch and yaw commands through /mavros/setpoint_raw/attitude?
Was the mode 'OFFBOARD' ?
Can you share some code?

Thank you, Yoni

[AlexisTM]

If you want to control the copter off the FCU board, you need the OFFBOARD mode.

To send attitude, you also need to send the thrust!

Easiest control is using velociy control.

Position control implementation is simply velociy*distance to point.

[mandelyoni]

ok, can you share some code?

thanks

[TSC21]

https://dev.px4.io/en/ros/mavros_offboard.html

[PX4BuildBot]

Hey, this issue has been closed because the label more_info_needed is set and there were no updates for 30 days. Feel free to reopen this issue if you deem it appropriate.

(This is an automated comment from GitMate.io.)

[marcelinomalmeidan]

Can anyone update px4's documentation? The documentation mentioned by @TSC21 is incomplete, since it only discusses the case for position control. I think this could save time for other people (like me) who need to work with attitude references.
BTW, the solution mentioned by @realdealneil worked for me, i.e., I had to publish into /mavros/setpoint_raw/attitude instead of the previous approach mavros/setpoint_attitude/attitude

[LorenzMeier]

@TSC21 Could you do that?

[TSC21]

Yeah sure. I will find a gap on my time this week to update the docs.

[PX4BuildBot]

Hey, this issue has been closed because the label more_info_needed is set and there were no updates for 30 days. Feel free to reopen this issue if you deem it appropriate.

(This is an automated comment from GitMate.io.)

[ghost]

Hello, guys!
I am running the offboard example on my quadcopter, with z=0.6. I am also using motion capture, but my drone doesn't really take off, it just arms and moves to the side, not up. Here is the log file. Please check it out and help me understand what am I doing wrong. I am struggling with this for quite some time now. :)

[AlexisTM]

@shortcut21 Could you try a higher height like 2m? There is something about a smooth takeoff feature.

In offboard mode I takeoff with a forced vertical speed of 2m/s.

[ghost]

Is there a way to change this? Since my motion capture device works in a pretty short range. Is there a way to change the vertical velocity?

[AlexisTM]

You can look here: https://github.com/PX4/Firmware/blob/e69049ec4183d820c6f716be1577df2ad07194b8/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1016-L1043

[ghost]

Can you tell me exactly which variable I need to change? Do I have to change the states.position(2) ? I need a lower altitude around 1 meter. After the change should I upload the firmware again?

I tried at z=2 it started to go up but also spinning .This is the log: https://review.px4.io/plot_app?log=be807c5a-aa13-4f36-9aac-53e8dd151292 .

[AlexisTM]

Can you double check all your motor directions and props?

[ghost]

They look fine to me. About the firmware, can you please help me there? Give me some guidance or tell me where to read.

[AlexisTM]

Short answer: I don't know. I just pointed you to the smooth takeoff feature by searching "smooth_takeoff" on the PX4 Github. The logic behind and how you want it is up to you :/

[ghost]

Thanks