View difference between Paste ID: aZ2bWEZT and v1FhvhQv
SHOW: | | - or go back to the newest paste.
1
set k_autopilot2_version to "0.9.3".
2
print "Kadin's autopilot2 V" + k_autopilot2_version:tostring.
3
4
5
set debug to false.
6
set slope_debug to true.
7
8
///
9
/// set aoa to 72. lock steering to srfprograde * r(-aoa, 0,0)
10
///
11
12-
print "There are three other versions of this code. This is the dev version.".
12+
print "There are three versions of this code.".
13-
print "Unstable/testing version, seems to be working version: !runscript NHwvPAp3".
13+
print "Unstable/development version, newest features, newest bugs: !runscript NHwvPAp3".
14
print "Recent working version, seems like it is fairly stable: !runscript 0pgtewL5". 
15
print "Oldest version, least features but also fairly stable: !runscript dHtuDeGx".
16
17
// 2021-09-05: completely re-wrote setup_runway and changed all references to variables matching regexp("^runway").
18
// added variable flaps, while maintaining flaps on/off and toggle flaps.
19
//						you can do : set flaps to "full", "half" or "none".
20
//						you cat set flaps to 0.5.
21
//						BUT it only works when the flaps tag is "flaps:NUMBER" where number is the full scale deflection angle.
22
//						So if you want full flaps to be 23, then set the tag to "flaps:23".
23
// added landing flare.
24
// added aoa_limit_enable - stop craft from pitching beyond aoa limits.
25
//							- NOTE: the current aoa limit stuff is WRONG
26
//							- AoA has nothing to do with your angle to "up"... yet this is what I do.
27
//							- I know it is wrong, but i'll fix it later and it's better than nothing.
28
// added function haoa() - hold angle of attack.
29
// added SG Explorer and various other craft
30
// latest change: added setup_runway function.
31
//latest change: hl() for hold line.: hl(runway_west,runway_east).
32
//latest change: blurb about version of the code.
33
//latest change: added hg() for hold glideslope: hg(target_slope, ground_target, voffset, hoffset).
34
35
set bank_change_rate to 3.				// How fast we change between bank angles .
36
set rudder_rate to 0.4.					// How much rudder to kick in to help increase the turn rate.
37
set default_bank_limit to 40.			// This is how many degrees of bank for a turn if none are specified.
38
set default_aoa_limit to 15.			// aoa limit +/-
39
set hud_message to "Want to have a go? Speak up in chat! I'm just testing code, and will happily stop and let you have a go!".
40
41
42
set flare_factor to ship:mass/5.
43
if ship:name:startswith("Mallard") {
44
	set vs to 65.				// stall speed in level flight - complete guess.
45
	set vsr to 60.	//normal stall speed.
46
	set vs0 to 40.	//stall speed with flaps out
47
	set vto to 100.	//take off speed
48
	set vfe to 65.	//speed above which you would remove flaps.
49
	set vlo to 60.	//speed below which you CAN extend/retract gear.
50
	set vc to 80.	//cruise speed.
51
	set vt to 45.	//threshold speed.
52
	set vtd to 40.	//touchdown speed.
53
	set alt_radar_adj to 3.0433.
54
55
	set bank_limit to 40.
56
57
	//Setup small outboard aerosurfaces
58
	for p in ship:partsnamed("smallCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy",false).
59
	for p in ship:partsnamed("smallCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy direction",true).
60
	//max out authority limiter if we are able to.
61
	for p in ship:partsnamed("smallCtrlSrf") { local m is p:getmodule("ModuleControlSurface"). if m:hasfield("authority limiter") m:setfield("authority limiter",100). }.
62
	//Setup Large elevons.
63
	for p in ship:partsnamed("airlinerCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy",false).
64
	for p in ship:partsnamed("airlinerCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy direction",true).
65
	if exists("0:/Kadins_local_KSP.txt") {		// in my own stock KSP, for some reason these elevons have deploy direction flipped.
66
		for p in ship:partsnamed("airlinerCtrlSrf") {
67
			if p:position:mag < 6 p:getmodule("ModuleControlSurface"):setfield("deploy direction",false).
68
		}
69
	}
70
	//max out authority limiter if we are able to.
71
	for p in ship:partsnamed("airlinerCtrlSrf") { local m is p:getmodule("ModuleControlSurface"). if m:hasfield("authority limiter") m:setfield("authority limiter",100). }.
72
73
	//set up tail fins
74
	for p in ship:partsnamed("tailfin") p:getmodule("ModuleControlSurface"):setfield("deploy",false).
75
	for p in ship:partsnamed("tailfin") p:getmodule("ModuleControlSurface"):setfield("deploy direction",false).
76
	//max out authority limiter if we are able to.
77
	for p in ship:partsnamed("tailfin") { local m is p:getmodule("ModuleControlSurface"). if m:hasfield("authority limiter") m:setfield("authority limiter",100). }.
78
79
	//use large elevons as flaps (with deploy angle to 100% of available authority).
80
	for p in ship:partsnamed("airlinerCtrlSrf") { local m is p:getmodule("ModuleControlSurface"). m:setfield("deploy angle", choose ( m:getfield("authority limiter") * 1.0) if m:hasfield("authority limiter") else m:getfield("deploy angle") ). }.
81
	for p in ship:partsnamed("airlinerCtrlSrf") set p:tag to "flaps:" + p:getmodule("ModuleControlSurface"):getfield("deploy angle").
82
	//for p in ship:partstaggedpattern("^flaps(:|$)") print "TAG = " + p:tag.
83
84
	//use tail fins as spoilers (with deploy angle to 80% of available authority)
85
	for p in ship:partsnamed("tailfin") { local m is p:getmodule("ModuleControlSurface"). m:setfield("deploy angle", choose ( m:getfield("authority limiter") * 0.8) if m:hasfield("authority limiter") else m:getfield("deploy angle") ). }.
86
	for p in ship:partsnamed("tailfin") set p:tag to "spoilers".
87
88
	// at one stage I was only using some of the large elevons for flaps...
89
	// p:position:mag < 6 is the inboard elevons
90
	// p:position:mag > 6 and < 8 is the middle elevons
91
	// p:position:mag > 8 is the outer large elevons.
92
93
94
	//setup airbrakes
95
	for p in ship:partsnamed("airbrake1") set p:tag to "airbrakes".
96
	for p in ship:partstagged("airbrakes")  p:getmodule("ModuleAeroSurface"):setfield("deploy",false).
97
98
	//setup brakes
99
	for p in ship:partsnamed("GearMedium") p:getmodule("ModuleWheelBrakes"):setfield("brakes",75).
100
	for p in ship:partsnamed("SmallGearBay") p:getmodule("ModuleWheelBrakes"):setfield("brakes",25). 
101
102
	//setup reversable jets
103-
	for config_p in ship:partsnamed("JetEngine") set config_p:tag to "jets".
103+
	for p in ship:partsnamed("JetEngine") set p:tag to "jets".
104
105
} else if ship:name:startswith("Aeris 3A") {
106
	set alt_radar_adj to 1.410271525383.
107
	for p in ship:partsnamed("R8winglet") if p:position:mag < 4 p:getmodule("ModuleControlSurface"):setfield("deploy angle",10).
108
	for p in ship:partsnamed("R8winglet") if p:position:mag < 4 set p:tag to "flaps".
109
	for p in ship:partsnamed("elevon3") p:getmodule("ModuleControlSurface"):setfield("deploy angle",15).
110
	for p in ship:partsnamed("elevon3") set p:tag to "flaps".
111
	for p in ship:partsnamed("R8winglet") { if p:position:mag > 4 p:getmodule("ModuleControlSurface"):setfield("deploy angle",15). }
112
	for p in ship:partsnamed("R8winglet") if p:position:mag > 4 set p:tag to "spoilers".
113
	for p in ship:partsnamed("JetEngine") set p:tag to "jets".
114
	for p in ship:partsnamed("SmallGearBay") if p:position:mag > 2 p:getmodule("ModuleWheelBrakes"):setfield("brakes",25). else p:getmodule("ModuleWheelBrakes"):setfield("brakes",100).
115
	set bank_limit to 60.
116
	set vs to 32.
117
	set vsr to 32.
118
	set vs0 to 32.
119
	set vfe to 35.
120
	set vlo to 35.
121
	set vto to 75.
122
	set vc to 60.
123
	set vtd to 32.
124
	set vt to 35.
125
} else if ship:name:startswith("Lockheed Jetstar") {
126
	set alt_radar_adj to 1.6479332447952.
127
	for p in ship:partsnamed("SmallGearBay") if p:position:mag < 2 p:getmodule("ModuleWheelBrakes"):setfield("brakes",100). else p:getmodule("ModuleWheelBrakes"):setfield("brakes",25).
128
	set bank_limit to 50.
129
	set vs to 60.
130
	set vsr to 60.
131
	set vs0 to 60.
132
	set vfe to 65.
133
	set vlo to 65.
134
	set vto to 100.
135
	set vc to 80.
136
	set vtd to 60.
137
	set vt to 60.
138
	set vrot to 60.
139
} else if ship:name:startswith("Reusable Moons Rocketship") {
140
	set bank_limit to 30.
141
	set alt_radar_adj to 4.58227.
142
	// can glide (constant airspeed) at about a -28 pitch down orientation.
143
	for p in ship:partsnamedpattern("gear") if p:position:mag < 7 p:getmodule("ModuleWheelBrakes"):setfield("brakes",200).
144
	// the shuttle rudder defaults to deploy angle 37.5 deg. but it only has an authority limiter of 9.75 deg.
145
	// so I'm assuming you will lose control authority if you deploy the tail fins as a speed brake.
146
	// also they both have deploy direction set to false... which I think will mean they don't activate opposite directions???
147
	// mpore experimentation is required.
148
	for p in ship:partsnamed("wingShuttleRudder") set p:tag to "spoilers".
149
	set vs to 75.
150
	set vsr to 75.
151
	set vs0 to 75.
152
	set vfe to 100.
153
	set vlo to 100.
154
	set vto to 150.
155
	set vc to 125.
156
	set vtd to 80.
157
	set vt to 90.
158
} else if ship:name:startswith("SG Rescue 2") {
159
	//AG1  = rapier engine
160
	//ag2 = toggle rapier mode
161
	//ag3 = airbrakes.
162
	//ag5 = cargobay
163
	//ag7,9,10 do drill stuff.
164
	// enable nuclear engines with:  list engines in es. for e in es if e:name:contains("nuc") e:activate
165
	//show engine mode for rapiers: list engines in es. for e in es if e:multimode print e:mode
166
	//All engines on: list engines in es. for e in es e:activate
167
	//use execute_node_old for any multi egine craft with some engines on or off or jet engines in space etc...
168
	//extend antenna: for a in ship:modulesnamed("ModuleDeployableAntenna") a:DoAction("extend antenna", true)
169
	set alt_radar_adj to 6.
170
	set vs to 120.
171
	set vsr to 120.
172
	set vs0 to 120.
173
	set vfe to 120.
174
	set vlo to 120.
175
	set vto to 120.
176
	set vc to 120.
177
	set vtd to 120.
178
	set vt to 120.
179
} else if ship:name:startswith("roach") {
180
	set alt_radar_adj to 1.90005767345428.
181
	// vertical stabilizers only have authority limiter = 15 so 10 for spoilers is about the most you can use safely.
182
	for p in ship:partsnamed("R8winglet") if p:position:mag > 3.3 p:getmodule("ModuleControlSurface"):setfield("deploy angle",10).
183
	for p in ship:partsnamed("R8winglet") if p:position:mag > 3.3 set p:tag to "spoilers".
184
	for p in ship:partsnamed("SmallGearBay") if p:position:mag < 1.2 p:getmodule("ModuleWheelBrakes"):setfield("brakes",25).
185
	set vs to 30.
186
	set vsr to 30.
187
	set vs0 to 30.
188
	set vfe to 60.
189
	set vlo to 60.
190
	set vto to 60.
191
	set vc to 100.
192
	set vtd to 30.
193
	set vt to 35.
194
	//set apid:KP to 0.2.
195
	//set apid:KI to 0.1.
196
	//set apid:KD to 0.2.
197
	//top speed around 876 at 2500m.
198
} else if ship:name:startswith("SG Explorer") {
199
	set alt_radar_adj to 3.75.
200
	//for p in ship:partsnamed("SmallGearBay") p:getmodule("ModuleWheelBrakes"):setfield("brakes",0).
201
	for p in ship:partsnamed("airbrake1") set p:tag to "airbrakes".
202
 	set sus to ship:modulesnamed("ModuleWheelSuspension").
203
	for w in sus { if w:hasevent("spring/damper: auto") w:doevent("spring/damper: auto").}.
204
	for w in sus { w:setfield("spring strength",2). w:setfield("damper strength",2).}.
205
	set sus to ship:modulesnamed("ModuleWheelBase").
206
	for w in sus { if w:hasevent("friction control: auto") w:doevent("friction control: auto").}.
207
	for w in sus { w:setfield("friction control",1).}.
208
	for m in ship:modulesnamed("ModuleWheelBrakes") m:setfield("brakes",200).
209
	set vs to 90.
210
	set vsr to 90.
211
	set vs0 to 90.
212
	set vfe to 120.
213
	set vlo to 120.
214
	set vto to 100.		// rotate at 90? 95?
215
	set vc to 150.
216
	set vtd to 95.
217
	set vt to 100.
218
	set STEERINGMANAGER:MAXSTOPPINGTIME to 20.
219
	set steeringmanager:rollpid:kd to 1.
220
	set bank_change_rate to 1.
221
	set steeringmanager:rollpid:ki to 0.1. set steeringmanager:rollpid:kd to 0.2. set steeringmanager:rollpid:kp to 0.4.
222
}  else {
223
	set vs to 100.
224
	set vsr to 100.
225
	set vs0 to 100.
226
	set vfe to 120.
227
	set vlo to 120.
228
	set vto to 120.
229
	set vc to 150.
230
	set vtd to 95.
231
	set vt to 100.
232
	set alt_radar_adj to 5.
233
}
234
235
236
// anomoly scanner.
237
//set as to addons:scansat:getanomalies(body). for anomaly in as print anomaly:name:padright(20) + anomaly:geoposition
238
239
240
// flying a heading near the pole:
241
//lock srfheading to body:geopositionof(srfprograde:vector:normalized):heading
242
// lock x to srfheading + turnrate.
243
244
// stall speed increases at high bank angles. Keep this updated so we can refer to this increased stall speed.
245
lock bank_stall_angle to arccos(vs^2/max(vs,airspeed)^2).
246
247
248
set control_revthrust to true.
249
set revthrust to false.
250
on revthrust {
251
	for p in ship:partstagged("jets") {
252
		local m is p:getmodule("ModuleAnimateGeneric").
253
		if revthrust and m:hasevent("reverse thr" + "ust") m:doevent("reverse thr" + "ust").
254
		else if m:hasevent("forward thr" + "ust") m:doevent("forward thr" + "ust").
255
	}
256
	return control_revthrust.
257
}
258
259
//on flaps {
260
//	for p in ship:partstagged("flaps") p:getmodule("ModuleControlSurface"):setfield("deploy",flaps).
261
//	if flaps set vs to vs0.
262
//	else set vs to vsr.
263
//	return true.
264
//}
265
266
267
// allow flaps to be set to other values.
268
set control_flaps to true.
269
set flaps to 0.
270
on flaps {
271
	if flaps:istype("Boolean") {
272
		if flaps set flaps to 1.
273
		else set flaps to 0.
274
	}
275
	if flaps:istype("String") {
276
		if list("On","All","Full"):find(flaps:trim) > -1 set flaps to 1.
277
		else if list("Off","None",""):find(flaps:trim) > -1 set flaps to 0.
278
		else if flaps:trim = "Half" set flaps to 0.5.
279
		else set flaps to 1.
280
	}
281
 	if flaps:istype("Scalar") {
282
		if flaps > 1 set flaps to 1.
283
		if flaps < 0 set flaps to 0.
284
		for p in ship:partstaggedpattern("^flaps(:|$)") {
285
			if flaps > 0 {
286
				local tag_parts is p:tag:split(":").
287
				if tag_parts:length > 1 {
288
					//tonumber(0) means if the flaps setting is bad, we will effectively have NO flaps
289
					p:getmodule("ModuleControlSurface"):setfield("deploy angle",tag_parts[1]:tonumber(0) * flaps).
290
					p:getmodule("ModuleControlSurface"):setfield("deploy",true).
291
				} else {
292
					//if the tag is just "flaps" then they are either on (any value above 0) or off (value is 0).
293
					if flaps < 1 set flaps to 1.
294
					p:getmodule("ModuleControlSurface"):setfield("deploy",true).
295
				}
296
			} else {
297
				p:getmodule("ModuleControlSurface"):setfield("deploy",flaps).
298
			}
299
		}
300
		//vsr is the normal stall speed. vs0 is stall speed with full flaps.
301
		//we will make a linear approximation of stall speed between vs0 and vsr proportional to flaps.
302
//TODO: this is saying stall speed changes based on what flap setting we WANT
303
//	it should be stall speed changes based on what the flaps are actually SET to.
304
//	i.e. we should check "deploy" and "deploy angle" compared to the tag "flaps:".
305
//	if "flap:###" tag was bad, and we are deploying flaps to 0, then we shoudl NOT be reducing stall speed!!!!
306
		set vs to vsr - (vsr - vs0) * flaps.
307
		//if flaps set vs to vs0.
308
		//else set vs to vsr.
309
	} else {
310
		print char(7) + "ERROR: unrecognised variable type " + flaps:typename + "for variable: flaps".
311
	}
312
	return control_flaps.
313
}
314
315
//merge airbrakes into spoilers for the moment.
316
set control_spoilers to true.
317
set spoilers to false.
318
on spoilers {
319
	for p in ship:partstagged("spoilers") p:getmodule("ModuleControlSurface"):setfield("deploy",spoilers).
320
	for p in ship:partstagged("airbrakes") p:getmodule("ModuleAeroSurface"):setfield("deploy",spoilers).
321
	return control_spoilers.
322
}
323
324
//set airbrakes to false.
325
//set control_airbrakes to true.
326
//on airbrakes {
327
//	for p in ship:partstagged("airbrakes")  p:getmodule("ModuleAeroSurface"):setfield("deploy",airbrakes).
328
//	return control_airbrakes.
329
//}.
330
331
on lights {
332
	set flaps to lights.
333
	return true.
334
}
335
336
337
function alt_radar {
338
	if defined alt_radar_adj return alt:radar - alt_radar_adj.
339
	else return alt:radar.
340
}
341
342
//compass heading of our current direction of travel. (compass travel).
343
function ct {
344
	if airspeed < 0.1 {
345
		return body:geopositionof(ship:facing:vector:normalized):heading.
346
	}   else {
347
		return body:geopositionof(srfprograde:vector:normalized):heading.
348
	}
349
}
350
351
//compass heading of the direction we are facing. (compass facing). 
352
function cf {
353
	return body:geopositionof(ship:facing:vector:normalized):heading.
354
}
355
356
// return the angle between two compass headings (second defaults to the direction we are travelling).
357
// -ve means you need to subtract from the current direction of travel to get to the first headings.
358
// +ve means you need to turn right (i.e.add to the current direction of travel).
359
function ang {
360
	parameter a.
361
	parameter b is ct().
362
	//extra 720 added for a bit more resiliency against supplied angles like "-370 degrees" etc.
363
	return mod(720 + a-b + 180 + 360,360) - 180.
364
}.
365
366
// Use set to change these values. locking steering and THEN doing more locks on the steering variables /seems/ to cause issues. 
367
set x to cf().
368
if alt_radar() > 10 set x to ct().
369
set p to 10.
370
set b to 0.
371
lock steering to heading(x,p,b).
372
set pitch_mode to "fixed".	// used to know when we are switching from hv() to ha() so we can do a pid:reset().
373
set hdg to x.
374
set hdg_mode to "fixed".
375
set pitch_info to p:tostring + " degrees".
376
set heading_info to x:tostring + " degrees".
377
378
set tpid to pidloop(0.5,0.01,0.1,0,1).		//used for holding speed
379
set vpid to pidloop(1,0.1,0.2,-45,45).		//used for holding_vertical_speeed
380
set apid to pidloop(0.5,0.02,0.4,-15,45).	//used for holding altitude
381
set rpid to pidloop(0.5,0.02,0.5,-10,60).	//used for flying NOE
382
set lpid to pidloop(0.2,0.02,0.05,-5,5).	//used for following a line
383
set gpid to pidloop(1,0.2,0.2,-45,45).		// used for gliding to maintain speed using pitch.
384
set spid to pidloop(1,0.2,0.2,-45,45).		// use for slipping.
385
set gpid:setpoint to vs + 20.
386
387
set using_wheelsteer to false.
388
set turn_dir to 0.
389
set turn_force_dir to 45.
390
set m_per_deg to (2 * constant():pi * body:radius) / 360.
391
392
// return the reciprocal runway name, if it does not looke like a runway name, or there is no reciprocal, then return "".
393
function recip {
394
	parameter r_str.
395
	if r_str:istype("String") {
396
		if r_str:matchespattern(" \d\d$") {
397
			local r_number is (r_str:substring(r_str:length-2,2)):tonumber.
398
			local r_name is r_str:substring(0,r_str:length -2).
399
			set r_number to mod(r_number + 18,36).
400
			if r_number = 0 set r_number to 36.
401
			if r_number < 10 set r_name to r_name + "0".
402
			set r_name to r_name + r_number:tostring.
403
			if places:haskey(r_name) return(r_name).
404
			else return "".
405
		} else {
406
			return "".
407
		}
408
	} else {
409
		return "".
410
	}
411
}.
412
413
set json_filename to "0:/json/" + body:name + ".json".
414
if exists(json_filename) {
415
	set places to readjson(json_filename).
416
} else if exists("0:/json/places.json") {
417
	set places to readjson("0:/json/places.json").
418
} else {
419
	set places to lexicon().
420
	// assume we are an airplane and on the end of the runway pointing at the far end. pretty big guesses!
421
	local rw_num is floor(cf()/10):tostring.
422
	until rw_num:length >= 2 {
423
		set rw_num to "0" + rw_num.
424
	}
425
	places:add("Guess " + rw_num,ship:geoposition).
426
	set rw_num to mod(floor(cf()/10) + 18,36).
427
	if rw_num = 0 set rw_num to 36.
428
	if rw_num < 10 set r_name to r_name:string + "0".
429
	places:add("Guess " + rw_num, body:geopositionof(ship:geoposition:position + ship:facing:forevector * 2400)).
430
}
431
432
set runway to lexicon("enabled",false).
433
set default_runway_final to 2000.
434
set default_runway_touchdown to 150.
435
set default_runway_turn to 1000.
436
set default_runway_glideslope to 4.
437
438
//setup_runway("KSC 27",lexicon("final",5000)).
439
//setup_runway("KSC 09",lexicon("touchdown",250)).
440
//setup_runway("KSC 09",lexicon("turn",1250)).
441
//setup_runway("KSC 09",lexicon("slope",20)).
442
//setup_runway(lexicon("start",places["KSC VAB Pad 1"],"end",places["KSC VAB Pad 2"]),lexicon("touchdown",-15)).
443
function setup_runway {
444
	parameter rw.
445
	parameter opts is lexicon().
446
447
	local new_runway is lexicon("enabled",true).
448
	if rw:istype("String") and rw = "" {
449
		print char(7) + "WARNING: no runway specified".
450
		return 0.
451
	} else if rw:istype("String") and places:haskey(rw) and places:haskey(recip(rw)) {
452
		set new_runway["name"] to rw.
453
		set new_runway["start"] to places[rw].
454
		set new_runway["end"] to places[recip(rw)].
455
	} else if rw:istype("Lexicon") and rw:haskey("start") and rw:haskey("end") and rw["start"]:istype("GeoCoordinates") and rw["end"]:istype("GeoCoordinates") {
456
		set new_runway["start"] to rw["start"].
457
		set new_runway["end"] to rw["end"].
458
		local numbers is floor(remote_heading(new_runway["start"],new_runway["end"])/10):tostring.
459
		if numbers = "0" set numbers to "36".
460
		if numbers:length = 1 set numbers to "0" + numbers.
461
		set new_runway["name"] to "Lat:" + round(new_runway["start"]:lat,3) +",Lng:" + round(new_runway["end"]:lng,3) + " " + numbers.
462
	} else {
463
			print char(7) + "ERROR: runway incorrectly specified".
464
			return 0.
465
	}
466
	set new_runway["normalized"] to ( new_runway["end"]:position - new_runway["start"]:position ):normalized.
467
	set new_runway["touchdown"] to body:geopositionof(new_runway["start"]:position + new_runway["normalized"] * (choose opts["touchdown"] if opts:haskey("touchdown") else default_runway_touchdown)).
468
	set new_runway["final"] to body:geopositionof(new_runway["normalized"] * -(choose opts["final"] if opts:haskey("final") else default_runway_final) + new_runway["start"]:position).
469
	set new_runway["slope"] to (choose opts["slope"] if opts:haskey("slope") else default_runway_glideslope).
470
	set new_runway["right"] to body:geopositionof(
471
		(new_runway["normalized"] * -(choose opts["final"] if opts:haskey("final") else default_runway_final)) +
472
		(vcrs(up:vector,new_runway["normalized"]):normalized*(choose opts["turn"] if opts:haskey("turn") else default_runway_turn)) + new_runway["start"]:position
473
	).
474
	set new_runway["left"] to body:geopositionof(
475
		( new_runway["normalized"] * -(choose opts["final"] if opts:haskey("final") else default_runway_final)) +
476
		(vcrs(new_runway["normalized"],up:vector):normalized*(choose opts["turn"] if opts:haskey("turn") else default_runway_turn)) + new_runway["start"]:position
477
	).
478
	set runway to new_runway.
479
	return 1.
480
}
481
482
483
function find_closest_runway {
484
	local closest_runway is "".
485
	local closest_distance is -1.
486
	for rw_name in places:keys {
487
		if rw_name:matchespattern(" \d\d$") {
488
			if recip(rw_name) <> "" {
489
				local dist is body:radius * constant:degtorad * vang(places[rw_name]:position - body:position, ship:position - body:position).
490
491
				if closest_distance = -1 or dist < closest_distance {
492
					set closest_runway to rw_name.
493
					set closest_distance to dist.
494
				}
495
			}
496
		}
497
	}
498
	return closest_runway.
499
}
500
501
if (setup_runway(find_closest_runway())) {
502
	if (ship:status = "landed" or ship:status = "prelaunch") and runway["start"]:distance < 250 and groundspeed < 15 {
503
		set hdg to runway["end"]:heading.
504
		//set x to runway["end"]:heading.
505
	}
506
} else {
507
	set runway to lexicon("enabled",false).
508
}
509
510
511
//Replying to @kadin955: is there an easy way to get the angle between the line to the start of the runway, and the line of the runway , using vectory thingies ?
512
513
//9:09Moderator9-Month Subscriber0zin: you see that "rw" vector at the start there? just vang(vxcl(up:vector,p1:position), rw)
514
//lock rw to vxcl(up:vector,p2:position-p1:position):normalized.
515
516
517
//given two points, work out the heading from the first to the second.
518
function remote_heading {
519
	parameter loc_from.
520
	parameter loc_to.
521
522
	local d_lat is loc_to:lat - loc_from:lat.
523
	local d_lng is loc_to:lng - loc_from:lng.
524
525
	if d_lng > 0 return 90 - arctan(d_lat/d_lng).
526
	else if d_lng < 0 return 270 - arctan(d_lat/d_lng).
527
	else if d_lng = 0 {
528
		if d_lat > 0 return 0.
529
		else if d_lat < 0 return 180.
530
		else return 0.
531
	}
532
}.
533
534
535
536
// truncate num so that it is between min and max (inclusive). optionally, do a call back to say you did something.  
537
function constrain {
538
	parameter num.
539
	parameter min is -1.
540
	parameter max is 1.
541
	parameter action is "".
542
	if num < min {
543
		if not (action = "") action(num,min).
544
		return(min).
545
	} else if num > max {
546
		if not (action = "") action(num,max).
547
		return(max).
548
	} else  {
549
		return num.
550
	}
551
}
552
553
set steeringmanager:pitchtorquefactor to 0.2.
554
set steeringmanager:yawtorquefactor to 0.5.
555
set steeringmanager:rollcontrolanglerange to 180.
556
steeringmanager:resetpids().
557
558
// default_bank_limit is how steep to turn if the pilot does not specify...
559
// bank_limit is how steep to turn (pilot can change this value but it defaults to default_bank_limit). 
560
set bank_limit to default_bank_limit.
561
562
set bank_fudge_factor to 3.		// we try to bank this many degrees for every degree of heading we want to change.
563
								// other constraints will stop us banking that exact ammount, this will have more
564
								// effect at very small changes in heading angle. heading angle change of 1 deg = 3deg bank.
565
566
set current_rudder to 0.
567
568
//safetey features.
569
set dynamic_brakes_enable to false.		// when ground handling, apply/release brakes to help maintain target speeds <= 15.
570
set electric_throttle_enable to false.	// kick in the throttle if ship:electric charge gets too low.
571
set wing_leveler_enable to true.		// when lower than 40m off the ground, level the wings.
572
										// this gives better climb rates, and might mean avoiding touch a wingtip on the ground.
573
set auto_spoilers_enable to true.		// set if you want spoilers to auto operate while holding a slope with hg().
574
set overspeed_spoilers_enable to false.	//	set if you want spoilers to deploy just based on over speed,
575
set auto_gear_enable to true.			// TODO: need an indicator like DBE, but... we alreayd have a gear indicator can we merge?
576
set auto_wheelsteer_enable to true.
577
set aoa_limit_enable to true.			//Limit pitch AoA to +/- default_aoa_limit from current direction of travel.
578
579
// 1 minute timer
580
on floor(time:seconds / 60) {
581
	if hud_message <> "" {
582
			hudtext(hud_message, 15, 2, 25, purple, false).
583
	}
584
	return true.
585
}
586
587
// terrain following radar - lookahead to see what the terrain is like - and peek out to the sides a bit too.
588
//Try to predict where we are heading by looking at how fast our direction of travel is changing.
589
set tft_prev_hdg to ct().
590
set tft_prev_time to time:seconds.
591
592
// hold a 2d array of vectors to draw.
593
set tfr_vecdraws to list().
594
set tfr_lookahead to 3.			// how many seconds ahead should the tfr radar look.
595
596
597
function tfr {
598
	parameter lookahead is 3.
599
	local max_ht is ship:geoposition:terrainheight.
600
	local alpha is ang(tft_prev_hdg,ct()) * 50 * groundspeed *(time:seconds - tft_prev_time ).
601
	set tft_prev_time to time:seconds.
602
	set tft_prev_hdg to ct().
603
	from { local dt is 1. } until dt > lookahead step { set dt  to dt + 1. } do {
604
		from { local base_rot is -5. } until base_rot > 5 step { set base_rot to base_rot +5. } do {
605
			local actual_rot is base_rot - alpha * dt.
606
			//local th is body:geopositionof(vxcl(ship:up,velocity:surface * dt * r(actual_rot,0,0))):terrainheight.
607
			local th is body:geopositionof(vxcl(up:vector,velocity:surface) * dt * r(actual_rot,0,0)):terrainheight.
608
			//local th is body:geopositionof(positionat(ship,time:seconds + dt )*r(actual_rot,0,0)):terrainheight.
609
			if dt -1 >= tfr_vecdraws:length {
610
print "add row = " + (dt -1).
611
				tfr_vecdraws:add(list()).
612
			}
613
			if base_rot/5 + 1 >= tfr_vecdraws[dt -1]:length {
614
print "add index = " + (dt -1) + "," + (base_rot/5 + 1).
615
				tfr_vecdraws[dt-1]:add( vecdraw(v(0,0,0),v(0,0,0),yellow,"",1,true)).
616
			}
617
			if th > max_ht { 
618
				set max_ht to th.
619
				set tfr_vecdraws[dt -1][base_rot/5 +1]:color to red.
620
			} else   {
621
				set tfr_vecdraws[dt -1][base_rot/5 +1]:color to yellow.
622
			}
623
			//set tfr_vecdraws[dt -1][base_rot /5 + 1]:vec to vxcl(ship:up,velocity:surface * dt * r(0,0,actual_rot)).
624
			set tfr_vecdraws[dt -1][base_rot /5 + 1]:vec to vxcl(up:vector,velocity:surface) * dt * r(0,0,actual_rot).
625
			tfr_vecdraws[dt -1][base_rot/5 +1]:show.
626
		}
627
	}
628
	return max_ht .
629
}.
630
631
print "DEBUG: setting up the gui".
632
set the_gui to gui(300).
633
set gui_fields to lexicon().
634
{
635
	// hide some local veriables that can be thrown away.
636
	gui_fields:add("gui",the_gui).
637
	local vl is the_gui:addvlayout().
638
	local hl1 is vl:addhlayout().
639
	hl1:addlabel("Throttle:").
640
	gui_fields:add("speed",hl1:addtextfield(tpid:setpoint:tostring)).
641
	hl1:addlabel("m/s").
642
	gui_fields:add("stall",hl1:addlabel("<color=black>Stall</color>")).
643
	local hl2 is vl:addhlayout().
644
	hl2:addlabel("Pitch:").
645
	gui_fields:add("pitch_mode",hl2:addtextfield(pitch_mode)).
646
	set gui_fields["pitch_mode"]:style:fontsize to 10.
647
	gui_fields:add("pitch_info",hl2:addtextfield(p:tostring)).
648
	set gui_fields["pitch_info"]:STYLE:HSTRETCH TO True.
649
	set gui_fields["pitch_info"]:STYLE:FONTSIZE TO 10.
650
	local hl3 is vl:addhlayout().
651
	hl3:addlabel("Heading:").
652
	gui_fields:add("heading_mode",hl3:addtextfield(hdg_mode)).
653
	set gui_fields["heading_mode"]:style:fontsize to 10.
654
	gui_fields:add("heading_info",hl3:addtextfield(hdg:tostring)).
655
	set gui_fields["heading_info"]:STYLE:HSTRETCH TO True.
656
	set gui_fields["heading_info"]:STYLE:FONTSIZE TO 10.
657
	local hl4 is vl:addhlayout().
658
	hl4:addlabel("Bank Ang:").
659
	gui_fields:add("bank",hl4:addtextfield(b:tostring)).
660
	local lab is hl4:addlabel("Limit:").
661
	set lab:style:align to "RIGHT".
662
	gui_fields:add("bank_limit",hl4:addtextfield(bank_limit:tostring)).
663
	local hl5 is vl:addhlayout().
664
	gui_fields:add("brakes", hl5:addlabel((choose "<color=green>Brakes</color>" if brakes else "<color=black>Brakes</color>"))).	
665
	gui_fields:add("gear", hl5:addlabel((choose "<color=green>Gear</color>" if gear else "<color=black>Gear</color>"))).	
666
	gui_fields:add("flaps", hl5:addlabel((choose "<color=green>Flaps</color>" if flaps else "<color=black>Flaps</color>"))).	
667
	gui_fields:add("spoilers", hl5:addlabel( (choose "<color=green>Spoilers</color>" if spoilers else "<color=black>Spoilers</color>"))).
668
	local hl6 is vl:addhlayout().
669
	gui_fields:add("revthrust",hl6:addlabel( (choose "<color=green>Reverse</color>" if revthrust else "<color=black>Reverse</color>"))).
670
	gui_fields:add("electric_throttle",hl6:addlabel( (choose "<color=green>ETE</color>" if electric_throttle_enable else "<color=black>ETE</color>"))).
671
	gui_fields:add("dynamic_brakes",hl6:addlabel( (choose "<color=green>DBE</color>" if dynamic_brakes_enable else "<color=black>DBE</color>"))).
672
	gui_fields:add("wing_leveler",hl6:addlabel( (choose "<color=green>WLE</color>" if wing_leveler_enable else "<color=black>WLE</color>"))).
673
	set the_gui:x to -100.
674
	set the_gui:y to 100.
675
	the_gui:show().
676
}
677
print "DEBUG: gui created.".
678
679
set main_loop_timestamp to 0.
680
set main_loop_updates_per_second to 0.
681
set debug_main_loop_updates to false.
682
683
// 1 second timer
684
on time:second {
685
	if debug_main_loop_updates print "Main loop updates per second: " + main_loop_updates_per_second.
686
687
	//Gear at least does not mess with stallspeeds in KSP (I think!? If so it does not have much effect).
688
	if auto_gear_enable and alt_radar() < 80 and verticalspeed < 0 if not gear gear on.
689
690
	if gui_fields["gui"]:visible {
691
		local blink is false.
692
		if mod(time:second,2) = 0 set blink to true.
693
		set gui_fields["speed"]:text to tpid:setpoint:tostring.
694
		if ship:status <> "LANDED" and (airspeed <= vs or b >= bank_stall_angle) set gui_fields["stall"]:text to "<color=" + (choose "red" if blink else "yellow") + ">Stall</color>".
695
		else set gui_fields["stall"]:text to "<color=black>Stall</color>".
696
		set gui_fields["pitch_mode"]:text to pitch_mode.
697
		set gui_fields["pitch_info"]:text to pitch_info.
698
		set gui_fields["heading_mode"]:text to hdg_mode.
699
		set gui_fields["heading_info"]:text to heading_info.
700
		//set gui_fields["bank"]:text to round(abs(90 - vang(ship:facing:starvector,up:vector)),1):tostring + " deg".
701
		set gui_fields["bank"]:text to round(vang(vcrs(up:vector,vxcl(up:vector,ship:facing:forevector):normalized):normalized,ship:facing:starvector),1):tostring + " deg".
702
		set gui_fields["bank_limit"]:text to bank_limit:tostring + " deg".
703
		if brakes set gui_fields["brakes"]:text to "<color=green>Brakes</color>".
704
		else set gui_fields["brakes"]:text to "<color=black>Brakes</color>".
705
		if gear {
706
			if defined vlo and  airspeed > vlo set gui_fields["gear"]:text to "<color=" + (choose "red" if blink else "yellow") + ">Gear</color>".
707
			else set gui_fields["gear"]:text to "<color=green>Gear</color>".
708
		} else {
709
			set gui_fields["gear"]:text to "<color=black>Gear</color>".
710
		}
711
		if flaps {
712
			if defined vfe and airspeed > vfe set gui_fields["flaps"]:text to "<color=" + (choose "red" if blink else "yellow") + ">Flaps</color>".
713
			else set gui_fields["flaps"]:text to "<color=green>Flaps</color>".
714
		} else {
715
			set gui_fields["flaps"]:text to "<color=black>Flaps</color>".
716
		}
717
		if spoilers set gui_fields["spoilers"]:text to "<color=green>Spoilers</color>".
718
		else set gui_fields["spoilers"]:text to "<color=black>Spoilers</color>".
719
		if revthrust set gui_fields["revthrust"]:text to "<color=green>Reverse</color>".
720
		else set gui_fields["revthrust"]:text to "<color=black>Reverse</color>".
721
722
		if electric_throttle_enable set gui_fields["electric_throttle"]:text to "<color=green>ETE</color>".
723
		else set gui_fields["electric_throttle"]:text to "<color=black>ETE</color>".
724
		if dynamic_brakes_enable set gui_fields["dynamic_brakes"]:text to "<color=green>DBE</color>".
725
		else set gui_fields["dynamic_brakes"]:text to "<color=black>DBE</color>".
726
		if wing_leveler_enable set gui_fields["wing_leveler"]:text to "<color=green>WLE</color>".
727
		else set gui_fields["wing_leveler"]:text to "<color=black>WLE</color>".
728
	}
729
730
	if main_loop_timestamp > 0 and main_loop_timestamp +5 < time:seconds {
731
		print "MAIN LOOP IS DEAD".
732
	}.
733
	return true.
734
}.
735
736
set circle_radius to 1250.
737
set slip_bank_angle to -20.
738
set runway_heading to 0.
739
set pitch_aoa to 0.
740
set side_slip to false.
741
set side_slip_enable to false.
742
743
//set runway_vecdraw to vecdraw(v(0,0,0),v(0,0,0),yellow,"",1,true,0.2,true,false).
744
//set touchdown_vecdraw to vecdraw(v(0,0,0),v(0,0,0),yellow,"",1,true,0.2,false,false).
745
//set touchdown_vecdraw:pointy to false.
746
//set glideslope_vecdraw to vecdraw(v(0,0,0),v(0,0,0),green,"",1,true,0.2,false,false).
747
//set glideslope_vecdraw:pointy to false.
748
749
//	set previous to v(0,0,0).
750
//	for i in range(0,trajectory:length) {
751
//		set trajectory[i]:start to previous:vec.
752
//		set trajectory[i]:vec to angleaxis( (i+0.5) * turn_rate,up:vector) * (my_horizon_forward:normalized * chord_length).
753
//		set previous to previous:vec + trajectory[i]:vec.
754
//	}
755
756
757
set draw_the_runway to false.
758
759
//slope_correction_factor is how aggressively to try to correct slope errors.
760
//at 0.8, the new slope we want to fly is taking us to the correct height, (1-0.8) = 20% of the way to the target point.
761
//at 0.5 we are trying to get to the correct height by the time we are half way to the target point.
762
set slope_correction_factor to 0.75.
763
764
// Fast timer
765
when true then {
766
	// get these calculations out of the main loop... if you want to change them call setup_runway() !
767
	//if defined(runway) and runway["enabled"] {
768
	//	set runway_normalized to ( runway_end:position - runway_start:position ):normalized.
769
	//	set runway_touchdown to body:geopositionof(runway_start:position + runway_normalized * runway_touchdown_distance).
770
	//	set runway_final to body:geopositionof(runway_normalized * -runway_final_distance + runway_start:position).
771
	//}
772
	if draw_the_runway {
773
		if runway["enabled"] {
774
			set runway_vecdraw:start to runway["start"]:position + up:vector * 3.
775
			set runway_vecdraw:vec to runway["end"]:position - runway["start"]:position - up:vector * runway["start"]:terrainheight + up:vector * runway["end"]:terrainheight.
776
			set touchdown_vecdraw:start to runway["touchdown"]:position + runway["normalized"] * 25 * angleaxis(-90,up:vector) + up:vector * 3.
777
			set touchdown_vecdraw:vec to runway["normalized"] * 50 *angleaxis(90,up:vector).
778
			set glideslope_vecdraw:start to runway["touchdown"]:position.
779
			set glideslope_vecdraw:vec to vxcl(up:vector,runway["normalized"]):normalized * angleaxis(180+runway["slope"] , touchdown_vecdraw:vec) * ((runway["final"]:position - runway["start"]:position):mag / cos(runway["slope"] )).
780
			if not(runway_vecdraw:show) or not(touchdown_vecdraw:show) or not(glideslope_vecdraw:show) {
781
				set runway_vecdraw:show to true.
782
				set touchdown_vecdraw:show to true.
783
				set glideslope_vecdraw:show to true.
784
			}
785
		}
786
	} else {
787
		//set runway_vecdraw:show to false.
788
		//set touchdown_vecdraw:show to false.
789
		//set glideslope_vecdraw:show to false.
790
	}
791
792
	set main_loop_updates_per_second to 1/(time:seconds - main_loop_timestamp).
793
	set main_loop_timestamp to time:seconds.
794
	if pitch_mode = "vert" {
795
		// pilot has asked to try to maintain climb/sink rate
796
		if aoa_limit_enable {
797
			local curr_pitch is 90 - vang(up:vector,ship:velocity:surface).
798
			set vpid:minoutput to curr_pitch - default_aoa_limit.
799
			set vpid:maxoutput to curr_pitch + default_aoa_limit.
800
		} else {
801
			set vpid:minoutput to -45.
802
			set vpid:maxoutput to 45.
803
		}
804
		set p to vpid:update(time:seconds,verticalspeed).
805
	} else if pitch_mode = "aoa" {
806
		//maintain an angle of attack.
807
		set p to 90 - vang(up:vector,ship:velocity:surface) + pitch_aoa.
808
	} else if pitch_mode = "glide" {
809
		of aoa_limit_enable {
810
			local curr_pitch is 90 - vang(up:vector,ship:velocity:surface).
811
			set gpid:minoutput to curr_pitch - default_aoa_limit.
812
			set gpid:maxoutput to curr_pitch + default_aoa_limit.
813
		} else {
814
			set gpid:minoutput to -45.
815
			set gpid:maxoutput to 45.
816
		}
817
		set p to -gpid:update(time:seconds,airspeed).
818
	} else if pitch_mode = "alt" {
819
		// pilot has asked to try to maintain altitude
820
		if aoa_limit_enable {
821
			local curr_pitch is 90 - vang(up:vector,ship:velocity:surface).
822
			set apid:minoutput to curr_pitch - default_aoa_limit.
823
			set apid:maxoutput to curr_pitch + default_aoa_limit.
824
		} else {
825
			set apid:minoutput to -15.
826
			set apid:maxoutput to 45.
827
		}
828
		set p to apid:update(time:seconds,altitude).
829
	} else if pitch_mode = "radar" {
830
		// pilot has ask to to fly NOE
831
//my current thinking is to NOT follow aoa limits, since in KSP we have reaction wheels to easily enable stall recover
832
//and landing gear in KSP has the ability to absorb huge impacts, and getting the gear into position is more important
833
//than avoiding the stall if you are that close to the ground.
834
		set p to rpid:update(time:seconds,altitude - tfr(tfr_lookahead)).
835
	} else if pitch_mode = "slope" {
836
		if aoa_limit_enable {
837
			local curr_pitch is 90 - vang(up:vector,ship:velocity:surface).
838
			set vpid:minoutput to curr_pitch - default_aoa_limit.
839
			set vpid:maxoutput to curr_pitch + default_aoa_limit.
840
		} else {
841
			set vpid:minoutput to -45.
842
			set vpid:maxoutput to 45.
843
		}
844
		if runway["final"]:altitudeposition(altitude):mag <1000 and airspeed > (vc + vt)/2 hs((vc + vt)/2).
845
		if runway["start"]:altitudeposition(altitude):mag < 1000 and airspeed > vt hs(vt).
846
		if runway["start"]:altitudeposition(altitude):mag < 100 or runway["touchdown"]:altitudeposition(altitude):mag < 100 and airspeed > vtd hs(vtd).
847
		if alt_radar < ship:mass * -verticalspeed / flare_factor {
848
print "DEBUG: Flaring!".
849
			set pitch_mode to "flare".
850
			set p to p + 3.
851
			flaps off.
852
			spoilers on.
853
if debug print "flare" + char(7).
854
		} else {
855
			if debug print "A_R= " + round(alt_radar,1) + " VS=" + round(verticalspeed,1) + "  TRIGGER=" + round(ship:mass * -verticalspeed / flare_factor,1).
856
			local actual_dist is (slope_target:position - ship:geoposition:position):mag + slope_distance_adj.
857
//Trying new calculation to get the horizontal distance to touchdown target.
858
			local actual_dist is slope_target:altitudeposition(altitude):mag + slope_distance_adj.
859
			local actual_vert is altitude - slope_target:terrainheight - slope_height_adj.
860
if actual_dist = 0 {
861
	print "DEBUG: divide by zero due to actual_dist = 0".
862
}
863
			local actual_slope is arctan(actual_vert / actual_dist).
864
			local new_dist is actual_dist * (1 - slope_correction_factor).
865
			local new_vert is altitude - slope_target:terrainheight - slope_height_adj - ( actual_dist * tan(runway["slope"] )*slope_correction_factor).
866
if airspeed = 0 {
867
	print "DEBUG: divide by zero due to airspeed = 0".
868
}
869
			set vpid:setpoint to ((-new_vert)/ (new_dist / airspeed)).
870
			set p to vpid:update(time:seconds,verticalspeed).
871
if new_dist = 0 {
872
	print "DEBUG: divide by zero due to new_dist = 0".
873
}
874
if slope_debug print "SL TGT: " + round(runway["slope"] ,1) + "  ACT: " + round(actual_slope,1) + " NEW: " + round(arctan(new_vert/new_dist),1) + " VS TG=" + round(vpid:setpoint,1).
875
			if tpid:setpoint * 1.1 < airspeed {
876
				if overspeed_spoilers_enable spoilers on.
877
			} else if tpid:setpoint * 1.05 < airspeed and actual_slope > runway["slope"] {
878
				if auto_spoilers_enable spoilers on.
879
			} else {
880
				if (overspeed_spoilers_enable or auto_spoilers_enable) spoilers off.
881
			}.
882
			if side_slip_enable {
883
				if tpid:setpoint * 1.1 < airspeed and (actual_slope + 2) > runway["slope"] and alt_radar > 200 set side_slip to true.
884
				else set side_slip to false.
885
			}
886-
//print "DEBUG: not applying brakes in the main loop".
886+
887
	} else if pitch_mode = "flare" {
888
		set pitch_info to "landing flare".
889
		if ship:status = "landed" {
890
			if flaps flaps off.
891
			if not spoilers spoilers on.
892
			set pitch_info to round(p,1) + " deg".
893
			set gui_fields["pitch_info"]:text to pitch_info.
894
			hs(0).
895
hp(0).
896
			set pitch_mode to "landed".
897
if debug print "landed".
898
		}
899
		//if verticalspeed > -0.25 set p to p -1.
900
		//if verticalspeed < -0.75 set p to p + 1.
901
		if tpid:setpoint > vtd hs(vtd).
902
		//set vpid:setpoint to -alt_radar /10.
903
		set pitch_info to round(p,1) + " degrees".
904
	} else if pitch_mode = "landed" {
905
		set pitch_info to "landed".
906
print "DEBUG: not applying brakes in the main loop".
907
		//if not brakes brakes on.
908
		if groundspeed < 0.1 {
909
			hp(0).
910
		}
911
	}  // else pitch_mode = "fixed" => just leave the pitch alone.
912
	if hdg_mode = "circle" {
913
		//TODO: support circle_target:istype("Vessel")...
914
		if circle_target:altitudeposition(altitude):mag > circle_radius {
915
			local alpha is arcsin( circle_radius / circle_target:altitudeposition(altitude):mag ).
916
			set hdg to circle_target:heading + (choose alpha if circle_dir = -1 else -alpha).
917
		} else {
918
			set hdg to circle_target:heading + 92 * -circle_dir.
919
		}
920
	} else if hdg_mode = "target" {
921
		if hdg_target:altitudeposition(altitude):mag < (2 * groundspeed) and not(hdg_target:istype("Vessel")) {
922
			// if we get "close", just keep flying that heading TODO: this should be more rigorous shouldn't it?
923
			//unless it is a vessel... then we just keep heading towards it.
924
			set hdg_mode to "heading".
925
			set heading_info to round(hdg,1):tostring + " degrees".
926
		}     else {
927
			set hdg to hdg_target:heading.
928
		}
929
	} else if hdg_mode = "lineup" {
930
		// try a multi-stage linup method
931
		set runway_heading to remote_heading(runway["start"],runway["end"]).
932
933
		if runway["touchdown"]:altitudeposition(altitude):mag < 150 {
934
			// just fly the runway heading now. Perhaps we are doing a touch-n-go or a go-around.
935
			set hdg to runway["end"]:heading.
936
			set hdg_mode to "fixed".
937
		} else if runway["start"]:altitudeposition(altitude):mag < 300 or abs(ang(runway["start"]:heading,ct())) > 90 {
938
			//continue lineup but now target runway["touchdown"].
939
			set hdg to (mod(360 + runway["touchdown"]:heading + ang(runway["touchdown"]:heading,runway_heading) ,360)).
940
		} else if runway["final"]:altitudeposition(altitude):mag < 600 or abs(ang(runway["final"]:heading,ct())) > 90 {
941
			//continue lineup but now target runway["start"].
942
			set hdg to (mod(360 + runway["start"]:heading + ang(runway["start"]:heading,runway_heading) ,360)).
943
		} else {
944
			// aim to lineup before runway["final"]
945
			set hdg to (mod(360 + runway["final"]:heading + ang(runway["final"]:heading,runway_heading) ,360)).
946
		}
947
	}
948
	if side_slip {
949
		set old_b to b.
950
		if alt_radar < 50 {
951
			set side_slip_enable to false.
952
			set side_slip to false.
953
		}
954
		if alt_radar < 100 {
955
			set b to constrain(slip_bank_angle/2,old_b -3, old_b + 3).
956
		} else {
957
			set b to constrain(slip_bank_angle,old_b -3, old_b + 3).
958
		}
959
		set x to ct() + spid:update(time:seconds,ang(hdg,ct())).
960
	} else 
961
	if hdg_mode = "fixed" or hdg_mode = "target" or hdg_mode = "circle" or hdg_mode = "lineup" {
962
		if ship:status = "PRELAUNCH" or ship:status = "LANDED" {
963
			set b to 0.
964
			set x to hdg.
965
			//still on the ground - try to steer, but as speed increases do a LOT less wheelsteering.
966
if auto_wheelsteer_enable {
967
			if abs(ang(hdg,cf())) < 1 {
968
				set ship:control:wheelsteer to 0.
969
				set using_wheelsteer to false.
970
			} else     {
971
				set using_wheelsteer to true.
972
				set ship:control:wheelsteer to constrain(
973
					constrain(
974
						-ang(hdg,cf())/10,
975
						-100/(groundspeed+1)^2,
976
						100/(groundspeed+1)^2
977
					),
978
					-1,
979
					1
980
				).
981
			}.
982
}.
983
			// we might be using throttle to maintain ship:electriccharge... so using brakes on the ground could be really important.
984
			if dynamic_brakes_enable {
985
				if tpid:setpoint < 0.1 {
986
					brakes on.
987
				} else {
988
					if groundspeed > tpid:setpoint * 1.1 brakes on.
989
					if groundspeed < tpid:setpoint  brakes off.
990
				}
991
			}
992
		}     else     {
993
			//in the air
994
			//if brakes brakes off.
995
			if using_wheelsteer {
996
				set ship:control:wheelsteer to 0.
997
				set using_wheelsteer to false.
998
			}.
999
			local bank_dir is 0.
1000
			if hdg_mode = "circle" {
1001
				set bank_dir to circle_dir.
1002
				set turn_force_dir to 90.
1003
			}
1004
			if hdg_mode = "fixed" set bank_dir to turn_dir.
1005
1006
			// force a turn in a particular direction if the angle is over turn_force_dir degrees.
1007
			// otherwise turn which ever way is smaller.
1008
			if abs(ang(hdg,ct())) < turn_force_dir set bank_dir to 0.
1009
			local hdg_error is ang(hdg,ct()).
1010
			// force us to turn the other way by faking how much we need to turn :-/
1011
			if bank_dir = -1 and hdg_error > 0 {
1012
				set hdg_error to hdg_error - 180.
1013
			} else if bank_dir = 1 and hdg_error < 0 {
1014
				set hdg_error to hdg_error + 180.
1015
			}
1016
1017
			// don't bank so much you stall,
1018
			// don't exceed normal bank limits,
1019
			// don't change bank too rapidly,
1020
			set b to constrain(
1021
				-hdg_error * bank_fudge_factor,
1022
				max(-bank_stall_angle,max(-bank_limit,b - bank_change_rate)),
1023
				min(bank_stall_angle,min(bank_limit,b + bank_change_rate))
1024
			).
1025
			set target_rudder to rudder_rate * -b.
1026
			if wing_leveler_enable {
1027
				// don't bank steeply close to the ground.
1028
				set b to constrain(b,-alt_radar(), alt_radar()).
1029
			}
1030
			set current_rudder to constrain(target_rudder,current_rudder -1, current_rudder + 1).
1031
			set x to mod(720 + ct() + current_rudder,360).
1032
		}
1033
	}
1034
	return true.
1035
}
1036
1037
// move the runway to left (-ve) or right (+ve), x meters.
1038
function rs {
1039
	parameter dist is 0.
1040
print  char(7)+"ERROR: rs() is unavailable.".
1041
return.
1042
	local offset is runway_normalized * dist * angleaxis(90,up:vector).
1043
	set runway_start to body:geopositionof( runway_start:position + offset).
1044
	set runway_end to body:geopositionof(runway_end:position + offset).
1045
}
1046
//move the runway d meters further away. (closer if d is negative).
1047
function rd {
1048
	parameter dist is 0.
1049
print  char(7)+"ERROR: rd() is unavailable.".
1050
return.
1051
	local offset is runway_normalized * dist.
1052
	set runway_start to body:geopositionof( runway_start:position + offset).
1053
	set runway_end to body:geopositionof(runway_end:position + offset).
1054
}
1055
1056
//rotate the runway thru angle rot_angle around the starting point.
1057
function ra {
1058
	parameter rot_angle is 0.
1059
print  char(7)+"ERROR: ra() is unavailable.".
1060
return.
1061
	set runway_end to body:geopositionof(
1062
		runway_start:position + (runway_end:position - runway_start:position) * angleaxis(rot_angle,up:vector)
1063
	).
1064
}
1065
1066
function rf {
1067
	local temp is runway_end.
1068
print  char(7)+"ERROR: rf() is unavailable.".
1069
return.
1070
	set runway_end to runway_start.
1071
	set runway_start to temp.
1072
}
1073
1074
// begin decending at slope "slope", once we are, work out where we will land, and create a "runway" there.
1075
function land_here {
1076
	parameter slope is 4.
1077
	hv(sin(slope) * airspeed).
1078
}
1079
1080
set valid_dirs to list("left","right","either").
1081
// try to hold a circle - if not landmark to reference is given, circle the current location.
1082
//if "either" direction is given (or no direction given) then a direction has to be determined here.
1083
function circle {
1084
	parameter ctgt is ship:geoposition.
1085
	parameter radius is 1000.
1086
	parameter dir is "either".
1087
	if not (ctgt:hassuffix("distance") and ctgt:hassuffix("heading")) and not places:haskey(ctgt) {
1088
		print char(7) + "ERROR: cannot circle - unrecognised target".
1089
		return.
1090
	}
1091
	if valid_dirs:find(dir) = -1 {
1092
		print char(7) + "ERROR: cannot circle - unrecognised direction".
1093
		return.
1094
	}
1095
	if places:haskey(ctgt) {
1096
		set heading_info to ctgt + " radius=" + round(circle_radius) + "m ".
1097
		set ctgt to places[ctgt].
1098
	} else {
1099
		set heading_info to "LAT:" + round(ctgt:lat,3) + " LNG:" + round(ctgt:lng,3).
1100
	}
1101
	set hdg_mode to "circle".
1102
	set circle_target to ctgt.
1103
	set circle_radius to radius.
1104
1105
	if dir = "either" {
1106
		if circle_target:altitudeposition(altitude):mag > circle_radius {
1107
			set circle_dir to (choose 1 if ang(circle_target:heading,ct()) > 0 else -1).
1108
		} else {
1109
			set circle_dir to (choose 1 if abs(ang(circle_target:heading -90 ,ct())) < 90 else -1).
1110
		}
1111
	} else {
1112
		set circle_dir to (choose -1 if dir = "left" else 1 ).
1113
	}
1114
	set heading_info to heading_info + (choose "left" if circle_dir  < 0 else "right").
1115
}.
1116
1117
1118
// provide target heading, a direction to force a turn in, and a limit of how far off a heading must be before forced dir will be used.
1119
// i.e. if the current heading is 45, the new target heading is 50 and the force_dir is left, and force_angle is 10 we will turn right.
1120
// be careful setting small force angles, any perturbation of the heading could result in complete 360degree turns.
1121
set force_dirs to list("closest","left","right").
1122
function tt {
1123
	parameter ttgt.
1124
	parameter force_dir is "closest".
1125
	parameter force_angle is 45.
1126
	if force_dirs:find(force_dir) = -1 {
1127
		print char(7) + "ERROR: " + force_dir + " is not a recognised direction".
1128
	}       else        {
1129
		set turn_force_dir to force_angle.
1130
		if force_dir = "closest" or abs(ang(ttgt,ct())) < force_angle {
1131
			set hdg_mode to "fixed".
1132
			set hdg to ttgt.
1133
			set turn_dir to 0.
1134
		} else if force_dir = "left" {
1135
			set hdg_mode to "fixed".
1136
			set hdg to ttgt.
1137
			set turn_dir to -1.
1138
		} else if force_dir = "right" {
1139
			set hdg_mode to "fixed".
1140
			set hdg to ttgt.
1141
			set turn_dir to 1.
1142
		}
1143
		set heading_info to hdg:tostring + " deg".
1144
	}
1145
	return true.
1146
}.
1147
1148
//pilot is requesting to fly to a particular location.
1149
function goto {
1150
	parameter where.
1151
	if where:istype("String") and places:haskey(where) {
1152
		set hdg_mode to "target".
1153
		set hdg_target to places[where].
1154
		set heading_info to where.
1155
	} else if where:istype("GeoCoordinates") {
1156
		set hdg_mode to "target".
1157
		set hdg_target to where.
1158
		set heading_info to "LAT:" + round(where:lat,3) + " LNG:" + round(where:lng,3).
1159
	} else if where:istype("Vessel") {
1160
		set heading_info to "TGT:" + where:name.
1161
		set hdg_mode to "target".
1162
		set hdg_target to where.
1163
	} else {
1164
		print "Sorry, I don't know where " + where + " is.".
1165
	}
1166
	return true.
1167
}.
1168
1169
// pilot is requesting to fly NOE at a certain height off the ground.
1170
function hr {
1171
	parameter rtgt.
1172
	if pitch_mode <> "radar" {
1173
		rpid:reset().
1174
		set pitch_mode to "radar".
1175
	}
1176
	set rpid:setpoint to rtgt.
1177
	set pitch_info to "NOE " + round(rtgt,1) + " m".
1178
	return true.
1179
}.
1180
1181
//pilot is requesting to hold a particular speed - if we are using throttle to maintain electric charge,
1182
//then when electriccharge is low, kick in enough throttle to maintain some charge!
1183
//in the air electric_throttle_enable might not be noticed, but on the ground a small change in throttle could
1184
//easily cause problems... so might be a good idea to also set dynamic_brakes_enable to true to prevent overspeeds.
1185
function hs {
1186
	parameter stgt is 0.
1187
	if stgt = "" {
1188
		unlock throttle.
1189
	} else {
1190
		set tpid:setpoint to stgt.
1191
		lock throttle to max(tpid:update(time:seconds,airspeed),choose 100 - ship:electriccharge if electric_throttle_enable else 0).
1192
	}
1193
	return true.
1194
}.
1195
1196
//pilot has requested to hold a particular pitch
1197
function hp {
1198
	parameter ptgt.
1199
	set pitch_mode to "fixed".
1200
	set p to ptgt.
1201
	set pitch_info to round(ptgt,1) + " deg".
1202
	return true.
1203
}
1204
1205
//pilot is requesting to hold a particular climb/sink rate 
1206
function hv {
1207
	parameter vtgt.
1208
	if pitch_mode <> "vert" and pitch_mode <> "slope" {
1209
		vpid:reset().
1210
	}
1211
	set pitch_mode to "vert".
1212
	set vpid:setpoint to vtgt.
1213
	if vtgt < 0 {
1214
		set pitch_info to "Descend " + round(vtgt,1) + " m/s".
1215
	} else if vtgt > 0 {
1216
		set pitch_info to "Climb " + round(vtgt,1) + " m/s".
1217
	} else {
1218
		set pitch_info to "Level Flight".
1219
	}.
1220
	return true.
1221
}.
1222
1223
//hold glideslope.
1224
function hg {
1225
	parameter slope is default_glideslope.
1226
	parameter gstgt is (choose runway["touchdown"] if runway["enabled"] else "").
1227
	parameter ht is 0.
1228
	parameter dist is 0.
1229
1230
	set runway["slope"] to slope.
1231
	set slope_target to gstgt.
1232
	set slope_height_adj to ht.
1233
	set slope_distance_adj to dist.
1234
	if pitch_mode <> "vert" and pitch_mode <> "slope" {
1235
		vpid:reset().
1236
	}
1237
	//main flight loop will calculate the vertspd to fly and set p accordingly.
1238
	set pitch_mode to "slope".
1239
	set pitch_info to round(slope,1) + " deg slope".
1240
	return true.
1241
}
1242
1243
1244
function hl {
1245
	parameter rw_name.
1246
	parameter the_final_distance is default_runway_final.
1247
	parameter the_touchdown_distance is default_runway_touchdown.
1248
	parameter the_turn_radius is default_runway_turn.
1249
1250
	local opts is lexicon("final",the_final_distance,"touchdown",the_touchdown_distance,"turn",the_turn_radius).
1251
1252
	if rw_name:istype("String") and places:haskey(rw_name) and places:haskey(recip(rw_name)) {
1253
		setup_runway(rw_name,opts).
1254
		set heading_info to "Runway " + runway["name"].
1255
		set hdg_mode to "lineup".
1256
	} else if rw_name:istype("List") and rw_name:length = 2 and rw_name[0]:istype("GeoCoordinates") and rw_name[1]:istype("GeoCoordinates") {
1257
		setup_runway(lexicon("start",rw_name[0],"end",rw_name[1]), opts).
1258
		set heading_info to "Runway " + runway["name"].
1259
		set hdg_mode to "lineup".
1260
	} else {
1261
		print char(7) + "ERROR: runway incorrectly specified".
1262
	}
1263
}
1264
1265
function make_runway {
1266
	parameter slope is 3.
1267
1268
	print char(7) + "ERROR: make_runway is not available.".
1269
	return.
1270
1271
	local sink is sin(slope) * airspeed.
1272
	local rw_start is ship:geoposition.
1273
	local projected_alt is altitude.
1274
	until projected_alt < rw_start:terrainheight {
1275
		set rw_start to body:geopositionof(rw_start:position + vxcl(up:vector,ship:velocity:surface)).
1276
		set projected_alt to projected_alt - sink.
1277
	}.
1278
	set rw_end to body:geopositionof(rw_start:position + rw_start:position:normalized * 2400).
1279
	setup_runway(lexicon("start",rw_start,"end",rw_end),lexicon("slope",slope)).
1280
	set draw_the_runway to true.
1281
}
1282
1283
1284
//pilot is requesting to hold a particular altitude. 
1285
function ha {
1286
	parameter atgt.
1287
	if pitch_mode <> "alt" {
1288
		apid:reset().
1289
		set pitch_mode to "alt".
1290
	}
1291
	set apid:setpoint to atgt.
1292
	set pitch_info to "" + round(atgt,1) + " m".
1293
}.
1294
1295
function haoa {
1296
	parameter tgt.
1297
	set pitch_mode to "aoa".
1298
	set pitch_aoa to tgt.
1299
	set pitch_info to "" + round(tgt,1) + " deg".
1300
}
1301
1302
function hm {
1303
	parameter spd is vs.
1304
	set gpid:setpoint to spd.
1305
	if pitch_mode <> "glide" gpid:reset().
1306
	set pitch_mode to "glide".
1307
	set pitch_info to "Speed: " + spd.
1308
}
1309
1310
function takeoff_and_land {
1311
	set bank_limit to 30.
1312
	stage.
1313
	brakes off.
1314
	hs(100).
1315
	hv(10).
1316
	set flaps to 0.5.
1317
	tt(runway["end"]:heading).
1318
	when altitude > 100 then hv(9).
1319
	when altitude > 120 then hv(8).
1320
	when altitude > 160 then hv(7).
1321
	when altitude > 200 then hv(6).
1322
	when altitude > 240 then hv(5).
1323
	when altitude > 280 then hv(4).
1324
	when altitude > 310 then hv(3).
1325
	when altitude > 330 then hv(2).
1326
	when altitude > 340 then hv(1).
1327
	when altitude > 345 then ha(350).
1328
	when alt:radar > 25 then gear off.
1329
	when airspeed > 60 then flaps off.
1330
	when airspeed > 75 then {
1331
		tt(275).
1332
		when abs(ang(ct(),275)) < 10 and abs(ang(ct(),runway["start"]:heading)) > 135 then {
1333
			hl(runway["name"]).
1334
			hg(4,runway["touchdown"]).
1335
			when runway["start"]:distance < 2000 then {
1336
				flaps on.
1337
				hs(vt + 10).
1338-
if exists("0:/Kadins_local_KSP.txt") {
1338+
1339-
	if runway["enabled"] {
1339+
1340-
		print "Taking off and landing on runway " + runway["name"].
1340+
1341-
		takeoff_and_land().
1341+
1342
				revthrust on.
1343-
		print "Cannot take off an land - no runway detected".
1343+
1344
				flaps off.
1345-
	print "Toggle RCS to disable autopilot.".
1345+
1346-
	rcs off.
1346+
1347-
	wait until rcs.
1347+
1348-
	rcs off.
1348+
1349-
	clearguis().
1349+
1350-
	clearvecdraws().
1350+
1351
					brakes on.
1352
				}.
1353
			}.
1354
		}.
1355
	}.
1356
}.
1357
1358