From db995cc8eed437aad737f1401c7e34a182a7f2fd Mon Sep 17 00:00:00 2001
From: Anton Gomez Alvedro <galvedro@gmail.com>
Date: Mon, 4 Nov 2013 14:09:03 +0100
Subject: [PATCH] Soaring instrumentation library for Nasal

A Nasal library for implementing instruments that are specific for soaring.
This version supports:

 + Total Energy compensated variometers
 + Netto variometers
 + Relative (aka Super-Netto) variometers
 + Configurable dampener for simulating mechanical needles
 + Averager
 + Speed to fly computer
 + Speed Command variometer
 + Yaw string (it's an instrument, isn't it?)
---
 .../Generic/soaring-instrumentation-sdk.nas   | 459 ++++++++++++++++++
 1 file changed, 459 insertions(+)
 create mode 100644 Aircraft/Generic/soaring-instrumentation-sdk.nas

diff --git a/Aircraft/Generic/soaring-instrumentation-sdk.nas b/Aircraft/Generic/soaring-instrumentation-sdk.nas
new file mode 100644
index 000000000..de085bc6a
--- /dev/null
+++ b/Aircraft/Generic/soaring-instrumentation-sdk.nas
@@ -0,0 +1,459 @@
+# Glider Instrumentation Toolkit
+# Author: Anton Gomez Alvedro (galvedro)
+# Licensed under GNU GPL
+#
+# Features:
+#   + Total energy compensated variometer
+#   + Netto variometer
+#   + Relative (Super Netto) variometer
+#   + Configurable dampener for simulating needle response time
+#   + Configurable averager
+#   + Speed to fly computer
+#
+# TODO:
+# - add wind correction to speed-to-fly
+# - final glide computer
+
+var MPS2KPH = 3.6;
+var sqr = func(x) {x * x}
+
+
+var InstrumentComponent = {
+	output: 0,
+	init: func { me.output = 0 },
+	update: func(dt) { },
+};
+
+# update_prop(property)
+# Helper generator for updating the given property on every element update
+#
+# Example:
+# var needle = Dampener.new(
+#	input: probe,
+#	dampening: 2.8,
+#	on_update: update_prop("/instrumentation/variometer/te-reading-mps"));
+
+var update_prop = func(property) {
+	func(value) { setprop(property, value) }
+};
+
+# InputSwitcher
+# Selects output from one of multiple components given as inputs
+#
+# var lcd_controller = InputSwitcher.new(
+#	inputs: Vector of objects connected to the input
+#	active_input: (optional) Input number that is active at start
+#	on_update: (optional) function to call whenever a new output is available
+
+var InputSwitcher = {
+
+	parents: [InstrumentComponent],
+
+	new: func(inputs, active_input = 0, on_update = nil) {
+		return {
+			parents: [me],
+			inputs: inputs,
+			active_input: active_input,
+			on_update: on_update
+		};
+	},
+
+	select_input: func(input_number) {
+		me.active_input = input_number;
+		me.update();
+	},
+
+	update: func {
+		me.output = me.inputs[me.active_input].output;
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# PropertyReader
+# Makes a property available at its output. Its purpose is to adapt properties
+# to the component model used by the library.
+#
+# var temperature = PropertyReader.new(
+#	property: Property to read from
+#	scale: Scale factor applied to the property value (output = scale * prop)
+
+var PropertyReader = {
+
+	parents: [InstrumentComponent],
+
+	new: func(property, scale = 1) {
+		return {
+			parents: [me],
+			property: property,
+			scale: scale
+		};
+	},
+
+	update: func {
+		me.output = me.scale * getprop(me.property);
+	}
+};
+
+# YawString
+# The most important instrument in a glider. Simple, cheap and effective!
+#
+# var string = YawString.new(
+#	on_update: update_prop("/instrumentation/yaw-string/deflection-deg");
+
+var YawString = {
+
+	parents: [InstrumentComponent],
+
+	new: func (on_update = nil) {
+		return {
+			parents: [me],
+			on_update: on_update
+		};
+	},
+
+	update: func {
+		var airspeed = getprop("velocities/airspeed-kt");
+		var noise = (airspeed < 54) ?
+			math.sin(math.pi * airspeed / 54) * rand() : 0;
+
+		me.output = noise + getprop("orientation/side-slip-deg");
+
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# TotalEnergyProbe
+# Computes total energy variation by reading current airspeed and altitude
+#
+# var probe = TotalEnergyProbe.new(
+#	on_update: (optional) function to call whenever a new output is available
+
+var TotalEnergyProbe = {
+
+	parents: [InstrumentComponent],
+	altitude: 0, # meters
+	airspeed: 0, # m/s
+
+	new: func(on_update = nil) {
+		return {
+			parents: [me],
+			on_update: on_update
+		};
+	},
+
+	init: func {
+		me.airspeed = getprop("/velocities/airspeed-kt") * KT2MPS;
+		me.altitude = getprop("/position/altitude-ft") * FT2M;
+		me.output = 0;
+	},
+
+	update: func(dt) {
+		var altitude_now = getprop("/position/altitude-ft") * FT2M;
+		var airspeed_now = getprop("/velocities/airspeed-kt") * KT2MPS;
+
+		me.output = (altitude_now - me.altitude) / dt;
+		me.output += (sqr(airspeed_now) - sqr(me.airspeed)) / (19.62 * dt);
+
+		me.altitude = altitude_now;
+		me.airspeed = airspeed_now;
+
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# Dampener
+# Simple IIR exponential filter. Appropriate and efficient for simulating
+# mechanical needle dampening.
+#
+# var needle = Dampener.new(
+#	input: Object connected to the dampeners input.
+#	dampening: (optional) Time constant for the filter in seconds
+#	scale: (optional) Scale factor applied to the input signal before filtering
+#	on_update: (optional) function to call whenever a new output is available
+
+var Dampener = {
+
+	parents: [InstrumentComponent],
+	dampening: 0, # time constant of the exponential filter (sec)
+	scale: 1,
+
+	new: func(input, dampening = 3, scale = 1, on_update = nil) {
+		return {
+			parents: [me],
+			input: input,
+			dampening: dampening,
+			scale: scale,
+			on_update: on_update,
+		};
+	},
+
+	update: func(dt) {
+		var alfa = math.exp(-dt / me.dampening);
+		me.output = me.output * alfa + me.input.output * me.scale * (1 - alfa);
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# Averager
+# Provides a windowed moving average of its input signal. Window size is
+# set on construction, and is given in samples (i.e. not seconds).
+#
+# var averager = Averager.new(
+#	input: Object connected to the averagers input.
+#	size: (optional) window size in samples
+#	on_update: (optional) function to call whenever a new output is available
+
+var Averager = {
+
+	parents: [InstrumentComponent],
+
+	new: func(input, buffer_size = 25, on_update = nil) {
+		var m = { parents: [me] };
+		m.input = input;
+		m.on_update = on_update;
+		m.size = buffer_size;
+		m.sum = m.wp = 0;
+
+		m.buffer = setsize([], buffer_size);
+		m.init();
+		return m;
+	},
+
+	init: func {
+		me.sum = me.wp = me.output = 0;
+		forindex (var i; me.buffer)
+			me.buffer[i] = 0;
+	},
+
+	update: func {
+		var new_value = me.input.output;
+
+		me.sum = me.sum + new_value - me.buffer[me.wp];
+		me.output = me.sum / me.size;
+
+		me.buffer[me.wp] = new_value;
+		if ((me.wp += 1) == me.size)
+			me.wp = 0;
+
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# PolarSolver
+# Helper object required for advanced soaring instrumentation.
+# Provides McCready speed-to-fly computations assuming a parabolic glider polar
+# (this approximation is frequently used in real instruments as well).
+#
+# Polar coeficients provided on construction correspond to the equation:
+# sink = coefs[0] * airspeed^2 + coefs[1] * airspeed + coefs[2]
+#
+# Note that sink is considered positive. Negative sink means.. lift!
+#
+# var solver = PolarSolver.new(
+#	polar_coefs: [0.000364277, -0.0479199, 2.31644]
+#	mass: Reference mass in Kg used while obtaining the polar above
+
+var PolarSolver = {
+
+	min_sink: 0, # minimum sink m/s, according to glider polar
+
+	new: func(polar_coefs, mass) {
+		var m = { parents: [me] };
+		m.reference_coefs = polar_coefs;
+		m.coefs = polar_coefs;
+		m.reference_mass = mass;
+		m.total_mass = mass;
+		m.min_sink = m.coefs[2] - (sqr(m.coefs[1]) / (4 * m.coefs[0]));
+		return m;
+	},
+
+	set_total_mass: func(mass) {
+		me.total_mass = mass;
+		var load_factor = math.sqrt(mass / me.reference_mass);
+
+		# Update active polar
+		me.coefs[0] = me.reference_coefs[0] / load_factor;
+		me.coefs[2] = me.reference_coefs[2] * load_factor;
+
+		me.min_sink = me.coefs[2] - (sqr(me.coefs[1]) / (4 * me.coefs[0]));
+	},
+
+	speed_to_fly: func(mc, airmass_sink) {
+		var speed = (mc + me.coefs[2] + airmass_sink) / me.coefs[0];
+		return (speed > 0) ? math.sqrt(speed) : 0;
+	},
+
+	ld: func(airspeed) {
+		return aispeed / me.sink(airspeed);
+	},
+
+	sink: func(airspeed) {
+		return me.coefs[0] * sqr(airspeed)
+		       + me.coefs[1] * airspeed + me.coefs[2];
+	}
+};
+
+# NettoVario
+# The Netto variometer substract glider's sink rate for current airpseed from a
+# total energy reading. The resulting value is airmass' lift/sink in m/s.
+#
+# var netto = NettoVario.new(
+#	te_probe: Object providing a total energy reading
+#	polar_solver: Object providing a McCready implementation
+#	on_update: (optional) function to call whenever a new output is available
+
+var NettoVario = {
+
+	parents: [InstrumentComponent],
+
+	new: func(te_probe, polar_solver, on_update=nil) {
+		return {
+			parents: [me],
+			probe: te_probe,
+			polar: polar_solver,
+			on_update: on_update
+		};
+	},
+
+	update: func {
+		me.output = probe.output
+		            + me.polar.sink(probe.airspeed);
+
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# RelativeVario
+# The Relative (aka Super Netto) variometer tell you what climb rate would you
+# get if you slowed down to optimal thermaling speed.
+#
+# var snetto = RelativeVario.new(
+#	te_probe: Object providing a total energy reading
+#	polar_solver: Object providing a McCready implementation
+#	on_update: (optional) function to call whenever a new output is available
+
+var RelativeVario = {
+
+	new: func(te_probe, polar_solver, on_update=nil) {
+		return {
+			parents: [me, NettoVario.new(te_probe, polar_solver, on_update)]
+		};
+	},
+
+	update: func {
+		me.output = probe.output
+		            + me.polar.sink(probe.airspeed)
+		            - me.polar.min_sink;
+
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# SpeedCmdVario
+# The speed command variometer tells you how fast or slow your airspeed is with
+# respect to the optimal speed-to-fly (computed according to McCready theory).
+#
+# var speedcmd = SpeedCmdVario.new(
+#	te_probe: Object providing a total energy reading
+#	polar_solver: Object providing a McCready implementation
+#	netto: (optional) Object providing a Netto reading
+#	on_update: (optional) function to call whenever a new output is available
+
+var SpeedCmdVario = {
+
+	parents: [InstrumentComponent],
+	mc: 0, # mccready setting
+
+	new: func(te_probe, polar_solver, netto = nil, on_update = nil) {
+		return {
+			parents: [me],
+			polar: polar_solver,
+			probe: te_probe,
+			netto: netto or NettoVario.new(te_probe, polar_solver),
+			update_netto: (netto == nil),
+			on_update: on_update
+		};
+	},
+
+	update: func {
+		if (me.update_netto) me.netto.update();
+
+		var target_speed = me.polar.speed_to_fly(me.mc, -me.netto.output);
+		me.output = me.probe.airspeed * MPS2KPH - target_speed;
+
+		if (me.on_update != nil) me.on_update(me.output);
+	}
+};
+
+# Instrument
+# Wraps a set of components and updates them periodically.
+# Takes care of critical sim signals (reinit, fdm-initialized, speed-up).
+#
+# var instrument = Instrument.new(
+#	components: List of components to update in the fast loop.
+#	update_period: (optional) Time in seconds between updates (fast components).
+#	enable: (optional) Enable instrument after creation.
+
+var Instrument = {
+
+	new: func(components, update_period = 0, enable = 1) {
+
+		var m = { parents: [me] };
+		m.initialized = 0;
+		m.enabled = enable;
+		m.update_period = update_period;
+		m.time_last = 0;
+		m.sim_speed = 1;
+		m.components = (components != nil)? components : [];
+
+		m.timer = maketimer(update_period,
+			func { call(me.update, [], m) });
+
+		setlistener("/sim/speed-up",
+			func(n) { m.sim_speed = n.getValue() }, 1, 0);
+
+		setlistener("sim/signals/reinit", func {
+			m.timer.stop();
+			m.initialized = 0;
+		});
+
+		setlistener("sim/signals/fdm-initialized", func {
+			if (m.timer.isRunning) m.timer.stop();
+			call(me.init, [], m);
+			if (m.enabled) m.timer.start();
+		});
+
+		return m;
+	},
+
+	init: func {
+		me.time_last = getprop("/sim/time/elapsed-sec");
+
+		foreach (var component; me.components)
+			component.init();
+
+		me.initialized = 1;
+	},
+
+	update: func {
+		var time_now = getprop("/sim/time/elapsed-sec");
+		var dt = (time_now - me.time_last) * me.sim_speed;
+		if (dt == 0) return;
+
+		me.time_last = time_now;
+
+		foreach (var component; me.components)
+			component.update(dt);
+	},
+
+	enable: func {
+		if (me.initialized) me.timer.start();
+		me.enabled = 1;
+	},
+
+	disable: func {
+		me.timer.stop();
+		me.enabled = 0;
+	}
+};
+