Altitude smooth mode doesn't work

#1

Hi all,

Somehow the z velocity setpoint isn’t smoothed in Altitude mode, although I have choose Smooth position control option for MPC_POS_MODE and set the max ACC up/down to 2m/s/s.

I feel that Altitude smooth flight task is overriden by normal Altitude flight task.
Here is the log, I kept the drone at fixed altitude and test the setpoint only.
https://review.px4.io/plot_app?log=813c76b8-0bb4-4a87-810b-c642533ee408

Please let me know your ideas.
Thanks.

#2

Hi @ttdat235, first question that I tried to answer: Did you change the code because I can’t find the hash from your log on the official PX4 repo. And if so did you change anything about the position controller or flight tasks?

#3

The altitude velocity setpoints in the log you provided look indeed unsmoothed. 3m/s change in a small fraction of a second:


This should not be possible since this part limits the acceleration of the setpoint situation dependent: https://github.com/PX4/Firmware/blob/master/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp#L144-L159

#4

I did some simulation tests on current master to check. With the defaults of 10m/s^2 MPC_ACC_UP_MAX & MPC_ACC_UP_MAX:
Log: https://logs.px4.io/plot_app?log=257c40c9-61f6-4541-900f-a5becb598f48

and with 1m/s^2:

It all worked as expected.
Now I’ll check your configuration and how long it should take to accelerate.

#5

In your log MPC_POS_MODE is set to the default value 1 and smoothing should be enabled.
Furthermore you have the defaults:

MPC_Z_VEL_MAX_DN, 1.0
MPC_Z_VEL_MAX_UP, 3.0

that correctly map full stick up to 3m/s
and you have the non-defaults constraining acceleration more:

MPC_ACC_DOWN_MAX, 2.0
MPC_ACC_UP_MAX, 2.0

If I use these settings on SITL master and do stick bang up down I get the expected smoothed result:
Log: https://logs.px4.io/plot_app?log=717bc430-8c89-4bab-88c9-fe1a80d9d7b5

#6

You have non-default values:

MPC_JERK_MAX, 0.0
MPC_JERK_MIN, 1.0

which only breaks things in vertical direction when you’re breaking and not when you’re accelerating. Why would you set JERK_MAX to zero?

I’m sorry but you need to provide more information about the version you use and what you changed. To rule out things I can encourage you to try latest master and resetting the involved parameters to defaults to check if it’s a configuration issue.

#7

@MaEtUgR thanks for your kind replies.
I figured out the problem. I’m using the previous firmware version which doesn’t have the case ManualAltitudeSmooth in mc_pos_control.cpp, so it will definitely choose ManualAltitude only.
This is the code from the latest stable version

I added this code to the position controller and everything is done.
Thanks.

#8

ok, I’m happy to hear that you found out. I can encourage you to use the latest version instead of just copying this part because then you’re on the latest version and debugging gets easier in case you have other problems. Fly safe and have fun.