Stabilization: Add optional integral decay to rate mode.

[ekaszubski]

This PR adds a tweak to optimize rate mode for HeliCP. It allows the user to configure an IntegralDecay for Pitch and Roll that causes the iAccumulator to smoothly settle to zero over time. The decay value defines how long it will take for the accumulator to drop to 1% of its starting value (assuming no extra error is accumulated). I'm currently using a value of 4 seconds.

Some background: I've struggled for a while to get my helis flying nice and "locked in" with any open-source software despite hours of tuning, trying different flight modes, etc. Recently, I bought a mini iKon2, and couldn't believe how well it flew with minimal setup/tuning, so I decided to take a simple pass at reverse engineering its control algorithms.

My main realization is that the swashplate on a CP heli appears to function more like a steering wheel on a car than the steering flaps on an airboat or hovercraft; the swashplate is almost like a mechanical rate control which must be held at a fixed displacement to attain a fixed rate, and when centered it actually abruptly stops the motion of the axis you're controlling. So trying to apply a negative angle to slow down is like overcorrecting when steering a car.

With this realization, I looked at the open-loop response of the iKon2 for roll and pitch (I focused on the P and the I terms) and found that the P-term only contributes ~2 degrees of tilt to the main rotor blades (whether from user input or vehicle motion), while the I-term contributes ~13 degrees. It also clearly has some kind of settling behavior that causes the swashplate to slowly re-center over about 4 seconds (regardless of initial displacement), as well as some small feed-forward component which I chose to ignore.

Starting with just the P-term, and then just I-term (for roll and pitch), I found that in dRonin-scaled values, for pitch:

• P-term is about 0.001 (10)
• I-term is about 0.03 (300!)
• I-limit is about 0.9 (I think default is 0.3)
• Somewhat arbitrarily chose 5e-6 for the D-term

And for roll:

• P-term is about 0.0005 (5)
• I-term is about 0.03 (300)
• I-limit is about 0.9 (I think default is 0.3)
• Somewhat arbitrarily chose 1e-6 for the D-term

Combined with the integral decay from this PR, the open-loop behavior for both FCs was very similar (both via transmitter control and from physically rotating the aircraft with the control sticks centered). I went out and flew and the heli was very nimble and very stable, almost on par with the iKon2. I also tried attitude mode (with this modified rate mode as the inner loop, and with very low attitude gains) and althold mode, and I've never seen it fly so well with any open-source hardware/software.

The iKon also has an attitude mode, and it's definitely doing something different than just passing the attitude error to the rate loop; I think it's just passing the attitude error straight to the actuators, since the swash is effectively already a rate control. But this mode also has a similar swashplate displacement decay, so maybe it's running and I-controller. I need to look into it a bit more. Regardless, our attitude mode worked great with this modified rate mode, but I used very low attitude p-gains since I wasn't sure how everything would behave.

[ekaszubski]

Oh @mlyle I also used the new nth-order filter; I used 3rd order with 40 Hz cutoff. From what I could tell, the heli never came close to any wobbling, but it's hard to tell how much of an impact the filter has vs the huge I-gain.

[mlyle]

This doesn't make much sense to me / doesn't really jibe with the concept of PID control.

An integral term that decays towards 0 is identical to a P term with a very low cutoff-frequency filter. So really we're talking about filter shaping.

Unlike a real I term, it can't stop steady-state error-- just like a P term. Once it takes the process to a low error, the "decay" and the amount integrated are equal. basically, your construction seems to be state_i+1 = state_i * decay_constant + (gain * error), which seems to be the same thing as taking the IIR lowpass filter recurrence relation filt_i+1 = filt_i * (1-alpha) + error * alpha and multiplying it by a gain.

[ekaszubski]

Thanks for the feedback. My understanding of the details of control systems isn't really that great, I'm just reporting what worked.

The performance of everything else I've tried and/or has been recommended has been mediocre compared to this strategy. As mentioned, I think it has to do with the swashplate already being a rate control, so to stop rotating you just want to center it instead of moving it in the opposite direction of the aircraft's movement. Would you use a PID controller to steer a car (via the steering wheel) based on yaw rate? What would the gains look like?

I took a few hours to try to match match the behavior of this iKon flight controller, and all of a sudden the heli flies almost perfectly. So there's definitely something about this strategy that is making the heli fly much better than before; I don't fully understand it, I'm just trying to contribute that info back to the project. If there's a better way to implement the controller based on what I've described and what the logs eventually show, I'd be happy to implement/test it.

[mlyle]

The math makes it look like to me that this is equivalent to A) only running a P term, with B) a different gyro filter shape.

There's two differences I see from just picking a lower gyro filter frequency: 1) filter you're constructing with the combination of the I term plus the decay applies to both stick input and gyros equally, while the gyro filter only applies to the gyros... 2) depending on the P/I gains, the filter may have two flattish regions-- there's the response from P gain with the 35Hz steep cutoff, and there's the response from this ~1Hz cutoff shallow slope filter added to it.

As mentioned, I think it has to do with the swashplate already being a rate control, so to stop rotating you just want to center it instead of moving it in the opposite direction of the aircraft's movement.

This is how most things controlled with PID are; the classic PID example of a burner heating a bunch of liquid is really just a rate control--- temperature should stop changing once you turn the burner off (after system delays).

I don't think the A effect above is helpful, for the most part. (That is-- exciting a control system less makes it more predictable, and setpoint change/stick movement is one kind of high frequency excitation... but you also want to be able to react to that quickly). The B part is interesting-- I wonder if there's effectively something "notched". That is, I think what you've created is equivalent to just P control, with an input filter looking like this in the frequency domain:

One thing that could be tried is adjusting the sensor filter to let you pick your own coefficients as an experiment, and choosing some coeffs that give you that kind of shaping.

[mlyle]

One "dumb" way to do this for prototyping/understanding would be just to hack in a 1Hz cutoff filter 1st order IIR (like the math you currently do) with the other sensor filter in the sensors module.

That is, output = 0.8 * 1Hz1storderfilter(input) + 0.2 * normalgyrofilter(input) or whatever.

Then set I=0, find P to get appropriate magnitude of response to stick movement, and then set I= 0.75 * P.

This still isn't "right" from a controls standpoint-- in that we don't really know why we're choosing this filter shape-- but if this happens to fly well it's at least conceptually simple-- we have "normal" PID control with some odd input shaping and that's ripe for further investigation.

Also, we don't lose the auto-trimming aspect of the I term (it'll be able to eliminate steady state error).

ALTERNATIVELY--- you could fly the base/normal control system with undertuned PI control and cutoff=35/3rd order--- and fly an autotune with current code and provide an autotune partition. We'd effectively get the step impulse response from that (I'm not sure you've seen the nextgen autotune stuff or that jupyter notebook, but it's a pretty useful snapshot of a log). And from there we could analytically decide what's best with filters etc (maybe first just trying the coeffs that autotune-ng gives us ;)

[ekaszubski]

Cool, thanks for the explanations, I'll try the 1Hz filter setup you described and possibly an autotune next weekend.

FYI I've tried three different "professional" heli FCs at this point, and they all have similar behavior to this (i.e. what appears to be a large I-gain, small P-gain, and some kind of swash settling behavior over time, but I could easily be wrong in my interpretation of what's happening behind the scenes). That is, when the heli is on the ground and I move the sticks, the swash moves as if the ail/ele stick is setting the swash's rate of rotation, but when I let off the stick, the swash slowly re-centers instead of staying where I left it. Similarly, if I pick up the heli and tilt it around, the swash stays fairly level relative to the ground, but as soon as I stop moving the heli, the swash begins to re-center relative to the heli.

[ekaszubski]

BTW the iKon2 has some pretty decent logging. I think I can log the desired rate, actual rate, and actuator output for ail/ele on it. Would that be useful?

[mlyle]

Cool, thanks for the explanations, I'll try the 1Hz filter setup you described and possibly an autotune next weekend.

OK. I think we should do one path or another-- "play" with things or see what the autotune says (or more specifically, what the graphs look like) for the current system. I am not sure how to interpret autotune output once there's a "weird" filter in place.

That is, when the heli is on the ground and I move the sticks, the swash moves as if the ail/ele stick is setting the swash's rate of rotation, but when I let off the stick, the swash slowly re-centers instead of staying where I left it.

What happens when the stick is not quite neutral-- that is, if you move the stick full left, then move it to deflected half left, does it settle to the "half left" position?

It may be a bit of strategy to fight integrator windup, where if the helicopter can't maneuver (e.g. on the ground) you don't get a nasty surprise. There may be an argument for pulling I towards 0 if both the rate is 0 and the stick is undeflected, though this has costs. (If the user has the stick neutral and is trying to just hover, and trims aren't perfect, any movements towards 0 will cause drift).

[ekaszubski]

What happens when the stick is not quite neutral-- that is, if you move the stick full left, then move it to deflected half left, does it settle to the "half left" position?

Yep, it settles to a "half left" position.

It may be a bit of strategy to fight integrator windup, where if the helicopter can't maneuver (e.g. on the ground) you don't get a nasty surprise.

I think this is a part of it, but then why wouldn't they just use a higher P-term and a lower I-term?

Graphs are below; blue is stick displacement, red is swash output; both values are on +/- 120 but my stick input only seems to go to +/- 90. These all include P/I/D terms but I can zero out any of the terms and log them again if you want to focus on just one term. This is with the heli sitting slightly nose-down but otherwise level. They also seem to only be calculating derivative from the gyros and not from stick input.

[ekaszubski]

More graphs (these are all of the Ele axis):

P-term only, stick input

I-term only, stick input

D-term only, stick input

Feedforward only, stick input

I-term only, set at 30 (iKon2 setup wizard sets it to 60)

I-term only, set at 30, with time on the same range as first I-term graph

P-term, gyro input

I-term, gyro input

D-term, gyro input

P-term, with swash output scaled by 10

D-term, with swash output scaled by 10

[ekaszubski]

On the iKon2, changing the I-gain +simultaneously changes the max swash deflection and the movement speed of the swash, which suggests that they're doing something like:

i_accumulator = bound_sym(i_accumulator + loop_error * dt, i_max);
output = ... + i_gain * i_accumulator + ...;

So they accumulate up to i_max degree-seconds, and the I-gain just scales that accumulated value.

The decay behavior appears unchanged when changing the I-gain. I did a quick fit in Desmos using the form I proposed in this PR (i_accumulator *= 0.01^(dt/decay_time)) and the corresponding decay_time that fits the iKon2 data is 6.7 seconds. Alternatively, the form i_acumulator *= 0.5^dt also fits the data fairly closely.

[ekaszubski]

The iKon2 also has an "agility" slider with the description:

"This parameter sets the overall helicopter flying characteristics. Higher agility values make the helicopter feel more natural and more flybar like. Lower agility values make the helicopter more locked in during flight."

If I set a slight positive elevator trim, with only the I-term enabled, then move the agility slider around, higher agility pulls the swashplate closer to center, and lower agility makes it deflect more. So agility appears to affect the swashplate settling behavior, either in how fast it settles, or how hard it pulls at a given stick deflection. This matches the parameter's description; lower values make the swash centering happen slower/less, i.e. the integral accumulator decays less/more slowly, which should make the heli feel more "locked in".

[ekaszubski]

@mlyle Just grabbed a log with OpenLager with this PR, requested vs actual rate look much better than in the past (in that the actual rate mostly resembles the requested rate in shape and amplitude), delay from manual command to gyro result appears to be about 100 ms. Note: this is with the decay time set to 6.7 seconds, and it was pretty windy outside.

Graphs; these all have manual and actuator scaled by 270 (which is what manual rate was set to)

[ekaszubski]

Will try AT with undertuned gains, 35 Hz/3rd order filter tomorrow. Is autotune-ng in next as of 8a8184b?

[ekaszubski]

Just flew with 40hz 1st order lowpass, 4 sec integral decay. Delay between commanded and actual looks like ~90 ms.

Graphs have manual roll/pitch and actuator roll/pitch scaled by 270, actuator thrust scaled by 200:

Flip to inverted, flip back upright

Flip to inverted, flip back upright

[ekaszubski]

Also flew this PR with nearly identical settings (used slightly higher P-gains) on my 500-size heli (other flights were on my 380-size) and it had very similar performance. I have not tried with no integral decay; will try before running AT.

I also bumped up the gain for attitude control from 10 to 30 and it felt really nice. I ran althold as well and everything looked good. Attitude mode in the air is great, but super sketchy during spinup and spindown due to the integral windup; like I mentioned before, I don't think the iKon2 uses the same inner rate loop when in attitude mode, because it doesn't have this windup problem.

[ekaszubski]

I studied the iKon2 attitude behavior some more and I think I have it mostly figured out:

• I can zero out all the rate gains and attitude mode performs exactly the same, so they're either using different inner-loop gains for attitude mode or they're not using an inner loop at all; I think it's the latter
• In attitude mode, when I move the sticks to an arbitrary deflection, the swash approaches a deflection that is proportional to the stick deflection over a few seconds. So I can't make it instantly deflect to its max angle, but it's definitely not just being integrated (it looks like the deflection exponentially decays toward the final deflection)
• If I move the heli itself, I can make the swash move extremely quickly. The swash also has the same settling behavior as in rate mode, but it only seems to apply to user input and not to attitude error
• I think this whole behavior exists to prevent the pilot from moving the swashplate too quickly; I tried modifying our attitude mode to pass the attitude PI output directly to the swash (instead of running it through the inner loop first) and the heli flew great and leveled really fast, but if I flicked the sticks, it moved almost too fast

TL;DR I think they're just filtering the stick inputs to prevent the swash (and thus the heli) from jerking around too much from user input, and then passing the attitude PI output straight to the swash, with no inner rate loop. I tweaked our attitude mode to do this (without the input filter) and it flew really well (and no integrator windup on the ground) but was almost too sensitive to fast stick inputs; will investigate the input filtering to see if it helps.

[ekaszubski]

I studied the iKon2's rate mode some more and I think I have a better idea of what the "agility" slider does; low agility values make the swash response more linear (more like a standard integral, or more "locked in" as described by the iKon documentation) with respect to stick inputs, and high agility values make the swash response look more like an exponential decay toward the final deflection. I'm not really sure how they accomplish the high-agility behavior, nor what its purpose is outside of a "control feel" tweak, but I don't think it benefits us much so I'm not going to investigate it further.

[ekaszubski]

@mlyle I thought some more about your previous comments and I don't think this integral decay stuff belongs in rate mode. If anything, I think it should replace virtualbar since virtualbar has a similar setup and parameters.

How do you feel about:

• Replacing/simplifying virtualbar and changing the virtualbar default settings to be better for helis (based on what I've learned here)
• Adding a mechanism to bypass the inner loop for attitude mode for helis; for example, if in attitude mode and all the rate PID gains are zero, bypass the rate PID and output directly to actuatordesired

[IMbackK]

I can confirm this PR flys great (merged with latest next branch). Im on a CX250 with P,I,D: 0.0004, 0.02, 15e-6 and a decay time of 5 sec.
Flys at least as good as a VStabi vbar.