UAV stops following Offboard velocity commands

Hello.

I am trying to land my quadcopter in offboard mode, because GPS is not available (and I could not get the Land function to work properly). Pixhawk 4 autopilot running PX4 firmware receives MAVSDK offboard commands (set_velocity_ned), and I cen see these setpoints in PX4 logs:

https://logs.px4.io/plot_app?log=f240667b-1793-4aef-ac66-043c09ff0b8a

At first the autopilot slowly increases it’s velocity, but then diverges away from the setpoint when closer to ground. It can be seen here:

Please ignore the part where the velocity setpoint messages are stopped at 38:52.

Can someone point out what could be causing this? When I run same code in gazebo simulator, the drone always tries to follow the velocity setpoint, even if at an offset or with a delay.

Hi @Themaksiest ,

It might be that it is because the hover thrust estimate goes to its minimum value before takeoff. I the version of the code you’re using, the hover thrust estimator starts on arming, which happens quite some time before takeoff. You can disable it using MPC_USE_HTE or upgrade to a newer release which has this issue fixed.

Hi. @bresch

Thanks for the response.

Could you elaborate on hover thrust estimator, or point me in a direction, where I can read more about it? How does it affect the velocity commands and what is it used for? I truly have no experience with this parameter other than that I know my drone is overpowered and the hover thrust is set to 20% which might affect the estimate.

Tried again with MPC_USE_HTE set to 0. Seems like nothing changed. When close to ground PX4 stops trying to achieve the setpoint velocity.
Logs can be found here: https://logs.px4.io/plot_app?log=cd70a2cb-42dd-4cd4-b869-ddab9cf3e14e

@Themaksiest Middle of offboard mode, the position setpoints are stopped and that is where your vehicle is changing behavior

If you want to control velocity, you need to not send position setpoints and set the position setpoint type mask accordingly

Hi @Jaeyoung-Lim . Thanks for the input.

I takeoff in offboard mode, by sending position command, then try to land by sending velocity commands.
In the image I pasted in the first post, between 38:48 and 38:49 when the Z setpoint changes sign is when I start sending velocity commands.
In this image you can see that velocity Z tries to match the Z setpoint, at the beginning as expected. Then Diverges closer to ground.

I am using MAVSDK without ROS. At first I am sending set_position_ned messages and then I stop sending them and begin sending set_velocity_ned. Unless I am unaware of something that goes on in the background of MAVSDK Offboard plugin, the position messages should not be sent when the velocity commands are sent.

Upgraded to latest stable version of 1.12.3 to try and test if this would fix the issue, but now the quadcopter is all jittery, so I will have to tune the PIDs. Lowered The initial P and D values, the jitters are now only present at some random moments when changing roll and pitch.

Hopefully I can get rid of those (no idea what is causing them, at the moment) and test if the new firmware solves my issue.

Hi. Upgraded Firmware, and somewhat tuned the rate controller for the drone to be flyable.

Unfortunately the issue of not always following the velocity command was not resolved.


Logs can be found here: https://logs.px4.io/plot_app?log=4dbc5b13-d92b-4c35-be2b-007e7a44fab4

Are there any other suggestions? Am I missing some functionality, that means that everything is working as it should?

Hi, I would suggest to decouple your offboard development with the altitude tracking issues you’re having. Do you have a log of the drone flying in stabilized and altitude (or position) modes?

I can also see in the log that you enabled EV pos and EV yaw. To make sure the issue isn’t related to that, just fly the drone in altitude with barometric height, without EV aiding.
Once you have the drone flying properly with a minimal configuration you can start to re-enable the other features piece by piece, but with everything together, it’s difficult to debug.

Hi @bresch.
Just made the stabilized and altitude mode flights, while communication between the offboard computer was disabled. I also switched EKF_AID_MASK from 24 to 1.
Here are the logs.
Stabilized:
https://logs.px4.io/plot_app?log=c6a6de6b-d8f6-462f-ab6e-c72f703398ca
Altitude:
https://logs.px4.io/plot_app?log=da652105-dcba-4d81-ad6b-332b1c444c81

To be honest I don’t see anything off in them. And the quadcopter flies fairly fine (Although I was just moving slowly through the room).
I would appreciate if you could advise me on the next steps, because I have no Idea what could be causing the behavior I am seeing when trying to use offboard control.

Hi @Themaksiest ,

You can see in the Altitude mode flight that the vertical velocity loop isn’t tracking well during takeoff. This is probably the same issue you’ve seen in offboard mode. What you need to do is to increase the gains of the vertical velocity controller to improve tracking (the integrator is apparently quite a bit too low).

Hi @bresch
Thank you for the response and advice.
What is the tool you are using for analysis? It does not look like Flight Review.

I tried increasing the I gain on Z velocity controller. Went from 2 (1…3) to 2.4 and it seem that responsiveness during flight is working ok (if I understand correctly), can’t tell if takeoff issue is fixed though. Could you take a look?

https://logs.px4.io/plot_app?log=62a46968-6671-4ad3-9bc8-b1bddf51b63a

Should I increase the P gain as well? Could there be an underlying issue with my Rate controller gains, or does that not affect the vertical velocity tracking so much? During flight the drone seems to fly stably, and responds well to RC commands. I think I am a little bit confused by all the available options.