Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/TPA #5861

Closed
wants to merge 20 commits into from
Closed

Feature/TPA #5861

wants to merge 20 commits into from

Conversation

anton-matosov
Copy link
Contributor

@anton-matosov anton-matosov commented Nov 15, 2016

Re-implement Throttle PID Attenuation to support per-component setting of attenuation break and slope rate.
Used a bit different function that has preductable curve based on inputs.
You can compare both functions here https://www.desmos.com/calculator/gn4mfoddje

(PR includes new ZMR250 frame rc and mix files)

@LorenzMeier
Copy link
Member

Awesome! Do you have some feedback on MC_RATE_I_MODE?

@anton-matosov
Copy link
Contributor Author

I have removed it from this PR, I am still trying to figure out how this code affects flight characteristics

@kd0aij
Copy link
Contributor

kd0aij commented Nov 15, 2016

@anton-matosov Interesting; have you done any testing to show the motivation for and effectiveness of also scaling the I and D gains? Also, what is more predictable about the scaling now?

@anton-matosov
Copy link
Contributor Author

My drone had D oscilations at 0.8+ throttle that I couldn't compensate with existing TPA. That was the main motivation for these changes.
While implementing it I have found that it's hard to predict what will be your PID rate attenuation with max throttle for particular break and slope values.
With my function you always have (slope at full throttle==PID scale factor) and linear function from breakpoint to full for the scale.
You can see graphs for both functions here https://www.desmos.com/calculator/gn4mfoddje and play with b (break) and r (rate/slope) sliders

@@ -1005,6 +1053,63 @@ MulticopterAttitudeControl::task_main()
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}

if (_v_control_mode.flag_control_termination_enabled) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This change should be in my RC failsafe PR #5863

@kd0aij
Copy link
Contributor

kd0aij commented Nov 16, 2016

@anton-matosov Thanks,that makes sense. I hadn't noticed a need for decreasing D when tuning and my procedure was just to increase the slope till the oscillations were inaudible. Hence I didn't really care what the actual attenuation at max throttle was, though I did calculate it afterward. With the new scaling, both r and b affect the slope, so it might be best to rename the r parameter to something which more accurately names its function.

@anton-matosov
Copy link
Contributor Author

Agreed. In other flight stacks it's called TPA Rate
I will rename it tonight

@LorenzMeier
Copy link
Member

@anton-matosov This looks awesome. Is it feasible to enable a safe default for all platforms? Do you have a chance to test with a 450 sized drone?

@LorenzMeier LorenzMeier added this to the Release v1.5.0 milestone Nov 19, 2016
@anton-matosov
Copy link
Contributor Author

Unfortunately I have only 250 racer and 550 hexa (which weight 2 kg). I was ok to test full throttle with racer, but not comfortable at all to do this with hexa.
IMO default for TPA - is no TPA, as usually PIDs are not tuned to their extremes so there are no oscillations at higher/full throttle.

# Conflicts:
#	src/modules/mc_att_control/mc_att_control_params.c
@anton-matosov
Copy link
Contributor Author

Can anyone please take a look at this PR?

Copy link
Member

@bkueng bkueng left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you change the autostart id 4070 to something else? This conflicts with the recently merged Intel Aero board.

@anton-matosov
Copy link
Contributor Author

Sure, will change to 4080

@anton-matosov
Copy link
Contributor Author

@kd0aij P gains don't even participate in the acro mode, only rates. The main influencer of the attitude hold for acro mode is I rate, I have found it should be in the range of 0.19-0.25 for my frame/motors/esc/battery/props setup.

@LorenzMeier parameters are actually very unique for frame/motors/esc/battery/props combination. If I simply change props I already need to re-tweak parameters (deviation depends on how big is the change).
ZMR250 and QAV250 frames are very similar, but other components are not.
In addition to the frame size we should consider combinations of other components:

  • battery 3s vs 4s (totally different P and D rates) + vendor (different discharge rate and curve which affects TPA settings)
  • esc (different refresh frequency and motor timings and firmware, can drastically affect final thrust and require different P and D rates)
  • motor (generally only KV matters, totally different P and D rates for 2050kv and 2400kv)
  • props (diameter and pitch affect tuning significantly, e.g. 5030 2 blade vs 5051 3 blade requires different P and D rates)
  • final weight also matter

Setup for which zmr250 config was created is as follows:

  • battery: 2200mah 3s
  • motors: 1808 V-Spec 2400kv
  • esc: Afro 20A Race Spec ESC running SimonK
  • props: Gemfan 5030 2 blade

I will put this info into config as well.

I have switched to 3 blades recently and tried 2 different kinds:

  • For DAL 5045 quad become a bit sluggish, so I had to lower D a little bit
  • For RaceKraft RK5051 I also got oscillations on higher throttle and sharp turns, so I will have to adjust P rate and TPA settings (didn't have time to find perfect settings yet).

I have some flight videos captured for current settings and RK5051 props. Once I get a change to process them I will share them in the px4 discussion forums.

@kd0aij
Copy link
Contributor

kd0aij commented Dec 10, 2016

@anton-matosov Of course I was referring to the rate P gains; sorry for the ambiguity in my comment.
The roll/pitchrate_p gains for the ZMR250 in this PR are .05, whereas those for the generic 250 are .19

@anton-matosov
Copy link
Contributor Author

Yep, that is what I am flying at, and with RK5051 props I will have to lower them even more

@anton-matosov
Copy link
Contributor Author

anton-matosov commented Dec 10, 2016

Here is one of the FPV flights https://youtu.be/-SNok411Btc
and other one https://youtu.be/dqQlU0KXfS8

@kd0aij
Copy link
Contributor

kd0aij commented Dec 10, 2016

Thanks for posting video; that helps a lot for comparison. I'm running 3S, 2300KV motors and 5.4.6 Hyperion bullnose 2-blade props at a field elevation of 1760m. Hard to believe the altitude makes such a big difference; I wonder if something else is different?

I'd appreciate it if you could post logs using the new logger, with lots of rapid roll/pitch action. (easier to do in stabilize mode without crashing) That would let me run an impulse response analysis for comparison with my racers.

BTW, the current TPA eliminated high-throttle oscillations for me without degrading overall performance.

@anton-matosov
Copy link
Contributor Author

You are welcome.
Sure I can do that. How to enable new logger? (I have never used it before)

@kd0aij
Copy link
Contributor

kd0aij commented Dec 10, 2016

Assuming you're using Pixracer, all you have to do is use QGC to set parameter SYS_LOGGER to "new logger".

@anton-matosov
Copy link
Contributor Author

anton-matosov commented Dec 10, 2016

I have just double checked the config and it seems to be wrong.
I will update it now

Never mind, looked at the wrong commit.

@kd0aij
Copy link
Contributor

kd0aij commented Dec 10, 2016

Why have you reduced the multirotor mixer roll_scale parameter for the zmr250? Are you using that to compensate for a lower moment of inertia on the roll axis? My approach would be to lower the rate_P gain for the roll axis instead.

@anton-matosov
Copy link
Contributor Author

Are you referring to MC_PITCH_TC and MC_ROLL_TC? these parameters work as simple PID fine tune scale factor (MC_ROLLRATE_P/I/D * 0.2/MC_ROLL_TC, same for pitch). There is no real need to change them, you can simply adjust your PIDs, but I have changed them during original tuning and got current PIDs with TC applied and it was flying really well so I decided to leave them.
These TC params are used by QGroundControl for "roll/pitch sensitivity". I have confused them with RC rates (which are actually "encrypted" in MC_ACRO_R/P/Y_MAX params).

In the next iteration of tuning for 3 blades I am going to remove MC_*_TC from config

@LorenzMeier
Copy link
Member

Thanks for the good details @anton-matosov, it makes a lot more sense now. It would be great if you could work with @DonLakeFlyer on a tuning UI for multicopters that is more user-friendly (new or different sliders).

@LorenzMeier
Copy link
Member

I've rebased this, removed the merge commit, removed all noisy (fix ABC) commits and was left with one clean commit. See master.

@LorenzMeier
Copy link
Member

@anton-matosov Thanks for all the hard work!

@@ -0,0 +1,10 @@
# R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
R: 4x 7654 10000 10000 0
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@anton-matosov I'm talking about the roll_scale parameter (7654) on this line

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, never mind my previous post then.
Mixer config is the best way to accommodate custom geometry. ZMR 250 is asymmetric and mixer compensates this.
For ZMR250 mixing for cleanflight/betaflight please refer to https://oscarliang.com/custom-motor-output-mix-quadcopter/

@anton-matosov
Copy link
Contributor Author

anton-matosov commented Dec 15, 2016

@LorenzMeier
I have performed some testing of the acro mode on SK450 frame with 11kv motors, 10x4.5 slow-fly props and 3s battery. And it doesn't seem to have any noticeable oscillations on full throttle punches.
I have not finished PID tuning tho (broke frame), but I have noticed that I can/need get much higher D rate gain compared to racer setup before I get response without overshoot and even much higher gain to get sluggish response or D style twitchiness.
So behavior of PID loop in acro for bigger frame is very different.

@anton-matosov anton-matosov deleted the feature/TPA branch December 15, 2016 07:17
@kd0aij
Copy link
Contributor

kd0aij commented Dec 15, 2016 via email

@anton-matosov
Copy link
Contributor Author

@kd0aij my 450 is flying pixhawk, not pixracer will new logger work there?

@kd0aij
Copy link
Contributor

kd0aij commented Dec 15, 2016

If you're building fmu-v2_default, you have to change the cmake config for that target to build logger instead of sdlog2. Then just set parameter SYS_LOGGER to the new logger and it should start up on boot.

@anton-matosov
Copy link
Contributor Author

@kd0aij here are some flips and rolls logs for my ZMR250 (I hope this is the right file, FlightPlot doesn't plot any of it for some reason)
ZMR250 flips and rolls logs.zip
Unfortunately I broke my SK450 before I got a chance to grab logs for flips and rolls. So will get them for you once I fix it.

@kd0aij
Copy link
Contributor

kd0aij commented Dec 18, 2016 via email

@kd0aij
Copy link
Contributor

kd0aij commented Dec 18, 2016

@anton-matosov Your tuning looks great! Here's a link to the plots: https://github.com/kd0aij/impulse-response/tree/master/sample_ulogs/ZMR250/flips_250/plots
The rate impulse response is very close to critically damped for both roll and pitch, and the accel tracking has a latency of only 30msec.

@anton-matosov
Copy link
Contributor Author

Thanks. I am very pleased to hear that I did it right 😊

@anton-matosov
Copy link
Contributor Author

The waterproof frame looks interesting. How do you recover your quad from the water? What if it flips over?

@kd0aij
Copy link
Contributor

kd0aij commented Dec 19, 2016

A canoe is a good idea... Even with a receiver antenna taped to the inside bottom of the hull, I've not managed to flip back upright. Seems I lose the RC signal. But I'm sure I've seen videos of copters flipping back upright; would probably work with external whip antennas. A boat (or maybe a Golden Retriever) would still be a good idea though.

@kd0aij
Copy link
Contributor

kd0aij commented Dec 19, 2016

BTW, this is my S250 (3S) tuning: https://github.com/kd0aij/impulse-response/blob/master/sample_ulogs/S250AQ/2016-12-13/plots/rate_impulse.png

A little faster than yours, but a bit under-damped.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

5 participants