fly/fgdata
1
0
Fork 0
Code Activity
fgdata/Nasal/fcs2.nas
Vivian Meazza 2c7075a9b0 Minor updates 
Signed-off-by: Vivian Meazza <vivian.meazza@lineone.net>
2010-06-04 11:14:33 +01:00

570 lines
20 KiB
Text
Raw Blame History

#
# Flight Control System by Tatsuhiro Nishioka
# $Id: fcs.nas,v 1.11 2008/08/28 02:41:04 tat Exp $
#
#This one simulates a P/R and Jaw SAS
var FCSFilter = {
new : func(input_path, output_path) {
var obj = { parents : [FCSFilter],
input_path : input_path,
output_path : output_path };
obj.axis_conv = {'roll' : 'aileron', 'pitch' : 'elevator', 'yaw' : 'rudder' };
obj.body_conv = {'roll' : 'v', 'pitch' : 'u' };
obj.last_body_fps = {'roll' : 0.0, 'pitch' : 0.0 };
obj.last_pos = {'roll' : 0.0, 'pitch' : 0.0, 'yaw' : 0.0};
return obj;
},
# read input command for a given axis
read : func(axis) {
if (me.input_path == nil or me.input_path == "") {
return getprop("/controls/flight/" ~ me.axis_conv[axis]);
} else {
var value = getprop(me.input_path ~ "/" ~ axis);
value = int(value * 1000) / 1000.0;
}
},
# write output command for a given axis
# this will be the output of an next command filter (like SAS)
write : func(axis, value) {
if (me.output_path == nil or me.output_path == '') {
setprop("/controls/flight/fcs/" ~ axis, me.limit(value, 1.0));
} else {
setprop(me.output_path ~ "/" ~ axis, me.limit(value, 1.0));
}
},
toggleFilterStatus : func(name) {
var messages = ["disengaged", "engaged"];
var path = "/controls/flight/fcs/" ~ name ~ "-enabled";
var status = getprop(path);
setprop(path, 1 - status);
#screen.log.write(name ~ " " ~ messages[1 - status]);
},
getStatus : func(name) {
var path = "/controls/flight/fcs/" ~ name ~ "-enabled";
return getprop(path);
},
limit : func(value, range) {
if (value > range) {
return range;
} elsif (value < -range) {
return - range;
}
return value;
},
max : func(val1, val2) {
return (val1 > val2) ? val1 : val2;
},
min : func(val1, val2) {
return (val1 > val2) ? val2 : val1;
},
calcCounterBodyFPS : func(axis, input, offset_deg) {
var position = getprop("/orientation/" ~ axis ~ "-deg");
var body_fps = 0;
var last_body_fps = me.last_body_fps[axis];
var reaction_gain = 0;
var heading = getprop("/orientation/heading-deg");
var wind_speed_fps = getprop("/environment/wind-speed-kt") * 1.6878099;
var wind_direction = getprop("/environment/wind-from-heading-deg");
var wind_direction -= heading;
var rate = getprop("/orientation/" ~ axis ~ "-rate-degps");
var gear_pos = getprop("/gear/gear[0]/compression-norm") + getprop("/gear/gear[1]/compression-norm");
var counter_fps = 0;
var fps_axis = me.body_conv[axis]; # convert from {roll, pitch} to {u, v}
var target_pos = offset_deg;
var brake_deg = 0;
body_fps = getprop("/velocities/" ~ fps_axis ~ "Body-fps");
if (axis == 'roll') {
var wind_fps = math.sin(wind_direction / 180 * math.pi) * wind_speed_fps;
} else {
var wind_fps = math.cos(wind_direction / 180 * math.pi) * wind_speed_fps;
}
var brake_freq = getprop("/controls/flight/fcs/gains/afcs/fps-" ~ axis ~ "-brake-freq");
var brake_gain = getprop("/controls/flight/fcs/gains/afcs/fps-brake-gain-" ~ axis);
body_fps -= wind_fps;
var dfps = body_fps - me.last_body_fps[axis];
var fps_coeff = getprop("/controls/flight/fcs/gains/afcs/fps-" ~ axis ~ "-coeff");
target_pos -= int(body_fps * 100) / 100 * fps_coeff;
if (axis == 'roll' and gear_pos > 0.0 and position > 0) {
target_pos -= position * gear_pos / 5;
}
reaction_gain = getprop("/controls/flight/fcs/gains/afcs/fps-reaction-gain-" ~ axis);
var brake_sensitivity = (axis == 'roll') ? 1 : 1;
if (math.abs(position + rate / brake_freq * brake_sensitivity) > math.abs(target_pos)) {
if (math.abs(dfps) > 1) {
dfps = 1;
}
var error_deg = target_pos - position;
brake_deg = (error_deg - rate / brake_freq) * math.abs(dfps * 10) * brake_gain;
if (target_pos > 0) {
brake_deg = me.min(brake_deg, 0);
} else {
brake_deg = me.max(brake_deg, 0);
}
}
counter_fps = me.limit((target_pos + brake_deg) * reaction_gain, 1.0);
setprop("/controls/flight/fcs/afcs/ah-" ~ fps_axis ~ "body-fps", body_fps);
setprop("/controls/flight/fcs/afcs/ah-" ~ fps_axis ~ "body-wind-fps", wind_fps);
setprop("/controls/flight/fcs/afcs/ah-" ~ axis ~ "-target-deg", target_pos);
setprop("/controls/flight/fcs/afcs/ah-" ~ axis ~ "-rate", rate);
setprop("/controls/flight/fcs/afcs/ah-delta-" ~ fps_axis ~ "body-fps", dfps);
setprop("/controls/flight/fcs/afcs/ah-" ~ axis ~ "-brake-deg", brake_deg);
setprop("/controls/flight/fcs/afcs/counter-fps-" ~ axis, counter_fps);
me.last_pos[axis] = position;
me.last_body_fps[axis] = body_fps;
return me.limit(counter_fps + input * 0.2, 1.0);
},
};
#
# AFCS : Automatic Flight Control System
#
var AFCS = {
new : func(input_path, output_path) {
var obj = FCSFilter.new(input_path, output_path);
obj.parents = [FCSFilter, AFCS];
setprop("/controls/flight/fcs/auto-hover-enabled", 0);
setprop("/controls/flight/fcs/gains/afcs/fps-brake-gain-pitch", 1.8);
setprop("/controls/flight/fcs/gains/afcs/fps-brake-gain-roll", 0.8);
setprop("/controls/flight/fcs/gains/afcs/fps-pitch-brake-freq", 3);
setprop("/controls/flight/fcs/gains/afcs/fps-pitch-coeff", -0.95);
setprop("/controls/flight/fcs/gains/afcs/fps-pitch-offset-deg", 0.9);
setprop("/controls/flight/fcs/gains/afcs/fps-reaction-gain-pitch", -0.8);
setprop("/controls/flight/fcs/gains/afcs/fps-reaction-gain-roll", 0.3436);
setprop("/controls/flight/fcs/gains/afcs/fps-roll-brake-freq", 8);
setprop("/controls/flight/fcs/gains/afcs/fps-roll-coeff", 0.8);
setprop("/controls/flight/fcs/gains/afcs/fps-roll-offset-deg", -0.8);
return obj;
},
toggleAutoHover : func() {
me.toggleFilterStatus("auto-hover");
},
toggleAirSpeedLock : func() {
me.toggleFilterStatus("air-speed-lock");
},
toggleHeadingLock : func() {
me.toggleFilterStatus("heading-lock");
},
toggleAltitudeLock : func() {
me.toggleFilterStatus("altitude-lock");
},
#
# auto hover : locks vBody_fps and uBody_fps regardless of wind speed/direction
#
autoHover : func(axis, input) {
if (axis == 'yaw') {
return input;
} else {
var offset_deg = getprop("/controls/flight/fcs/gains/afcs/fps-" ~ axis ~ "-offset-deg");
return me.calcCounterBodyFPS(axis, input, offset_deg);
}
},
altitudeLock : func(axis, input) {
# not implemented yet
return input;
},
headingLock : func(axis, input) {
# not implementet yet
return input;
},
apply : func(axis) {
var input = me.read(axis);
var hover_status = me.getStatus("auto-hover");
if (hover_status == 0) {
me.write(axis, input);
return;
}
me.write(axis, me.autoHover(axis, input));
}
};
#
# SAS : Stability Augmentation System - a rate damper
#
var SAS = {
#
# new
# initial_gains: hash of initial gains for rate damping
# sensitivities: hash of minimum rates (deg/sec) that enables rate damper
# authority_limit: shows how much SAS can take over control
# 0 means no stability control, 1.0 means SAS fully takes over pilot control
# input_path: is a base path to input axis; nil for using raw input from KB/JS
# output_path: is a base path to output axis; nis for using /controls/flight/fcs
#
# with input_path / output_path, you can connect SAS, CAS, and more control filters
#
new : func(initial_gains, sensitivities, authority_limit, input_path, output_path) {
var obj = FCSFilter.new(input_path, output_path);
obj.parents = [FCSFilter, SAS];
obj.authority_limit = authority_limit;
obj.sensitivities = sensitivities;
obj.initial_gains = initial_gains;
props.globals.getNode("/controls/flight/fcs/gains/sas", 1).setValues(obj.initial_gains);
setprop("/controls/flight/fcs/sas-enabled", 1);
return obj;
},
toggleEnable : func() {
me.toggleFilterStatus("sas");
},
#
# calcGain - get gain for each axis based on air speed and dynamic pressure
# axis: one of 'roll', 'pitch', or 'yaw'
#
calcGain : func(axis) {
var mach = getprop("/velocities/mach");
var initial_gain = getprop("/controls/flight/fcs/gains/sas/" ~ axis);
var gain = initial_gain - 0.1 * mach * mach;
if (math.abs(gain) < math.abs(initial_gain) * 0.01 or gain * initial_gain < 0) {
gain = initial_gain * 0.01;
}
return gain;
},
calcAuthorityLimit : func() {
var mach = getprop("/velocities/mach");
var min_mach = 0.038;
var limit = me.authority_limit;
if (math.abs(mach < min_mach)) {
limit += (min_mach - math.abs(mach)) / min_mach * (1 - me.authority_limit) * 0.95;
}
setprop("/controls/flight/fcs/gains/sas/authority-limit", limit);
return limit;
},
#
# apply - apply SAS damper to a given input axis
# axis: one of 'roll', 'pitch', or 'yaw'
#
apply : func(axis) {
var status = me.getStatus("sas");
var input = me.read(axis);
if (status == 0) {
me.write(axis, input);
return;
}
var mach = getprop("/velocities/mach");
var value = 0;
var rate = getprop("/orientation/" ~ axis ~ "-rate-degps");
var gain = me.calcGain(axis);
var limit = me.calcAuthorityLimit();
if (math.abs(rate) >= me.sensitivities[axis]) {
value = - gain * rate;
if (value > limit) {
value = limit;
} elsif (value < - limit) {
value = - limit;
}
}
me.write(axis, value + input);
}
};
#
# CAS : Control Augmentation System - makes your aircraft more meneuverable
#
var CAS = {
new : func(input_gains, output_gains, sensitivities, input_path, output_path) {
var obj = FCSFilter.new(input_path, output_path);
obj.parents = [FCSFilter, CAS];
obj.sensitivities = sensitivities;
obj.input_gains = input_gains;
obj.output_gains = output_gains;
props.globals.getNode("/controls/flight/fcs/gains/cas/input", 1).setValues(obj.input_gains);
props.globals.getNode("/controls/flight/fcs/gains/cas/output", 1).setValues(obj.output_gains);
setprop("/autopilot/locks/altitude", '');
setprop("/autopilot/locks/heading", '');
setprop("/controls/flight/fcs/cas-enabled", 1);
return obj;
},
calcRollRateAdjustment : func {
var position = getprop("/orientation/roll-deg");
return math.abs(math.sin(position / 180 * math.pi)) / 6;
},
calcSideSlipAdjustment : func {
var mach = getprop("/velocities/mach");
var slip = getprop("/orientation/side-slip-deg");
if (mach < 0.015) { # works only if air speed > 10kt
slip = 0;
}
var anti_slip_gain = getprop("/controls/flight/fcs/gains/cas/output/anti-side-slip-gain");
var roll_deg = getprop("/orientation/roll-deg");
var gain_adjuster = me.min(math.abs(mach) / 0.060, 1) * me.limit(0.2 + math.sqrt(math.abs(roll_deg)/10), 3);
anti_slip_gain *= gain_adjuster;
setprop("/controls/flight/fcs/cas/anti-side-slip", slip * anti_slip_gain);
return slip * anti_slip_gain;
},
# FIXME: command for CAS is just a temporal one
calcCommand: func (axis, input) {
var output = 0;
var mach = getprop("/velocities/mach");
var input_gain = me.calcGain(axis);
var output_gain = getprop("/controls/flight/fcs/gains/cas/output/" ~ axis);
var target_rate = input * input_gain;
var rate = getprop("/orientation/" ~ axis ~ "-rate-degps");
var drate = target_rate - rate;
var locks = {'pitch' : getprop("/autopilot/locks/altitude"),
'roll' : getprop("/autopilot/locks/heading")};
setprop("/controls/flight/fcs/cas/target_" ~ axis ~ "rate", target_rate);
setprop("/controls/flight/fcs/cas/delta_" ~ axis, drate);
if (axis == 'roll' or axis == 'pitch') {
if (math.abs(input > 0.7) or locks[axis] != '') {
output = drate * output_gain;
} else {
output = me.calcAttitudeCommand(axis);
}
if (axis == 'roll' and math.abs(mach) < 0.035) {
# FIXME: I don't know if OH-1 has this one
output += me.calcCounterBodyFPS(axis, input, -0.8);
}
} elsif (axis == 'yaw') {
output = drate * output_gain + me.calcSideSlipAdjustment();
} else {
output = drate * output_gain;
}
return output;
},
toggleEnable : func() {
me.toggleFilterStatus("cas");
},
calcAttitudeCommand : func(axis) {
var input_gain = getprop("/controls/flight/fcs/gains/cas/input/attitude-" ~ axis);
var output_gain = getprop("/controls/flight/fcs/gains/cas/output/" ~ axis);
var brake_freq = getprop("/controls/flight/fcs/gains/cas/output/" ~ axis ~ "-brake-freq");
var brake_gain = getprop("/controls/flight/fcs/gains/cas/output/" ~ axis ~ "-brake");
var trim = getprop("/controls/flight/" ~ me.axis_conv[axis] ~ "-trim");
var current_deg = getprop("/orientation/" ~ axis ~ "-deg");
var rate = getprop("/orientation/" ~ axis ~ "-rate-degps");
var target_deg = (me.read(axis) + trim) * input_gain;
var command_deg = 0;
if (target_deg != 0) {
command_deg = (0.094 * math.ln(math.abs(target_deg)) + 0.53) * target_deg;
}
var error_deg = command_deg - current_deg;
var brake_deg = (error_deg - rate / brake_freq) * math.abs(error_deg) * brake_gain;
if (command_deg > 0) {
brake_deg = me.min(brake_deg, 0);
} else {
brake_deg = me.max(brake_deg, 0);
}
var monitor_prefix = me.output_path ~ "/" ~ axis;
setprop(monitor_prefix ~ "-target_deg", target_deg);
setprop(monitor_prefix ~ "-error_deg", error_deg);
setprop(monitor_prefix ~ "-brake_deg", brake_deg);
setprop(monitor_prefix ~ "-deg", current_deg);
setprop(monitor_prefix ~ "-rate", -rate);
return (error_deg + brake_deg) * output_gain;
},
# FixMe: gain should be calculated using both speed and dynamic pressure
calcGain : func(axis) {
var mach = getprop("/velocities/mach");
var input_gain = getprop("/controls/flight/fcs/gains/cas/input/" ~ axis);
var gain = input_gain;
if (axis == 'pitch') {
gain += 0.1 * mach * mach;
} elsif (axis== 'yaw') {
gain *= ((1 - mach) * (1 - mach));
}
if (gain * input_gain < 0.0 ) {
gain = 0;
}
return gain;
},
apply : func(axis) {
var input = me.read(axis);
var status = me.getStatus("cas");
var cas_command = 0;
# FIXME : hmm, a bit nasty. CAS should be enabled even with auto-hover....
if (status == 0 or (me.getStatus("auto-hover") == 1 and axis != 'yaw')) {
me.write(axis, input);
return;
}
cas_command = me.calcCommand(axis, input);
me.write(axis, cas_command);
}
};
#
# Tail hstab, "stabilator," for stabilize the nose
#
var Stabilator = {
new : func() {
var obj = { parents : [Stabilator] };
setprop("/controls/flight/fcs/gains/stabilator", -1.8);
setprop("/controls/flight/fcs/auto-stabilator", 1);
# 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160, 170, 180, .....
me.gainTable = [-0.9, -0.8, 0.1, -0.5, 0.0, 0.7, 0.8, 0.9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.9, 0.8, 0.6, 0.4, 0.2, -1.0];
return obj;
},
toggleManual : func {
var status = getprop("/controls/flight/fcs/auto-stabilator");
getprop("/controls/flight/fcs/auto-stabilator", 1 - status);
},
apply : func(delta) {
setprop("/controls/flight/fcs/auto-stabilator", 0);
var value = getprop("/controls/flight/fcs/stabilator");
getprop("/controls/flight/fcs/stabilator", value + delta);
},
calcPosition : func() {
var speed = getprop("/velocities/mach") / 0.001497219; # in knot
var index = int(math.abs(speed) / 10);
if (index >= size(me.gainTable) - 1) {
index = size(me.gainTable) - 2;
}
var mod = math.mod(int(math.abs(speed)), 10);
var position = me.gainTable[index] * ((10 - mod) / 10) + me.gainTable[index-1] * (mod) / 10;
if (speed < -20) {
position = - position;
}
return position;
},
update : func() {
var status = getprop("/controls/flight/fcs/auto-stabilator");
if (status == 0) {
return;
}
var gain = getprop("/controls/flight/fcs/gains/stabilator");
var mach = getprop("/velocities/mach");
var throttle = getprop("/controls/flight/throttle");
var stabilator_norm = 0;
stabilator_norm = me.calcPosition();
setprop("/controls/flight/fcs/stabilator", stabilator_norm);
}
};
var TailRotorCollective = {
new : func(minimum=0.10, maximum=1.0, low_limit=0.00011, high_limit=0.0035) {
var obj = FCSFilter.new("/controls/engines/engine[1]", "/controls/flight/fcs/tail-rotor");
obj.parents = [FCSFilter, TailRotorCollective];
obj.adjuster = 0.0;
setprop("/controls/flight/fcs/tail-rotor/src-minimum", minimum);
setprop("/controls/flight/fcs/tail-rotor/src-maximum", maximum);
setprop("/controls/flight/fcs/tail-rotor/low-limit", low_limit);
setprop("/controls/flight/fcs/tail-rotor/high-limit", high_limit);
setprop("/controls/flight/fcs/gains/tail-rotor/error-adjuster-gain", -0.5);
return obj;
},
update : func() {
var throttle = me.read("throttle");
var pedal_pos_deg = getprop("/controls/flight/fcs/yaw");
var cas_input = cas.read('yaw');
var cas_input_gain = cas.calcGain('yaw');
var target_rate = cas_input * cas_input_gain;
var rate = getprop("/orientation/yaw-rate-degps");
var error_rate = getprop("/controls/flight/fcs/cas/delta_yaw");
var error_adjuster_gain = getprop("/controls/flight/fcs/gains/tail-rotor/error-adjuster-gain");
var minimum = getprop("/controls/flight/fcs/tail-rotor/src-minimum");
var maximum = getprop("/controls/flight/fcs/tail-rotor/src-maximum");
var low_limit = getprop("/controls/flight/fcs/tail-rotor/low-limit");
var high_limit = getprop("/controls/flight/fcs/tail-rotor/high-limit");
var output = 0;
var range = maximum - minimum;
if (throttle < minimum) {
output = low_limit;
} elsif (throttle > maximum) {
output = high_limit;
} else {
output = low_limit + (throttle - minimum) / range * (high_limit - low_limit);
}
# CAS driven tail rotor thrust adjuster
me.adjuster = error_rate * error_adjuster_gain;
me.adjuster = me.limit(me.adjuster, 0.3);
output += me.adjuster;
setprop("/controls/flight/fcs/tail-rotor/error-rate", error_rate);
setprop("/controls/flight/fcs/tail-rotor/adjuster", me.adjuster);
me.write("throttle", output);
}
};
var sas = nil;
var cas = nil;
var afcs = nil;
var stabilator = nil;
var tail = nil;
var count = 0;
var sensitivities = {'roll' : 0.0, 'pitch' : 0.0, 'yaw' : 1.125 };
var sas_initial_gains = {'roll' : 0.0011, 'pitch' : -0.0042, 'yaw' : 0.004 };
var cas_input_gains = {'roll' : 30, 'pitch' : -60, 'yaw' : 30,
'attitude-roll' : 80, 'attitude-pitch' : -80 };
var cas_output_gains = {'roll' : 0.06, 'pitch' : -0.1, 'yaw' : 0.5,
'roll-brake-freq' : 10, 'pitch-brake-freq' : 3,
'roll-brake' : 0.4, 'pitch-brake' : 6,
'anti-side-slip-gain' : -4.5};
var update = func {
count += 1;
# AFCS, CAS, and SAS run at 60Hz
if (math.mod(count, 2) == 0) {
return;
}
cas.apply('roll');
cas.apply('pitch');
cas.apply('yaw');
afcs.apply('roll');
afcs.apply('pitch');
afcs.apply('yaw');
sas.apply('roll');
sas.apply('pitch');
sas.apply('yaw');
stabilator.update();
tail.update();
}
var initialize = func {
cas = CAS.new(cas_input_gains, cas_output_gains, sensitivities, nil, "/controls/flight/fcs/cas");
afcs = AFCS.new("/controls/flight/fcs/cas", "/controls/flight/fcs/afcs");
sas = SAS.new(sas_initial_gains, sensitivities, 3, "/controls/flight/fcs/afcs", "/controls/flight/fcs");
stabilator = Stabilator.new();
tail = TailRotorCollective.new();
setlistener("/rotors/main/cone-deg", update);
}
_setlistener("/sim/signals/fdm-initialized", initialize);
View git blame Copy permalink