diff --git a/Nasal/radar.nas b/Nasal/radar.nas new file mode 100644 index 0000000..1ccb60d --- /dev/null +++ b/Nasal/radar.nas @@ -0,0 +1,125 @@ +#Parking radar by Sidi Liang +var Radar = { + #//Class for any Parking Radar (currently only support terrain detection) + #//height: height of installation above ground;installCoord: coord of installation; maxRange: max radar range + #//For this vehicle: height 0.3m; installCoord:3.78m; maxRange:7m; maxWidth:3m + #//To start scanning: myRadar.init(); + #//To Stop: myRadar.stop(); + new: func(height, installCoord, maxRange, maxWidth) { + return { parents:[Radar, followme.Appliance.new()], height: height, installCoord:installCoord, maxRange:maxRange, maxWidth:maxWidth}; + }, + height: 0.3, #METERS + installCoord:3.7, #METERS + maxRange:7, #METERS + maxWidth:2, #METERS + radarTimer: nil, + + vehiclePosition: nil, + coord: nil, + vehicleHeading: nil, + + backLonRange: nil, + backLatRange: nil, + widthLonRange:nil, + widthLatRange:nil, + + init: func(){ + me.getCoord(); + me.backLatRange = me.calculateLatChange(me.maxRange); + me.backLonRange = me.calculateLonChange(me.maxRange, me.coord); + me.widthLatRange = me.calculateLatChange(me.maxWidth); + me.widthLonRange = me.calculateLonChange(me.maxWidth, me.coord); + if(me.radarTimer == nil) me.radarTimer = maketimer(0.2, func me.update()); + me.radarTimer.start(); + }, + stop: func(){ + me.radarTimer.stop(); + }, + + getCoord: func(){ + me.vehicleHeading = props.getNode("/orientation/heading-deg",1).getValue(); + me.vehiclePosition = geo.aircraft_position(); + me.coord = geo.Coord.new(me.vehiclePosition); + me.coord.apply_course_distance(me.vehicleHeading, -me.installCoord); + #var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", me.coord); + }, + calculateLonChange: func(meters, coord){ + var earthLength = 2 * 6378137 * math.pi; #equator + var lat2EarthLength = earthLength * math.cos(math.abs(coord.lat())); + var lonChange = meters * (360/lat2EarthLength); + return math.abs(lonChange); + }, + calculateLatChange: func(meters){ + return meters * 0.000008983152841195214; + }, + calculateMeterChangebyLat: func(lat){ + return lat / 0.000008983152841195214; + }, + getElevByCoord: func(coord){ + return geo.elevation(coord.lat(), coord.lon()); + }, + position_change: func(position_val,value){ + if(position_val+value>180) + position_val += value-360; + else if(position_val+value<-180) + position_val += value+360; + else + position_val += value; + return position_val; + }, + judgeElev: func(targetElev){ + myElev = me.getElevByCoord(me.coord); + if((myElev + me.height) < targetElev){ + return 1; + }else{ + return 0; + } + }, + warn: func(targetCoord){ + var meters = me.coord.distance_to(targetCoord); + print("Caution! something behind at approximatly "~meters~" meters"); + var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", targetCoord); + }, + sample: func(stepLat, stepLon, searchLat, searchLon){ # returns an elevtion + var latChange = math.sin(math.pi * me.vehicleHeading/180); + var lonChange = -math.cos(math.pi * me.vehicleHeading/180); + var coord = geo.Coord.new(); + coord.set_latlon(me.position_change(searchLat,stepLat*latChange), me.position_change(searchLon,stepLon*lonChange)); + return coord; + }, + update: func(){ + me.getCoord(); + var searchDis = 0.01;#Meters + var searchStep = 0.1;#Meters + while(searchDis <= me.maxRange){ + var searchCoord = geo.Coord.new(); + searchCoord.set_latlon(me.coord.lat(), me.coord.lon()); + searchCoord.apply_course_distance(me.vehicleHeading, 0-searchDis); + for(var i = 0; i > (0 - me.widthLatRange/2); i -= 0.000001){ + var percentage = (0-i)/(me.widthLatRange/2); #use approximate value to reduce cost + var stepLon = 0 - me.widthLonRange * percentage; + var targetCoord = me.sample(i, stepLon, searchCoord.lat(), searchCoord.lon()); + targetElev = me.getElevByCoord(targetCoord); + if(me.judgeElev(targetElev)){ + me.warn(targetCoord); + return; + } + } + for(var i = 0; i < me.widthLatRange/2; i += 0.000001){ + var percentage = i/(me.widthLatRange/2); #use approximate value to reduce cost + var stepLon = me.widthLonRange * percentage; + var targetCoord = me.sample(i, stepLon, searchCoord.lat(), searchCoord.lon()); + targetElev = me.getElevByCoord(targetCoord); + if(me.judgeElev(targetElev)){ + me.warn(targetCoord); + return; + } + } + searchStep += 0.1; + searchDis += searchStep; + } + print("All clear"); + }, +}; + +var parkingRadar = Radar.new(0.3, 3.7, 7, 2); diff --git a/Nasal/waypoint.ac b/Nasal/waypoint.ac new file mode 100644 index 0000000..e6c3ff9 --- /dev/null +++ b/Nasal/waypoint.ac @@ -0,0 +1,276 @@ +AC3Db +MATERIAL "autopush cursor" rgb 0.0000 0.0000 0.0000 amb 0.0000 0.0000 0.0000 emis 1.000 0.173 0.545 spec 0.0000 0.0000 0.0000 shi 50 trans 0.0000 +OBJECT world +name "Blender_exporter_v2.26__waypoint.ac" +kids 3 +OBJECT poly +name "Waypoint" +data 11 +Circle.mesh +crease 40.0 +numvert 33 +0.03902 0.2 -0.19616 +0.07654 0.2 -0.18478 +0.11111 0.2 -0.16629 +0.14142 0.2 -0.14142 +0.16629 0.2 -0.11111 +0.18478 0.2 -0.07654 +0.19616 0.2 -0.03902 +0.2 0.2 0 +0.19616 0.2 0.03902 +0.18478 0.2 0.07654 +0.16629 0.2 0.11111 +0.14142 0.2 0.14142 +0.11111 0.2 0.16629 +0.07654 0.2 0.18478 +0.03902 0.2 0.19616 +0 0.2 0.2 +-0.03902 0.2 0.19616 +-0.07654 0.2 0.18478 +-0.11111 0.2 0.16629 +-0.14142 0.2 0.14142 +-0.16629 0.2 0.11111 +-0.18478 0.2 0.07654 +-0.19616 0.2 0.03902 +-0.2 0.2 0 +-0.19616 0.2 -0.03902 +-0.18478 0.2 -0.07654 +-0.16629 0.2 -0.11111 +-0.14142 0.2 -0.14142 +-0.11111 0.2 -0.16629 +-0.07654 0.2 -0.18478 +-0.03902 0.2 -0.19616 +0 0.2 -0.2 +0 0.2 0 +numsurf 32 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +18 0.090277 0.292133 +17 0.101082 0.26903 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +31 0.125 0.75 +30 0.112807 0.745197 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +4 0.176967 0.638893 +3 0.169194 0.676777 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +17 0.101082 0.26903 +16 0.112807 0.254804 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +30 0.112807 0.745197 +29 0.101082 0.73097 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +3 0.169194 0.676777 +2 0.159723 0.707868 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +16 0.112807 0.254804 +15 0.125 0.25 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +29 0.101082 0.73097 +28 0.090277 0.707868 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +2 0.159723 0.707868 +1 0.148918 0.73097 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +15 0.125 0.25 +14 0.137193 0.254804 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +28 0.090277 0.707868 +27 0.080806 0.676777 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +1 0.148918 0.73097 +0 0.137193 0.745197 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +14 0.137193 0.254804 +13 0.148918 0.26903 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +27 0.080806 0.676777 +26 0.073033 0.638893 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +0 0.137193 0.745197 +31 0.125 0.75 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +13 0.148918 0.26903 +12 0.159723 0.292133 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +26 0.073033 0.638893 +25 0.067258 0.595671 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +12 0.159723 0.292133 +11 0.169194 0.323224 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +25 0.067258 0.595671 +24 0.063701 0.548773 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +11 0.169194 0.323224 +10 0.176967 0.361108 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +24 0.063701 0.548773 +23 0.0625 0.5 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +10 0.176967 0.361108 +9 0.182743 0.404329 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +23 0.0625 0.5 +22 0.063701 0.451228 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +9 0.182743 0.404329 +8 0.186299 0.451228 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +22 0.063701 0.451228 +21 0.067258 0.404329 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +8 0.186299 0.451228 +7 0.1875 0.5 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +21 0.067258 0.404329 +20 0.073033 0.361108 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +7 0.1875 0.5 +6 0.186299 0.548773 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +20 0.073033 0.361108 +19 0.080806 0.323223 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +6 0.186299 0.548773 +5 0.182742 0.595671 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +19 0.080806 0.323223 +18 0.090277 0.292133 +SURF 0X0 +mat 0 +refs 3 +32 0.125 0.5 +5 0.182742 0.595671 +4 0.176967 0.638893 +kids 0 +OBJECT poly +name "WingtipL" +data 15 +Circle.mesh.001 +crease 40.0 +numvert 4 +-0.1 0.2 -0.1 +-0.1 0.2 0.1 +0.1 0.2 -0.1 +0.1 0.2 0.1 +numsurf 1 +SURF 0X0 +mat 0 +refs 4 +0 0 0 +1 0 0 +3 0 0 +2 0 0 +kids 0 +OBJECT poly +name "WingtipR" +data 15 +Circle.mesh.007 +crease 40.0 +numvert 4 +-0.1 0.2 -0.1 +-0.1 0.2 0.1 +0.1 0.2 -0.1 +0.1 0.2 0.1 +numsurf 1 +SURF 0X0 +mat 0 +refs 4 +0 0 0 +1 0 0 +3 0 0 +2 0 0 +kids 0 diff --git a/followme_e-tron-set.xml b/followme_e-tron-set.xml index ac25965..cfd3f31 100644 --- a/followme_e-tron-set.xml +++ b/followme_e-tron-set.xml @@ -708,6 +708,7 @@ Aircraft/followme_e-tron/Nasal/electrical.nas Aircraft/followme_e-tron/Nasal/systems.nas Aircraft/followme_e-tron/Nasal/steering.nas + Aircraft/followme_e-tron/Nasal/radar.nas