Updated autopush
Signed-off-by: merspieler <merspieler@airmail.cc>
This commit is contained in:
parent
65d42fdf6a
commit
36190eac0b
4 changed files with 89 additions and 54 deletions
|
|
@ -37,6 +37,11 @@
|
|||
<driver>
|
||||
<F_V type="float">5.5</F_V>
|
||||
<K_psi type="float">0.03</K_psi>
|
||||
<F_V type="float">8.0</F_V>
|
||||
<K_psi type="float">0.03</K_psi>
|
||||
<F_psi type="float">1.0</F_psi>
|
||||
<K_psidot type="float">0.03</K_psidot>
|
||||
<F_psidot type="float">1.0</F_psidot>
|
||||
</driver>
|
||||
<route>
|
||||
<show type="bool"/>
|
||||
|
|
|
|||
|
|
@ -8,7 +8,6 @@
|
|||
# Distribute under the terms of GPLv2.
|
||||
|
||||
|
||||
var _enabled = 0;
|
||||
var _K_p = nil;
|
||||
var _F_p = nil;
|
||||
var _K_i = nil;
|
||||
|
|
@ -134,10 +133,9 @@ var _stop = func() {
|
|||
|
||||
setlistener("/sim/model/autopush/enabled", func(p) {
|
||||
var enabled = p.getValue();
|
||||
if ((enabled > _enabled) and getprop("/sim/model/autopush/available")) {
|
||||
if ((enabled) and getprop("/sim/model/autopush/available")) {
|
||||
_start();
|
||||
} else if (enabled < _enabled) {
|
||||
} else {
|
||||
_stop();
|
||||
}
|
||||
_enabled = enabled;
|
||||
});
|
||||
}, 1, 0);
|
||||
|
|
|
|||
|
|
@ -15,14 +15,19 @@ 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 = 1;
|
||||
var _to_wp = 0;
|
||||
var _is_last_wp = 0;
|
||||
var _is_reverse_wp = 0;
|
||||
|
||||
|
|
@ -45,12 +50,17 @@ var _loop = func() {
|
|||
stop();
|
||||
return;
|
||||
}
|
||||
var psi = getprop("/orientation/heading-deg") + _push * 180.0;
|
||||
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 deltaA = geo.normdeg180(A - psi);
|
||||
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;
|
||||
|
|
@ -69,12 +79,11 @@ var _loop = func() {
|
|||
_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 ~ ", psi_target " ~ geo.normdeg(A) ~ ", deltapsi " ~ deltapsi ~ ", deltapsi_steer " ~ _sign * deltaA);
|
||||
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);
|
||||
steering = math.min(math.max(_sign * _K_psi * deltaA, -1.0), 1.0);
|
||||
setprop("/sim/model/autopush/steer-cmd-norm", steering);
|
||||
}
|
||||
|
||||
|
|
@ -100,7 +109,7 @@ var start = func() {
|
|||
if ((_route == nil) or size(_route) < 2) {
|
||||
gui.popupTip("Pushback route empty or invalid");
|
||||
return;
|
||||
}else{
|
||||
} else {
|
||||
autopush_route.done();
|
||||
}
|
||||
_K_V = getprop("/sim/model/autopush/driver/K_V");
|
||||
|
|
@ -108,12 +117,18 @@ var start = func() {
|
|||
_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 == 1) {
|
||||
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) {
|
||||
|
|
@ -123,9 +138,9 @@ var start = func() {
|
|||
}
|
||||
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 " ~ int(geo.normdeg(psi_twy + 180.0 - magvar())) ~ ".");
|
||||
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 " ~ int(geo.normdeg(psi_twy - magvar())) ~ ".");
|
||||
screen.log.write("(pushback): Tow facing " ~ math.round(geo.normdeg(psi_twy - magvar()), 1.0) ~ ".");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -136,7 +151,5 @@ var pause = func() {
|
|||
|
||||
var stop = func() {
|
||||
pause();
|
||||
_to_wp = 1;
|
||||
_is_last_wp = 0;
|
||||
_is_reverse_wp = 0;
|
||||
_to_wp = 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,15 +15,25 @@ var _user_point_modes = dynarr.dynarr.new(4); # Modes: 0 = Bezier node, 1 = Bezi
|
|||
var _route = [];
|
||||
var _route_hdg = [];
|
||||
var _route_reverse = [];
|
||||
var _view_index = nil;
|
||||
var _top_view_index = nil;
|
||||
var _top_view_heading_offset_deg = 180.0;
|
||||
var _reset_view_index = nil;
|
||||
var _view_z_offset = nil;
|
||||
var _view_pitch_offset_deg = nil;
|
||||
var _view_heading_offset_deg = nil;
|
||||
var _user_point_models = [];
|
||||
var _waypoint_models = [];
|
||||
var _N = 0;
|
||||
var _show = 0;
|
||||
var _view_changed_or_external = 0;
|
||||
var _R_turn_min = 0;
|
||||
var _invalid = 0;
|
||||
|
||||
# Make top-down view point north in old FG.
|
||||
var __fg_version = num(string.replace(getprop("/sim/version/flightgear"),".",""));
|
||||
if (__fg_version < 201920) {
|
||||
_top_view_heading_offset_deg = 94.5;
|
||||
}
|
||||
|
||||
|
||||
var _add = func(pos) {
|
||||
if (_N) {
|
||||
|
|
@ -77,11 +87,11 @@ var _stop = func(fail = 0) {
|
|||
_listener = nil;
|
||||
if (!fail) {
|
||||
settimer(func() {
|
||||
_reset_view();
|
||||
_finalize_top_view();
|
||||
gui.popupTip("Done");
|
||||
}, 1.0);
|
||||
} else {
|
||||
_reset_view();
|
||||
_finalize_top_view();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -129,31 +139,36 @@ var _clear_waypoint_models = func() {
|
|||
setsize(_waypoint_models, 0);
|
||||
}
|
||||
|
||||
var _set_view = func() {
|
||||
if (!getprop("/sim/current-view/internal")){
|
||||
_view_changed_or_external = 1;
|
||||
var top_view = func() {
|
||||
if (_view_listener != nil) {
|
||||
return;
|
||||
}
|
||||
_view_index = getprop("/sim/current-view/view-number");
|
||||
# While "Chase View Without Yaw" would have looked better, only "Model View" resets its z-offset back to normal by itself.
|
||||
setprop("/sim/current-view/view-number", view.indexof("Model View"));
|
||||
_top_view_index = view.indexof("Chase View Without Yaw");
|
||||
_reset_view_index = getprop("/sim/current-view/view-number");
|
||||
setprop("/sim/current-view/view-number", _top_view_index);
|
||||
_view_pitch_offset_deg = getprop("/sim/current-view/pitch-offset-deg");
|
||||
_view_heading_offset_deg = getprop("/sim/current-view/heading-offset-deg");
|
||||
_view_z_offset = getprop("/sim/current-view/z-offset-m");
|
||||
setprop("/sim/current-view/z-offset-m", -500.0);
|
||||
setprop("/sim/current-view/heading-offset-deg", _top_view_heading_offset_deg);
|
||||
setprop("/sim/current-view/pitch-offset-deg", 90.0);
|
||||
setprop("/sim/current-view/heading-offset-deg", 0.0);
|
||||
_view_changed_or_external = 0;
|
||||
_view_listener = setlistener("/sim/current-view/name", func {
|
||||
_view_changed_or_external = 1;
|
||||
});
|
||||
_finalize_top_view();
|
||||
}, 0, 0);
|
||||
}
|
||||
|
||||
var _reset_view = func() {
|
||||
if (!_view_changed_or_external) {
|
||||
setprop("/sim/current-view/view-number", _view_index);
|
||||
}
|
||||
if (_view_listener != nil) {
|
||||
removelistener(_view_listener);
|
||||
_view_listener = nil;
|
||||
var _finalize_top_view = func() {
|
||||
if (_view_listener == nil) {
|
||||
return;
|
||||
}
|
||||
removelistener(_view_listener);
|
||||
_view_listener = nil;
|
||||
# Go back to the view to restore settings, in case user has switched away.
|
||||
setprop("/sim/current-view/view-number", _top_view_index);
|
||||
setprop("/sim/current-view/z-offset-m", _view_z_offset);
|
||||
setprop("/sim/current-view/heading-offset-deg", _view_heading_offset_deg);
|
||||
setprop("/sim/current-view/pitch-offset-deg", _view_pitch_offset_deg);
|
||||
setprop("/sim/current-view/view-number", _reset_view_index);
|
||||
if (!_show) {
|
||||
_clear_user_point_models();
|
||||
_clear_waypoint_models();
|
||||
|
|
@ -238,20 +253,24 @@ var _calculate_bezier = func(user_points) {
|
|||
len += user_points[i].distance_to(user_points[i + 1]);
|
||||
}
|
||||
|
||||
var step = _R_turn_min / len;
|
||||
if (len < _R_turn_min) {
|
||||
route.add(geo.Coord.new(user_points[PNumber - 1]))
|
||||
} else {
|
||||
var step = _R_turn_min / len;
|
||||
|
||||
for (var i = step; i < 1 - step; i+= step) {
|
||||
# start iterating from 1 cause we don't need to iterate over Pn
|
||||
for (var j = 1; j < PNumber; j += 1) {
|
||||
for (var k = 0; k < PNumber - j; k += 1) {
|
||||
pointList[j][k] = geo.Coord.new(pointList[j - 1][k]);
|
||||
var dist = pointList[j - 1][k].distance_to(pointList[j - 1][k + 1]);
|
||||
var course = pointList[j - 1][k].course_to(pointList[j - 1][k + 1]);
|
||||
pointList[j][k].apply_course_distance(course, dist * i);
|
||||
for (var i = step; i < 1 - step; i+= step) {
|
||||
# start iterating from 1 cause we don't need to iterate over Pn
|
||||
for (var j = 1; j < PNumber; j += 1) {
|
||||
for (var k = 0; k < PNumber - j; k += 1) {
|
||||
pointList[j][k] = geo.Coord.new(pointList[j - 1][k]);
|
||||
var dist = pointList[j - 1][k].distance_to(pointList[j - 1][k + 1]);
|
||||
var course = pointList[j - 1][k].course_to(pointList[j - 1][k + 1]);
|
||||
pointList[j][k].apply_course_distance(course, dist * i);
|
||||
}
|
||||
}
|
||||
pointList[PNumber - 1][0].set_alt(geo.elevation(pointList[PNumber - 1][0].lat(),pointList[PNumber - 1][0].lon()));
|
||||
route.add(geo.Coord.new(pointList[PNumber - 1][0]));
|
||||
}
|
||||
pointList[PNumber - 1][0].set_alt(geo.elevation(pointList[PNumber - 1][0].lat(),pointList[PNumber - 1][0].lon()));
|
||||
route.add(geo.Coord.new(pointList[PNumber - 1][0]));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -317,21 +336,21 @@ var _check_turn_radius = func() {
|
|||
setlistener("/sim/model/autopush/route/show", func(p) {
|
||||
var show = p.getValue();
|
||||
if (_listener == nil) {
|
||||
if (show > _show) {
|
||||
if (show) {
|
||||
_place_user_point_models();
|
||||
_place_waypoint_models();
|
||||
} else if (show < _show) {
|
||||
} else {
|
||||
_clear_user_point_models();
|
||||
_clear_waypoint_models();
|
||||
}
|
||||
}
|
||||
_show = show;
|
||||
});
|
||||
}, 1, 0);
|
||||
|
||||
|
||||
var enter = func() {
|
||||
clear();
|
||||
_set_view();
|
||||
top_view();
|
||||
_R_turn_min = getprop("/sim/model/autopush/min-turn-radius-m");
|
||||
var wp = geo.aircraft_position();
|
||||
var H = geo.elevation(wp.lat(), wp.lon());
|
||||
|
|
|
|||
Loading…
Reference in a new issue