Menu
▾
▴
Re: [Flightgear-devel] route manager crash
|
From: Tobias D. <tda...@gm...> - 2018-06-27 06:42:05
|
On Tue, Jun 26, 2018 at 03:47:19PM +0100, James Turner wrote:
>
> I don’t suppose you’d mind contributing that back to the C++ code? :)
Hmm, right now it's all Nasal, and of course anyone who wants to should
feel free to take it and whatever they want with it. Not sure if I want
to rewrite it in C++ though; specifically, I'm not 100% sure whether it
would work for other aircraft types.
Anyway, here's the patch for the CRJ700-family:
diff --git a/Nasal/instrumentation.nas b/Nasal/instrumentation.nas
index 843f8a6..1ad4922 100644
--- a/Nasal/instrumentation.nas
+++ b/Nasal/instrumentation.nas
@@ -233,6 +233,356 @@ var nav_annunciator = func ()
setlistener("autopilot/internal/vor1-captured", nav_annunciator, 0, 0);
setlistener("autopilot/internal/vor2-captured", nav_annunciator, 0, 0);
+var mk_coords = func (lat, lon)
+{
+ var c = geo.Coord.new();
+ c.set_latlon(lat, lon);
+ return c;
+}
+
+var fdm_set_target_radial = func (radial)
+{
+ if (radial == 'auto' or radial == '') {
+ setprop("autopilot/internal/target/radial", -1);
+ setprop("autopilot/internal/target/capmode", "homing");
+ }
+ else {
+ setprop("autopilot/internal/target/radial", radial);
+ setprop("autopilot/internal/target/capmode", "intercept");
+ }
+}
+
+var get_std_turn_radius = func (airspeed) {
+ return airspeed / (15 * math.pi);
+}
+
+var fdm_configure_hold = func (fp, mode, phase)
+{
+ if (fp == nil) return;
+ var wpid = fp.current;
+ var wp = fp.getWP(wpid);
+ if (wp == nil) return;
+ var my_coords = mk_coords(
+ getprop("position/latitude-deg"),
+ getprop("position/longitude-deg"));
+ var wp_coords = mk_coords(wp.lat, wp.lon);
+ var target_dist = my_coords.distance_to(wp_coords) * M2NM;
+ var my_course = getprop("orientation/track-deg");
+ var pattern_speed = 230; # TODO: get real speed
+ var turn_radius = get_std_turn_radius(pattern_speed);
+ var leg_len_nm = 4; # arbitrary default
+ if (!wp.hld_is_distance) {
+ leg_len_nm = wp.hld_time_or_distance * pattern_speed / 3600;
+ }
+ else {
+ leg_len_nm = wp.hld_time_or_distance;
+ }
+ var radial_fwd = wp.hld_inbound_radial;
+ var radial_rev = geo.normdeg(radial_fwd + 180);
+ var diagonal_angle = R2D * math.atan2(2 * turn_radius, leg_len_nm);
+ var diagonal_fwd = radial_fwd;
+ if (wp.hld_is_left_handed) {
+ diagonal_fwd = geo.normdeg(radial_fwd - diagonal_angle);
+ }
+ else {
+ diagonal_fwd = geo.normdeg(radial_fwd + diagonal_angle);
+ }
+ var diagonal_rev = geo.normdeg(diagonal_fwd + 180);
+
+ var mapping =
+ { "hold":
+ { "inbound": [ "F", radial_fwd ]
+ , "outbound": [ "O", radial_rev ]
+ }
+ , "direct":
+ { "approach": [ "F", 'auto' ]
+ , "outbound": [ "O", radial_rev ]
+ }
+ , "teardrop":
+ { "approach": [ "F", 'auto' ]
+ , "diagonal": [ "O", diagonal_rev ]
+ },
+ , "parallel":
+ { "approach": [ "F", 'auto' ]
+ , "reverse": [ "X", radial_rev ]
+ , "diagonal": [ "F", diagonal_fwd ]
+ }
+ };
+
+ var selection = mapping[mode][phase];
+ var target = selection[0];
+ var radial = selection[1];
+
+ var target_coords = mk_coords(wp.lat, wp.lon);
+ if (target == "X" or target == "O") {
+ target_coords.apply_course_distance(radial_rev, leg_len_nm * NM2M);
+ }
+ if (target == "O") {
+ if (wp.hld_is_left_handed) {
+ target_coords.apply_course_distance(radial_fwd - 90, 2 * turn_radius * NM2M);
+ }
+ else {
+ target_coords.apply_course_distance(radial_fwd + 90, 2 * turn_radius * NM2M);
+ }
+ }
+ setprop("autopilot/internal/target/lat", target_coords.lat());
+ setprop("autopilot/internal/target/lon", target_coords.lon());
+ fdm_set_target_radial(radial);
+ setprop("autopilot/internal/target/id", wpid);
+ setprop("autopilot/internal/target/mode", mode);
+ setprop("autopilot/internal/target/phase", phase);
+ # no turn anticipation in pattern
+ setprop("autopilot/internal/target/anticipate", 0);
+}
+
+var fdm_configure_target = func (fp, wpid)
+{
+ if (fp == nil) return;
+ var wp = fp.getWP(wpid);
+ if (wp == nil) return;
+ if (wp.fly_type == "Hold") {
+ var my_coords = mk_coords(
+ getprop("position/latitude-deg"),
+ getprop("position/longitude-deg"));
+ var wp_coords = mk_coords(wp.lat, wp.lon);
+ var target_bearing = my_coords.course_to(wp_coords);
+ var my_course = getprop("orientation/track-deg");
+ var turn_heading = geo.normdeg180(target_bearing - my_course);
+
+ if (math.abs(turn_heading) >= 85) {
+ # We're moving away from the fix; this means we've already entered
+ # the holding area.
+ # So we will decide our hold entry procedure based on current
+ # heading relative to the holding radial, and we will jump directly
+ # to the second phase of the hold entry pattern.
+ var heading_diff = wp.hld_inbound_radial - my_course;
+ if (wp.hld_is_left_handed) {
+ heading_diff = -heading_diff;
+ }
+ if (math.abs(heading_diff) <= 90) {
+ # direct entry
+ fdm_configure_hold(fp, "hold", "outbound");
+ }
+ else if (heading_diff > 0) {
+ # parallel entry
+ fdm_configure_hold(fp, "parallel", "reverse");
+ }
+ else {
+ # teardrop entry
+ fdm_configure_hold(fp, "teardrop", "diagonal");
+ }
+ }
+ else {
+ # We're moving towards the fix; this means we have yet to enter the
+ # holding area, so we will start with the approach phase of the
+ # holding entry pattern.
+ var heading_diff = wp.hld_inbound_radial - target_bearing;
+ if (wp.hld_is_left_handed) {
+ heading_diff = -heading_diff;
+ }
+ if (math.abs(heading_diff) <= 90) {
+ # direct entry
+ fdm_configure_hold(fp, "direct", "approach");
+ }
+ else if (heading_diff > 0) {
+ # parallel entry
+ fdm_configure_hold(fp, "parallel", "approach");
+ }
+ else {
+ # teardrop entry
+ fdm_configure_hold(fp, "teardrop", "approach");
+ }
+ }
+ }
+ else {
+ var leg_bearing = wp.leg_bearing;
+ setprop("autopilot/internal/target/lat", wp.lat);
+ setprop("autopilot/internal/target/lon", wp.lon);
+ fdm_set_target_radial(leg_bearing);
+ setprop("autopilot/internal/target/id", wpid);
+ setprop("autopilot/internal/target/mode", "normal");
+ setprop("autopilot/internal/target/phase", "approach");
+ if (wp.fly_type == "flyBy") {
+ if (wpid + 1 >= fp.getPlanSize()) {
+ # last leg in plan, nothing to anticipate
+ setprop("autopilot/internal/target/anticipate", 0);
+ }
+ else {
+ var next_wp = fp.getWP(wpid + 1);
+ if (next_wp.fly_type == "Hold") {
+ # approaching a holding pattern, no turn anticipation
+ setprop("autopilot/internal/target/anticipate", 0);
+ }
+ else {
+ # next leg is a normal one, enable turn anticipation
+ setprop("autopilot/internal/target/anticipate", next_wp.leg_bearing - wp.leg_bearing);
+ }
+ }
+ }
+ else {
+ # current leg is a fly-over, no turn anticipation
+ setprop("autopilot/internal/target/anticipate", 0);
+ }
+ }
+}
+
+var fdm_course_update = func ()
+{
+ if (getprop("autopilot/internal/target/lat") == nil or
+ getprop("autopilot/internal/target/lon") == nil)
+ return;
+ # get target coords
+ var target_fix = mk_coords(
+ getprop("autopilot/internal/target/lat"),
+ getprop("autopilot/internal/target/lon"));
+
+ # get aircraft position
+ var my_coords = mk_coords(
+ getprop("position/latitude-deg"),
+ getprop("position/longitude-deg"));
+ var my_course = getprop("orientation/track-deg");
+
+ # relative position towards fix
+ var target_course = my_coords.course_to(target_fix);
+ var target_dist = my_coords.distance_to(target_fix) * M2NM;
+ var capmode = getprop("autopilot/internal/target/capmode");
+ setprop("autopilot/internal/target/course", target_course);
+ setprop("autopilot/internal/target/dist", target_dist);
+ # default is to home in on the target wp
+ var course = target_course;
+ var airspeed = getprop("velocities/airspeed-kt");
+ var turn_radius = get_std_turn_radius(airspeed);
+ setprop("autopilot/internal/target/turn_radius", turn_radius);
+
+ if (capmode == "intercept" and target_dist > turn_radius) {
+ var target_radial = getprop("autopilot/internal/target/radial");
+ setprop("autopilot/internal/target/effective-radial", target_radial);
+
+ var bearing_err = geo.normdeg180(target_radial - target_course);
+ setprop("autopilot/internal/target/bearing_err", bearing_err);
+
+ var track_err = math.sin(D2R * bearing_err) * target_dist;
+ setprop("autopilot/internal/target/track_err", track_err);
+
+ if (math.abs(track_err) < 0.1 or
+ math.sgn(track_err) == math.sgn(geo.normdeg180(my_course - target_radial))) {
+ # moving away from the target, or already on track; just align to
+ # target radial
+ course = target_radial;
+ }
+ else {
+ # moving towards the target; calculate interception course
+ var cos_course_err = math.max(math.cos(60), (turn_radius - math.abs(track_err)) / turn_radius);
+ var course_err = math.acos(cos_course_err) * R2D;
+ setprop("autopilot/internal/target/course_err", course_err);
+ if (track_err > 0) {
+ course = target_radial - course_err;
+ }
+ else {
+ course = target_radial + course_err;
+ }
+ }
+ }
+ else {
+ setprop("autopilot/internal/target/effective-radial", geo.normdeg(target_course));
+ }
+ setprop("autopilot/internal/target-crs", geo.normdeg(course));
+ setprop("autopilot/internal/target/effective-crs", geo.normdeg(course));
+}
+
+var fdm_next_target = func (fp)
+{
+ var mode = getprop("autopilot/internal/target/mode");
+ var phase = getprop("autopilot/internal/target/phase");
+
+ if (mode == "hold") {
+ if (phase == "outbound") {
+ fdm_configure_hold(fp, "hold", "inbound");
+ }
+ else {
+ fdm_configure_hold(fp, "hold", "outbound");
+ }
+ }
+ else if (mode == "parallel") {
+ if (phase == "approach") {
+ fdm_configure_hold(fp, "parallel", "reverse");
+ }
+ else if (phase == "reverse") {
+ fdm_configure_hold(fp, "parallel", "diagonal");
+ }
+ else {
+ # diagonal
+ fdm_configure_hold(fp, "hold", "inbound");
+ }
+ }
+ else if (mode == "teardrop") {
+ if (phase == "approach") {
+ fdm_configure_hold(fp, "teardrop", "diagonal");
+ }
+ else {
+ # diagonal
+ fdm_configure_hold(fp, "hold", "inbound");
+ }
+ }
+ else if (mode == "direct") {
+ fdm_configure_hold(fp, "hold", "outbound");
+ }
+ else {
+ # assume mode "normal"
+ var wpid = getprop("autopilot/internal/target/id");
+ fp.current = wpid + 1;
+ }
+}
+
+var fdm_update_target = func (fp)
+{
+ if (getprop("autopilot/internal/target/lat") == nil or
+ getprop("autopilot/internal/target/lon") == nil)
+ return;
+ var my_coords = mk_coords(
+ getprop("position/latitude-deg"),
+ getprop("position/longitude-deg"));
+ var target_coords = mk_coords(
+ getprop("autopilot/internal/target/lat"),
+ getprop("autopilot/internal/target/lon"));
+ var airspeed = getprop("velocities/airspeed-kt");
+ var turn_radius = get_std_turn_radius(airspeed);
+ var capmode = getprop("autopilot/internal/target/capmode");
+ var leg_bearing = 'auto';
+ if (capmode == "intercept") {
+ leg_bearing = getprop("autopilot/internal/target/radial");
+ }
+ var turn_anticipation_dist = 0;
+ if (leg_bearing != 'auto') {
+ var anticipation_angle = getprop("autopilot/internal/target/anticipate");
+ if (math.abs(anticipation_angle) > 90) {
+ turn_anticipation_dist = turn_radius;
+ }
+ else {
+ turn_anticipation_dist =
+ turn_radius *
+ math.tan(0.5 * D2R * anticipation_angle);
+ }
+ }
+ var target_dist =
+ my_coords.distance_to(target_coords) * M2NM;
+ if (target_dist <= turn_anticipation_dist or target_dist < 0.1) {
+ fdm_next_target(fp);
+ }
+}
+
+var fdm_update = func ()
+{
+ var fp = flightplan();
+ var configured_wpid = getprop("autopilot/internal/target/id");
+ var current_wpid = fp.current;
+ if (configured_wpid != current_wpid) {
+ fdm_configure_target(fp, current_wpid);
+ }
+ fdm_update_target(fp);
+ fdm_course_update();
+}
+
var vs_annunciator = func ()
{
var ref = sprintf("%1.1f", getprop("controls/autoflight/vertical-speed-select")/1000);
diff --git a/Nasal/master.nas b/Nasal/master.nas
index 9942a4c..df0388a 100644
--- a/Nasal/master.nas
+++ b/Nasal/master.nas
@@ -109,6 +109,7 @@ var fast_loop = Loop(0, func {
# Instruments.
eicas_messages_page1.update();
eicas_messages_page2.update();
+ CRJ700.fdm_update();
# Model.
wipers[0].update();
diff --git a/Systems/CRJ700-autopilot.xml b/Systems/CRJ700-autopilot.xml
index f2c94d2..2602a47 100644
--- a/Systems/CRJ700-autopilot.xml
+++ b/Systems/CRJ700-autopilot.xml
@@ -82,7 +82,7 @@
<debug type="bool">false</debug>
<gain>1</gain>
<input>
- <property>autopilot/route-manager/wp[0]/true-bearing-deg</property>
+ <property>autopilot/internal/target-crs</property>
<offset>
<property>orientation/heading-deg</property>
<scale>-1</scale>
Definitely needs some more tweaking though, and especially the hold code needs
debugging, or maybe even an entirely different approach. The way it works now
is that the autopilot can either follow a radial, defined as a point in space
and a course, or it can just home in on a point in space. For a hold, it also
tracks the entry mode (teardrop, parallel, direct) and the phase (which
depends on the mode, but always starts with "approach" and ends in an
"inbound" - "outbound" loop). It calculates a second virtual fix, right
where the turn onto the inbound leg should be (from the holding fix, 1 minute
worth of flying along the outbound radial, and then two standard-turn
radii to the left or right, depending on the direction of the holding
pattern), and then the inbound leg is just a matter of intercepting the
(Fix, Radial) ray, and the outbound leg is intercepting (Opposite Fix,
Opposite Radial).
The problem seems to be that turning this tightly doesn't leave much
room for error, so when the turn overshoots due to wind, the autopilot
has trouble correcting, and ends up circling the next fix.
So I was thinking maybe I should take a dumber approach, and just base
the outbound leg at least on course and time alone - when reaching the
fix, turn right until the course matches the opposite of the hold
radial, then fly straight for 1 minute (or whatever time the hold
specifies), then home in on the fix (or intercept the (Fix, Radial)
ray). This requires a timer though, and it won't work for
distance-defined holding patterns (then again, those can of course just
measure distance from the fix).
Oh, and also I have patched fgfs to expose holding point data to Nasal,
like so:
commit ca377c4ef297f0d6ed21f2a93a2bcbee3f2c12ca
Author: Tobias Dammers <tda...@gm...>
Date: Thu Jun 21 18:29:49 2018 +0200
Expose holding point data in nasal
diff --git a/src/Scripting/NasalPositioned.cxx b/src/Scripting/NasalPositioned.cxx
index 22953bb2b..0d5fda303 100644
--- a/src/Scripting/NasalPositioned.cxx
+++ b/src/Scripting/NasalPositioned.cxx
@@ -494,6 +494,42 @@ static const char* waypointCommonGetMember(naContext c, Waypt* wpt, const char*
*out = waypointAirport(c, wpt);
} else if (!strcmp(fieldName, "runway")) {
*out = waypointRunway(c, wpt);
+ } else if (!strcmp(fieldName, "hld_is_left_handed")) {
+ Hold* hold = dynamic_cast<Hold*>(wpt);
+ if (hold)
+ *out = naNum(hold->isLeftHanded());
+ else
+ return nullptr;
+ } else if (!strcmp(fieldName, "hld_is_distance")) {
+ Hold* hold = dynamic_cast<Hold*>(wpt);
+ if (hold)
+ *out = naNum(hold->isLeftHanded());
+ else
+ return nullptr;
+ } else if (!strcmp(fieldName, "hld_by")) {
+ Hold* hold = dynamic_cast<Hold*>(wpt);
+ if (hold)
+ *out = stringToNasal(c, hold->isDistance() ? "dist" : "time");
+ else
+ return nullptr;
+ } else if (!strcmp(fieldName, "hld_inbound_radial")) {
+ Hold* hold = dynamic_cast<Hold*>(wpt);
+ if (hold)
+ *out = naNum(hold->inboundRadial());
+ else
+ return nullptr;
+ } else if (!strcmp(fieldName, "hld_heading_radial_deg")) {
+ Hold* hold = dynamic_cast<Hold*>(wpt);
+ if (hold)
+ *out = naNum(hold->inboundRadial());
+ else
+ return nullptr;
+ } else if (!strcmp(fieldName, "hld_time_or_distance")) {
+ Hold* hold = dynamic_cast<Hold*>(wpt);
+ if (hold)
+ *out = naNum(hold->timeOrDistance());
+ else
+ return nullptr;
} else {
return nullptr; // member not found
}
|
