Howto:Implementing a PID Controller in Nasal
This article is a stub. You can help the wiki by expanding it. |
Autoflight |
---|
Autopilot |
Route manager |
Specific autopilots |
Miscellaneous |
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
systime()
maketimer()
- thread.newthread()
Code
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");