406 lines
10 KiB
Text
406 lines
10 KiB
Text
# Glider Instrumentation Toolkit
|
|
#
|
|
# Copyright (C) 2013-2014 Anton Gomez Alvedro
|
|
#
|
|
# This program is free software; you can redistribute it and/or
|
|
# modify it under the terms of the GNU General Public License as
|
|
# published by the Free Software Foundation; either version 2 of the
|
|
# License, or (at your option) any later version.
|
|
#
|
|
# This program is distributed in the hope that it will be useful, but
|
|
# WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
# General Public License for more details.
|
|
#
|
|
# You should have received a copy of the GNU General Public License
|
|
# along with this program; if not, write to the Free Software
|
|
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
#
|
|
# 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
|
|
|
|
io.include("update_loop.nas");
|
|
|
|
var MPS2KPH = 3.6;
|
|
var sqr = func(x) {x * x}
|
|
|
|
|
|
var InstrumentComponent = {
|
|
parents: [Updatable],
|
|
output: 0,
|
|
reset: func { me.output = 0 },
|
|
};
|
|
|
|
# This alias is for keeping backwards compatibility.
|
|
# TODO: Refactor aircrafts that use it and remove this.
|
|
var Instrument = UpdateLoop;
|
|
|
|
# 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
|
|
};
|
|
},
|
|
|
|
reset: 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.reset();
|
|
return m;
|
|
},
|
|
|
|
reset: 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);
|
|
}
|
|
};
|