Roll Pitch rate limiting removed?

321 views
Skip to first unread message

Caleb Woodruff

unread,
Oct 22, 2015, 2:56:12 PM10/22/15
to drones-discuss
This patch removed rate limiting and added the sqrt controller to angular rates. https://github.com/diydrones/ardupilot/commit/afcd1c6ec326e631592847812a88559f3e95d4b0#diff-5ce13b8aa9036e1cf94bdc302cd38e93

I haven't been able to find any analysis of the sqrt controller. It seems to work out to sqrt(2*a(err-a/(2*sqrt(p_gain)) where a has units deg/sec^2, err is deg, and p_gain is (deg/sec)/deg. 
If I have correctly stated the formula the units don't seem to work out. Can anyone point me to an explanation for how this is supposed to work? I can see how the shape of this function might be nice but the tuning is escaping me.

Second question: Why were hard limits on rate removed? Rate limits are very helpful on my very large X8 frame.
Thanks,
--Caleb

Caleb Woodruff

unread,
Oct 22, 2015, 5:16:02 PM10/22/15
to drones-discuss
I found my error on the formula, should is sqrt(2*a(err-a/(2*p_gain^2))) so the units work out. I still haven't figured out why the rate limit was removed.

Leonard Hall

unread,
Oct 23, 2015, 7:24:32 AM10/23/15
to drones-discuss
Hi Caleb,

The Stab_P correction requests a rate of P x angle_error. As the copter corrects for an angle error it must decelerate to follow the P x angle_error curve. As the angle_error becomes larger the maximum acceleration becomes larger. At some point the copter can't accelerate that much and it overshoots, bigger still and it oscillates.

The rate limits are not there to limit the maximum rate, they are their to limit the maximum acceleration. The square root controller does this without limiting the maximum rate. This means the copter is able to correct large angle errors quicker with higher rates without exceeding the maximum acceleration capabilities of the airframe.

There is a log post here that goes into some depth at the end of the post.

Caleb Woodruff

unread,
Oct 23, 2015, 11:53:00 AM10/23/15
to drones-discuss
Thanks Leonard that is exactly the information I was looking for.

Maybe you can suggest which parameter to adjust. I work with a very large x8 (40" rotors). I have to do all my tuning to avoid excessive overshoot when slowing down from a dash into wind. The problem seems to be that as soon as the copter pitches up the 40" diameter times 8 rotors sail area causes the vehicle to climb. The z controller notices the climb and throttles down all motors so I lose control authority just when I need it to avoid massive overshoot in the pitch motion. We lost one vehicle to this problem. I can also recreate this with a 3dr x8 with 10 m/s or greater air speed.

In AC 3.2 I was able to work around the problem by setting the pitch and roll rate limits to about 30 deg/sec. I think that limit was applied just after the earth to body frame translation in your diagram here http://api.ning.com/files/MHYT4l2C-QNKVVfyfeXN0uN1IOe9qU2M4zeuheEnQWJVI3ZqhZJetfHjdnES7P-SfkFfDi6kM6WO8PevIZ2KOw__/ArduCopterV2.9AttitudePIDs.png

With Copter 3.3 I haven't been able to find an acceleration limit that leaves me with acceptable responsiveness in normal flight but still allows me to safely fly into wind. 40 deg/sec/sec works ok for dash but if there is any wind the lag from input is so much that the pilot can't control the vehicle.

Do you have any suggestions as to other gains I might look at?
Thanks,
--Caleb

Leonard Hall

unread,
Oct 23, 2015, 8:29:42 PM10/23/15
to drones-discuss
Hi Caleb,

Can you provide me with some photos and a log showing this behaviour so I can see what parameters you have set?

Have you done an autotune? If so can I see the log?

There are a couple of parameters I will be looking at.

ATC_ACCEL_P_MAX, This should be as low as you feel comfortable.
ATC_ACCEL_R_MAX, This should be as low as you feel comfortable.
ATC_ACCEL_Y_MAX, This should be as low as you feel comfortable.
ATC_RATE_FF_ENAB,1 (make sure this is one)
MOT_THST_BAT_MAX, just to make sure you have set it
MOT_THST_BAT_MIN,just to make sure you have set it
MOT_THST_EXPO,just to make sure you have set it
MOT_THST_MAX, just to make sure you have set it
MOT_YAW_HEADROOM,200 (If yaw control is part of the saturation problem then this may need to be reduced I will need to look at the log)
THR_MID, (this is probably very low but you may need to increase it ensure you have a normal takeoff weight)

I am happy to talk on skype and help you directly. I have an X8 that sounds like it is just a little smaller I hope to start flying for work in the next couple of months but until then 17" props is as large as I have.

Making sure we handle the "Large" class of multi rotor is very important!!!!

Chat soon,
Leonard

Leonard Hall

unread,
Oct 23, 2015, 9:04:51 PM10/23/15
to drones-discuss
MOT_THST_BAT_MIN is the main parameter that may be helpful here. You may need to increase it to 0.9.

Just be mindful that if you see oscillation in your copter switch to Stabilize because Alt Hold may not be able to force your throttle low enough to stop the copter climbing.

Chat soon,
Leonard

Leonard Hall

unread,
Oct 24, 2015, 6:43:50 AM10/24/15
to drones-discuss
Hi Caleb,

RC_Feel should be low too, like 10.

Chat soon,
Leonard

Caleb Woodruff

unread,
Nov 3, 2015, 7:18:07 PM11/3/15
to drones-discuss
Hey Leonard,
This video will give you an overview of what I'm working with. This is an older smaller version.

I would love to have your input on this.
--Caleb
Reply all
Reply to author
Forward
0 new messages