Last active
April 4, 2024 21:22
-
-
Save MikahB/d474a04092bbeff9fa25d3f53f3e41a7 to your computer and use it in GitHub Desktop.
Very Basic Airplane Simulator
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import asyncio | |
| import time | |
| from math import sin, cos, atan2, tan, radians, degrees | |
| import random | |
| from windlock_beta.flight_control.flight_controller import ControlInput | |
| GRAVITY_ACC = 9.80665 # m/s^2 | |
| def mph_to_ms(speed_mph): | |
| return speed_mph * 0.44704 | |
| def mph_to_kts(speed_mph): | |
| return speed_mph * 0.868976 | |
| def mph_to_fps(speed_mph): | |
| return speed_mph * 5280 / (60 * 60) | |
| class Sim172: | |
| def __init__(self): | |
| # Internal members | |
| self.__is_simulating = False # True when simulation is running, false otherwise | |
| self.__is_stalled = False # True when the airplane stalls, false otherwise | |
| self.__last_timestamp = time.time_ns() # last time the simulation ran, use to cal time delta | |
| self.__yoke_translation = 0 # mm from 0 representing neutral trim level. Negative means towards console (down elevator) | |
| self.__yoke_rotation = 0 # degrees from level (neutral aileron). Negative is left | |
| self.__current_pitch_input_mag = 0 # mm/sec - signs match yoke_translation | |
| self.__current_pitch_input_exp = None # integer, nanoseconds since epoch when current input stops | |
| self.__current_roll_input_mag = 0 # degrees/sec - signs match yoke_rotation | |
| self.__current_roll_input_exp = None # integer, nanoseconds since epoch when current input stops | |
| self.__current_turb_roll_mag = 0 # We'll treat turbulence events just like a brief control input | |
| self.__current_turb_pitch_mag = 0 # But turbulence has separate roll/pitch with a single expiration date | |
| self.__current_turb_exp = 0 | |
| self.__next_turb_event = 0 | |
| # Environment | |
| self.wind_direction = 0 # degrees direction wind is coming from (0 = blowing from North to South) | |
| self.wind_speed = 0 # mph - wind speed | |
| self.turbulence_freq = 20 # how frequent is turbulence: 0 is never, 100 is constant | |
| self.turbulence_magnitude = 60 # how aggressive is turbulence: 0 is none, 100 is maximum | |
| # Aircraft Characteristics | |
| self.max_sustainable_climb_rate = 500 # feet per minute, beyond this will decelerate | |
| self.stall_speed = 65 # mph, below this will stall | |
| self.level_cruise_speed = 90 # if not gaining or losing altitude, target speed | |
| self.roll_input_sensitivity = 0.5 # unitless, degrees/sec of rotation for each degree of yoke rotation | |
| self.roll_input_damping = 10 # represents the mass of the aircraft resisting roll input - 100 = cannot change | |
| self.roll_self_correction = 10 # unitless, represents effect of dihedral | |
| self.pitch_input_sensitivity = 0.3 # unitless, degrees/sec of pitch rotation for each mm of yoke translation | |
| self.pitch_input_damping = 10 # represents the mass of the aircraft resisting picth input - 100 = cannot change | |
| # Instrument Readings - Read Only | |
| self.__bank_angle = 0 # degrees, negative is left, positive is right | |
| self.__pitch_angle = 0 # degrees, negative is down, positive is up | |
| self.__current_heading = 0 # degrees, 0 is North | |
| self.__current_airspeed = 0 # mph, current speed of the aircraft | |
| self.__altitude = 0 # feet above ground | |
| self.__climb_rate = 0 # feet per second | |
| @property | |
| def bank_angle(self): | |
| return self.__bank_angle | |
| @property | |
| def pitch_angle(self): | |
| return self.__pitch_angle | |
| @property | |
| def current_heading(self): | |
| return self.__current_heading | |
| @property | |
| def current_airspeed(self): | |
| return self.__current_airspeed | |
| @property | |
| def current_altitude(self): | |
| return self.__altitude | |
| @property | |
| def curent_climb_rate(self): | |
| return self.__climb_rate | |
| @property | |
| def yoke_position(self): | |
| return self.__yoke_translation, self.__yoke_rotation | |
| async def run_simulation(self): | |
| while self.__is_simulating: | |
| await asyncio.sleep(0.01) | |
| current_time = time.time_ns() # save for checking if inputs are expired | |
| delta_t_ms = (current_time - self.__last_timestamp) / 1000000 # convert to milliseconds | |
| if delta_t_ms == 0: | |
| continue | |
| delta_t_sec = delta_t_ms / 1000 | |
| # Check for turbulence | |
| if self.__next_turb_event <= current_time and self.__current_turb_exp <= current_time: | |
| #print('[*] New Turbulence',end='\r\n') | |
| turb = self.get_turbulence() | |
| self.__current_turb_roll_mag = turb[0] | |
| self.__current_turb_pitch_mag = turb[1] | |
| self.__current_turb_exp = turb[2] | |
| self.__next_turb_event = time.time_ns() + ((100 - self.turbulence_freq)/100) * random.random() * 1000000000 | |
| else: | |
| self.__current_turb_exp = 0 | |
| self.__current_turb_pitch_mag = 0 | |
| self.__current_turb_roll_mag = 0 | |
| # First, check for any inputs | |
| if self.__current_roll_input_exp: | |
| if self.__current_roll_input_exp > current_time: | |
| # New Yoke Rotation = Current Rotation + RollInputMagnitude * TimeSpan | |
| self.__yoke_rotation += self.__current_roll_input_mag * delta_t_sec * (1 - (self.roll_input_damping / 100)) | |
| else: | |
| self.__current_roll_input_exp = None | |
| if self.__current_pitch_input_exp: | |
| if self.__current_pitch_input_exp > current_time: | |
| self.__yoke_translation += self.__current_pitch_input_mag * delta_t_sec * (1 - (self.pitch_input_damping / 100)) | |
| else: | |
| self.__current_pitch_input_exp = None | |
| # Now our controls are in the right place, so make the plane respond to current inputs | |
| # We will basically assume no slippage in the air for now | |
| # Aircraft attitude first | |
| self.__bank_angle += self.roll_input_sensitivity * self.__yoke_rotation * delta_t_sec \ | |
| + self.__current_turb_roll_mag * delta_t_sec \ | |
| - 0.75 * sin(radians(self.__bank_angle)) * delta_t_sec # Dihedral effect | |
| # For pitch angle, calculate contribution from Control Input, then from Turbulence | |
| pitch_control_contrib = self.pitch_input_sensitivity * self.__yoke_translation * (delta_t_sec) | |
| pitch_bank_contrib = -6.0 * abs(sin(radians(self.__bank_angle))) * delta_t_sec | |
| pitch_turbulence_contrib = self.__current_turb_pitch_mag * delta_t_sec | |
| self.__pitch_angle += (pitch_control_contrib + pitch_bank_contrib + pitch_turbulence_contrib) | |
| # Calculate altitude and speed first since they affect how much we're turning | |
| slippage = 0.7 # fudge factor to get climb rate and angle closer to reality | |
| new_climb_rate = slippage * (sin(radians(self.pitch_angle)) * mph_to_fps(self.__current_airspeed) * delta_t_sec) * 60 / delta_t_sec | |
| # Now, deduct some climb rate if we're banked | |
| #new_climb_rate -= (sin(radians(self.bank_angle))) * mph_to_fps(self.__current_airspeed) * delta_t_sec * 60 | |
| self.__climb_rate = 0.75 * new_climb_rate + 0.25 * self.__climb_rate | |
| # Adjust speed - need to figure out how to do this better at some point | |
| self.__current_airspeed -= sin(radians(self.pitch_angle)) * (delta_t_sec) | |
| # Calculate radius of turn based on speed and bank angle | |
| if self.bank_angle != 0.0: | |
| turn_radius_ft = (mph_to_kts(self.__current_airspeed)**2) / (11.29 * tan(radians(self.bank_angle))) | |
| calc_heading = self.__current_heading + degrees((mph_to_fps(self.__current_airspeed) * delta_t_sec) / turn_radius_ft) | |
| if calc_heading >= 360: | |
| self.__current_heading = calc_heading - 360 | |
| elif calc_heading < 0: | |
| self.__current_heading = calc_heading + 360 | |
| else: | |
| self.__current_heading = calc_heading | |
| # Finally, update altitude | |
| if self.pitch_angle != 0.0: | |
| self.__altitude += self.__climb_rate * (delta_t_sec) / 60 | |
| #print('[*] SimPlane Turb Roll: {0:6.2f}, Pitch: {1:6.2f}' \ | |
| # .format(self.__current_turb_roll_mag, self.__current_turb_pitch_mag), end='\r') | |
| self.__last_timestamp = current_time # update for next loop | |
| async def start_simulating(self, altitude=1000, heading=0): | |
| self.__is_simulating = True | |
| self.__current_airspeed = self.level_cruise_speed | |
| self.__last_timestamp = time.time_ns() | |
| self.__altitude = altitude | |
| self.__current_heading = heading | |
| asyncio.ensure_future(self.run_simulation()) | |
| def stop_simulating(self): | |
| self.__is_simulating = False | |
| def make_control_inputs(self, roll_input: ControlInput = None, pitch_input: ControlInput = None): | |
| if roll_input: | |
| self.__current_roll_input_mag = roll_input.magnitude | |
| self.__current_roll_input_exp = time.time_ns() + 1000000 * roll_input.milliseconds | |
| if pitch_input: | |
| self.__current_pitch_input_mag = pitch_input.magnitude | |
| self.__current_pitch_input_exp = time.time_ns() + 1000000 * pitch_input.milliseconds | |
| # Returns a tuple with roll_mag, pitch_mag, expiration | |
| def get_turbulence(self): | |
| max_mag = 10.0 # degrees per second pitch or bank angle | |
| roll_mag = max_mag * random.random() * (self.turbulence_magnitude / 100) | |
| roll_mag *= [-1, 1][random.randrange(2)] | |
| pitch_mag = max_mag * random.random() * (self.turbulence_magnitude / 100) | |
| pitch_mag *= [-1, 1][random.randrange(2)] | |
| # Turbulence events can last anywhere from 200 to 800 milliseconds | |
| duration = random.randrange(200, 800) | |
| exp = 1000000 * duration + time.time_ns() | |
| return roll_mag, pitch_mag, exp |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment