One of the features of extreme high altitude flight is the high true airspeeds that are needed to generate the dynamic pressure required for airborne flight.  To minimise these extreme speeds for my high altitude glider project, I wanted to increase the flyable operation envelope closer to the airframe's stall speed, helping the reign-in the stratospheric true airspeeds!

As I was playing about tuning the roll and pitch control PID loops using my airframe model in an X-plane HIL simulation, I discovered that a relatively large amount of derivative could yield some dramatic improvements to roll control when on the edge of the stall condition.  The D term could be wound up at least an order of magnitude higher than could be tolerated at higher speeds.

To make use of this feature, I forensic'd my way through the codebase and the APM libraries to add a non-linearity to the PID speed scalar as it is applied to the derivative term.  I did this by tweaking the PID.css file in the PID Arduino library folder (not the AP_PID, as it took me a while to discover..!)  The mods are:

PID::get_pid(int32_t error, uint16_t dt, float scalar)
{
 float output  = 0;
  float delta_time = (float)dt / 1000.0;
 float deriv_scalar = (scalar - 0.5);

 // Compute proportional component
 output += error * _kp;

 // Compute derivative component if time has elapsed
 if ((fabs(_kd) > 0) && (dt > 0)) {
  float derivative = (error - _last_error) / delta_time;

  // discrete low pass filter, cuts out the
  // high frequency noise that can drive the controller crazy
  float RC = 1/(2*M_PI*_fCut);
  derivative = _last_derivative +
          (delta_time / (RC + delta_time)) * (derivative - _last_derivative);

  // update state
  _last_error   = error;
  _last_derivative    = derivative;

  // add in derivative component
  deriv_scalar *= deriv_scalar;
  deriv_scalar = max(0,deriv_scalar);
  output     += _kd * derivative * deriv_scalar;
 }

 // scale the P and D components
 output *= scalar;

(Oh, and I corrected the spelling of scalar too!)

I have also expanded the clipping of the speed scalar delivered to the PID controller from the normal 0.5-2.0 in the standard APM codebase, to 0.01-5.0  I also chose to tune the controller with the reference speed to be the stall speed of the aircraft (7m/s) instead of the default cruise speed (25m/s).

The funny (scalar-0.5) term I have used to increase the decay of the term away from the stall speed without getting into complicated and CPU intensive maths.  This allows a higher derivative gain to be used without unduly influencing normal flight. 

So, having got this compiled and working in APM and X-plane, here are the results:

When I glide towards the stall speed in stabilise mode, holding as best I can, a level flight path, but with the roll control derivative term zeroed, which is more or less how I have tuned the controller till now, the airframe slows to approximately 16-17kts before a divergent roll instability develops.  This instability can develop at slightly higher speeds too, but the main characteristic is once it develops, it is properly divergent.  There is no saving it, except for a rapid pitch down and acceleration.

With the non-linear D term applied, I can now achieve relatively reliable roll control very close to the stall speed.  The divergent instability is still there, but it is at a higher frequency and a much lower speed, clearing the way for operation much closer to the stall break than was possible before.

This work was done in X-plane 9, using an airframe model of my own forward-swept flying wing concept.  The APM code was modified from Arduplane 2.28, using Michael Oborne's AP Mission Planner 1.1.42