Armor/general_tools
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
zhouyr9
2026-01-16 16:09:51 +08:00
parent ec4c06ea8c
commit c81fc697d2
2 changed files with 425 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/KSP_tools/KRPC_intergration/example_landing.py
+292
View File
@@ -0,0 +1,292 @@
######################################################################
### Automated Landing Library and Example
######################################################################
### Like all of the scripts in my folder here, this file contains
### functions you might want to include into your own scripts for
### actual use and a demo in the 'main' function that you can just
### run to see how it works.
###
### This file shows how to complete an automated landing. It
### does so in three phases - a deorbital burn, a suicide burn to a
### safety altitude and speed, and then a speed limited descent to
### final touch down.
######################################################################
import krpc
import math
import time
import numpy as np
from pid import PID
##############################################################################
### Main function - demonstrates the use of this library. Simply lands
### a vessel. Only propulsive landing - so really designed for planets
### without an atmosphere.
##############################################################################
def main():
''' Main function that demonstrates how to use the pieces of
this landing library.
Connects to KRPC server, performs a deorbit burn, then a
suicide burn, then a final descent.
'''
conn = krpc.connect()
sc = conn.space_center
v = sc.active_vessel
deorbit(v, .3)
suicide_burn(conn, v)
final_descent(v)
###############################################################################
### High Level Functions
###
### These are the ones you'd actually maybe want to call from your script.
###############################################################################
def deorbit(vessel, eccentricity):
'''executes a deorbit burn until the given eccentricity
is achieved. If periapsis remains above the equatorial
radius, it continues burning.
'''
print("Deorbiting Vessel...")
vessel.control.speed_mode = vessel.control.speed_mode.surface
ap = vessel.auto_pilot
ap.sas = True
time.sleep(.1)
ap.sas_mode = ap.sas_mode.retrograde
while (vessel.orbit.eccentricity < eccentricity or
vessel.orbit.periapsis_altitude > 0):
vessel.control.throttle = 1.0
vessel.control.throttle = 0.0
def suicide_burn(conn, vessel, autowarp=True):
'''
Performs a 'Suicide Burn' using the calculator class.
'''
print("Calculating Suicide Burn...")
telem = vessel.flight(vessel.orbit.body.reference_frame)
vessel.control.speed_mode = vessel.control.speed_mode.surface
rf = vessel.orbit.body.reference_frame
ap = vessel.auto_pilot
ap.sas = True
time.sleep(.1)
ap.sas_mode = ap.sas_mode.retrograde
##calculate initial burn
computer = suicide_burn_calculator(conn, vessel, 5000)
computer.update() # run once with altitude of 5000m to get an estimated groundtrack distance
touchdown = coords_down_bearing(telem.latitude, telem.longitude, (180 + telem.heading), computer.ground_track,
vessel.orbit.body)
# update actual safe altitude over the groundtrack
computer.alt = check_terrain(telem.latitude, telem.longitude, touchdown[0], touchdown[1], vessel.orbit.body)
# Initial burn - aiming for the 'safe alt' (highest point over course)
# and a vertical velocity of 10 m/s
burn_until_velocity(conn, vessel, telem, 10, computer, True)
## update computer ti aim for 10m above current altitude.
computer.alt = vessel.orbit.body.surface_height(telem.latitude, telem.longitude) + 10
# Second half of burn - aiming for ground alt + 10m and a vertical
# velocity of 5 m/s
burn_until_velocity(conn, vessel, telem, 5, computer, True)
def burn_until_velocity(conn, vessel, telem, thresh, computer, autowarp):
'''
Helper Function for suicide_burn function - actually executes a burn
at the calculated time and burns until it reaches the threshold
vertical velocity.
'''
countdown = computer.update()
print("Burn in {} seconds".format(countdown))
if autowarp and (countdown > thresh):
conn.space_center.warp_to(conn.space_center.ut + countdown - 10) # warp to 10 seconds before burn
while countdown > 0.0: # Wait until suicide burn
time.sleep(.1)
countdown = computer.update()
while telem.vertical_speed < (-1 * thresh): # Loop until we're ready for final descent
countdown = computer.update()
vessel.control.throttle = .95 # 95% throttle
if countdown < 0.0: # use the emergencty 5% of throttle when needed
vessel.control.throttle = 1.0
vessel.control.throttle = 0.0
def final_descent(v):
''' manages final descent. At the moment keeps vertical velocity limited to 1/10 of the
current altitude above terrain - so at 200m would try to descend at 20m/s and at 10 m/s locks
descent speed to 1m/s.
'''
print("final descent")
telem = v.flight(v.orbit.body.reference_frame)
v.control.speed_mode = v.control.speed_mode.surface
ap = v.auto_pilot
ap.sas = True
time.sleep(.1)
ap.sas_mode = ap.sas_mode.retrograde
p = PID(.25, .25, .025)
while v.situation is not v.situation.landed:
# ap.sas_mode = ap.sas_mode.retrograde # SAS leaves retrograde mode if velocity gets to zero.
safe_descent = telem.surface_altitude / -10
# print safe_descent
if safe_descent < -15.0:
safe_descent = -15.0
p.setpoint(safe_descent)
v.control.throttle = p.update(telem.vertical_speed)
v.control.throttle = 0
###############################################################################
## Burn Calculator Class
###############################################################################
class suicide_burn_calculator(object):
'''
Class that calculates time until suicide burn.
'''
def __init__(self, conn, v, alt):
self.conn = conn
self.v = v
self.sc = conn.space_center
self.burn_time = np.inf
self.burn_duration = np.inf
self.ground_track = np.inf
self.effective_decel = np.inf
self.radius = np.inf
self.angle_from_horizontal = np.inf
self.impact_time = np.inf
self.alt = alt
self.rf = self.sc.ReferenceFrame.create_hybrid(
position=self.v.orbit.body.reference_frame,
rotation=self.v.surface_reference_frame)
def update(self):
'''
Returns an estimate of how many seconds until you need to burn at 95% throttle to avoid crashing.
This gives a 5% safety margin.
I do not even PRETEND to understand all of the math in this function. It's essentially a porting
of the routine from the Mechjeb orbit extensions.
'''
if self.v.orbit.periapsis_altitude > 0: ## We're not on a landing trajectory yet.
self.burn_time = np.inf
self.burn_duration = np.inf
self.ground_track = np.inf
self.effective_decel = np.inf
self.angle_from_horizontal = np.inf
self.impact_time = np.inf
return self.burn_time
rf = self.v.orbit.body.reference_frame
# calculate sin of angle from horizontal -
v1 = self.v.velocity(self.rf)
v2 = (0, 0, 1)
self.angle_from_horizontal = angle_between(v1, v2)
sine = math.sin(self.angle_from_horizontal)
# estimate deceleration time
g = self.v.orbit.body.surface_gravity
T = (self.v.max_thrust / self.v.mass) * .95 # calculating with 5% safety margin!
self.effective_decel = .5 * (-2 * g * sine + math.sqrt((2 * g * sine) * (2 * g * sine) + 4 * (T * T - g * g)))
self.decel_time = self.v.flight(self.rf).speed / self.effective_decel
# estimate time until burn
radius = self.v.orbit.body.equatorial_radius + self.alt
TA = self.v.orbit.true_anomaly_at_radius(radius)
TA = -1 * TA # look on the negative (descending) side of the orbit
self.impact_time = self.v.orbit.ut_at_true_anomaly(TA)
self.burn_time = self.impact_time - self.decel_time / 2
self.ground_track = ((self.burn_time - self.sc.ut) * self.v.flight(self.rf).speed) + (
.5 * self.v.flight(self.rf).speed * self.decel_time)
return self.burn_time - self.sc.ut
###############################################################################
## Ground Navigation Functions - Probably ought to move to their
## own library file some day.
###############################################################################
def coords_down_bearing(lat, lon, bearing, distance, body):
'''
Takes a latitude, longitude and bearing in degrees, and a
distance in meters over a given body. Returns a tuple
(latitude, longitude) of the point you've calculated.
'''
bearing = math.radians(bearing)
R = body.equatorial_radius
lat = math.radians(lat)
lon = math.radians(lon)
lat2 = math.asin(math.sin(lat) * math.cos(distance / R) +
math.cos(lat) * math.sin(distance / R) * math.cos(bearing))
lon2 = lon + math.atan2(math.sin(bearing) * math.sin(distance / R
) * math.cos(lat), math.cos(distance / R) - math.sin(lat
) * math.sin(
lat2))
lat2 = math.degrees(lat2)
lon2 = math.degrees(lon2)
return (lat2, lon2)
def check_terrain(lat1, lon1, lat2, lon2, body):
'''
Returns an estimate of the highest terrain altitude betwen
two latitude / longitude points.
'''
lat = lat1
lon = lon1
highest_lat = lat
highest_lon = lon
highest_alt = body.surface_height(lat, lon)
latstep = (lat2 - lat1) / 20
lonstep = (lon2 - lon1) / 20
for x in range(20):
test_alt = body.surface_height(lat, lon)
if test_alt > highest_alt:
highest_lat = lat
highest_lon = lon
highest_alt = test_alt
lat = lat + latstep
lon = lon + lonstep
return highest_alt
###############################################################################
## Vector Math Functions - Probably ought to move to their
## own library file some day.
###############################################################################
def unit_vector(vector):
""" Returns the unit vector of the vector provided. """
return vector / np.linalg.norm(vector)
def angle_between(v1, v2):
""" Returns the angle in radians between vectors 'v1' and 'v2'"""
v1_u = unit_vector(v1)
v2_u = unit_vector(v2)
return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))
main()
src/KSP_tools/KRPC_intergration/pid.py
+133
View File
@@ -0,0 +1,133 @@
##############################################################################
### PID Controller Library and Example
##############################################################################
### Like all of the scripts in my folder here, this file contains
### functions you might want to include into your own scripts for
### actual use and a demo in the 'main' function that you can just
### run to see how it works.
###
### This file includes a simple and generic PID controller that we use in
### many of the other examples when we want to smoothly control one value
### based on our measurement of another. The PID class docstring explains
### the basics of using it in your project. The demo code below that
### shows how to use the PID controller to hold a vertical velocity with
### variation of engine thrust.
### From https://github.com/krpc/krpc-library/blob/master/Art_Whaleys_KRPC_Demos/pid.py
##############################################################################
import time
import krpc
class PID(object):
'''
Generic PID Controller Class
Based on the PID recipe at :
http://code.activestate.com/recipes/577231-discrete-pid-controller/
and the code and discussions in the blog at:
http://brettbeauregard.com/blog/2011/04/
improving-the-beginners-pid-introduction/
An instance is created with the format
your_pid=PID(P=.0001, I=0.00001, D=0.000001)
Finding the right values for those three gain numbers is called 'tuning' and
that's beyond the scope of this doc string!
Use your_pid.setpoint(X) to set the target output value of the controller.
Regularly call your_pid.update(Y), passing it the input data that the
controller should respond to.
output_data = your_pid.update(input_data)
'''
def __init__(self, P=1.0, I=0.1, D=0.01):
self.Kp = P # P controls reaction to the instantaneous error
self.Ki = I # I controls reaction to the history of error
self.Kd = D # D prevents overshoot by considering rate of change
self.P = 0.0
self.I = 0.0
self.D = 0.0
self.SetPoint = 0.0 # Target value for controller
self.ClampI = 1.0 # clamps i_term to prevent 'windup.'
self.LastTime = time.time()
self.LastMeasure = 0.0
def update(self, measure):
now = time.time()
change_in_time = now - self.LastTime
if not change_in_time:
change_in_time = 1.0 # avoid potential divide by zero if PID just created.
error = self.SetPoint - measure
self.P = error
self.I += error
self.I = self.clamp_i(self.I) # clamp to prevent windup lag
self.D = (measure - self.LastMeasure) / (change_in_time)
self.LastMeasure = measure # store data for next update
self.lastTime = now
return (self.Kp * self.P) + (self.Ki * self.I) - (self.Kd * self.D)
def clamp_i(self, i):
if i > self.ClampI:
return self.ClampI
elif i < -self.ClampI:
return -self.ClampI
else:
return i
def setpoint(self, value):
self.SetPoint = value
self.I = 0.0
##############################################################################
## Demo Code Below This Line!
##############################################################################
Target_Velocity = 5 # The value we're trying to limit ourselves to
##############################################################################
## Main -- only run when we execute this file directly.
## ignored when we import the PID into other files!
##############################################################################
def main():
# Setup KRPC
conn = krpc.connect()
sc = conn.space_center
v = sc.active_vessel
telem = v.flight(v.orbit.body.reference_frame)
# Create PID controller.
p = PID(P=.25, I=0.025, D=0.0025)
p.ClampI = 20
p.setpoint(Target_Velocity)
# starting with locked SAS and throttle at full
v.control.sas = True
v.control.throttle = 1.0
while not v.thrust: # stage if we just launched a new rocket
v.control.activate_next_stage()
# Loop Forever, or until you get the point of this example and stop it.
while True:
the_pids_output = p.update(telem.vertical_speed)
v.control.throttle = the_pids_output
print('Vertical V:{:03.2f} PID returns:{:03.2f} Throttle:{:03.2f}'
.format(telem.vertical_speed,
the_pids_output,
v.control.throttle))
time.sleep(.1)
if __name__ == '__main__':
main()
print('--')