Automatic servo trim

[avsaase]

Closes #5750 and #5719.

When the total rotation of the airplane is near zero and the sticks are centered, the servo midpoint is adjusted in the direction of the current servo position. The I-term is scaled down accordingly to avoid jumps in the servo position. On the bench, it doesn't perfectly cancel out but in real-world flying conditions, this should not be noticeable. (The I-term seems to slowly shrink with the model sitting still when AUTOTRIM is not enabled. Not sure why that is.)

The servos that are trimmed are configured using an extra <trim> flag in the servo command. The complete command is now

servo <n> <min> <max> <mid> <rate> <trim>

For testing purposes, I have hijacked the normal AUTOTRIM mode so that the original midpoints are restored when you disable the mode. If this works correctly I want to make it into a feature that can be permanently enabled

Ignore all this. Start reading at #6708 (comment).

[avsaase]

I had a chance to test this today. Here is the dvr with the AUTOTRIM debug mode enabled: https://www.youtube.com/watch?v=0to2egfySns. Track [1] is the number of midpoint updates applied, [2] and [4] are the servo midpoints, [3] is the total rotation of the airplane, [5] and [7] are the roll and pitch I-terms. This plane is already trimmed mechanically so this test doesn't show what happens when the trim is way off, but at least it is clear nothing terrible happens.

[avsaase]

I'm still not completely sure about what to do with the I-term. If the I-term serves as a dynamic trim, then scaling it down at the same rate as the servo midpoints are updated makes sense IMO. But the I-term does not only trim the plane and also helps with the stabilization. During maneuvers, the I-term can grow quite a bit, and scaling it down by 5-10% once the maneuvers are over may be too much. An alternative would be to just leave the I-term process alone and limit the size of the midpoint updates to 10us or so to avoid noticeable jumps in the servo position. Feedback is more than welcome.

[Jetrell]

An alternative would be to just leave the I-term process alone and limit the size of the midpoint updates to 10us or so to avoid noticeable jumps in the servo position.

That sounds like the safer way to approach it.

It may take a little longer to trim the surfaces if they are considerably out at launch. But I don't see that as a big deal. Unless you are aiming for this feature to be able to bring a wildly out of trim plane, back into trim very quickly for a beginner?

[avsaase]

I think I found a better way to do it. Instead of reading the current servo positions I take the I-term from the rate controller, convert it to μs PWM using the mixer and servo rates, and add part of if to the servo midpoints. I then subtract the same amount from the I-term so that it cancels out exactly.

This also avoids changing the servo command because it just trims the servos that are driven by the stabilzied_roll/pitch/yaw mixer rules.

[avsaase]

Okay, this works quite well now. When the plane is flying straight and the I-term is more than servo_autotrim_iterm (default 10 5), then servo_autotrim_iterm is converted to μs servo output and added to the midpoints of all servos drive that axis' PIDs. The I-term is decreased by the same amount so the servos stay in the exact same position. The result is that the I-term is slowly carried over to the servo midpoints.

I did not apply any limiting to the servo midpoints, so if the plane is mechanically trimmed very badly the servo midpoints could in theory go all the way to 1000 or 2000. One reason is that I didn't immediately see a clean way to stop the updates for all servos tied to a specific axis and stop the I-term adjustment when only one servo on that axis goes outside 1300-1700 or something like that. Servos can also be driven by multiple axes which also complicates things. I'd say the pilot is responsible for making sure the trim is not too far off when the servos are at 1500, and after the maiden flight he should check the midpoints to see if any further mechanical adjustments are necessary.

WDYT?

[Jetrell]

@avsaase The idea seems good.

How do you think it will work under conditions like these...
Many air frame's require control surface offset. Or pilots even apply dynamic offsets. Like throttle to elevator mixing or rudder to aileron mixing.

e.g. If a plane is flying at 50% throttle and it has a tendency to climb a few degrees, at that throttle/speed setting.
With 'hands off', the autotrim will trim the servo's for level flight, by applying some down elevator.
But once the pilot moves the throttle; autotrim will attempt to re-trim the servo's again, to suit the amount of elevator needed to fly level, at that applied thrust.
It most certainly will be very active in some air frame's if I-term is set low in Air mode. But this is what its designed to do.

I only mention it, to help you think of any edge cases that autotrim may encounter when being continuously active?

Will this setting fw_iterm_throw_limit have any effect on preventing servo mid points being driven fully to either end?

[avsaase]

Many air frame's require control surface offset. Or pilots even apply dynamic offsets. Like throttle to elevator mixing or rudder to aileron mixing.

e.g. If a plane is flying at 50% throttle and it has a tendency to climb a few degrees, at that throttle/speed setting.
With 'hands off', the autotrim will trim the servo's for level flight, by applying some down elevator.
But once the pilot moves the throttle; autotrim will attempt to re-trim the servo's again, to suit the amount of elevator needed to fly level, at that applied thrust.
It most certainly will be very active in some air frame's if I-term is set low in Air mode. But this is what its designed to do.

It's important to distinguish between level flight (the wings are parallel to the ground and the plane is not climbing or descending) and flying straight ahead (no rotation around each of the three axes). The I-term works to keep the rotation at zero but doesn't care about altitude or which attitude the plane is at. With this PR the servo midpoints serve as a slow integrator of the I-term so it behaves in the exact same way. If throttle changes affect the rotation of the airplane (pitch up on full throttle is not uncommon) then the I-term will fight this change and this PR will slowly adjust the midpoints to maintain straight flight when switching to manual mode. As a result, the trim will always only be good for the throttle level you last flew at. This is no different from the current autotune mode or a manually trimmed plane. I could add a check that the trims are only adjusted when the throttle is not zero, but there may be glider pilots that also want to use this feature at zero throttle.

Will this setting fw_iterm_throw_limit have any effect on preventing servo mid points being driven fully to either end?

No, it doesn't make it completely impossible, but less likely. The I-term by default is limited to 165, but every time part of the I-term is transferred to the servo midpoints there is new room for the I-term to grow to 165. So over time, this could drive the servos to their endpoints. However, if 165 is not enough to maintain total rotation less than 20 deg/s, then the midpoints will not be updated. If stick input is needed to fly straight, then the midpoints will also not be updated. So if the trims are way off they won't be changed, unless turbulence gets the rotation within the threshold 20 deg/s threshold.

I only mention it, to help you think of any edge cases that autotrim may encounter when being continuously active?

This is the main challenge with this PR. When autotrim is permanently enabled we have to be sure it doesn't cause problems. Now that is it linked directly to the I-term it should be quite safe but it certainly needs more testing. Ideally, it would be tied to a proper flying/not flying detection to avoid changing the midpoints while the plane is on the ground. For the start of the flight I got around this by not changing the trims in the first 30 seconds after arming. At the end of the flight the shock of the landing could cause some I-term windup which will cause problems if you don't disarm immediately and the I-term is allowed to creep into the trims. Maybe on disarm the trims from 10 seconds ago should be save instead of the current trims?

If you can think of any other edge cases please let me know.

[avsaase]

I added a check for a valid GPS heading to avoid updating the trims when the plane is not flying. This is the easiest flyong/not-flying detection I could think of, but it's unfortunate to require GPS for a feature that doesn't really need it. I also limited the servo midpoints to the 1300-1700 range to always leave some room for maneuvering. With or without this feature, the advice will still be to adjust mechanically if the midpoints are more than 50-100us from center.

For now, you can select continuous autotrim as a mode when feature FW_AUTOTRIM is not enabled. I don't intend to keep it like this for the final 3.0 release because one selectable autotrim mode is enough. But for testing I'd advise using the mode rather than the feature so you can restore the old trims when anything goes wrong. Once we know for sure that this is safe and the midpoint backup is no longer needed I can clean it up a bit.

[avsaase]

I have now flown with this several times with feature FW_AUTOTRIM, including some longer range flights, and I have not noticed any problems. Randomly switching to manual throughout the flight shows that the trim is quite good, although maybe not always perfect. I consider this safe to fly with so I think we can merge it.