SHOW:
|
|
- or go back to the newest paste.
1 | // assumes stable and circular (ap - pe < 500) orbit | |
2 | // set guidanceactive to false and vacLocLanderFinished to true, if you want to take over during landing | |
3 | ||
4 | ||
5 | if not (defined tgt) { | |
6 | if hastarget AND target:body = body AND (target:status = "landed" OR target:status = "splashed") { | |
7 | set tgt to target:geoposition. | |
8 | } | |
9 | else set tgt to ship:geoposition. | |
10 | } | |
11 | ||
12 | if not (defined terrainMargin) set terrainMargin to 1. | |
13 | if not (defined minApproachAngle) set minApproachAngle to 0. | |
14 | if not (defined terrRes) set terrRes to 30. | |
15 | if not (defined precision) set precision to terrainMargin/20. | |
16 | if not (defined minPeStep) set minPeStep to 1. | |
17 | ||
18 | set phases to list("INIT", "PLANNING", "SYNCRONISATION", "DEORBIT", "STAGING", "COASTING", "ACTIVE_GUIDANCE", "LANDING", "GUIDANCE_RELEASED", "TOUCHDOWN", "LANDED", "STOPPED"). | |
19 | ||
20 | ||
21 | //user callable: | |
22 | function land { | |
23 | parameter minangle is 0. | |
24 | //lands at tgt | |
25 | ||
26 | - | if apoapsis - periapsis > 500 { |
26 | + | if obt:eccentricity > 0.006 { |
27 | print "Orbit is not circular enough.". | |
28 | print "aborting". | |
29 | return false. | |
30 | } | |
31 | ||
32 | if not (defined lowestSafeAltitude) set lowestSafeAltitude to periapsis - 100. | |
33 | set deorbitAltitude to periapsis + (apoapsis - periapsis)/2. | |
34 | ||
35 | set phase to "". | |
36 | on phase {print "Phase: " + phase. return (phase <> "LANDED" AND phase <> "STOPPED").}. | |
37 | ||
38 | engage(minangle). | |
39 | }. | |
40 | ||
41 | function launch { | |
42 | parameter _targetAltitude. | |
43 | parameter minangle is 0. | |
44 | ||
45 | if status <> "landed" { | |
46 | print "can only launch if landed". | |
47 | print "aborting". | |
48 | return false. | |
49 | } | |
50 | ||
51 | set tgt to ship:geoposition. | |
52 | if not (defined lowestSafeAltitude) OR lowestSafeAltitude >= (_targetAltitude - 100) set lowestSafeAltitude to _targetAltitude - 100. | |
53 | if not (defined thrSmoothing) set thrSmoothing to 100. | |
54 | set ascentAngle to calculateAscent(_targetAltitude). | |
55 | ||
56 | lock rawvertacc to ship:maxthrust/ship:mass * sin(ascentAngle). | |
57 | lock desvertacc to (choose getGravAcc(altitude) if (90 - vang(up:vector, velocity:orbit) > ascentAngle) else (rawvertacc + getGravAcc(altitude))). | |
58 | lock lp to max(minangle, arcsin(desvertacc/(ship:maxthrust/ship:mass))). | |
59 | ||
60 | on throttle lock steering to heading(90, lp). | |
61 | on status gear off. | |
62 | lock thr to (_targetAltitude - apoapsis)/1000 + 1e-3. | |
63 | for i in range(thrSmoothing) {lock throttle to min(i/thrSmoothing, thr). wait 0.01.}. | |
64 | lock throttle to thr. | |
65 | when apoapsis >= _targetAltitude then { | |
66 | lock throttle to 0. | |
67 | set steer to velocityat(ship, time:seconds + eta:apoapsis):orbit. | |
68 | lock steering to steer. | |
69 | print "at ap in " + round(eta:apoapsis) + " seconds". | |
70 | print "launch finished". | |
71 | } | |
72 | }. | |
73 | ||
74 | function touchandgo { | |
75 | parameter _targetAltitude is periapsis. | |
76 | parameter minangle is 0. | |
77 | ||
78 | fillOptionalParams(). | |
79 | ||
80 | if not (defined lowestSafeAltitude) OR lowestSafeAltitude >= (_targetAltitude - 100) set lowestSafeAltitude to _targetAltitude - 100. | |
81 | if not (defined thrSmoothing) set thrSmoothing to 100. | |
82 | ||
83 | set vacLocLanderFinished to false. | |
84 | set ascentAngle to calculateAscent(_targetAltitude). | |
85 | ||
86 | when vacLocLanderFinished then { | |
87 | lock rawvertacc to ship:maxthrust/ship:mass * sin(ascentAngle). | |
88 | lock desvertacc to (choose getGravAcc(altitude) if (90 - vang(up:vector, velocity:orbit) > ascentAngle) else (rawvertacc + getGravAcc(altitude))). | |
89 | lock lp to max(minangle, arcsin(desvertacc/(ship:maxthrust/ship:mass))). | |
90 | ||
91 | lock steering to heading(90, lp). | |
92 | lock thr to (_targetAltitude - apoapsis)/1000 + 1e-3. | |
93 | for i in range(thrSmoothing) {lock throttle to min(i/thrSmoothing, thr). wait 0.01.}. | |
94 | lock throttle to thr. | |
95 | when apoapsis >= _targetAltitude then { | |
96 | lock throttle to 0. | |
97 | set steer to velocityat(ship, time:seconds + eta:apoapsis):orbit. | |
98 | lock steering to steer. | |
99 | print "at ap in " + round(eta:apoapsis) + " seconds". | |
100 | print "launch finished". | |
101 | } | |
102 | } | |
103 | ||
104 | set landedSettleTime to 0. | |
105 | land(minangle). | |
106 | } | |
107 | ||
108 | function grabandgo { | |
109 | parameter _targetAltitude is periapsis. | |
110 | parameter minangle is 0. | |
111 | ||
112 | fillOptionalParams(). | |
113 | ||
114 | if not (defined lowestSafeAltitude) OR lowestSafeAltitude >= (_targetAltitude - 100) set lowestSafeAltitude to _targetAltitude - 100. | |
115 | if not (defined thrSmoothing) set thrSmoothing to 50. | |
116 | ||
117 | set vacLocLanderFinished to false. | |
118 | set ascentAngle to calculateAscent(_targetAltitude). | |
119 | ||
120 | set iParts to ship:parts:length. | |
121 | list engines in ieng. | |
122 | ||
123 | when ship:parts:length > iParts then { | |
124 | for e in ship:engines if not ieng:contains(e) e:shutdown(). | |
125 | lock rawvertacc to ship:maxthrust/ship:mass * sin(ascentAngle). | |
126 | lock desvertacc to (choose getGravAcc(altitude) if (90 - vang(up:vector, velocity:orbit) > ascentAngle) else (rawvertacc + getGravAcc(altitude))). | |
127 | lock lp to max(minangle, arcsin(desvertacc/(ship:maxthrust/ship:mass))). | |
128 | ||
129 | lock steering to heading(90, lp). | |
130 | lock thr to (_targetAltitude - apoapsis)/1000 + 1e-3. | |
131 | for i in range(thrSmoothing) {lock throttle to min(i/thrSmoothing, thr). wait 0.01.}. | |
132 | gear off. | |
133 | lock throttle to thr. | |
134 | when apoapsis >= _targetAltitude then { | |
135 | lock throttle to 0. | |
136 | set steer to velocityat(ship, time:seconds + eta:apoapsis):orbit. | |
137 | lock steering to steer. | |
138 | print "at ap in " + round(eta:apoapsis) + " seconds". | |
139 | print "launch finished". | |
140 | } | |
141 | } | |
142 | ||
143 | set landedSettleTime to 0. | |
144 | set landingDescendSpeed to 0.5. | |
145 | set gearDeployAltitude to 0. | |
146 | landingOverride on. | |
147 | land(minangle). | |
148 | } | |
149 | ||
150 | ||
151 | ||
152 | ||
153 | //parameter checks | |
154 | function fillOptionalParams { | |
155 | if not (defined landingHeight) { | |
156 | local BottomBounds is ship:Bounds:furthestcorner(-ship:facing:vector). | |
157 | set landingHeight to (BottomBounds - vxcl(ship:facing:vector, BottomBounds)):mag. | |
158 | } | |
159 | set landingAltitude to tgt:terrainHeight + landingHeight. | |
160 | ||
161 | if not (defined landingVelocity) { | |
162 | if not (defined landingDescendSpeed) set landingDescendSpeed to 2. | |
163 | if not (defined landingGroundSpeed) set landingGroundSpeed to 0. | |
164 | } | |
165 | else { | |
166 | if (defined landingGroundSpeed) { | |
167 | questionableParams:add(list("landingVelocity, landingGroundSpeed", "note that landingVelocity always overrides landingGroundSpeed!")). | |
168 | } | |
169 | if (defined landingDescendSpeed) { | |
170 | questionableParams:add(list("landingVelocity, landingDescendSpeed", "note that landingVelocity always overrides landingDescendSpeed!")). | |
171 | } | |
172 | local landingHoriVel is vxcl((tgt:position - body:position), landingVelocity). | |
173 | local landingVertVel is landingVelocity - landingHoriVel. | |
174 | set landingGroundSpeed to landingHoriVel:mag. | |
175 | set landingDescendSpeed to landingVertVel:mag * ((vang(landingVertVel, (tgt:position - body:position)) - 90)/90). | |
176 | } | |
177 | ||
178 | if not (defined landingStage) set landingStage to stage:number. | |
179 | if not (defined terrainMargin) set terrainMargin to 5. | |
180 | if not (defined shipRollTime) set shipRollTime to 30. | |
181 | if not (defined deadzoneRCSstarboard) set deadzoneRCSstarboard to 0.3. | |
182 | if not (defined deadzoneRCStop) set deadzoneRCStop to 0.3. | |
183 | if not (defined glideTime) set glideTime to shipRollTime/2. | |
184 | if not (defined landedSettleTime) set landedSettleTime to 5. | |
185 | if not (defined thrustfactor) set thrustfactor to 1. | |
186 | ||
187 | if not (defined gearDeployAltitude) { | |
188 | if not (defined gearDeployTime) set gearDeployTime to 11. | |
189 | if glideTime >= gearDeployTime set gearDeployAltitude to landingDescendSpeed * gearDeployTime. | |
190 | else set gearDeployAltitude to 4 * landingDescendSpeed * gearDeployTime. | |
191 | } | |
192 | ||
193 | if not (defined debugPrinting) set debugPrinting to false. | |
194 | }. | |
195 | ||
196 | function checkParamsValidity { | |
197 | ||
198 | if deorbitAltitude < lowestSafeAltitude { | |
199 | invalidParams:add(list("lowestSafeAltitude, deorbitAltitude", "lowestSafeAltitude(" + lowestSafeAltitude + ") cannot be above deorbitAltitude(" + deorbitAltitude + ").")). | |
200 | } | |
201 | ||
202 | if deorbitAltitude <= 0 { | |
203 | invalidParams:add(list("deorbitAltitude", "must be bigger than 0 (" + deorbitAltitude + ").")). | |
204 | } | |
205 | ||
206 | if lowestSafeAltitude <= 0 { | |
207 | questionableParams:add(list("lowestSafeAltitude", "should be bigger than 0 (" + lowestSafeAltitude + ") unless you want to penetrate some terrain.")). | |
208 | } | |
209 | ||
210 | if terrainMargin < 0 { | |
211 | questionableParams:add(list("terrainMargin", "should be bigger than 0 (" + terrainMargin + ") unless you dont care about impacting some terrain.")). | |
212 | } | |
213 | ||
214 | if shipRollTime <= 0 { | |
215 | questionableParams:add(list("shipRollTime", "should be bigger than 0(" + shipRollTime + "). Will use 60 instead.")). | |
216 | set shipRollTime to 60. | |
217 | } | |
218 | ||
219 | if glideTime < 0 AND not (defined forceNegativeGlide AND forceNegativeGlide){ | |
220 | invalidParams:add(list("glideTime", "cannot be smaller than 0(" + glideTime + "). This would result in an 'upwards' landing. If you want to force this, set param 'forceNegativeGlide' to true!")). | |
221 | } | |
222 | ||
223 | if gearDeployAltitude <= 0 { | |
224 | questionableParams:add(list("gearDeployAltitude", "should be bigger than 0(" + gearDeployAltitude + ") unless you want to land with retracted gear or deploy it manually.")). | |
225 | } | |
226 | ||
227 | return invalidParams:length = 0. | |
228 | }. | |
229 | ||
230 | ||
231 | //util | |
232 | function getApproachAngle { | |
233 | parameter _Ap. | |
234 | parameter _Pe. | |
235 | parameter _alt. | |
236 | ||
237 | if _alt <= _Pe OR _alt >= _Ap return 0. | |
238 | ||
239 | local h is body:radius + _alt. | |
240 | local c is _Ap - _Pe. | |
241 | local q is 2 * body:radius + _Ap + _Pe - h. | |
242 | ||
243 | local gamma is arccos((q^2 + h^2 - c^2) / (2 * q * h)). | |
244 | ||
245 | return gamma/2. | |
246 | }. | |
247 | ||
248 | function getAltitudeAtAnomaly { | |
249 | parameter _ta. | |
250 | parameter _Ap is apoapsis. | |
251 | parameter _Pe is periapsis. | |
252 | ||
253 | local sma is (_Ap + _Pe)/2 + body:radius. | |
254 | local le is sma - _Pe - body:radius. | |
255 | local ecc is le/sma. | |
256 | ||
257 | return sma * ((1 - ecc^2) / (1 + ecc * cos(_ta))) - body:radius. | |
258 | }. | |
259 | ||
260 | function getAnomaly { | |
261 | parameter _Ap. | |
262 | parameter _Pe. | |
263 | parameter _alt. | |
264 | ||
265 | if _alt <= _Pe return 0. | |
266 | if _alt >= _Ap return 180. | |
267 | ||
268 | local h is body:radius + _alt. | |
269 | local c is _Ap - _Pe. | |
270 | local q is 2 * body:radius + _Ap + _Pe - h. | |
271 | ||
272 | local beta is arccos((h^2 + c^2 - q^2) / (2 * h * c)). | |
273 | ||
274 | return 180 - beta. | |
275 | }. | |
276 | ||
277 | function calculateDeorbit { | |
278 | parameter minApproachAngle is 0. | |
279 | parameter terrRes is 30. //terrain check interval in meters (horizontal). | |
280 | parameter precision is terrainMargin/20. | |
281 | parameter minPeStep is 1. | |
282 | ||
283 | set deorbitPeriapsis to landingAltitude. | |
284 | ||
285 | until getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude) >= minApproachAngle { | |
286 | set deorbitPeriapsis to deorbitPeriapsis - minPeStep * 1000. | |
287 | print (" Approach angle: " + round(getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
288 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
289 | } | |
290 | until getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude) <= minApproachAngle { | |
291 | set deorbitPeriapsis to deorbitPeriapsis + minPeStep * 100. | |
292 | print (" Approach angle: " + round(getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
293 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
294 | } | |
295 | until getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude) >= minApproachAngle { | |
296 | set deorbitPeriapsis to deorbitPeriapsis - minPeStep * 10. | |
297 | print (" Approach angle: " + round(getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
298 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
299 | } | |
300 | until getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude) <= minApproachAngle { | |
301 | set deorbitPeriapsis to deorbitPeriapsis + minPeStep. | |
302 | print (" Approach angle: " + round(getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
303 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
304 | } | |
305 | ||
306 | set brr to 360/body:rotationPeriod. | |
307 | set deorbitSMA to (deorbitAltitude + deorbitPeriapsis)/2 + body:radius. | |
308 | set deorbitSpeed to getOrbitSpeed(deorbitSMA, deorbitAltitude). | |
309 | set deorbitPeriod to getOrbitPeriod(deorbitSMA). | |
310 | ||
311 | local stepsize is terrRes * 180 / (constant:pi * body:radius). | |
312 | local relAngle is 0. | |
313 | local landingAnomaly is getAnomaly(deorbitAltitude, deorbitPeriapsis, landingAltitude). | |
314 | ||
315 | local done is false. | |
316 | until done { | |
317 | set relAngle to relAngle + stepsize. | |
318 | set currLng to mod(900 + tgt:lng - 90 + deorbitPeriod * relAngle/360 * brr/4 + longitudeAtAnomalyOfAN(90 - relAngle, tgt:lat), 360) - 180. | |
319 | set currLat to getLatOfLng(currLng, tgt:lat, deorbitPeriod, brr, tgt:lng). | |
320 | set currTerrain to getTerrainMax(latlng(currLat, currLng), stepsize/3, stepsize/3). | |
321 | set currAlt to getAltitudeAtAnomaly(landingAnomaly + relAngle, deorbitAltitude, deorbitPeriapsis). | |
322 | if currAlt >= lowestSafeAltitude { | |
323 | set done to true. | |
324 | if debugPrinting print "above lsa.". | |
325 | break. | |
326 | } | |
327 | ||
328 | print (" Approach angle: " + round(getApproachAngle(deorbitAltitude, deorbitPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
329 | print (" Terrain check: " + round(100 * (currAlt - landingAltitude) / (lowestSafeAltitude - landingAltitude)) + "%"):padright(terminal:width) at(0,1). | |
330 | ||
331 | if currAlt < currTerrain + terrainMargin { | |
332 | local PeStep is (body:radius + deorbitPeriapsis)/10. | |
333 | ||
334 | until abs(currAlt - currTerrain - terrainMargin) < precision { | |
335 | if (currAlt - landingAltitude) < terrainMargin AND (currAlt - landingHeight) >= currTerrain { | |
336 | break. | |
337 | } | |
338 | if debugPrinting print (round(currAlt - currTerrain - terrainMargin, 5) + ", " + round(landingAnomaly + relAngle, 3) + ", " + round(PeStep, 5) + ", " + round(deorbitPeriapsis, 4)):padright(terminal:width) at(0,2). | |
339 | if currAlt > (currTerrain + terrainMargin) { | |
340 | if PeStep <= minPeStep break. | |
341 | ||
342 | set deorbitPeriapsis to deorbitPeriapsis + PeStep. | |
343 | set PeStep to PeStep/2. | |
344 | if PeStep <= minPeStep set PeStep to minPeStep. | |
345 | set deorbitPeriapsis to deorbitPeriapsis - PeStep. | |
346 | } | |
347 | else { | |
348 | set deorbitPeriapsis to deorbitPeriapsis - PeStep. | |
349 | } | |
350 | ||
351 | if deorbitPeriapsis < -body:radius { | |
352 | print "ERROR: " + deorbitPeriapsis + ", " + PeStep. | |
353 | set deorbitPeriapsis to -body:radius. | |
354 | } | |
355 | ||
356 | set landingAnomaly to getAnomaly(deorbitAltitude, deorbitPeriapsis, landingAltitude). | |
357 | set currAlt to getAltitudeAtAnomaly(landingAnomaly + relAngle, deorbitAltitude, deorbitPeriapsis). | |
358 | set deorbitSMA to (deorbitAltitude + deorbitPeriapsis)/2 + body:radius. | |
359 | set deorbitSpeed to getOrbitSpeed(deorbitSMA, deorbitAltitude). | |
360 | set deorbitPeriod to getOrbitPeriod(deorbitSMA). | |
361 | ||
362 | if deorbitPeriapsis <= -body:radius { | |
363 | print "XXX " + deorbitPeriapsis. | |
364 | break. | |
365 | } | |
366 | } | |
367 | } | |
368 | ||
369 | if deorbitPeriapsis <= -body:radius break. | |
370 | } | |
371 | ||
372 | //TODO: get rid of simResult structure | |
373 | set simResult:finalAngle to 180 - landingAnomaly. | |
374 | set simResult:duration to getDeorbitDuration(deorbitAltitude, deorbitPeriapsis, landingAnomaly). | |
375 | set simResult:ignitionTime to simResult:duration - 15. | |
376 | ||
377 | if debugPrinting { | |
378 | print " " + deorbitPeriapsis. | |
379 | print " " + landingAnomaly. | |
380 | print " " + simResult:finalAngle. | |
381 | } | |
382 | ||
383 | set deorbitCalcFinished to true. | |
384 | }. | |
385 | ||
386 | function calculateAscent { | |
387 | parameter targetAltitude. | |
388 | parameter minApproachAngle is 0. | |
389 | parameter terrRes is 30. //terrain check interval in meters (horizontal). | |
390 | parameter precision is terrainMargin/20. | |
391 | parameter minPeStep is 1. | |
392 | ||
393 | if not (defined landingHeight) AND status = "landed" { | |
394 | set landingHeight to alt:radar. | |
395 | set landingAltitude to tgt:terrainHeight + landingHeight. | |
396 | } | |
397 | if not (defined lowestSafeAltitude) set lowestSafeAltitude to targetAltitude - 100. | |
398 | ||
399 | set ascentPeriapsis to landingAltitude. | |
400 | ||
401 | until getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude) >= minApproachAngle { | |
402 | set ascentPeriapsis to ascentPeriapsis - minPeStep * 1000. | |
403 | print (" Ascent angle: " + round(getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
404 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
405 | } | |
406 | until getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude) <= minApproachAngle { | |
407 | set ascentPeriapsis to ascentPeriapsis + minPeStep * 100. | |
408 | print (" Ascent angle: " + round(getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
409 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
410 | } | |
411 | until getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude) >= minApproachAngle { | |
412 | set ascentPeriapsis to ascentPeriapsis - minPeStep * 10. | |
413 | print (" Ascent angle: " + round(getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
414 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
415 | } | |
416 | until getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude) <= minApproachAngle { | |
417 | set ascentPeriapsis to ascentPeriapsis + minPeStep. | |
418 | print (" Ascent angle: " + round(getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
419 | print (" Terrain check: 0%"):padright(terminal:width) at(0,1). | |
420 | } | |
421 | ||
422 | set brr to 360/body:rotationPeriod. | |
423 | set ascentSMA to (targetAltitude + ascentPeriapsis)/2 + body:radius. | |
424 | set ascentApSpeed to getOrbitSpeed(ascentSMA, targetAltitude). | |
425 | set ascentPeriod to getOrbitPeriod(ascentSMA). | |
426 | ||
427 | local stepsize is terrRes * 180 / (constant:pi * body:radius). | |
428 | local relAngle is 0. | |
429 | local ascentAnomaly is getAnomaly(targetAltitude, ascentPeriapsis, landingAltitude). | |
430 | ||
431 | local done is false. | |
432 | until done { | |
433 | set relAngle to relAngle + stepsize. | |
434 | set currLng to mod(900 + tgt:lng - 90 - ascentPeriod * relAngle/360 * brr/4 + longitudeAtAnomalyOfAN(90 + relAngle, tgt:lat), 360) - 180. | |
435 | set currLat to getLatOfLng(currLng, tgt:lat, ascentPeriod, brr, tgt:lng). | |
436 | set currTerrain to getTerrainMax(latlng(currLat, currLng), stepsize/3, stepsize/3). | |
437 | set currAlt to getAltitudeAtAnomaly(ascentAnomaly + relAngle, targetAltitude, ascentPeriapsis). | |
438 | if currAlt >= lowestSafeAltitude { | |
439 | set done to true. | |
440 | if (defined debugPrinting) AND debugPrinting print "above lsa.". | |
441 | break. | |
442 | } | |
443 | ||
444 | print (" Ascent angle: " + round(getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude), 2) + " deg"):padright(terminal:width) at(0,0). | |
445 | print (" Terrain check: " + round(100 * (currAlt - landingAltitude) / (lowestSafeAltitude - landingAltitude)) + "%"):padright(terminal:width) at(0,1). | |
446 | ||
447 | if currAlt < currTerrain + terrainMargin { | |
448 | local PeStep is (body:radius + ascentPeriapsis)/10. | |
449 | ||
450 | until abs(currAlt - currTerrain - terrainMargin) < precision { | |
451 | if (currAlt - landingAltitude) < terrainMargin AND (currAlt - landingHeight) >= currTerrain { | |
452 | break. | |
453 | } | |
454 | if (defined debugPrinting) AND debugPrinting print (round(currAlt - currTerrain - terrainMargin, 5) + ", " + round(ascentAnomaly + relAngle, 3) + ", " + round(PeStep, 5) + ", " + round(ascentPeriapsis, 4)):padright(terminal:width) at(0,2). | |
455 | if currAlt > (currTerrain + terrainMargin) { | |
456 | if PeStep <= minPeStep break. | |
457 | ||
458 | set ascentPeriapsis to ascentPeriapsis + PeStep. | |
459 | set PeStep to PeStep/2. | |
460 | if PeStep <= minPeStep set PeStep to minPeStep. | |
461 | set ascentPeriapsis to ascentPeriapsis - PeStep. | |
462 | } | |
463 | else { | |
464 | set ascentPeriapsis to ascentPeriapsis - PeStep. | |
465 | } | |
466 | ||
467 | if ascentPeriapsis < -body:radius { | |
468 | print "ERROR: " + ascentPeriapsis + ", " + PeStep. | |
469 | set ascentPeriapsis to -body:radius. | |
470 | } | |
471 | ||
472 | set ascentAnomaly to getAnomaly(targetAltitude, ascentPeriapsis, landingAltitude). | |
473 | set currAlt to getAltitudeAtAnomaly(ascentAnomaly + relAngle, targetAltitude, ascentPeriapsis). | |
474 | set ascentSMA to (targetAltitude + ascentPeriapsis)/2 + body:radius. | |
475 | set ascentApSpeed to getOrbitSpeed(ascentSMA, targetAltitude). | |
476 | set ascentPeriod to getOrbitPeriod(ascentSMA). | |
477 | ||
478 | if ascentPeriapsis <= -body:radius { | |
479 | print "XXX " + ascentPeriapsis. | |
480 | wait until false. | |
481 | break. | |
482 | } | |
483 | } | |
484 | } | |
485 | ||
486 | if ascentPeriapsis <= -body:radius break. | |
487 | } | |
488 | ||
489 | set ascentAngle to getApproachAngle(targetAltitude, ascentPeriapsis, landingAltitude). | |
490 | return ascentAngle. | |
491 | }. | |
492 | ||
493 | function getTerrainMax { | |
494 | parameter _gp. | |
495 | parameter dlng. | |
496 | parameter dlat. | |
497 | ||
498 | local th is list(). | |
499 | th:add(_gp:terrainHeight). | |
500 | th:add(latlng(_gp:lat, _gp:lng - dlng):terrainHeight). | |
501 | th:add(latlng(_gp:lat, _gp:lng + dlng):terrainHeight). | |
502 | th:add(latlng(_gp:lat + dlat, _gp:lng):terrainHeight). | |
503 | th:add(latlng(_gp:lat + dlat, _gp:lng - dlng):terrainHeight). | |
504 | th:add(latlng(_gp:lat + dlat, _gp:lng + dlng):terrainHeight). | |
505 | th:add(latlng(_gp:lat - dlat, _gp:lng):terrainHeight). | |
506 | th:add(latlng(_gp:lat - dlat, _gp:lng - dlng):terrainHeight). | |
507 | th:add(latlng(_gp:lat - dlat, _gp:lng + dlng):terrainHeight). | |
508 | ||
509 | local hmax is th[0]. | |
510 | for h in th { | |
511 | if h > hmax set hmax to h. | |
512 | } | |
513 | ||
514 | return hmax. | |
515 | }. | |
516 | ||
517 | function getDeorbitDuration { | |
518 | parameter _Ap. | |
519 | parameter _Pe. | |
520 | parameter _ta. | |
521 | parameter _step is 1. | |
522 | ||
523 | local beta is 180 - _ta. | |
524 | local rlist is list(). | |
525 | local ca is 0. | |
526 | until ca >= beta { | |
527 | rlist:add(getAltitudeAtAnomaly(180 - ca, _Ap, _Pe) + body:radius). | |
528 | set ca to ca + _step. | |
529 | } | |
530 | rlist:add(getAltitudeAtAnomaly(beta, _Ap, _Pe) + body:radius). | |
531 | local fa is beta - ca + _step. | |
532 | ||
533 | local smajor is (_Ap + _Pe)/2 + body:radius. | |
534 | local sminor is smajor * sqrt(1 - ((_Ap - _Pe) / (2 * smajor))^2). | |
535 | local areaSpd is constant:pi * smajor * sminor / getOrbitPeriod(smajor). | |
536 | ||
537 | local duration is 0. | |
538 | local i is 1. | |
539 | until i >= rlist:length { | |
540 | if i = rlist:length - 1 set _step to fa. | |
541 | ||
542 | local aa is (rlist[i] + rlist[i-1])/2. | |
543 | local w is 2 * aa * sin(_step/2). | |
544 | local x is aa * cos(_step/2). | |
545 | local area is x * w/2. | |
546 | set duration to duration + area/areaSpd. | |
547 | set i to i + 1. | |
548 | } | |
549 | ||
550 | return duration. | |
551 | }. | |
552 | ||
553 | function planFlyover { | |
554 | parameter tgt is tgt. | |
555 | parameter allowOrbitReversal is false. | |
556 | ||
557 | local obtNormVec is vcrs(velocity:orbit, up:vector). | |
558 | local tgtNormVec is vcrs(latlng(tgt:lat, mod(181 + tgt:lng, 360) - 180):position - tgt:position, tgt:position - body:position). | |
559 | if allowOrbitReversal AND vang(obtNormVec, tgtNormVec) > 90 set tgtNormVec to -tgtNormVec. | |
560 | local ascAnVec is vcrs(obtNormVec, tgtNormVec). | |
561 | local descAnVec is vcrs(tgtNormVec, obtNormVec). | |
562 | local diffLDN is body:geopositionof(descAnVec + body:position):lng - tgt:lng. | |
563 | local diffLAN is body:geopositionof(ascAnVec + body:position):lng - tgt:lng. | |
564 | local alpha is vang(obtNormVec, tgtNormVec). | |
565 | ||
566 | if diffLDN < -180 set diffLDN to diffLDN + 360. | |
567 | if diffLAN < -180 set diffLAN to diffLAN + 360. | |
568 | local nodeVec is choose descAnVec if diffLDN > 0 else ascAnVec. | |
569 | local na is vang((nodeVec + body:position), up:vector). | |
570 | if vang(vcrs(nodeVec, up:vector), obtNormVec) > 90 set na to 360 - na. | |
571 | local nts is time:seconds + na * orbit:period/360. | |
572 | local spd is velocityAt(ship, nts):orbit:mag. | |
573 | add node(nts, 0, (choose spd if diffLDN > 0 else -spd) * sin(alpha), spd * cos(alpha) - spd). | |
574 | } | |
575 | ||
576 | //returns terrain normalVector at given latlng | |
577 | function getTerrainNormal { | |
578 | parameter posll. | |
579 | parameter resolution is 1. | |
580 | ||
581 | local _delta is 360 * resolution / (2 * constant:pi * body:radius). | |
582 | local _a is latlng(posll:lat, mod(180 + posll:lng + _delta, 360) - 180). | |
583 | local _b is latlng(mod(90 + posll:lat + _delta, 360) - 90, posll:lng). | |
584 | local terrNorm is vcrs(_b:position - posll:position, _a:position - posll:position):normalized. | |
585 | ||
586 | return terrNorm. | |
587 | }. | |
588 | ||
589 | //returns terrain slope in degrees at given latlng | |
590 | function getTerrainSlope { | |
591 | parameter posll. //geoCoordinates | |
592 | ||
593 | return vang(posll:position - body:position, getTerrainNormal(posll)). | |
594 | }. | |
595 | ||
596 | //returns magnitude of gravity in at given altitude | |
597 | function getGravAcc { | |
598 | parameter _alt. | |
599 | ||
600 | local r is body:radius + _alt. | |
601 | return body:mu/(r^2). | |
602 | }. | |
603 | ||
604 | //returns orbital speed as scalar in m/s at given semimajoraxis and altitude | |
605 | function getOrbitSpeed { | |
606 | parameter _sma. | |
607 | parameter _alt. | |
608 | ||
609 | return sqrt(body:mu * ((2/(body:radius + _alt)) - (1/_sma))). | |
610 | }. | |
611 | ||
612 | //returns orbital period in sec at given semimajoraxis | |
613 | function getOrbitPeriod { | |
614 | parameter _sma. | |
615 | ||
616 | return 2 * constant:pi * sqrt(_sma^3 / body:mu). | |
617 | }. | |
618 | ||
619 | //returns corrected steering vector to match delta-V vector under gravity | |
620 | function getDriftVector { | |
621 | parameter acceleration. //magnitute of your acceleration (through engines) as scalar (m/s^2) | |
622 | parameter targetVector. //direction you want to accelerate in as (normalized) vector | |
623 | parameter gravity. //gravity-acceleration as vector | |
624 | ||
625 | if targetVector:mag = 0 return targetVector. | |
626 | if acceleration = 0 return v(0,0,0). | |
627 | ||
628 | //intersect sphere with straight: | |
629 | local A is targetVector:x^2 + targetVector:y^2 + targetVector:z^2. | |
630 | local B is -2 * targetVector:x * gravity:x -2 * targetVector:y * gravity:y -2 * targetVector:z * gravity:z. | |
631 | local C is gravity:x^2 + gravity:y^2 + gravity:z^2 - acceleration^2. | |
632 | ||
633 | local t is -0.5*(B/A) + sqrt((0.5*(B/A))^2 - C/A). | |
634 | ||
635 | local aVec is t * targetVector - gravity. | |
636 | return aVec. | |
637 | }. | |
638 | ||
639 | //returns longitude delta relative to longitude at ascent-node given anomaly-angle as (true_anomaly + argument_of_periapsis) | |
640 | function longitudeAtAnomalyOfAN { | |
641 | parameter _anom, _inc. | |
642 | ||
643 | if abs(_inc) >= 90 { | |
644 | if _anom < 90 OR _anom > 270 return 0. | |
645 | if _anom = 90 OR _anom = 270 return 90. | |
646 | if _anom > 90 AND _anom < 270 return 180. | |
647 | } | |
648 | else if abs(_inc) < 0.1 { | |
649 | return _anom. | |
650 | } | |
651 | return arcsin(round(sin(_anom) * cos(_inc) / sqrt(1 - (sin(_anom) * sin(_inc))^2), 10)). | |
652 | }. | |
653 | ||
654 | //returns latitude given longitude, inclination, period (sec), bodyRotationRate(deg/sec) and longitudeOfFirstHighpoint ('groundtrack') | |
655 | function getLatOfLng { | |
656 | parameter _lng. //longitude. NOTE: | |
657 | // As the groundtrack shifts each orbit, (_lng + 360) will not return the same as (_lng)! (unless orbital period = x * body:rotationperiod; x E N...) | |
658 | // _lng = [0, 360[ refers to the initial orbit where it hits latlng(_inc, _hp) | |
659 | // _lng = [360, 720[ refers to the orbit after the initial orbit etc., where an orbit starts at lng0 and ends at lng360. | |
660 | // | |
661 | parameter _inc. //orbital inclination | |
662 | parameter _per. //orbital period | |
663 | parameter _brr. //body rotation rate (deg per second) | |
664 | parameter _hp. //longitude of first max latitude (_inc > 0), or first min latitude (_inc < 0) | |
665 | ||
666 | return _inc * cos(360 * (_lng - _hp) / (360 - _per * _brr)). | |
667 | }. | |
668 | ||
669 | //returns altitude of glideslope at current distance from target. | |
670 | function getGlideAltitude { | |
671 | parameter spd_h, spd_v, d. | |
672 | ||
673 | if spd_h = 0 return false. | |
674 | return d * spd_v/spd_h. | |
675 | }. | |
676 | ||
677 | //transforms gps-coordinates into a geoposition. | |
678 | function geopositionFromCoordinates { | |
679 | parameter _degN, _minN, _secN, _prefixN. | |
680 | parameter _degE, _minE, _secE, _prefixE. | |
681 | ||
682 | local pN is list("NORTH", "N"). | |
683 | local pS is list("SOUTH", "S"). | |
684 | local pE is list("EAST", "E"). | |
685 | local pW is list("WEST", "W"). | |
686 | ||
687 | local lat is 0. | |
688 | local lng is 0. | |
689 | ||
690 | if pN:contains(_prefixN) set lat to (_degN + _minN/60 + _secN/3600). | |
691 | else if pS:contains(_prefixN) set lat to -(_degN + _minN/60 + _secN/3600). | |
692 | else { | |
693 | print _prefixN + " is not a valid north/south direction.". | |
694 | return false. | |
695 | } | |
696 | ||
697 | if pE:contains(_prefixE) set lng to (_degE + _minE/60 + _secE/3600). | |
698 | else if pW:contains(_prefixE) set lng to -(_degE + _minE/60 + _secE/3600). | |
699 | else { | |
700 | print _prefixE + " is not a valid east/west direction.". | |
701 | return false. | |
702 | } | |
703 | ||
704 | return latlng(lat, lng). | |
705 | }. | |
706 | ||
707 | //plans and executes deorbit maneuver given anomaly and speed | |
708 | function deorbit { //assumes eccentricity = 0 | |
709 | parameter _anomaly. | |
710 | parameter _speed. | |
711 | ||
712 | set orbitSpeed to getOrbitSpeed((apoapsis + periapsis)/2 + body:radius, (apoapsis + periapsis)/2). | |
713 | set deorbitDV to _speed - orbitSpeed. | |
714 | ||
715 | lock maxAcceleration to ship:maxthrust/ship:mass. | |
716 | if maxAcceleration = 0 { //no thrust | |
717 | print "no thrust abort placeholder". | |
718 | } | |
719 | set burnDuration to deorbitDV/maxAcceleration. | |
720 | ||
721 | add node(time:seconds + mod(720 + _anomaly - orbit:trueAnomaly, 360) * orbit:period/360, 0, 0, deorbitDV). | |
722 | set executeNodeComplete to false. | |
723 | //executeNode("lower_Pe", deorbitPeriapsis). | |
724 | executeNode(). | |
725 | when executeNodeComplete then set deorbitComplete to true. | |
726 | }. | |
727 | ||
728 | function executeNode { | |
729 | parameter _condition is "NONE". | |
730 | parameter _conValue is 0. | |
731 | ||
732 | set nd to nextnode. | |
733 | if nd:deltav:mag = 0 { | |
734 | remove nd. | |
735 | set executeNodeComplete to true. | |
736 | return false. | |
737 | } | |
738 | lock max_acc to ship:maxthrust/ship:mass. | |
739 | set burnDuration to nd:deltav:mag/max_acc. | |
740 | set ignitionTime to burnDuration/2. | |
741 | lock steering to nd:deltav. | |
742 | when vang(nd:deltav, ship:facing:vector) < 1 then { | |
743 | warpto(time:seconds + nd:eta - ignitionTime - shipRollTime). | |
744 | when vang(nd:deltav, ship:facing:vector) < 0.05 then { | |
745 | warpto(time:seconds + nd:eta - ignitionTime - shipRollTime/5). | |
746 | when nd:eta <= (ignitionTime) then { | |
747 | set done to false. | |
748 | if _condition = "lower_Pe" { | |
749 | lock throttle to min(3*nd:deltav:mag/max_acc, min(max((periapsis - _conValue)/8000, 0.0001), 1)). | |
750 | lock steering to retrograde. | |
751 | when done OR periapsis <= _conValue then { | |
752 | if done return false. | |
753 | set done to true. | |
754 | lock throttle to 0. | |
755 | } | |
756 | } | |
757 | else { | |
758 | lock throttle to min(3*nd:deltav:mag/max_acc, 1). | |
759 | set dv0 to nd:deltav. | |
760 | when done OR vdot(dv0, nd:deltav) < 0 then { | |
761 | if done return false. | |
762 | lock throttle to 0. | |
763 | set done to true. | |
764 | } | |
765 | when done OR nd:deltav:mag < 0.1 then { | |
766 | if done return false. | |
767 | unlock steering. | |
768 | set steering to nd:deltav. | |
769 | lock throttle to max(min(nd:deltav:mag/max_acc, 1), 0.0001). | |
770 | wait until vdot(dv0, nd:deltav) < 0.2. | |
771 | lock throttle to 0. | |
772 | set done to true. | |
773 | } | |
774 | } | |
775 | ||
776 | when done then { | |
777 | unlock steering. | |
778 | unlock throttle. | |
779 | ||
780 | remove nd. | |
781 | set executeNodeComplete to true. | |
782 | } | |
783 | } | |
784 | } | |
785 | } | |
786 | }. | |
787 | ||
788 | function engage { | |
789 | parameter minangle is 0. | |
790 | //parameter checks | |
791 | set invalidParams to list(). | |
792 | set questionableParams to list(). | |
793 | ||
794 | set deorbitAltitude to periapsis. | |
795 | if not (defined lowestSafeAltitude) set lowestSafeAltitude to periapsis - 100. | |
796 | if lowestSafeAltitude > deorbitAltitude - 100 set lowestSafeAltitude to deorbitAltitude - 100. | |
797 | ||
798 | fillOptionalParams(). | |
799 | if not checkParamsValidity() { | |
800 | print "Found invalid parameters:". | |
801 | print " ". | |
802 | for i in invalidParams { | |
803 | print i[0] + ": " + i[1]. | |
804 | print " ". | |
805 | } | |
806 | print "aborting". | |
807 | return false. | |
808 | } | |
809 | if questionableParams:length > 0 { | |
810 | print "WARNING! Found questionable parameters:". | |
811 | print " ". | |
812 | for i in questionableParams { | |
813 | print i[0] + ": " + i[1]. | |
814 | print " ". | |
815 | } | |
816 | print "continuing". | |
817 | } | |
818 | ||
819 | set phase to "INIT". | |
820 | set guidanceStatus to "INIT". | |
821 | set vacLocLanderFinished to false. | |
822 | ||
823 | //main code | |
824 | set phase to "PLANNING". | |
825 | set guidanceStatus to "SLEEPING". | |
826 | set simResult to lexicon("STATUS", "EMPTY"). | |
827 | set deorbitCalcFinished to false. | |
828 | ||
829 | //TODO: get real descentStage values. | |
830 | if not (defined descentStageThrust) set descentStageThrust to ship:availablethrust. | |
831 | if not (defined descentStageMassFlow) { | |
832 | set descentStageMassFlow to 0. | |
833 | list engines in elist. | |
834 | for en in elist set descentStageMassFlow to descentStageMassFlow + en:maxMassFlow. | |
835 | } | |
836 | if not (defined descentStageMass) set descentStageMass to ship:mass. | |
837 | ||
838 | ||
839 | ||
840 | calculateDeorbit(minangle). | |
841 | ||
842 | when deorbitCalcFinished then { | |
843 | set phase to "SYNCRONISATION". | |
844 | set deorbitLongitude to mod(900 + tgt:lng - 90 + longitudeAtAnomalyOfAN(90 - simResult:finalAngle, abs(tgt:lat)) + 360/body:rotationperiod * simResult:duration, 360) - 180. | |
845 | //set inclinationDeadzone to arctan(((simResult:burn:stepLog[0][1] - landingAltitude) * crossrangeAbility)/body:radius). | |
846 | ||
847 | set deorbitComplete to false. | |
848 | ||
849 | //approach | |
850 | set guidanceStatus to "EXPERIMENTAL APPROACH". | |
851 | planFlyover(tgt). | |
852 | set executeNodeComplete to false. | |
853 | executeNode(). | |
854 | when executeNodeComplete then { | |
855 | set phase to "DEORBIT". | |
856 | set touchdownArgumentOfAN to (choose 90 if tgt:lat > 0 else 270). | |
857 | set currANLNG to mod(900 + longitude - longitudeAtAnomalyOfAN(orbit:trueAnomaly + orbit:argumentOfPeriapsis, orbit:inclination), 360) - 180. | |
858 | set tarANLNG to mod(900 + tgt:lng - touchdownArgumentOfAN, 360) - 180. | |
859 | set diffANLNG to mod(720 + tarANLNG - currANLNG, 360). | |
860 | set deorbitLAN to mod(720 + orbit:LAN + diffANLNG + 0.5 * orbit:period * 360/body:rotationPeriod + 90 * orbit:period/body:rotationPeriod, 360). | |
861 | ||
862 | set totangle to 360 - vang(-body:position, tgt:position - body:position). | |
863 | set relangle to totangle - simResult:finalAngle + simResult:duration * 360/body:rotationperiod. | |
864 | set anom to mod(360 + orbit:trueAnomaly + relAngle + relAngle * orbit:period/body:rotationPeriod, 360). | |
865 | ||
866 | deorbit(anom, deorbitSpeed). | |
867 | } | |
868 | ||
869 | when deorbitComplete then { | |
870 | if vacLocLanderFinished return false. | |
871 | set timestampDeorbit to time:seconds. | |
872 | ||
873 | if stage:number > landingStage { | |
874 | set phase to "STAGING". | |
875 | lock steering to up. | |
876 | ||
877 | on time:second { | |
878 | if stage:ready AND vang(steering:vector, ship:facing:vector) < 1 { | |
879 | stage. | |
880 | } | |
881 | return stage:number > landingStage. | |
882 | } | |
883 | ||
884 | when stage:number <= landingStage then { | |
885 | set phase to "COASTING". | |
886 | lock steering to srfRetrograde. | |
887 | } | |
888 | } | |
889 | else { | |
890 | set phase to "COASTING". | |
891 | lock steering to srfRetrograde. | |
892 | } | |
893 | } | |
894 | ||
895 | when phase = "COASTING" AND time:seconds >= min(timestampDeorbit + simResult:ignitionTime * 0.8, timestampDeorbit + simResult:ignitionTime - shipRollTime * 3) then { | |
896 | if vacLocLanderFinished return false. | |
897 | set warp to 0. | |
898 | matchGlide(). | |
899 | } | |
900 | ||
901 | when phase = "GUIDANCE_RELEASED" then { | |
902 | if vacLocLanderFinished return false. | |
903 | lock throttle to min(3-vang(steering:vector, ship:facing:vector), getGravAcc(altitude)*ship:mass/ship:availableThrust + max(0, 1 - (altitude - (tgt:terrainHeight + landingHeight)))*(-verticalSpeed - landingDescendSpeed)/ship:availableThrust). | |
904 | when status = "landed" OR status = "splashed" then { | |
905 | lock throttle to 0. | |
906 | if ship:groundspeed > 0.1 { | |
907 | set phase to "TOUCHDOWN". | |
908 | brakes on. | |
909 | when velocity:surface:mag < 0.01 then { | |
910 | unlock steering. | |
911 | set phase to "STOPPED". | |
912 | } | |
913 | } | |
914 | else { | |
915 | lock steering to ship:facing. | |
916 | set phase to "LANDED". | |
917 | } | |
918 | } | |
919 | } | |
920 | } | |
921 | ||
922 | when (phase = "LANDED" OR phase = "STOPPED") AND not (defined landingOverride) then { | |
923 | lock throttle to 0. | |
924 | lock steering to getTerrainNormal(latlng(latitude, longitude)):direction + R(0, 0, ship:facing:roll). | |
925 | set releaseTimestamp to time:seconds + landedSettleTime. | |
926 | when time:seconds > releaseTimestamp then { | |
927 | unlock steering. | |
928 | set vacLocLanderFinished to true. | |
929 | print "landing finished". | |
930 | } | |
931 | } | |
932 | }. | |
933 | ||
934 | function centerOfMass { | |
935 | parameter _plist. | |
936 | ||
937 | local tmass is 0. | |
938 | local tvec is v(0,0,0). | |
939 | ||
940 | for p in _plist { | |
941 | set tmass to tmass + p:mass. | |
942 | set tvec to tvec + p:mass * p:position. | |
943 | } | |
944 | ||
945 | return tvec/tmass. | |
946 | ||
947 | } | |
948 | ||
949 | //manages descent with active guidance towards landing spot | |
950 | function matchGlide { | |
951 | local elist is list(). | |
952 | local avThrust is 0. | |
953 | local mxThrust is 0. | |
954 | local mxMass is 0. | |
955 | local avMass is 0. | |
956 | local slopeAlt is 0. | |
957 | local expMaxAcceleration is 0. | |
958 | list engines in elist. | |
959 | //TODO: only list active and usable engines | |
960 | for e in elist { | |
961 | set avThrust to avThrust + e:availableThrust. | |
962 | set mxThrust to mxThrust + e:maxThrust. | |
963 | set mxMass to mxMass + e:maxMassFlow. | |
964 | } | |
965 | set avMass to mxMass * avThrust / mxThrust. | |
966 | ||
967 | //main code | |
968 | lock landingPoint to (tgt:position - body:position):normalized * (body:radius + landingAltitude). | |
969 | if not (defined landingVelocity) { | |
970 | lock _landingVelocity to -landingPoint:normalized * landingDescendSpeed + vxcl(landingPoint, landingPoint + body:position):normalized * landingGroundSpeed. | |
971 | } | |
972 | else set _landingVelocity to landingVelocity. | |
973 | lock startPoint to landingPoint - _landingVelocity*glideTime. | |
974 | set brr to 360/body:rotationPeriod. | |
975 | ||
976 | lock vel_h to vxcl(ship:up:vector, velocity:surface). | |
977 | lock vel_v to velocity:surface - vel_h. | |
978 | lock vel_cr to vxcl(vxcl(ship:up:vector, startPoint + body:position), vel_h). | |
979 | lock vel_dr to vel_h - vel_cr. | |
980 | ||
981 | lock timeStart to vxcl(ship:up:vector, startPoint + body:position):mag / vel_h:mag. | |
982 | //TODO: find a way to make predAltitude more precise. | |
983 | lock predAltitude to (positionAt(ship, time:seconds + timeStart) - body:position):mag - body:radius. | |
984 | ||
985 | lock expMaxAcceleration to avThrust/ship:mass - getGravAcc(startPoint:mag - body:radius). | |
986 | lock maxAcceleration to ship:availableThrust/ship:mass. | |
987 | lock dVGlide to _landingVelocity - velocity:surface. | |
988 | lock btGlide to dvGlide:mag/expMaxAcceleration. | |
989 | lock predAltError to predAltitude - (startPoint:mag - body:radius). | |
990 | ||
991 | if landingGroundSpeed < 1e-5 lock slopeAlt to altitude. | |
992 | else lock slopeAlt to getGlideAltitude(landingGroundSpeed, landingDescendSpeed, vxcl(ship:up:vector, landingPoint + body:position):mag) + landingPoint:mag - body:radius. | |
993 | ||
994 | set phase to "ACTIVE_GUIDANCE". | |
995 | set guidanceStatus to "ARMED". | |
996 | set guidanceActive to true. | |
997 | set velCrMatch to false. | |
998 | set expAltMatch to false. | |
999 | set rcsActive to false. | |
1000 | set hoverslam to false. | |
1001 | ||
1002 | if abs(predAltError) > 1e-3 { | |
1003 | set guidanceStatus to "REDUCING_DOWNRANGE_ALTITUDE_ERROR". | |
1004 | if predAltError > 10 { | |
1005 | lock steering to heading(mod(180 + body:geopositionof(startPoint + body:position):heading, 360), 0). | |
1006 | lock throttle to min(2 - vang(steering:vector, facing:vector), min(0.3, max(predAltError/100, 0.001))). | |
1007 | when predAltError < 10 and not hoverslam then { | |
1008 | lock throttle to 0. | |
1009 | set expAltMatch to true. | |
1010 | } | |
1011 | } | |
1012 | else if predAltError < 5 { | |
1013 | lock steering to heading(mod(180 + body:geopositionof(startPoint + body:position):heading, 360), 90). | |
1014 | lock throttle to min(2 - vang(steering:vector, facing:vector), min(0.3, max(-predAltError/100, 0.001))). | |
1015 | when predAltError > 8 and not hoverslam then { | |
1016 | lock throttle to 0. | |
1017 | set expAltMatch to true. | |
1018 | } | |
1019 | } | |
1020 | else set expAltMatch to true. | |
1021 | } | |
1022 | else set expAltMatch to true. | |
1023 | ||
1024 | when expAltMatch and not hoverslam then { | |
1025 | if vacLocLanderFinished return false. | |
1026 | if vel_cr:mag > 1e-3 { | |
1027 | set guidanceStatus to "REDUCING_CROSSRANGE_SPEED". | |
1028 | lock steering to -vel_cr. | |
1029 | lock throttle to min(5 - vang(steering, facing:vector), max(vel_cr:mag/maxAcceleration, 0.005)). | |
1030 | when vel_cr:mag < 1 and not hoverslam then { | |
1031 | local vel_cr0 is vel_cr. | |
1032 | lock steering to -vel_cr0. | |
1033 | lock throttle to min(2 - vang(steering, facing:vector), max(vel_cr:mag/maxAcceleration, 0.01)). | |
1034 | when vdot(vel_cr0, vel_cr) < 0 and not hoverslam then { | |
1035 | lock throttle to 0. | |
1036 | set velCrMatch to true. | |
1037 | } | |
1038 | } | |
1039 | } | |
1040 | else set velCrMatch to true. | |
1041 | } | |
1042 | ||
1043 | when guidanceActive AND (velCrMatch OR hoverslam) then { | |
1044 | if vacLocLanderFinished return false. | |
1045 | if not hoverslam { | |
1046 | set guidanceStatus to "RCS_ERROR_REDUCTION". | |
1047 | lock steering to dVGlide + getGravAcc(altitude) * btGlide * ship:up:vector. | |
1048 | } | |
1049 | ||
1050 | set rcsPrestate to lexicon(). | |
1051 | set rcsPrestate:active to RCS. | |
1052 | set rcsPrestate:thrusters to list(). | |
1053 | ||
1054 | set rcsThrusters to ship:modulesnamed("modulercsfx"). | |
1055 | for t in rcsThrusters { | |
1056 | if t:allEventNames:contains("show actuation toggles") t:doevent("show actuation toggles"). | |
1057 | rcsPrestate:thrusters:add(list(t:getField("yaw"), t:getField("pitch"), t:getField("roll"), t:getField("port/stbd"), t:getField("dorsal/ventral"), t:getField("fore/aft"))). | |
1058 | } | |
1059 | ||
1060 | ||
1061 | lock downrange to vxcl(ship:up:vector, velocity:surface):normalized. | |
1062 | lock nstar to vcrs(downrange, ship:up:vector):normalized. | |
1063 | ||
1064 | lock velTop to vxcl(nstar, vel_h). | |
1065 | lock velStar to vxcl(downrange, vel_h). | |
1066 | lock dvStar to vxcl(vxcl(ship:up:vector, _landingVelocity), vxcl(ship:up:vector, dVGlide)). | |
1067 | lock dvTop to vxcl(ship:up:vector, dvGlide) - dvStar. | |
1068 | ||
1069 | set rcsActive to true. | |
1070 | when vang(steering, facing:vector) < 5 AND abs(90 - vang(ship:facing:topvector, nstar)) < 5 then { | |
1071 | for t in rcsThrusters { | |
1072 | if not rcsPrestate:active { | |
1073 | t:setField("yaw", false). | |
1074 | t:setField("pitch", false). | |
1075 | t:setField("roll", false). | |
1076 | } | |
1077 | t:setField("port/stbd", true). | |
1078 | t:setField("dorsal/ventral", true). | |
1079 | t:setField("fore/aft", true). | |
1080 | rcs on. | |
1081 | } | |
1082 | on round(time:seconds*3) { | |
1083 | if rcsActive { | |
1084 | if guidanceStatus = "MINIMIZING_GROUNDSPEED" { | |
1085 | set ship:control:top to 2 * velTop:mag * ((vang(velTop, downrange)-90)/90). | |
1086 | set ship:control:starboard to velStar:mag * ((vang(velStar, nstar)-90)/90). | |
1087 | } | |
1088 | else if guidanceStatus = "MATCHING_GLIDESLOPE" { | |
1089 | set ship:control:top to -3 * dvTop:mag * ((vang(dvTop, vxcl(ship:up:vector, _landingVelocity))-90)/90). | |
1090 | set ship:control:starboard to dvStar:mag * ((vang(dvStar, vxcl(ship:up:vector, _landingVelocity)))/90). //missing '-90' is intentional! | |
1091 | set ship:control:fore to max(slopeAlt, landingPoint:mag - body:radius) - altitude. | |
1092 | } | |
1093 | else if guidanceStatus = "MATCHING_GROUNDSPEED" { | |
1094 | set ship:control:top to 0. | |
1095 | set ship:control:starboard to 0. | |
1096 | } | |
1097 | else if abs(90 - vang(ship:facing:topvector, nstar)) < 5 { | |
1098 | set ship:control:starboard to 5*vdot(vcrs(startPoint + body:position, ship:up:vector):normalized, -vel_cr). | |
1099 | if guidanceStatus <> "HOVERSLAM_EXECUTION" set ship:control:top to -predAltError/30. | |
1100 | else set ship:control:top to 0. | |
1101 | ||
1102 | if abs(ship:control:starboard) < deadzoneRCSstarboard set ship:control:starboard to 0. | |
1103 | if abs(ship:control:top) < deadzoneRCStop set ship:control:top to 0. | |
1104 | } | |
1105 | else { | |
1106 | set ship:control:starboard to 0. | |
1107 | set ship:control:top to 0. | |
1108 | } | |
1109 | return true. | |
1110 | } | |
1111 | else { | |
1112 | set rcs to rcsPrestate:active. | |
1113 | local _i is 0. | |
1114 | for t in rcsThrusters { | |
1115 | t:setField("yaw", rcsPrestate:thrusters[_i][0]). | |
1116 | t:setField("pitch", rcsPrestate:thrusters[_i][1]). | |
1117 | t:setField("roll", rcsPrestate:thrusters[_i][2]). | |
1118 | t:setField("port/stbd", rcsPrestate:thrusters[_i][3]). | |
1119 | t:setField("dorsal/ventral", rcsPrestate:thrusters[_i][4]). | |
1120 | t:setField("fore/aft", rcsPrestate:thrusters[_i][5]). | |
1121 | set _i to _i + 1. | |
1122 | } | |
1123 | ||
1124 | set ship:control:starboard to 0. | |
1125 | set ship:control:top to 0. | |
1126 | return false. | |
1127 | } | |
1128 | } | |
1129 | } | |
1130 | } | |
1131 | ||
1132 | when rcsActive OR timeStart < btGlide + shipRollTime/2 then { | |
1133 | if vacLocLanderFinished return false. | |
1134 | if not rcsActive { | |
1135 | set guidanceStatus to "HOVERSLAM_PREPARATION". | |
1136 | lock steering to dVGlide + getGravAcc(altitude) * btGlide * ship:up:vector. | |
1137 | } | |
1138 | set hoverslam to true. | |
1139 | lock throttle to 0. | |
1140 | ||
1141 | when guidanceActive AND (vxcl(ship:up:vector, startPoint + body:position):mag <= (0.5 * (vxcl(ship:up:vector, velocity:surface):mag)^2 / (vxcl(ship:up:vector, ship:availableThrust * thrustFactor * steering:normalized):mag/ship:mass))) then { | |
1142 | set phase to "FINAL_APPROACH". | |
1143 | set guidanceStatus to "HOVERSLAM_EXECUTION". | |
1144 | if not gear when alt:radar <= gearDeployAltitude then gear on. | |
1145 | lock throttle to max(1e-50, min(1, dVGlide:mag/(max(1, timeStart) * ship:availableThrust/ship:mass))). | |
1146 | lock steering to dVGlide + getGravAcc(altitude) * (btGlide/throttle) * ship:up:vector. | |
1147 | ||
1148 | when guidanceActive AND (vdot(velocity:surface, startPoint + body:position) < 0 OR dVGlide:mag < 0.5 OR (ship:groundspeed - landingGroundSpeed) < 0.5) then { | |
1149 | set phase to "LANDING". | |
1150 | set guidanceStatus to "MATCHING_GROUNDSPEED". | |
1151 | lock downrange to heading(90, 0):vector:normalized. | |
1152 | lock nstar to -heading(90, 0):starvector:normalized. | |
1153 | lock throttle to max(dvGlide:mag/(ship:availableThrust/ship:mass), getGravAcc(altitude)*ship:mass/ship:availableThrust + max(0, 1 - (altitude - max(slopeAlt, landingPoint:mag - body:radius)))*(-verticalSpeed - landingDescendSpeed)/ship:availableThrust). | |
1154 | when guidanceActive AND (abs(ship:groundspeed - landingGroundSpeed) < 0.5 ) then { | |
1155 | if landingGroundSpeed < 1e-5 { | |
1156 | set guidanceStatus to "MINIMIZING_GROUNDSPEED". | |
1157 | lock steering to heading(90, 90, 180). | |
1158 | lock throttle to min(10-vang(steering:vector, ship:facing:vector), getGravAcc(altitude)*ship:mass/ship:availableThrust + max(0, 1 - (altitude - max(slopeAlt, landingPoint:mag - body:radius)))*(-verticalSpeed - landingDescendSpeed)/ship:availableThrust). | |
1159 | } | |
1160 | else { | |
1161 | gear on. | |
1162 | set guidanceStatus to "MATCHING_GLIDESLOPE". | |
1163 | lock throttle to 0. | |
1164 | lock steering to heading(body:geopositionof(velocity:surface:normalized * 100):heading, 90, 180). | |
1165 | lock throttle to min(3-vang(steering:vector, ship:facing:vector), getGravAcc(altitude)*ship:mass/ship:availableThrust + max(0, 1 - (altitude - max(slopeAlt, landingPoint:mag - body:radius)))*(-verticalSpeed - landingDescendSpeed)/ship:availableThrust). | |
1166 | when guidanceActive AND mod(720 + longitude - tgt:lng, 360) > 180 then { | |
1167 | rcs off. | |
1168 | set rcsActive to false. | |
1169 | } | |
1170 | } | |
1171 | } | |
1172 | } | |
1173 | } | |
1174 | } | |
1175 | ||
1176 | when guidanceActive AND (status = "landed" OR status = "splashed" or altitude <= (landingPoint:mag - body:radius)) then { | |
1177 | if vacLocLanderFinished return false. | |
1178 | lock throttle to 0. | |
1179 | rcs off. | |
1180 | set rcsActive to false. | |
1181 | set guidanceActive to false. | |
1182 | if status = "landed" OR status = "splashed" { | |
1183 | if ship:groundspeed > 0.1 { | |
1184 | set phase to "TOUCHDOWN". | |
1185 | brakes on. | |
1186 | when velocity:surface:mag < 0.01 then { | |
1187 | unlock steering. | |
1188 | set phase to "STOPPED". | |
1189 | } | |
1190 | } | |
1191 | else { | |
1192 | set phase to choose "LANDED" if status = "landed" else "SPLASHED". | |
1193 | } | |
1194 | set guidanceStatus to "ENDED". | |
1195 | } | |
1196 | else { | |
1197 | set phase to "GUIDANCE_RELEASED". | |
1198 | set guidanceStatus to "ENDED". | |
1199 | } | |
1200 | } | |
1201 | }. | |
1202 | until not hasnode {remove nextnode. wait 0.}. | |
1203 | clearscreen. for i in range(2) print " ". |