Howto:Implementing a PID Controller in Nasal

From FlightGear wiki
Jump to navigation Jump to search
This article is a stub. You can help the wiki by expanding it.

Background

For the time being, the autopilot/property-rule subsystems are unfortunately not accessible from Nasal space, i.e. custom controllers cannot be set up dynamically (on demand), they have to be set up manually in XML space, and there's usually a hard-coded ownship assumption, or a fixed number of generic controllers must be set up/used instead.

However, PID controllers are potetnially useful in many other scenarios.

Thus, aircraft developers tend to come up with their own, non-generic, PID controllers implemented in scripting space.

Unfortunately, these are rarely, if ever, reusable, and they also don't lend themselves to being run asynchronously in a worker thread, because multi-threaded property tree is inherently problematic.

This has also implications for other use-cases in FlightGear, such as AI aircraft (Scripted AI Objects) not able to make use of PID controllers.

Objective

Demonstrate how a PID controller can be implemented in Nasal. The controller will be designed so that it can be run in a Nasal background thread, with only the synchronization part (i.e. copying properties) will be done in the main loop.

Design

There will be a low-level Controller class that will merely deal with native Nasal types, so that this can be executed in a worker thread. On top of this, there will be a property-enabled wrapper, which handles main loop synchronization, i.e. by copying/updating all relevant properties and saving them to a vector, which can then be processed further by the worker thread, whose results will then be copied to the main loop once it has finished.

The whole thing may be made available via Richard's Emesary framework.

Building blocks

Code

WIP.png Work in progress
This article or section will be worked on in the upcoming hours or days.
See history for the latest developments.
# PID Controller class:
var Controller = {

  new: func (k_p, k_i, k_d, dt) {
    var m = {parents:[Controller]};
    var i_max=0;
    if (typeof(k_p) == 'hash') {
      var options = k_p;
      k_p = options.k_p;
      k_i = options.k_i;
      k_d = options.k_d;
      dt = options.dt;
      i_max = options.i_max;
    }

    # PID constants
    m.k_p = (typeof(k_p) == 'scalar') ? k_p : 1;
    m.k_i = k_i or 0;
    m.k_d = k_d or 0;

    # Interval of time between two updates
    # If not set, it will be automatically calculated
    m.dt = dt or 0;

    # Maximum absolute value of sumError
    m.i_max = i_max or 0;

    m.sumError  = 0;
    m.lastError = 0;
    m.lastTime  = 0;

    m.target    = 0; # default value, can be modified with .setTarget
    return m;
  }, # new()

  setTarget: func (target) {
    me.target = target;
  }, # setTarget()

  update: func(currentValue) {
    me.currentValue = currentValue;

    # Calculate dt
    var dt = me.dt;
    if (!dt) {
      var currentTime = systime();
      if (me.lastTime == 0) { # First time update() is called
        dt = 0;
      } else {
        dt = (currentTime - me.lastTime) / 1000; # in seconds
      }
      me.lastTime = currentTime;
    }
    if (typeof(dt) != 'number' or dt == 0) {
      dt = 1;
    }

    var error = (me.target - me.currentValue);
    me.sumError = me.sumError + error*dt;
    if (me.i_max > 0 and Math.abs(me.sumError) > me.i_max) {
      var sumSign = (me.sumError > 0) ? 1 : -1;
      me.sumError = sumSign * me.i_max;
    }

    var dError = (error - me.lastError)/dt;
    me.lastError = error;

    return (me.k_p*error) + (me.k_i * me.sumError) + (me.k_d * dError);
  }, # update()

  reset: func() {
    me.sumError  = 0;
    me.lastError = 0;
    me.lastTime  = 0;
  } # reset()
}; # Controller


var options = {
    k_p: 0.5,
    k_i: 0.1,
    k_d: 0.2,
    dt: 1
};

# Create/test the controller
var ctr = Controller.new(options.k_p, options.k_i, options.k_d, options.dt);

ctr.setTarget(120);
ctr.reset();
ctr.dt = 2; # 2 seconds between updates
var correction = ctr.update(115);
if (correction !=4) {
	print("Correction not working:", correction);
} else
	print("Correction working properly");


thread.newthread( func() {
#  maketimer(0.00, ctr, func ctr.update(110) ).start();
});

print("PID module parsed");