574
edits
User:Necolatis/terrain detection from nasal: Difference between revisions
Jump to navigation
Jump to search
User:Necolatis/terrain detection from nasal (view source)
Revision as of 02:20, 5 January 2017
, 5 January 2017Second example
(1st example done. For moderators: please don't delete this page.) |
(Second example) |
||
| Line 21: | Line 21: | ||
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> | ||