Advertisement
Guest User

Rudimentary auto land , needs lib_PID

a guest
Jun 5th, 2015
280
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.91 KB | None | 0 0
  1. // kOS lander script
  2. // calculates desired speed at altitude until 50 m AGL
  3. // gear down at 100 m AGL
  4.  
  5. clearscreen.
  6. sas on.
  7. set sasmode to "retrograde".
  8.  
  9. print" Lander script".
  10. print" Landing on " + ship:body:name.
  11. print" Altitude AGL = ".
  12. print" Desired speed = ".
  13. print" Actual speed = ".
  14. print" Speed Error = ".
  15. print" ".
  16. print" Throttle Setting = ".
  17.  
  18. // load PID functions
  19. run lib_pid.
  20.  
  21. set ship:control:pilotmainthrottle to 0.
  22.  
  23. // Call to update the display of numbers.
  24. function display_block {
  25. parameter startCol, startRow. // define where the block of text should be positioned
  26.  
  27. print round(alt:radar,2) + " m " at (startCol,startRow).
  28. print round(seekSpeed,2) + " m/s " at (startCol,startRow+1).
  29. print round(currentSpeed,2) + " m/s " at (startCol,startRow+2).
  30. print round(seekSpeed - currentSpeed,2) + " m/s " at (startCol,startRow+3).
  31. print round(myth,3) + " " at (startCol,startRow+5).
  32. }.
  33.  
  34. // Start with 0 throttle.
  35. set myTh to 0.
  36.  
  37. lock throttle to myTh.
  38.  
  39. // Initialize PID.
  40. set myPID to PID_init( 0.20, 0.08, 0.1, 0, 1 ). // Kp, Ki, Kd, min, max control range vals.
  41.  
  42. gear on. gear off. // on then off because of the weird KSP 'have to hit g twice' bug.
  43.  
  44. // desired speed at final altitude.
  45. set finalSpeed to 1.
  46. set finalAltitude to 250.
  47.  
  48. // desired deceleration in m/s.
  49. set shipG to 10.
  50.  
  51. // current gravity.
  52. lock g to ship:body:mu / (ship:body:radius^2+ alt:radar).
  53. //lock g to ship:body:mu / ship:body:radius^2.
  54.  
  55. // current radar altitude.
  56. lock radarAlt to alt:radar.
  57.  
  58. // current speed.
  59. lock currentSpeed to ship:velocity:surface:mag.
  60.  
  61. // in runstate 0 we coast to deceleration altitude.
  62. set runState to 0.
  63. until runstate > 0 {
  64. set seekSpeed to sqrt(finalSpeed^2 + 2 * (shipG+g) * (radarAlt - finalAltitude) ).
  65. if seekSpeed < finalSpeed { set seekSpeed to finalSpeed.}
  66. // currentSpeed and seekSpeed are reversed due to vector math.
  67. set myTh to PID_seek( myPID, currentSpeed, seekSpeed).
  68. display_block(20,2).
  69. wait 0.001.
  70. if radarAlt < finalAltitude {
  71. gear on.
  72. sas off.
  73. lock steering to heading(0,90).
  74. set runState to 1.
  75. }
  76. }
  77.  
  78. set myTh to 0.
  79. // Now we wait until we are mostly aligned.
  80. until( abs(heading(0,90):pitch - facing:pitch) < 0.15 and abs(heading(0,90):yaw - facing:yaw) < 0.15) {
  81. display_block(20,2).
  82. wait 0.001.
  83. }
  84.  
  85. // Now we need to switch to vertical speed...
  86. lock currentSpeed to ship:verticalspeed.
  87.  
  88. until runState > 1 {
  89. if radarAlt < 10 { set seekSpeed to -1. }
  90. if radarAlt >= 10 { set seekSpeed to -5. }
  91. if radarAlt >= 50 { set seekSpeed to -10. }
  92. if radarAlt > 200 { set seekSpeed to -20. }
  93. set myTh to PID_seek( myPID, seekSpeed, currentSpeed).
  94. display_block(20,2).
  95. wait 0.001.
  96. if ship:status = "LANDED" {
  97. set runState to 2.
  98. }
  99. }
  100.  
  101. rcs off.
  102. sas off.
  103. unlock steering.
  104. set ship:control:pilotmainthrottle to 0.
  105. unlock throttle.
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement