p38192567
/
followme_e-tron
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Safety: Added AEB AutoSpeed: Bug fix

This commit is contained in:
Sidi Liang 2020-04-06 00:02:11 +08:00
parent 1ae9532e97
commit d2acc94496
No known key found for this signature in database
GPG Key ID: 79F0A6B20B72F42F
4 changed files with 112 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Nasal/auto_speed.nas
View File

@ -25,7 +25,8 @@ var autoSpeedMainLoop = func(){
}else{
throttle = 0;
}
throttleNode.setValue(throttle);
if(active) throttleNode.setValue(throttle);
else throttleNode.setValue(0);
leftBrakes.setValue(brakes);
rightBrakes.setValue(brakes);
lastDeltaSpeed = deltaSpeed;
@ -43,9 +44,9 @@ var startAutoSpeed = func(){
}
var stopAutoSpeed = func(){
active = 0;
autoSpeedTimer.stop();
props.getNode("/sim/messages/copilot",1).setValue("ze dong chao sue see tong yee guan bee. Auto Speeding System is off.");
active = 0;
throttleNode.setValue(0);
}
@ -84,4 +85,4 @@ var targetSpeedChange = func(speed){
return 0;
}
targetSpeed = speed;
}
}

59
Nasal/radar.nas
View File

@ -2,12 +2,22 @@
var Radar = {
#//Class for any Parking Radar (currently only support terrain detection) which scans in a sector
#//height: height of installation above ground;installCoordX: X coord of installation; installCoordY: Y coord of installation; maxRange: max radar range; maxWidth: width of the sector
#//orientationMode 0:towards back, 180:towards front, 90: towards left, 270:towards right, custom values are accepted(in degrees)
#//Node that 0 degrees is the backward(180 degrees heading of the vehicle), which is a little weird because I forget about it when designing the system
#//and the support of other install orientations are added afterwards
#//For a typical parking radar, set orientationMode=0(or leave it as default)
#//warnEnabled: set it to 1 (or leave it as default) enables the internal warning system(typecally used for a parking radar)
#//Notice: when warnEnabled set to 1, there's nothing being outputed to radarOutput!
#//For follow me EV: height 0.3m; installCoordX:0m; installCoordY:3.8m; maxRange:3m;maxWidth:3m
#//To start scanning: myRadar.init();
#//To Stop: myRadar.stop();
new: func(height, installCoordX, installCoordY, maxRange, maxWidth) {
return { parents:[Radar, followme.Appliance.new()], height: height, installCoordX:installCoordX, installCoordY:installCoordY, maxRange:maxRange, maxWidth:maxWidth};
new: func(height, installCoordX, installCoordY, maxRange, maxWidth, orientationMode=0, warnEnabled=1, debug=0) {
return { parents:[Radar, Appliance.new()], height: height, installCoordX:installCoordX, installCoordY:installCoordY, maxRange:maxRange, maxWidth:maxWidth, orientationMode:orientationMode, warnEnabled:warnEnabled, debug:debug, radarOutput:10000};
},
debug: 0,#if debug = 1, shows marker and prints info
warnEnabled: 1,#1 enables the internal warning system(typecally used for a parking radar) as 0 disables it
height: 0.3, #METERS
installCoordX: 0, #METERS
installCoordY: 3.8, #METERS
@ -17,6 +27,7 @@ var Radar = {
updateInterval:0.25,#SEC
searchAngle: 0, #RAD, half of the search angle
tanSearchAngle: 0,
orientationMode:0, #Deg
vehiclePosition: nil,
coord: nil,
@ -33,6 +44,8 @@ var Radar = {
warningSound: "parking_radar.wav",
lastDis: 0,
radarOutput: 10000,#The value which radar returns in meters
init: func(){
me.searchAngle = math.acos(me.maxRange / math.sqrt((2/me.maxWidth)*(2/me.maxWidth) + me.maxRange*me.maxRange));
me.tanSearchAngle = math.tan(me.searchAngle);
@ -42,14 +55,23 @@ var Radar = {
me.widthLatRange = me.calculateLatChange(me.maxWidth);
me.widthLonRange = me.calculateLonChange(me.maxWidth, me.coord);
if(me.radarTimer == nil) me.radarTimer = maketimer(me.updateInterval, func me.update());
if(me.warningTimer == nil) me.warningTimer = maketimer(me.warningInterval, func me.warn());
if(me.warnEnabled and me.warningTimer == nil) me.warningTimer = maketimer(me.warningInterval, func me.warn());
me.radarTimer.start();
print("Parking radar initialized!");
playAudio("parking_radar_init.wav");
if(me.warnEnabled){
print("Parking radar initialized!");
playAudio("parking_radar_init.wav");
}else{
#print("Radar initialized!");
}
},
stop: func(){
print("Parking radar stopped!");
me.warningTimer.stop();
if(me.warnEnabled){
print("Parking radar stopped!");
playAudio("parking_radar_init.wav");
}else{
#print("Radar Stopped!");
}
if(me.warnEnabled) me.warningTimer.stop();
me.radarTimer.stop();
},
toggle: func(){
@ -61,7 +83,7 @@ var Radar = {
},
getCoord: func(){
me.vehicleHeading = props.getNode("/orientation/heading-deg",1).getValue();
me.vehicleHeading = geo.normdeg(props.getNode("/orientation/heading-deg",1).getValue() + me.orientationMode);
me.vehiclePosition = geo.aircraft_position();
me.coord = geo.Coord.new(me.vehiclePosition);
me.coord.apply_course_distance(geo.normdeg(me.vehicleHeading-90), me.installCoordX);
@ -113,14 +135,14 @@ var Radar = {
if(meters <= 0.5){
me.warningInterval = 0.2;
me.warningSound = "parking_radar_long.wav";
#print("Caution! Something behind at less than 0.5 meters!");
if(me.debug) print("Caution! Something detected at less than 0.5 meters!");
return;
}else{
me.warningSound = "parking_radar.wav";
}
meters = sprintf("%.3f", meters);
if(meters != me.lastDis) me.warningInterval = (meters)/me.maxRange;
#print("Caution! Something behind at approximatly "~meters~" meters");
if(me.debug) print("Caution! Something detected at approximatly "~meters~" meters");
me.lastDis = meters;
},
sample: func(stepLat, stepLon, searchLat, searchLon){ # returns an elevtion
@ -128,7 +150,7 @@ var Radar = {
var lonChange = -math.cos(me.vehicleHeading * D2R);
var sampleCoord = geo.Coord.new();
sampleCoord.set_latlon(me.position_change(searchLat,stepLat*latChange), me.position_change(searchLon,stepLon*lonChange));
#var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", sampleCoord.lat(), sampleCoord.lon(), me.coord.alt());
if(me.debug) var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", sampleCoord.lat(), sampleCoord.lon(), me.coord.alt());
return sampleCoord;
},
update: func(){
@ -150,8 +172,9 @@ var Radar = {
targetElev = me.getElevByCoord(targetCoord);
if(me.judgeElev(targetElev)){
var meters = me.coord.distance_to(targetCoord);
#var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", targetCoord.lat(), targetCoord.lon(), me.coord.alt());
me.warnControl(meters);
if(me.debug) var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", targetCoord.lat(), targetCoord.lon(), me.coord.alt());
if(me.warnEnabled) me.warnControl(meters);
else me.radarOutput = meters;
return;
}
}
@ -162,16 +185,18 @@ var Radar = {
targetElev = me.getElevByCoord(targetCoord);
if(me.judgeElev(targetElev)){
var meters = me.coord.distance_to(targetCoord);
#var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", targetCoord.lat(), targetCoord.lon(), me.coord.alt());
me.warnControl(meters);
if(me.debug) var model = geo.put_model(getprop("sim/aircraft-dir")~"/Nasal/waypoint.ac", targetCoord.lat(), targetCoord.lon(), me.coord.alt());
if(me.warnEnabled) me.warnControl(meters);
else me.radarOutput = meters;
return;
}
}
searchStep += 0.1;
searchDis += searchStep;
}
#print("All clear");
me.warnControl(10000);
if(me.debug) print("All clear");
if(me.warnEnabled) me.warnControl(10000);
else me.radarOutput = 10000;
},
};

66
Nasal/systems.nas
View File

@ -471,17 +471,60 @@ var resetOnPosition = func(){
var Safety = {
new: func(airbagAccelerationLimit=140, sideAirbagAccelerationLimit=72){
return {parents: [Safety], airbagAccelerationLimit:airbagAccelerationLimit, sideAirbagAccelerationLimit:sideAirbagAccelerationLimit};
var newSafety = { parents:[Safety] };
newSafety.airbagAccelerationLimit = airbagAccelerationLimit;
newSafety.sideAirbagAccelerationLimit = sideAirbagAccelerationLimit;
newSafety.frontRadar = Radar.new(0.3, 0, 0, 10, 1, 180, 0, 0);
return newSafety;
},
isOn: 0,
safetySystemTimer: nil,
updateInterval: 0.01,
frontRadarEnabled: 0,
aebActivated: 0,
#Airbag
accXProp: props.getNode("/fdm/jsbsim/accelerations/a-pilot-x-ft_sec2", 1),
accYProp: props.getNode("/fdm/jsbsim/accelerations/a-pilot-y-ft_sec2", 1),
frontAirbagProp: props.getNode("/systems/safety/airbag/front", 1),
sideAirbagProp: props.getNode("/systems/safety/airbag/side", 1),
airbagAccelerationLimit: 140, #To be configured,m/s^2
sideAirbagAccelerationLimit: 72, #To be configured,m/s^2
#Frontwards radar
frontRadar: nil,
enableFrontRadar: func(){
#Enables the front radar
me.frontRadarEnabled = 1;
me.frontRadar.init();
me.frontRadar.stop();
print("Front radar enabled");
},
disableFrontRadar: func(){
#Disables the front radar
me.frontRadar.stop();
me.frontRadarEnabled = 0;
},
aebActive: func(){
me.aebActivated = 1;
#engine.engine_1.engineSwitch.switchDisconnect();
props.getNode("/",1).setValue("/controls/gear/brake-left", 1);
props.getNode("/",1).setValue("/controls/gear/brake-right", 1);
props.getNode("/",1).setValue("/controls/gear/brake-parking", 1);
playAudio("parking_radar_init.wav");
print("AEB Activated!");
},
aebStop: func(){
me.aebActivated = 0;
print("AEB Stopped");
#engine.engine_1.engineSwitch.switchConnect();
props.getNode("/",1).setValue("/controls/gear/brake-left", 0);
props.getNode("/",1).setValue("/controls/gear/brake-right", 0);
props.getNode("/",1).setValue("/controls/gear/brake-parking", 0);
},
update: func(){
#print("running");
#Front airbag
@ -496,10 +539,28 @@ var Safety = {
me.sideAirbagProp.setValue(1);
me.emergencyMode();
}
#AEB, Automatic Emergency Brake
var currentSpeed = props.getNode("/", 1).getValue("sim/multiplay/generic/float[15]")*1.852;#In km/h
if(currentSpeed > 20){
if(me.frontRadarEnabled){
me.frontRadar.init();
if(me.frontRadar.radarOutput <= 8 and !me.aebActivated){
me.aebActive();
me.emergencyMode();
}else if(me.frontRadar.radarOutput >= 8 and me.aebActivated){
me.aebStop();
}
}
}else{
if(me.frontRadarEnabled and me.frontRadar.radarTimer.isRunning) me.frontRadar.stop();
}
},
emergencyMode: func(){
indicatorController.falseLightOn(); #Active malfunction light
indicatorController.setMode(3); #Active malfunction light
indicatorController.falseLight = 1;
if(autospeed.autoSpeedTimer.isRunning) autospeed.stopAutoSpeed();
if(autopilot.road_check_timer.isRunning) autopilot.road_check_timer.stop();
},
@ -531,6 +592,7 @@ var Safety = {
};
var safety = Safety.new(140, 72);
safety.init();
safety.enableFrontRadar();
var brakesABS = func(){
var gearFrtLftSpeed = math.round(props.getNode("/",1).getValue("/fdm/jsbsim/gear/unit/wheel-speed-fps"));
var gearFrtRgtSpeed = math.round(props.getNode("/",1).getValue("/fdm/jsbsim/gear/unit[1]/wheel-speed-fps"));

4
followme_e-tron-set.xml
View File

@ -838,11 +838,11 @@
<nasal>
<followme>
<file>Aircraft/followme_e-tron/Nasal/electrical.nas</file>
<file>Aircraft/followme_e-tron/Nasal/radar.nas</file>
<file>Aircraft/followme_e-tron/Nasal/systems.nas</file>
<file>Aircraft/followme_e-tron/Nasal/plate.nas</file>
<file>Aircraft/followme_e-tron/Nasal/steering.nas</file>
<file>Aircraft/followme_e-tron/Nasal/radar.nas</file>
<file>Aircraft/followme_e-tron/Nasal/save.nas</file>
<file>Aircraft/followme_e-tron/Nasal/save.nas</file>
</followme>
<!--<screen>
<file>Aircraft/followme_e-tron/Nasal/SmartScreen.nas</file>