Source Autopush driver.nas
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
# AUTOPUSH
# Pushback driver class.
#
# Command the pushback to tow/push the aircraft.
#
# Copyright (c) 2018 Autopush authors:
# Michael Danilov <mike.d.ft402 -eh- gmail.com>
# Joshua Davidson http://github.com/Octal450
# Merspieler http://gitlab.com/merspieler
# Distribute under the terms of GPLv2.
var _K_V = nil;
var _F_V = nil;
var _R_turn_min = nil;
var _D_stop = nil;
var _K_psi = nil;
var _F_psi = nil;
var _K_psidot = nil;
var _F_psidot = nil;
var _debug = nil;
var _psi = nil;
var _time = nil;
var _route = nil;
var _route_reverse = nil;
var _push = nil;
var _sign = nil;
var _to_wp = 0;
var _is_last_wp = 0;
var _is_reverse_wp = 0;
var _advance_wp = func(flip_sign = 0) {
_to_wp += 1;
_is_last_wp = (_to_wp == (size(_route) - 1));
_is_reverse_wp = (_route_reverse[_to_wp]);
if (flip_sign) {
_sign *= -1;
_push = !_push;
}
if (_debug == 1) {
print("autopush_driver to_wp " ~ _to_wp);
}
}
var _loop = func() {
if (!getprop("/sim/model/autopush/connected")) {
stop();
return;
}
var (A, D) = courseAndDistance(_route[_to_wp]);
D *= NM2M;
var (psi_leg, D_leg) = courseAndDistance(_route[_to_wp - 1], _route[_to_wp]);
var deltapsi = geo.normdeg180(A - psi_leg);
var psi = getprop("/orientation/heading-deg") + _push * 180.0;
var deltaA = math.min(math.max(_K_psi * geo.normdeg180(A - psi), -_F_psi), _F_psi);
var time = getprop("/sim/time/elapsed-sec");
var dt = time - _time;
var minus_psidot = (dt > 0.002) * math.min(math.max(_K_psidot * (_psi - psi) / dt, -_F_psidot), _F_psidot);
_psi = psi;
_time = time;
# TODO Either use _K_V and total remaining distance or turn radius to calculate speed.
# TODO Make slider input override speed.
var V = _F_V;
if (_is_reverse_wp or _is_last_wp) {
if ((D < _D_stop) or (abs(deltapsi) > 90.0)) {
if (_is_last_wp) {
_done();
return;
}
if (_is_reverse_wp) {
_advance_wp(1);
}
}
} else {
if ((D < _R_turn_min) or (abs(deltapsi) > 90.0)) {
_advance_wp();
}
}
var steering = math.min(math.max(_sign * (deltaA + minus_psidot), -1.0), 1.0);
if (_debug > 1) {
print("autopush_driver to_wp " ~ _to_wp ~ ", A " ~ geo.normdeg(A) ~ ", deltaA " ~ deltaA ~ ", minus_psidot " ~ minus_psidot);
}
setprop("/sim/model/autopush/target-speed-km_h", _sign * V);
setprop("/sim/model/autopush/steer-cmd-norm", steering);
}
var _timer = maketimer(0.051, func{_loop()});
var _done = func() {
stop();
autopush_route.clear();
screen.log.write("(pushback): Pushback complete, please set parking brake.");
}
var start = func() {
if (_timer.isRunning) {
gui.popupTip("Already moving");
return;
}
if (!getprop("/sim/model/autopush/connected")) {
gui.popupTip("Pushback not connected");
return;
}
_route = autopush_route.route();
_route_reverse = autopush_route.route_reverse();
if ((_route == nil) or size(_route) < 2) {
gui.popupTip("Pushback route empty or invalid");
return;
} else {
autopush_route.done();
}
_K_V = getprop("/sim/model/autopush/driver/K_V");
_F_V = getprop("/sim/model/autopush/driver/F_V");
_R_turn_min = getprop("/sim/model/autopush/min-turn-radius-m");
_D_stop = getprop("/sim/model/autopush/stopping-distance-m");
_K_psi = getprop("/sim/model/autopush/driver/K_psi");
_F_psi = getprop("/sim/model/autopush/driver/F_psi");
_K_psidot = getprop("/sim/model/autopush/driver/K_psidot");
_F_psidot = getprop("/sim/model/autopush/driver/F_psidot");
_debug = getprop("/sim/model/autopush/debug") or 0;
if (!_to_wp) {
var (psi_park, D_park) = courseAndDistance(_route[0], _route[1]);
_push = (abs(geo.normdeg180(getprop("/orientation/heading-deg") - psi_park)) > 90.0);
_sign = 1.0 - 2.0 * _push;
_advance_wp();
_psi = 0.0;
}
_time = getprop("/sim/time/elapsed-sec");
_timer.start();
var endsign = _sign;
for (ii = _to_wp; ii < size(_route_reverse); ii += 1) {
if (_route_reverse[ii]) {
endsign = -endsign;
}
}
var (psi_twy, D_twy) = courseAndDistance(_route[size(_route) - 2], _route[size(_route) - 1]);
if (endsign < 0.0) {
screen.log.write("(pushback): Push back facing " ~ math.round(geo.normdeg(psi_twy + 180.0 - magvar()), 1.0) ~ ".");
} else {
screen.log.write("(pushback): Tow facing " ~ math.round(geo.normdeg(psi_twy - magvar()), 1.0) ~ ".");
}
}
var pause = func() {
_timer.stop();
setprop("/sim/model/autopush/target-speed-km_h", 0.0);
}
var stop = func() {
pause();
_to_wp = 0;
}