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

For discussion: Integrated yaw control #6432

Merged
merged 4 commits into from Dec 15, 2018

Conversation

Projects
None yet
8 participants
@joelucid
Copy link
Contributor

joelucid commented Jul 23, 2018

Just had this epiphany this weekend about how radically different yaw is from the other axis. Started out with this thinking process on why yaw has better response for small rate changes, but worse for large ones than R/P (from rcgroups):

"Actually thinking about it I can see why yaw would behave differently: prop thrust is proportional to rpm squared and roll acceleration is proportional to 2 x thrust (for both motors). Yaw control on the other hand not only has the component from rotor drag which would also be proportional to 2 x rpm^2, but also an inertia based component: since the total angular momentum is preserved in the total system spinning motors 1 and 4 up causes an increase in angular velocity of the quad in the opposite direction. The angular acceleration caused by this second effect is proportional to the angular acceleration of props. Note the difference: the acceleration due to prop thrust/drag depends on rpm, the on based on yaw inertia on the rate of change of rpm.

You can see where this is going: As the yaw move starts the props accelerate and immediately provide the inertia based yaw acceleration to the quad. Actually that acceleration will be largest right at the beginning of the move (modulo low rpm esc issues). The thrust based roll move needs some time to really get going since it's based on rpm, not rotor acceleration. The rotor drag component of yaw seems to be lower than the rotor thrust component of roll, so that as the props speed up yaw falls back in response. So we would expect very good yaw authority at the beginning of a move, but then decreasing relative to roll authority as the prop acceleration power is shot.

And now I finally understand, why you don't need D on yaw: As you approach target yaw rate the motors spin down. But other than in the roll/pitch case reducing the motor rpm actively and immediately accelerates the quad in the opposite yaw direction via the inertia effect which is proportional to rate of rpm change instead of rpm. So yaw kind of has an active brake built in.

Finally this is why you should use extremely high I on yaw. The significant inertia based effect means that yaw will behave very differently depending on the current acceleration of the props. So assuming a linear system as a PID does is actually not very correct. The errors will be large. Would be interesting to modify the mixer to take this more complex dependency on RPM into account to more effectively translate yaw pid sum to motor signal. One more reason to want real time RPM telemetry from the ESCs."

Thinking more about it here are some conclusions:

Since the derivative of RPM causes yaw action a correct mixer would integrate the yaw pids and give the result to the motors. We don't and what does that mean? It means that what we configure as P is really D and our I is P. This is why we don't need a D currently - we already have it.

In light of #6355: what does this mean for FF? First of all what we call P today is based on error, so it is a D with setpoint weight of 1. Yaw has always had FF! We've now added a FF component which is not integrated, so it effectively acts as kick, providing some extra punch when what would be normal FF changes.

I've implemented an experimental change in this PR to test integrated control for yaw. Works pretty well. P and D now work as expected. I is not needed since it's an integrative process. And FF can now be specified and acts as normal FF.

On my 2204 5" I need p_yaw=30, d_yaw=20, i_yaw=0 and f_yaw=130. With these settings and using smart_feedforward response is best - almost too good.

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Jul 23, 2018

No need to request style changes yet since this is only an experiment for discussion.

@DieHertz

This comment has been minimized.

Copy link
Member

DieHertz commented Jul 23, 2018

Now we need to come up with a way to abstract it out :)

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Jul 23, 2018

From rcgroups:

Windup prevention is already there via the yaw pidsum limit. I itself isn’t needed at all due to the integrative nature of the process.

A big advantage for F is that since it’s now integrated step increases don’t lead to step changes in motor output. That’s pretty important since on yaw a step increase in motors is a step increase in quad acceleration. Which is different from R/P.

With that rc interpolation isn’t needed for yaw. And for P/D I’ve found that some minor smoothing of the motor curve is enough to prevent ugly noises from the motors. Plus I have a better way to smooth it than low pass filtering worked out. With that it should be possible to further reduce latency while keeping motors smooth on quads with stronger motors.

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Jul 23, 2018

Here's a log with some yaw activity. This has smart_feedforward enabled.

LOG00068.BFL.zip

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Jul 24, 2018

Absolutely fantastic yaw responses, except when you hit acc_limit_yaw of 100 at 33s :-)
That limit is the cause of the straight line where it won't keep up with command at that point, causing a big error and leading to tendency to overshoot.
Please see / support my PR to fix this issue:
https://github.com/betaflight/betaflight/pull/6354/files

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Jul 24, 2018

@ctzsnooze acc_limit_yaw is at 0 for this quad. If you compare the gyro line with motors you’ll see the actual cause of the straight line: the ability to use the inertia based reverse torque naturally stops when we can no longer increase motor rpm. At that point all that’s left is rotor drag to increase yaw rate. That’s fairly slow but doesn’t lose effectiveness much as yaw rate increases. The overshoot is due to absolute control making up for the slow move.

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Jul 24, 2018

Here's a pic that shows that gyro acceleration is actually faster than the straight line early in the move. Just as one would expect from inertia based reverse torque diminishing as motor rpm maxes out.

yawspin

@stale

This comment has been minimized.

Copy link

stale bot commented Aug 23, 2018

This issue / pull request has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs within a week.

@stale stale bot added the Inactive label Aug 23, 2018

@Yamaford

This comment has been minimized.

Copy link

Yamaford commented Aug 29, 2018

Hush it stale bot!

@stale stale bot removed the Inactive label Aug 29, 2018

@cudakeer808

This comment has been minimized.

Copy link

cudakeer808 commented Aug 29, 2018

Don’t know if I fully grasp this but the current mixer need modification to actually use the diagonal torque bias efficiently?

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Sep 10, 2018

@bexamous @ctzsnooze, so integrated yaw works very well from my perspective. The advantages in summary:

  • Easier to tune since the parameters then mean the same as on the other axis.
  • Configurable FF (instead of the current FF which is really a boost on change of stick speed). FF leads to less rc noise on the motors, so you can use less rc smoothing -> less delay.
  • No undue interaction with ITerm mechanisms: without integrated yaw I is P. But I is also subject to iterm windup protection, iterm relax etc, which isn't really appropriate for P.

I've now tested it on 5" and 4" and I personally will not go back. Would appreciate any feedback if anybody gets a chance to test it. The PR is at #6432.

When I built primarily weaker quads I never understood the need for any rc smoothing. That changed when I went to 2306. But I found that the motor noises were primarily mediated via yaw due to the FF/boost connection. With integrated yaw much less of rc is needed, giving less latency.

Anyway - I'm not in a hurry with this PR. Currently developing a whole set of changes related to better responsiveness and I'm fine to get these integrated together when they are all ready. Still I think this one would be useful on it own so if others find it useful as well I'd be open to get it done earlier.

@@ -399,6 +399,8 @@ static FAST_RAM_ZERO_INIT float acErrorLimit;

void pidResetITerm(void)
{
extern float integratedYaw;
integratedYaw *= 0.99f;

This comment has been minimized.

@borisbstyle

borisbstyle Sep 12, 2018

Contributor

Why do you want yaw to slowly unwind in here?

This comment has been minimized.

@joelucid

joelucid Sep 12, 2018

Author Contributor

I don't want yaw to go crazy while hand starting

This comment has been minimized.

@borisbstyle

borisbstyle Sep 12, 2018

Contributor

Why not just setting it to zero like the rest?

@@ -399,6 +399,8 @@ static FAST_RAM_ZERO_INIT float acErrorLimit;

void pidResetITerm(void)
{
extern float integratedYaw;

This comment has been minimized.

@borisbstyle

borisbstyle Sep 12, 2018

Contributor

extern should be defined in header files

This comment has been minimized.

@joelucid

joelucid Sep 12, 2018

Author Contributor

Sure. And it will be. Please consider my previous comment:
"No need to request style changes yet since this is only an experiment for discussion."

This comment has been minimized.

@DieHertz

DieHertz Sep 12, 2018

Member

Not necessarily, as it allows to minimize scope

float motorMixMax = 0, motorMixMin = 0;
float motorMixMax = 0, motorMixMin = 0;
extern float dT;
integratedYaw =

This comment has been minimized.

@borisbstyle

borisbstyle Sep 12, 2018

Contributor

Isn't this just easier to apply earlier:
integratedYaw = constrainf(integratedYaw + pidData[FD_YAW].Sum , -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw);

@borisbstyle
Copy link
Contributor

borisbstyle left a comment

Please try to integrate this within pidSum and make it configurable.
Perhaps an integral gain parameter could also be added

@@ -739,6 +739,8 @@ float applyThrottleLimit(float throttle)
return throttle;
}


float integratedYaw;

This comment has been minimized.

@borisbstyle

borisbstyle Sep 12, 2018

Contributor

Keeping it all in pid.c will also prevent globals

@borisbstyle

This comment has been minimized.

Copy link
Contributor

borisbstyle commented Sep 14, 2018

your PR actually gave me the same oscillating problem. I believe I used wrong hex before

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Sep 15, 2018

Which exact pid settings did you use? I get oscillations when I use an kI other than zero with integrated yaw.

@borisbstyle

This comment has been minimized.

Copy link
Contributor

borisbstyle commented Sep 19, 2018

@joelucid in my case I used pretty much the default pids.

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Oct 6, 2018

One thing that’s been bothering me with integrated yaw is that with fairly high P the motors didn’t sound completely clean. Been looking at that problem a bit today and found small oscillations on yaw.
Here’s the problem: if the pids target rpm’ then the motors will increase rpm to increase yaw rate and then leave the props at the higher rpm. But the faster motors wil of course also generate a drag related increase in yaw rate which causes overshoot until the motors have been slowed down again.
The right way to apply a zero pid sum to the motors is to slowly decrease rpm to offset the drag component with an inertia component until rpm is back at neutral.
Added something like this in and now the motors are clean. In fact I don’t think they have ever been this clean with these 2306 motors. And that’s without any rc smoothing, but some limiting of max rpm acceleration in the mixer.

@stale

This comment has been minimized.

Copy link

stale bot commented Nov 5, 2018

This issue / pull request has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs within a week.

@stale stale bot added the Inactive label Nov 5, 2018

@mikeller mikeller added this to the 4.0 milestone Nov 5, 2018

@stale stale bot removed the Inactive label Nov 5, 2018

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Nov 8, 2018

Still want to do this dear bot, please show mercy.

@mikeller

This comment has been minimized.

Copy link
Member

mikeller commented Nov 9, 2018

@joelucid:

Still want to do this dear bot, please show mercy.

Then get it to work and we can discuss some more...

@cudakeer808

This comment has been minimized.

Copy link

cudakeer808 commented Nov 9, 2018

Or push so the daring have something to chime in 😉

@joelucid joelucid force-pushed the joelucid:integrated_yaw branch from dfb070e to d451267 Dec 9, 2018

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Dec 9, 2018

Ok - this should work now. if you want to try set use_integrated_yaw=on and then use something like p_yaw=30, d_yaw=15, i_yaw = 0, f_yaw=60 or so to start.

} pidProfile_t;

This comment has been minimized.

@mikeller

mikeller Dec 9, 2018

Member

Extra newline.

{ "use_integrated_yaw", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, use_integrated_yaw) },
{ "integrated_yaw_relax", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) },


This comment has been minimized.

@mikeller

mikeller Dec 9, 2018

Member

Extra newline.

@wind0r

This comment has been minimized.

Copy link
Member

wind0r commented Dec 10, 2018

@joelucid is 0 a "valid" setting for integrated_yaw_relax? will it work? will anyone ever set it to that value?

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Dec 10, 2018

@wind0r yeah it's a possible value - it's what I had in there for a long time. Probably unlikely that a lot of people will want that but possible nonetheless.

@@ -1241,7 +1247,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
#endif
// calculating the PID sum
pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;

This comment has been minimized.

@mikeller

mikeller Dec 11, 2018

Member

I know this isn't much code change, but in general it can't hurt, so I think we should make this functionality conditional to USE_INTEGRATED_YAW_CONTROL or similar.

@wind0r

This comment has been minimized.

Copy link
Member

wind0r commented Dec 11, 2018

@joelucid just asked because it would be awesome to just use 0 as the disabled variable similar to how every other feature does it. so we can remove this useIntegratedYaw and replace it in the if with integratedYawRelax != 0

@joelucid

This comment has been minimized.

Copy link
Contributor Author

joelucid commented Dec 11, 2018

@wind0r yeah that's possible. I find it pretty confusing though since the relax part attenuates the integrated yaw. No attenuation should not switch the feature off. But I'm open to do it if more people would prefer you suggestion.

@mikeller mikeller merged commit 60118da into betaflight:master Dec 15, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.