Trad Heli New Rotor Speed Control Mode 3 Throttle Curve with cubic spline fit

@ChrisOlson and I have been working on an improvement to the Mode 3 V throttle curve for the Rotor Speed Control. This Mode was intended for users that wanted to have a 3D heli set up but did not have a rotor governor. The current mode 3 was a linear interpolation between the points based on collective position. This is mode was very basic and was not that ideal for even 3D set ups.

After talking with Chris, it was felt that a 5 point curve with a cubic spline interpolation, similar to nearly all RC Heli transmitters, would be sufficient to provide the necessary throttle response. This would be versatile for any heli set up requiring a throttle curve, which includes some governors as well. This would also be able to replace the Mode 2 set point set up since users can just set up a flat throttle curve at the set point value. The new 5 point throttle curve would be very intuitive for virtually any heli pilot since it mimics how they setup the throttle curve in their transmitter.
The three parameters for the existing mode 3 set up, H_RSC_POWER_LOW, H_RSC_POWER_HIGH, and H_RSC_POWER_NEGC are replaced by the 5 parameters for the 5 point curve which are: H_RSC_THRCRV_0, H_RSC_THRCRV_25, H_RSC_THRCRV_50, H_RSC_THRCRV_75, and H_RSC_THRCRV_100.

The code uses the throttle_in, which is the collective pitch setting for tradheli, sent to the motors class and sets the throttle out to the main rotor RSC servo based on the spline interpolation of the 5 point curve. Here are some examples. The joystick position would be the collective pitch settting between 0 and 100 percent and the output position would be the servo position to the motor or ESC.
Typical UAV throttle curve

Typical 3D throttle curve

Here is the link to my respository with the change implemented in 3.5.5.
https://github.com/bnsgeyer/ardupilot/tree/spline

@ChrisOlson has compiled, loaded and tested this change. I’ll let him discuss the results of his tests.

Bill, thanks for posting this!

I initially did a lot of bench testing, logging the ThO vs SERVO8 output, trying various curves, and graphing them in APM Planner2. Once I was satisfied it works on the bench I did a ground runup without blades to test it for runaway, or improper throttle response. It passed the ground runup test easily. I made a vid of that run

The weather improved in the mean time and I flew it. I first disabled the governor and flew it just on the throttle curve to fine tune the points and get smooth response with no governor. I ended up with a 25-30-35-50-100 throttle curve for the 766G. Then I enabled the governor with a 30% “stick switch”. What the “stick switch” does in a piston or turbine governor is enable the governor at a pre-set point on the throttle curve. With combustion engines this allows starting the engine and idling at ground idle-rotors not turning for warmup. Disengaging the throttle hold (enabling “motor interlock” in ArduPilot parlance) spools up to ground idle-rotors turning with the collective at bottom. As the collective is advanced, when the 30% throttle point on the curve is reached the governor takes over and takes 10 seconds to reach governed headspeed.

The points on the throttle curve with this new system do not correspond to any collective pitch angles as the old system did. It is like the typical RC radio and the five points correspond to percentage of collective range. So with a typical UAV setup running, say -3 to 11 degrees pitch, we have 14 degrees of collective range. So, as an example, the H_RSC_THRCRV_50 setting corresponds to 50% of the range, which is 7 degrees, or 4 degrees of positive pitch. This will be instantly understood by anybody that has ever set up a throttle/pitch curve for a RC heli - it is standard procedure. The old system was somewhat confusing, for all but 3D setups using a V-curve.

Hover collective pitch ranges from usually 5 degrees to 7.5 degrees of positive pitch for most helicopters. So we’re “on the curve” between the H_RSC_THRCRV_50 and 75 points in hover with the example noted above. This smoothly increases power, or throttle opening, as the main loads to takeoff pitch without the governor having to do anything if the curve is set properly.

The response of combustion engines to throttle opening is not linear. It is more the shape of the curve that Bill posted above. There is quite fast response to small throttle opening changes at <50% throttle opening. From 50% throttle opening to WOT, there is much less change in power output vs throttle.

All modern governors, as explained in the video, “read” the throttle curve for feedforward to provide faster response to load changes. That curve has to fit the torque curve of the engine or you get either over-speed or “lugging” on quick collective lead-in.

After making a few tweaks to the governor settings to fit the tuned throttle curve it was as close to scale quality throttle handling as it gets in helicopters. I was able to achieve picture perfect transition from the governor back to the throttle curve after landing, when reducing power to ground idle-rotors turning, before disengaging the clutch. Huge improvement over the old three-point linear “curve”.

The final thing this does is provide fallback to the throttle curve if the governor sensor fails in flight, which absolutely requires a properly tuned throttle curve for a piston or turbine engine.

H_RSC_MODE 2 is redudant and not needed. Simply set a flat throttle curve like is done in RC radios for electrics with governor. MODE 2 is also redudant with the current 3-point curve, so I’m not totally certain why MODE 2 even exists. If you want 75% for an electric governor, just set all the throttle curve points to 750 and it works identical to MODE 2.

This will fly an electric with no governor. It is done with RC radios all the time. It’s just a mattter of fine tuning the curve to fit the motor’s torque. Electrics are considerably different from combustion engines. Testing on an electric platform is underway. Should have feedback on that soon.

I wrote a while ago a bezier function generator as a demo for template metaprogramming. You can get any nx * ny * nz * … dimensional bezier-curve/plane/volume/…-function completely at compile time. Dunno, maybe you want to use bezier curves over splines because they are more easier to use. Downside, the the n-dimensional requires C++17.

The generator:

https://github.com/dgrat/StaticBezier/blob/master/bezier_generator.h

Daniel,
Thanks for the info and link to your Bezier function. I had to modify the spline function I used because it was using open cv2 libraries. But it is a pretty simple function and works well for our intended use.
Thanks again!
Bill

I liked the cubic spline math though - it was really easy to tune the points to get a smooth curve that fit. I was pretty close “out of the box” using the typical numbers for a zero-pitch Condition Normal throttle curve commonly used in RC radios for piston engines. That part only took me maybe 10 minutes to fine tweak the points, doing some punchouts to load the engine to see how it responded. It took longer to tune the governor to fit the characteristics of the throttle curve than it did to tune the actual curve itself. But part of that is the fact that on MavLink can make settings very fast and convenient. Setting the governor requires shutting down, hooking up the programming box to the governor to make settings.