User:Necolatis/terrain detection from nasal: Difference between revisions
Jump to navigation
Jump to search
(1st example done. For moderators: please don't delete this page.) |
mNo edit summary |
||
(2 intermediate revisions by the same user not shown) | |||
Line 1: | Line 1: | ||
This page is deprecated. See [[Terrain Detection]] for how it ended up being implemented. | |||
This is mostly a page for Richard and James to see that many use cases are easily solved in Nasal, with the new wrapper: | This is mostly a page for Richard and James to see that many use cases are easily solved in Nasal, with the new wrapper: | ||
Line 5: | Line 8: | ||
<syntaxhighlight lang="nasal"> | <syntaxhighlight lang="nasal"> | ||
var myPos = geo.aircraft_position(); | var myPos = geo.aircraft_position(); | ||
var otherPos = geo.aircraft_position().apply_course_distance(100, | var otherPos = geo.aircraft_position().apply_course_distance(100, 20000);# another plane 20 Km out | ||
# Check for terrain between own aircraft and other: | # Check for terrain between own aircraft and other: | ||
Line 21: | Line 24: | ||
print("The planes has clear view of each other"); | print("The planes has clear view of each other"); | ||
} | } | ||
} | |||
</syntaxhighlight> | |||
== Find terrain from radar beam== | |||
In this example, we have a Mig21 with a forward down looking terrain detecting radar. | |||
The radar beam is fixed 15 degs down from the nose of the aircraft. | |||
<syntaxhighlight lang="nasal"> | |||
# First we need this helper method: | |||
var getGPS = func(x, y, z) { | |||
# | |||
# get Coord from body structural position. x,y,z must be in meters. | |||
# derived from Vivian's code in AIModel/submodel.cxx. | |||
# | |||
var ac = geo.aircraft_position(); | |||
if(x == 0 and y==0 and z==0) { | |||
return geo.Coord.new(ac); | |||
} | |||
var ac_roll = getprop("orientation/roll-deg"); | |||
var ac_pitch = getprop("orientation/pitch-deg"); | |||
var ac_hdg = getprop("orientation/heading-deg"); | |||
var in = [0,0,0]; | |||
var trans = [[0,0,0],[0,0,0],[0,0,0]]; | |||
var out = [0,0,0]; | |||
in[0] = -x * M2FT; | |||
in[1] = y * M2FT; | |||
in[2] = z * M2FT; | |||
# Pre-process trig functions: | |||
var cosRx = math.cos(-ac_roll * D2R); | |||
var sinRx = math.sin(-ac_roll * D2R); | |||
var cosRy = math.cos(-ac_pitch * D2R); | |||
var sinRy = math.sin(-ac_pitch * D2R); | |||
var cosRz = math.cos(ac_hdg * D2R); | |||
var sinRz = math.sin(ac_hdg * D2R); | |||
# Set up the transform matrix: | |||
trans[0][0] = cosRy * cosRz; | |||
trans[0][1] = -1 * cosRx * sinRz + sinRx * sinRy * cosRz ; | |||
trans[0][2] = sinRx * sinRz + cosRx * sinRy * cosRz; | |||
trans[1][0] = cosRy * sinRz; | |||
trans[1][1] = cosRx * cosRz + sinRx * sinRy * sinRz; | |||
trans[1][2] = -1 * sinRx * cosRx + cosRx * sinRy * sinRz; | |||
trans[2][0] = -1 * sinRy; | |||
trans[2][1] = sinRx * cosRy; | |||
trans[2][2] = cosRx * cosRy; | |||
# Multiply the input and transform matrices: | |||
out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2]; | |||
out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2]; | |||
out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2]; | |||
# Convert ft to degrees of latitude: | |||
out[0] = out[0] / (366468.96 - 3717.12 * math.cos(ac.lat() * D2R)); | |||
# Convert ft to degrees of longitude: | |||
out[1] = out[1] / (365228.16 * math.cos(ac.lat() * D2R)); | |||
# Set position: | |||
var mlat = ac.lat() + out[0]; | |||
var mlon = ac.lon() + out[1]; | |||
var malt = (ac.alt() * M2FT) + out[2]; | |||
var c = geo.Coord.new(); | |||
c.set_latlon(mlat, mlon, malt * FT2M); | |||
return c; | |||
} | |||
#in this example we will just assume the radar is at the center of aircraft, thats pretty trivial to change though. | |||
var myPos = geo.aircraft_position(); | |||
#first find a coord in the radar beam in aircraft coordinates: | |||
var beam_x = -5000*math.cos(-15*D2R);#15 deg down, 5Km out | |||
var beam_y = 0; | |||
var beam_z = 5000*math.sin(-15*D2R); | |||
var beam = getGPS(beam_x, beam_y, beam_z); | |||
#we now find the vector the beam is pointed in: | |||
v = get_cart_ground_intersection(myPos.x(), myPos.y(), myPos.z(), beam.x()-myPos.x(), beam.y()-myPos.y(), beam.z()-myPos.z()); | |||
if (v == nil) { | |||
printf("No terrain"); | |||
} else { | |||
var terrain = geo.Coord.new(); | |||
terrain.set_xyz(v[0],v[1],v[2]); | |||
var terrainDist = myPos.direct_distance_to(terrain); | |||
printf("terrain found %0.1f meters down the beam", terrainDist); | |||
} | } | ||
</syntaxhighlight> | </syntaxhighlight> |
Latest revision as of 22:10, 13 August 2017
This page is deprecated. See Terrain Detection for how it ended up being implemented.
This is mostly a page for Richard and James to see that many use cases are easily solved in Nasal, with the new wrapper:
Check for terrain between 2 planes
This example is typically for radar use.
var myPos = geo.aircraft_position();
var otherPos = geo.aircraft_position().apply_course_distance(100, 20000);# another plane 20 Km out
# Check for terrain between own aircraft and other:
v = get_cart_ground_intersection(myPos.x(), myPos.y(), myPos.z(), otherPos.x()-myPos.x(), otherPos.y()-myPos.y(), otherPos.z()-myPos.z());
if (v == nil) {
printf("No terrain, planes has clear view of each other");
} else {
var terrain = geo.Coord.new();
terrain.set_xyz(v[0],v[1],v[2]);
var maxDist = myPos.direct_distance_to(otherPos);
var terrainDist = myPos.direct_distance_to(terrain);
if (terrainDist < maxDist) {
print("terrain found between the planes");
} else {
print("The planes has clear view of each other");
}
}
Find terrain from radar beam
In this example, we have a Mig21 with a forward down looking terrain detecting radar. The radar beam is fixed 15 degs down from the nose of the aircraft.
# First we need this helper method:
var getGPS = func(x, y, z) {
#
# get Coord from body structural position. x,y,z must be in meters.
# derived from Vivian's code in AIModel/submodel.cxx.
#
var ac = geo.aircraft_position();
if(x == 0 and y==0 and z==0) {
return geo.Coord.new(ac);
}
var ac_roll = getprop("orientation/roll-deg");
var ac_pitch = getprop("orientation/pitch-deg");
var ac_hdg = getprop("orientation/heading-deg");
var in = [0,0,0];
var trans = [[0,0,0],[0,0,0],[0,0,0]];
var out = [0,0,0];
in[0] = -x * M2FT;
in[1] = y * M2FT;
in[2] = z * M2FT;
# Pre-process trig functions:
var cosRx = math.cos(-ac_roll * D2R);
var sinRx = math.sin(-ac_roll * D2R);
var cosRy = math.cos(-ac_pitch * D2R);
var sinRy = math.sin(-ac_pitch * D2R);
var cosRz = math.cos(ac_hdg * D2R);
var sinRz = math.sin(ac_hdg * D2R);
# Set up the transform matrix:
trans[0][0] = cosRy * cosRz;
trans[0][1] = -1 * cosRx * sinRz + sinRx * sinRy * cosRz ;
trans[0][2] = sinRx * sinRz + cosRx * sinRy * cosRz;
trans[1][0] = cosRy * sinRz;
trans[1][1] = cosRx * cosRz + sinRx * sinRy * sinRz;
trans[1][2] = -1 * sinRx * cosRx + cosRx * sinRy * sinRz;
trans[2][0] = -1 * sinRy;
trans[2][1] = sinRx * cosRy;
trans[2][2] = cosRx * cosRy;
# Multiply the input and transform matrices:
out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
# Convert ft to degrees of latitude:
out[0] = out[0] / (366468.96 - 3717.12 * math.cos(ac.lat() * D2R));
# Convert ft to degrees of longitude:
out[1] = out[1] / (365228.16 * math.cos(ac.lat() * D2R));
# Set position:
var mlat = ac.lat() + out[0];
var mlon = ac.lon() + out[1];
var malt = (ac.alt() * M2FT) + out[2];
var c = geo.Coord.new();
c.set_latlon(mlat, mlon, malt * FT2M);
return c;
}
#in this example we will just assume the radar is at the center of aircraft, thats pretty trivial to change though.
var myPos = geo.aircraft_position();
#first find a coord in the radar beam in aircraft coordinates:
var beam_x = -5000*math.cos(-15*D2R);#15 deg down, 5Km out
var beam_y = 0;
var beam_z = 5000*math.sin(-15*D2R);
var beam = getGPS(beam_x, beam_y, beam_z);
#we now find the vector the beam is pointed in:
v = get_cart_ground_intersection(myPos.x(), myPos.y(), myPos.z(), beam.x()-myPos.x(), beam.y()-myPos.y(), beam.z()-myPos.z());
if (v == nil) {
printf("No terrain");
} else {
var terrain = geo.Coord.new();
terrain.set_xyz(v[0],v[1],v[2]);
var terrainDist = myPos.direct_distance_to(terrain);
printf("terrain found %0.1f meters down the beam", terrainDist);
}