From c81fc697d2e649911c6b8c422c3c26291afb037a Mon Sep 17 00:00:00 2001 From: zhouyr9 Date: Fri, 16 Jan 2026 16:09:51 +0800 Subject: [PATCH] update --- .../KRPC_intergration/example_landing.py | 292 ++++++++++++++++++ src/KSP_tools/KRPC_intergration/pid.py | 133 ++++++++ 2 files changed, 425 insertions(+) create mode 100644 src/KSP_tools/KRPC_intergration/example_landing.py create mode 100644 src/KSP_tools/KRPC_intergration/pid.py diff --git a/src/KSP_tools/KRPC_intergration/example_landing.py b/src/KSP_tools/KRPC_intergration/example_landing.py new file mode 100644 index 0000000..640df80 --- /dev/null +++ b/src/KSP_tools/KRPC_intergration/example_landing.py @@ -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() + + diff --git a/src/KSP_tools/KRPC_intergration/pid.py b/src/KSP_tools/KRPC_intergration/pid.py new file mode 100644 index 0000000..ccfe5cc --- /dev/null +++ b/src/KSP_tools/KRPC_intergration/pid.py @@ -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('--') \ No newline at end of file