Sync Autopush.

parent d79bde75
......@@ -15,7 +15,12 @@ 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;
......@@ -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);
}
......@@ -108,13 +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) {
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) {
......
......@@ -12,6 +12,9 @@
<driver>
<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"/>
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment