From d3c9d28a44aa12cf63afbb0a70f0eacfd965a07f Mon Sep 17 00:00:00 2001
From: Sidi Liang <1467329765@qq.com>
Date: Sun, 16 Feb 2020 22:14:41 +0800
Subject: [PATCH] ParkingRadar: Added feature
---
Nasal/radar.nas | 125 ++++++++++++++++++
Nasal/waypoint.ac | 276 ++++++++++++++++++++++++++++++++++++++++
followme_e-tron-set.xml | 1 +
3 files changed, 402 insertions(+)
create mode 100644 Nasal/radar.nas
create mode 100644 Nasal/waypoint.ac
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 @@
<file>Aircraft/followme_e-tron/Nasal/electrical.nas</file>
<file>Aircraft/followme_e-tron/Nasal/systems.nas</file>
<file>Aircraft/followme_e-tron/Nasal/steering.nas</file>
+ <file>Aircraft/followme_e-tron/Nasal/radar.nas</file>
</followme>
<!--<screen>
<file>Aircraft/followme_e-tron/Nasal/SmartScreen.nas</file>