113 lines
3.2 KiB
Text
113 lines
3.2 KiB
Text
# AUTOPUSH
|
|
# Pushback driver class.
|
|
#
|
|
# Command the pushback to tow/push the aircract facing set heading.
|
|
#
|
|
# Copyright (c) 2018 Michael Danilov <mike.d.ft402 -eh- gmail.com>
|
|
# Distribute under the terms of GPLv2.
|
|
|
|
|
|
# FIXME When facing, acccount for current steering position, to work without dry steering.
|
|
|
|
|
|
var _K_V_push = nil;
|
|
var _F_V_push = nil;
|
|
var _S_min = nil;
|
|
var _K_psi_push = nil;
|
|
var _K_V_turn = nil;
|
|
var _F_V_turn = nil;
|
|
var _K_psi_turn = nil;
|
|
var _V_min = nil;
|
|
|
|
var _turning = nil;
|
|
var _sign = nil;
|
|
var _target = nil;
|
|
var _psi_parking = nil;
|
|
var _psi_turn = nil;
|
|
|
|
var loop = func() {
|
|
if(!getprop("/sim/model/pushback/connected")){
|
|
stop();
|
|
return;
|
|
}
|
|
var V = 0.0;
|
|
var steering = 0.0;
|
|
var deltapsi = 0.0;
|
|
var psi = getprop("/orientation/heading-deg");
|
|
if(!_turning){
|
|
# Initially follow a straight line parallel to our initial heading.
|
|
var (A, S) = courseAndDistance(_target);
|
|
S *= NM2M;
|
|
# Stop when the bearing flips, guarante against infinite pushing.
|
|
if(
|
|
(abs(S) < _S_min) +
|
|
(abs(saw180(_psi_parking - A - math.min(_sign, 0.0) * 180.0) > 90.0))
|
|
){
|
|
_turning = 1;
|
|
screen.log.write("(pushback): Turning to face heading " ~ int(_psi_turn) ~ ".");
|
|
}
|
|
deltapsi = saw180(_psi_parking - psi);
|
|
V = math.min(math.max(_K_V_push * _sign * (S / _S_min), -_F_V_push), _F_V_push);
|
|
steering = math.min(math.max(_K_psi_push * _sign * deltapsi, -1.0), 1.0);
|
|
}else{
|
|
# Turn (almost) in place.
|
|
deltapsi = saw180(_psi_turn - psi);
|
|
V = math.min(math.max(_K_V_turn * _sign * abs(deltapsi), -_F_V_turn), _F_V_turn);
|
|
# We are done when the heading error is small.
|
|
if(abs(V) < _V_min){
|
|
stop();
|
|
return;
|
|
}
|
|
steering = math.min(math.max(_K_psi_turn * _sign * deltapsi, -1.0), 1.0);
|
|
}
|
|
setprop("/sim/model/pushback/target-speed-km_h", V);
|
|
setprop("/sim/model/pushback/driver/steer-cmd-norm", steering);
|
|
}
|
|
|
|
var timer = maketimer(0.051, func{loop()});
|
|
|
|
var start = func() {
|
|
_K_V_push = getprop("/sim/model/pushback/driver/K_V_push");
|
|
_F_V_push = getprop("/sim/model/pushback/driver/F_V_push");
|
|
_S_min = getprop("/sim/model/pushback/driver/S_min-m");
|
|
_K_psi_push = getprop("/sim/model/pushback/driver/K_psi_push");
|
|
_K_V_turn = getprop("/sim/model/pushback/driver/K_V_turn");
|
|
_F_V_turn = getprop("/sim/model/pushback/driver/F_V_turn");
|
|
_K_psi_turn = getprop("/sim/model/pushback/driver/K_psi_turn");
|
|
_V_min = getprop("/sim/model/pushback/driver/V_min");
|
|
if(!getprop("/sim/model/pushback/connected")){
|
|
return;
|
|
}
|
|
towdist = getprop("/sim/model/pushback/driver/tow-distance-m");
|
|
_target = geo.aircraft_position();
|
|
_psi_parking = getprop("/orientation/heading-deg");
|
|
_target.apply_course_distance(_psi_parking, towdist);
|
|
_psi_turn = getprop("/sim/model/pushback/driver/face-heading-deg");
|
|
_sign = 1.0 - 2.0 * (towdist < 0.0);
|
|
_turning = 0;
|
|
timer.start();
|
|
if(_sign < 0.0){
|
|
screen.log.write("(pushback): Pushing back.");
|
|
}else{
|
|
screen.log.write("(pushback): Towing.");
|
|
}
|
|
}
|
|
|
|
var stop = func() {
|
|
if(timer.isRunning){
|
|
screen.log.write("(pushback): Done.");
|
|
}
|
|
timer.stop();
|
|
setprop("/sim/model/pushback/target-speed-km_h", 0.0);
|
|
}
|
|
|
|
# Sawtooth: -180..+180 deg.
|
|
var saw180 = func(p) {
|
|
while (p <= -180.0){
|
|
p += 360.0;
|
|
}
|
|
while (p > 180.0){
|
|
p -= 360.0;
|
|
}
|
|
return p;
|
|
}
|