1
0
Fork 0

Merge branch 'autopush'

This commit is contained in:
legoboyvdlp R 2019-10-24 16:58:05 +01:00
commit 1308850cad
5 changed files with 115 additions and 82 deletions

View file

@ -15,34 +15,7 @@
</hud>
<model>
<autopush>
<connected type="bool">0</connected>
<position-norm type="double">0</position-norm>
<enabled type="int"/>
<available type="int">1</available>
<chocks alias="/services/chocks/nose"/>
<steer-cmd-norm alias="/fdm/jsbsim/hydraulics/tiller/autopush-cmd"/>
<yaw alias="/fdm/jsbsim/fcs/steer-deg"/>
<yaw-mult type="float">1.0</yaw-mult>
<target-speed-km_h type="float">0.0</target-speed-km_h>
<K_p type="float">0.5</K_p>
<F_p type="float">0.15</F_p>
<K_i type="float">0.25</K_i>
<F_i type="float">0.1</F_i>
<K_d type="float">0.0</K_d>
<F_d type="float">0.0</F_d>
<pitch-deg type="float">0.0</pitch-deg>
<min-turn-radius-m type="float">15</min-turn-radius-m>
<stopping-distance-m type="float">5.0</stopping-distance-m>
<driver>
<F_V type="float">5.5</F_V>
<K_psi type="float">0.03</K_psi>
</driver>
<route>
<show type="bool"/>
</route>
<debug type="bool">false</debug>
</autopush>
<autopush include="AircraftConfig/autopush-config.xml"/>
<icing>
<iceable>
<name>Wing</name>

View file

@ -0,0 +1,30 @@
<PropertyList>
<steer-cmd-norm alias="/fdm/jsbsim/hydraulics/tiller/autopush-cmd"/>
<yaw alias="/fdm/jsbsim/fcs/steer-deg"/>
<yaw-mult type="float">1.0</yaw-mult>
<pitch-deg type="float">0.0</pitch-deg>
<min-turn-radius-m type="float">15</min-turn-radius-m>
<stopping-distance-m type="float">5.0</stopping-distance-m>
<chocks alias="/services/chocks/nose"/>
<available type="int">1</available>
<enabled type="int"/>
<connected type="bool">false</connected>
<target-speed-km_h type="float">0.0</target-speed-km_h>
<K_p type="float">0.5</K_p>
<F_p type="float">0.15</F_p>
<K_i type="float">0.25</K_i>
<F_i type="float">0.1</F_i>
<K_d type="float">0.0</K_d>
<F_d type="float">0.0</F_d>
<driver>
<F_V type="float">5.5</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"/>
</route>
<debug type="int">0</debug>
</PropertyList>

View file

@ -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);

View file

@ -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;
}

View file

@ -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);
var _finalize_top_view = func() {
if (_view_listener == nil) {
return;
}
if (_view_listener != nil) {
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,6 +253,9 @@ var _calculate_bezier = func(user_points) {
len += user_points[i].distance_to(user_points[i + 1]);
}
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) {
@ -254,6 +272,7 @@ var _calculate_bezier = func(user_points) {
route.add(geo.Coord.new(pointList[PNumber - 1][0]));
}
}
}
return route.get_sliced();
}
@ -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());