FlightTask: add 1st order lpf on yawrate satepoint for smooth motion #13430

bresch · 2019-11-11T12:27:32Z

Currently, there is no way to have a smooth yaw stop as the yaw locking mechanism is based on the position of the yaw stick: when the stick goes back to zero, the yaw setpoint is set to the current yaw and since the vehicle is moving, it is not physically possible to stop without overshoot.

To fix this issue, the yawspeed setpoint (generated from the stick position) is passed through a 1st order lowpass filter. The yaw lock now occurs when the output of the filter is at zero.
As shown below, there is no overshoot anymore:

Note: this modification affects altitude and position controlled modes (i.e.:not stabilized, not auto)

MaEtUgR

We definitely need to solve this but after reviewing and thinking about it I favor the slew-rate implementation. Please let me create a separate pr to compare. It's already implemented in https://github.com/PX4/Firmware/pull/12072/files#diff-cd11905871652d5f1467db1dfc9cecd2R61-R96

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

src/modules/mc_pos_control/mc_pos_control_params.c

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

bresch · 2019-11-13T16:32:04Z

@MaEtUgR Thanks for the review. I'm waiting for the slew-rate PR to compare then.
I was also considering slew-rate when I did this PR but I decided to go with the filter; I thought the behavior would be more intuitive (linear instead of nonlinear) but I didn't actually try.

MaEtUgR

We can test the differences to #13464 later on. It's easy to change again based on our findings from the comparison and any of the solutions is better than none.

I added a commit for my parameter description suggestions. You can change and squash them if you like.

bresch · 2019-11-18T21:05:55Z

@PX4/testflights Could you please test this PR? In altitude and position control, the yaw "stop" should be smoother than before.

dannyfpv · 2019-11-19T18:23:17Z

Tested on Pixhawk 4 v4 f-450

Modes Tested:
Altitude mode: Yaw not working
Position mode: Yaw not working
Stabilized mode: yaw working fine

log:
https://review.px4.io/plot_app?log=6c8619cd-b35e-4323-95c2-abd143e40d0e

Tony3dr · 2019-11-19T18:39:32Z

@bresch it seems like the Yaw is not working on Pixhawk 4, PixRacer,Pixhawk Pro and NXP FMUK66.

bresch · 2019-11-19T21:22:06Z

@dannyfpv @Tony3dr that's really strange but thanks for the test, I'll have a look at the log

jorge789 · 2019-11-19T21:33:35Z

Tested on PixRacer V4:

Modes Tested:
Altitude mode: Yaw not working
Position mode: Yaw not working
Stabilized mode: yaw working fine

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.
Note:
In position and Altitude mode Yaw does not work, in stabilized it seems to work fine

Log: https://review.px4.io/plot_app?log=5ff1f19a-ae37-47fe-a6f9-1c8ca1410508

Tested on Pixhawk V4pro:

Modes Tested:
Altitude mode: Yaw not working
Position mode: Yaw not working
Stabilized mode: yaw working fine

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.
Note:
In position and Altitude mode Yaw does not work, in stabilized it seems to work fine**

Log: https://review.px4.io/plot_app?log=d46b6772-8e8f-40a3-b646-4c490ab7f3b1

Tested on NXP_FMUK66_V3:

Modes Tested:
Altitude mode: Yaw not working
Position mode: Yaw not working

Procedure
Arm and Takeoff in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.
Note:
In position and Altitude mode Yaw does not work, in stabilized it seems to work fine**
Log: https://review.px4.io/plot_app?log=729b9acc-4425-4cf1-ba4d-07ca9817e1bf

bresch · 2019-11-20T08:48:02Z

@PX4/testflights I was able to recreate the issue and fixed it, it's now ready for testing again.

MaEtUgR

Looks good, I did all my checks and added some minor changes that don't change the behavior in the last commit. Ready to merge.

MaEtUgR · 2019-11-22T07:45:01Z

@PX4/testflights Could you give this another go? We're interested in your judgment about stick yawing when flying in Altitude and Position mode. It should be smoother and have no overshoot when stopping quickly in yaw.

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

src/modules/mc_pos_control/mc_pos_control_params.c

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

MaEtUgR

@bkueng ?

jorge789 · 2019-11-23T00:31:32Z

Tested on PixRacer V4:

- Modes Tested
Position Mode: YAW GOOD.
Altitude Mode: YAW GOOD.

Procedure
Takeoff in position mode, the yaw was tested and it worked well. After flying for about 1 minute, changed to altitude Mode, yaw seems to work fine and was smooth.

Log: https://review.px4.io/plot_app?log=54e39fe6-ab5d-4f77-9446-539aa857547a

Junkim3DR · 2019-11-23T05:04:10Z

Tested on NXP FMUK66 v3:

- Modes Tested
Position Mode: YAW GOOD.
Altitude Mode: YAW GOOD.

Procedure
Takeoff in position mode, the yaw was tested and it worked well. After flying for about 1 minute, changed to altitude Mode, yaw seems to work fine and was smooth.

Log: https://review.px4.io/plot_app?log=10ab6b43-de22-49f7-88db-ae03a7e5d113

Tony3dr · 2019-11-24T04:22:04Z

Tested on Pixhawk 4 V5:
Modes Tested
Position Mode: YAW GOOD.
Altitude Mode: YAW GOOD.

Procedure
Takeoff in position mode, the yaw was tested and it worked well. After flying for about 1 minute, changed to altitude Mode, yaw seems to work fine and was smooth.

Log: https://review.px4.io/plot_app?log=26192193-9762-4ac3-8da6-752171432b30&fbclid=IwAR3h5_wU5RDtIlpHSMCIoSSP7ow4tJ9WhUl_Q4LiT0-MKsPmJjpGF9esGt0

…filter state insted of using the previous setpoint

@bkueng

… comments FlightTaskManualAltitude: declaration order Addressing @bkueng's review comments.

mortenfyhn · 2020-02-21T11:08:34Z

This feature is really useful, so I just wanted to drop a comment to say thanks. Keep up the good work!

bresch added Admin: Enhancement (improvement) 💡 Multirotor 🛸 labels Nov 11, 2019

bresch requested review from bkueng, MaEtUgR and RomanBapst November 11, 2019 12:27

bresch self-assigned this Nov 11, 2019

MaEtUgR reviewed Nov 13, 2019

View reviewed changes

bresch force-pushed the pr-man-yawrate-filter-upstream branch from b2b0916 to c577967 Compare November 13, 2019 16:54

MaEtUgR mentioned this pull request Nov 13, 2019

FlightTask: Yawspeed slew rate for smooth motion #13464

Closed

MaEtUgR force-pushed the pr-man-yawrate-filter-upstream branch from c577967 to 2ed1631 Compare November 18, 2019 10:57

MaEtUgR previously approved these changes Nov 18, 2019

View reviewed changes

bresch dismissed MaEtUgR’s stale review via 370dcef November 20, 2019 08:44

bresch force-pushed the pr-man-yawrate-filter-upstream branch from 4f917aa to 370dcef Compare November 20, 2019 08:44

MaEtUgR previously approved these changes Nov 21, 2019

View reviewed changes

bkueng reviewed Nov 22, 2019

View reviewed changes

bresch dismissed MaEtUgR’s stale review via d8b4a09 November 22, 2019 10:37

bresch force-pushed the pr-man-yawrate-filter-upstream branch from 41d611c to d8b4a09 Compare November 22, 2019 10:37

bresch force-pushed the pr-man-yawrate-filter-upstream branch from d8b4a09 to 3d380dc Compare November 22, 2019 10:40

MaEtUgR force-pushed the pr-man-yawrate-filter-upstream branch from 9c83a1c to 7bc2789 Compare November 22, 2019 14:34

MaEtUgR previously approved these changes Nov 22, 2019

View reviewed changes

bresch and others added 3 commits November 25, 2019 11:05

FlightTask: add 1st order lpf on yawrate satepoint for smooth motion

3600fb2

FlightTaskManualAltitude: Use dedicated member variable for yawspeed …

0cc9be8

…filter state insted of using the previous setpoint

FlightTaskManualAltitude: yawspeed filter remove line with no effect,…

1581985

… comments FlightTaskManualAltitude: declaration order Addressing @bkueng's review comments.

bresch dismissed MaEtUgR’s stale review via 1581985 November 25, 2019 09:13

bresch force-pushed the pr-man-yawrate-filter-upstream branch from d418122 to 1581985 Compare November 25, 2019 09:13

bkueng approved these changes Nov 25, 2019

View reviewed changes

bresch merged commit 4a28c81 into master Nov 25, 2019

bresch deleted the pr-man-yawrate-filter-upstream branch November 25, 2019 10:55

MaEtUgR mentioned this pull request Dec 4, 2019

Yaw during takeoff #13675

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

FlightTask: add 1st order lpf on yawrate satepoint for smooth motion #13430

FlightTask: add 1st order lpf on yawrate satepoint for smooth motion #13430

bresch commented Nov 11, 2019

MaEtUgR left a comment

bresch commented Nov 13, 2019

MaEtUgR left a comment

bresch commented Nov 18, 2019

dannyfpv commented Nov 19, 2019 •

edited by Tony3dr

Loading

Tony3dr commented Nov 19, 2019 •

edited

Loading

bresch commented Nov 19, 2019

jorge789 commented Nov 19, 2019 •

edited by Tony3dr

Loading

bresch commented Nov 20, 2019

MaEtUgR left a comment

MaEtUgR commented Nov 22, 2019

MaEtUgR left a comment

jorge789 commented Nov 23, 2019 •

edited by Tony3dr

Loading

Junkim3DR commented Nov 23, 2019 •

edited

Loading

Tony3dr commented Nov 24, 2019 •

edited

Loading

mortenfyhn commented Feb 21, 2020

FlightTask: add 1st order lpf on yawrate satepoint for smooth motion #13430

FlightTask: add 1st order lpf on yawrate satepoint for smooth motion #13430

Conversation

bresch commented Nov 11, 2019

MaEtUgR left a comment

Choose a reason for hiding this comment

bresch commented Nov 13, 2019

MaEtUgR left a comment

Choose a reason for hiding this comment

bresch commented Nov 18, 2019

dannyfpv commented Nov 19, 2019 • edited by Tony3dr Loading

Tony3dr commented Nov 19, 2019 • edited Loading

bresch commented Nov 19, 2019

jorge789 commented Nov 19, 2019 • edited by Tony3dr Loading

Tested on PixRacer V4:

Tested on Pixhawk V4pro:

bresch commented Nov 20, 2019

MaEtUgR left a comment

Choose a reason for hiding this comment

MaEtUgR commented Nov 22, 2019

MaEtUgR left a comment

Choose a reason for hiding this comment

jorge789 commented Nov 23, 2019 • edited by Tony3dr Loading

Tested on PixRacer V4:

Junkim3DR commented Nov 23, 2019 • edited Loading

Tested on NXP FMUK66 v3:

Tony3dr commented Nov 24, 2019 • edited Loading

mortenfyhn commented Feb 21, 2020

dannyfpv commented Nov 19, 2019 •

edited by Tony3dr

Loading

Tony3dr commented Nov 19, 2019 •

edited

Loading

jorge789 commented Nov 19, 2019 •

edited by Tony3dr

Loading

jorge789 commented Nov 23, 2019 •

edited by Tony3dr

Loading

Junkim3DR commented Nov 23, 2019 •

edited

Loading

Tony3dr commented Nov 24, 2019 •

edited

Loading