[VTOL] HIL with offboard attitude setpoints not working

TSt · October 1, 2019, 4:12pm

Hi,

I’m unable to get offboard attitude control working for VTOL. I’m using MAVSDK with a slightly modified version of the offboard_velocity.cpp example (only running the attitude part of it) and simulate it with HIL (Gazebo).

The issue is that my setpoints are overwritten from somewhere with the latest attitude setpoint before the switch to offboard (latest setpoint from Takeoff state in this case, the same happens when Loiter was the last previous state). A handful of the good setpoints are logged, though. I couldn’t pin down where and why the old setpoint is still kept and overwrites my new ones. Any help would be very appreciated!

Here’s a log from yesterday’s master branch:
https://logs.px4.io/plot_app?log=fc60426e-a06d-441f-b309-d3ee00d207a1

My guess was that it’s a problem with _mc_virtual_att_sp not being updated with the offboard setpoint, so I changed the ORB_ID of this line to mc_virtual_attitude_setpoint

Here’s the corresponding log :
https://logs.px4.io/plot_app?log=6b832832-7029-4d79-b9cc-6519da3c02cc

At least it now also shows some +20° roll angle setpoints, but it’s still not working.

The script works well for Quadcopters, so it has to be something specific to VTOLs:
https://logs.px4.io/plot_app?log=1bc8e39e-e545-4ffe-97ae-7cc937cdfeea

My final goal is to create a new VTOL attitude controller for quadplanes for my Master thesis, for which I need to have a good frequency response model. Which is why I would like to be able to act directly on the attitude setpoints to get good data.

Thanks a lot for any help!

P.S. This post is a follow-up of 12972, but in the meantime I have switched to HIL instead of SITL. I think that the underlying problem is the same, though.

P.P.S Another weird thing happened: with the exactly same setup (same firmware version, same offboard script, same Pixhawk 4 in HIL mode) as for the second log, one time the drone fell out of the sky. Restarting Gazebo solved the problem, but it was strange anyway:

https://logs.px4.io/plot_app?log=829778d8-ea18-4156-b7b5-e21bebec2eca

TSt · October 3, 2019, 2:29pm

Update: It now works in SITL, but not HIL. With this change:

Here’s a log from a mostly successful SITL simulation:
https://logs.px4.io/plot_app?log=6fc2d31b-8894-488d-8756-f85111fef2bc

I only don’t understand why the drone pitches down to -20° just after the switch to offboard (the pitch rate setpoint apparently asks for it, but I wouldn’t know why). Apart from that if follows the roll setpoint

I have no idea as to why it doesn’t work in HIL, but for the time being I’m happy that I can at least work with SITL. (even though I’m a little scared of the day when I first test it on a real drone )

Time to play around to get something useful out of it.

(The change in my code is not really suitable for a PR right now as it would screw up non-VTOLs. Maybe I get around to do something a bit more proper in the future if I find a good way to initialise the ORB ID accordingly with the new uorb API.)

jahendry33 · October 3, 2019, 6:28pm

Hey Thomas,

Sorry that I totally forgot to get back to you yesterday. I’ve spent a bit of time looking into your problem and while its very different to the problem I had it looks like you’ve stumbled across of potentially interesting bug and what you’ve found so far is totally worth a PR.

You’ve already seen the distinction between attitude control a standard multirotor and VTOL, specifically the use of the following respective uorb topics:

vehicle_attitude_setpoint
_mc_virtual_att_sp

These uorb topics feed into two entirely different attitude controllers where if you’re a standard multicopter the mc_att_control module is used and if you’re VTOL vtol_att_control. So basically that neatly explains why your fix worked in SITL as the MAVSDK interface was publishing to the wrong uorb topic. Mavlink and by extension MAVSDK cant properly control attitude in offboard mode as you’ve found and thats worth raising an issue on. The documentation or mavlink receiver handler should be updated. Congrats on that.

As for why its not working in HIL, things aren’t immediately obvious. Don’t fly until you’ve fixed this problem. Heres a couple things you should try in HIL mode to dig a bit deeper:

Put some PX4_WARN messages in the handler method for the set attitude method found here to make sure the mavlink messages are coming in correctly. You could also set some SITL breakpoints here to see if the weird rate setpoints behaviour are being sent in the first mavlink message

2.If you’re experience the behaviour seen in the second graph while in HIL the problem is the position and attitude controllers fighting for control of the pipeline. Ultimately i solved this problem by setting a guard clause to disable the following line of code here which is the basis of the architectural flaw i mentioned earlier. Basically the position controller tries to set an attitude at the same time as the mavlink message which causes the square wave type of setpoints, really screwed me for a while.

Hopefully one of those helps. Good luck on the Thesis.

Jamie

TSt · October 4, 2019, 9:05am

Hi Jamie,

Thanks a lot for your insight!

I agree that ultimately this is a bug that is worth a PR, but my current solution screws up all non-VTOL vehicles. Until I get around to fix that (I need to sort out other things first), my code is not ready for integration in master. Neither for as long as it doesn’t work for HIL and is untested in real life. I can raise an issue on Github though.

It’s still a great source of confusion for me as to why HIL isn’t working. The architectural flaw that you mentioned doesn’t seem to be responsible for my problem. This statement should protect my offboard setpoints. I set a debug breakpoint at publish_attitude() and it never triggered during offboard control. I also verified via NSH that indeed no flight task is active when I send the MAVlink attitude message.

I also tried to add some PX4_WARNs in the handle_message_set_attitude_target. Weird thing is that they never get logged in HIL, whereas in SITL they appear. Also, the breakpoints I set in this function never triggered in HIL, even though in the log I have a handful of the correct attitude setpoints. I don’t understand. At all.

Also, the VSCode debugger doesn’t run very reliably with HIL, roughly half of the time it doesn’t even let me arm the drone even though I wouldn’t know what I did differently compared to when it works.

TSt · October 4, 2019, 9:37am

Issue opened on Github: #13092

jahendry33 · October 4, 2019, 9:50am

Hey Thomas,

Yeah it’d be quite a bit of work to fix it in a consistent way, Auterion will pick it up if you raise the Github issue though so yeah go for that.

Thats interesting, as a side note do you know how the flight task is disabled when you are sending attitude setpoints? Offboard mode corresponds directly to a flight task here and I couldn’t ever figure out how that method ever returned false in offboard mode.

Yeah I’ve experienced really similar issues with PX4_WARN actually, we had some success using PX4_ERROR which takes higher priority in publishing to the logs if you want to try that. Definitely sounds like a mavlink issue in that case though. Can you see steady receiver traffic on the mavlink instance when you type mavlink status in the console on the instance you’re sending the setpoints from?

TSt · October 4, 2019, 10:09am

When you send attitude setpoints only the flight task shouldn’t be active because the second part of the if-statement is false. Neither of those flags should be activated if you only have attitude control. --> offboard mode without running a flight task

Ok, I might try with PX4_ERROR at some point. Not sure I understand your last question. Do you mean in the NSH of the Pixhawk I’m running HIL on? I’m launching an executable the standard MAVSDK example way to send my setpoints, so I unfortunately can’t interact with my script when it’s running. If you have a better solution for sending offboard setpoints, I’m all ears.

jahendry33 · October 4, 2019, 2:46pm

Hmm, I see what you’re saying. I’d have to look through the pipeline to see how the flight task is dynamically reactivated to allow for position/velocity control again.

In the nsh> console. It should show you that traffic is at least being received over the serial port. You can also use the listener command in the console to see if anything is being publish to uorb when youre sending via MAVSDK.

Yeah there is another way to attack the problem. You could set up a fast RTPS bridge between your companion computer and Pixhawk which gives you total access to uorb from offboard. This would at least sidestep all the issues which seem to be present in MAVLink and publish straight to uorb. The tutorial is here. Let me know if you have any further luck.

Thanks,
Jamie