From d16ace00a71f1302ae61510b86a37b71b5449124 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Mon, 4 Nov 2024 23:35:08 -0500 Subject: [PATCH 01/63] initial framework --- main.py | 16 ++++++++++++---- simulator/__init__.py | 0 simulator/sim_config.yaml | 6 ++++++ simulator/sim_imu.py | 15 +++++++++++++++ utils.py | 26 ++++++++++++++++++++++---- 5 files changed, 55 insertions(+), 8 deletions(-) create mode 100644 simulator/__init__.py create mode 100644 simulator/sim_config.yaml create mode 100644 simulator/sim_imu.py diff --git a/main.py b/main.py index aab94485..60171760 100644 --- a/main.py +++ b/main.py @@ -21,6 +21,7 @@ PORT, SERVO_PIN, ) +from simulator.sim_imu import SimIMU from utils import arg_parser @@ -41,13 +42,20 @@ def main(args: argparse.Namespace) -> None: sim_time_start = time.time() if args.mock: - # If we are running a simulation, then we will replace our hardware objects with mock + # If we are running a mock simulation, then we will replace our hardware objects with mock # objects that just pretend to be the real hardware. This is useful for testing the # software without having to fly the rocket. MockIMU pretends to be the imu by reading # previous flight data from a log file - imu = MockIMU( - args.path, real_time_simulation=not args.fast_simulation, start_after_log_buffer=True - ) + if not args.sim: + imu = MockIMU( + args.path, + real_time_simulation=not args.fast_simulation, + start_after_log_buffer=True + ) + # If we are running the simulator for generating datasets, we will replace our IMU object + # with a sim variant, similar to running a mock simulation. + else: + imu = SimIMU() # MockFactory is used to create a mock servo object that pretends to be the real servo servo = ( Servo(SERVO_PIN) diff --git a/simulator/__init__.py b/simulator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml new file mode 100644 index 00000000..c788f262 --- /dev/null +++ b/simulator/sim_config.yaml @@ -0,0 +1,6 @@ +# config file for simulator + +sim_apogee: 1800 # The simulator will generate a dataset where the apogee will be around this value (in meters) +time_step: 0.002 # The time step between data points the simulator will generate (in seconds) +motor_thrust_curve: 0 # placeholder +drag_coefficient: 0.4 # coefficient of drag for the rocket \ No newline at end of file diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py new file mode 100644 index 00000000..4c9b137f --- /dev/null +++ b/simulator/sim_imu.py @@ -0,0 +1,15 @@ +"""Module for simulating the IMU on the rocket using generated data.""" + +from airbrakes.hardware.imu import IMU + + +class SimIMU(IMU): + """ + A mock implementation of the IMU for testing purposes. It doesn't interact with any hardware + and returns randomly generated data. + """ + def __init__(self): + """ + Initializes the object that pretends to be an IMU for testing purposes by returning + randomly generated data. + """ diff --git a/utils.py b/utils.py index d26f7e65..c5a89852 100644 --- a/utils.py +++ b/utils.py @@ -99,17 +99,35 @@ def arg_parser() -> argparse.Namespace: default=False, ) + parser.add_argument( + "-s", + "--sim", + help="runs the data simulator alongside the mock simulator, to randomly generate a dataset", + action="store_true", + default=False, + ) + args = parser.parse_args() # Check if the user has passed any options that are only available in simulation mode: if ( - any([args.real_servo, args.keep_log_file, args.fast_simulation, args.debug, args.path]) + any([ + args.real_servo, + args.keep_log_file, + args.fast_simulation, + args.debug, + args.path, + args.sim, + ]) and not args.mock ): parser.error( - "The `--real-servo`, `--keep-log-file`, `--fast-simulation`, `--debug`, and `--path` " - "options are only available in simulation mode. Please pass `-m` or `--mock` " - "to run in simulation mode." + "The `--real-servo`, `--keep-log-file`, `--fast-simulation`, `--debug`, `--path`, " + "and `--sim` options are only available in simulation mode. Please pass `-m` or " + "`--mock` to run in simulation mode." ) + if args.sim and args.path: + parser.error("The `--path` option is not able to be used with the `--sim` option") + return args From bb4037f020af3684df59e32bf4147bf21197e10a Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Tue, 5 Nov 2024 21:47:47 -0500 Subject: [PATCH 02/63] simulating thrust curves --- main.py | 2 +- simulator/data_gen.py | 319 ++++++++++++++++++++ simulator/sim_config.yaml | 17 +- simulator/sim_imu.py | 52 ++++ simulator/test.py | 34 +++ simulator/thrust_curves/AeroTech_L1940X.csv | 25 ++ utils.py | 18 +- 7 files changed, 454 insertions(+), 13 deletions(-) create mode 100644 simulator/data_gen.py create mode 100644 simulator/test.py create mode 100644 simulator/thrust_curves/AeroTech_L1940X.csv diff --git a/main.py b/main.py index 60171760..e37e6ad6 100644 --- a/main.py +++ b/main.py @@ -50,7 +50,7 @@ def main(args: argparse.Namespace) -> None: imu = MockIMU( args.path, real_time_simulation=not args.fast_simulation, - start_after_log_buffer=True + start_after_log_buffer=True, ) # If we are running the simulator for generating datasets, we will replace our IMU object # with a sim variant, similar to running a mock simulation. diff --git a/simulator/data_gen.py b/simulator/data_gen.py new file mode 100644 index 00000000..31fad97b --- /dev/null +++ b/simulator/data_gen.py @@ -0,0 +1,319 @@ +"""Module that creates randomly generated data to sent to the simulator IMU""" + +import csv +from pathlib import Path + +import numpy as np +import numpy.typing as npt +import yaml +from scipy.spatial.transform import Rotation as R + +from airbrakes.data_handling.imu_data_packet import ( + EstimatedDataPacket, + RawDataPacket, +) +from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY +from utils import deadband + + +class DataGenerator: + """ + Uses config settings to generate realistic flight trajectories. Returns raw and + estimated data points for the sim IMU to combine into data packets + """ + + __slots__ = ( + "_config", + "_current_timestamp", + "_last_est_packet", + "_last_raw_packet", + "_last_velocity", + "_thrust_data", + "_vertical_index", + ) + + def __init__(self): + """Initializes the data generator object""" + self._current_timestamp: np.float64 = np.float64(0.0) + self._last_est_packet: EstimatedDataPacket | None = None + self._last_raw_packet: RawDataPacket | None = None + self._last_velocity: np.float64 = np.float64(0.0) + + # loads the sim_config.yaml file + config_path = Path("simulator/sim_config.yaml") + with config_path.open(mode="r", newline="") as file: + self._config: dict = yaml.safe_load(file) + + # finds the vertical index of the orientation. For example, if -y is vertical, the index + # will be 1. + self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] + self._thrust_data: npt.NDArray = self._load_thrust_curve() + + def _load_thrust_curve(self) -> npt.NDArray: + """ + Loads the thrust curve from the motor specified in the configs. + + :return: numpy array containing tuples with the time and thrust at that time. + """ + # gets the path of the csv based on the config file + csv_path = Path(f"simulator/thrust_curves/{self._config["motor"]}.csv") + + # initializes the list for timestamps and thrust values + motor_timestamps = [ + 0, + ] + motor_thrusts = [ + 0, + ] + + start_flag = False # flag to identify when the metadata/header rows are skipped + + with csv_path.open(mode="r", newline="") as thrust_csv: + reader = csv.reader(thrust_csv) + + for row in reader: + # Start appending data only after the header row + if row == ["Time (s)", "Thrust (N)"]: + start_flag = True + continue # Skip header row itself + + if start_flag: + # Convert time and thrust values to floats and append as a tuple + time = float(row[0]) + thrust = float(row[1]) + motor_timestamps.append(time) + motor_thrusts.append(thrust) + + return np.array([motor_timestamps, motor_thrusts]) + + def _first_update(self) -> npt.NDArray: + """ + Sets up the initial values for the estimated and raw data packets. This should + only be called once, and all values will be approximate launch pad conditions. + + :return: numpy array containing a raw data packet and an estimated data packet + respectively, immitating initial conditions on the launch pad. + """ + orientation = self._config["rocket_orientation"] + initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) + initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) + + return np.array( + [ + RawDataPacket( + 0, + scaledAccelX=orientation[0], + scaledAccelY=orientation[1], + scaledAccelZ=orientation[2], + scaledGyroX=0.0, + scaledGyroY=0.0, + scaledGyroZ=0.0, + deltaVelX=0.0, + deltaVelY=0.0, + deltaVelZ=0.0, + deltaThetaX=0.0, + deltaThetaY=0.0, + deltaThetaZ=0.0, + ), + EstimatedDataPacket( + 0, + estOrientQuaternionW=initial_quaternion[0], + estOrientQuaternionX=initial_quaternion[1], + estOrientQuaternionY=initial_quaternion[2], + estOrientQuaternionZ=initial_quaternion[3], + estCompensatedAccelX=GRAVITY * orientation[0], + estCompensatedAccelY=GRAVITY * orientation[1], + estCompensatedAccelZ=GRAVITY * orientation[2], + estPressureAlt=0, + estGravityVectorX=-GRAVITY * orientation[0], + estGravityVectorY=-GRAVITY * orientation[1], + estGravityVectorZ=-GRAVITY * orientation[2], + estAngularRateX=0, + estAngularRateY=0, + estAngularRateZ=0, + ), + ] + ) + + def generate_data_packet(self) -> npt.NDArray: + """ + Generates data points and sends to queue + + :return: Numpy list containing a raw data packet, an estimated data packet, or both. + """ + # gets the next timestamp + next_timestamp = self.update_timestamp(self._current_timestamp) + # If the simulation has just started, we want to just generate the initial data points + # and initialize "self._last_" variables + if any(packet is None for packet in [self._last_est_packet, self._last_raw_packet]): + self._last_raw_packet, self._last_est_packet = self._first_update() + self._current_timestamp = next_timestamp + return np.array([self._last_raw_packet, self._last_est_packet]) + + raw_time_step = self._config["raw_time_step"] + est_time_step = self._config["est_time_step"] + + # creates a boolean array to indicate whether the next timestamp will have a raw + # data packet, an estimated data packet, or both. + packet_type_flag = np.array( + [ + any(np.isclose(next_timestamp % raw_time_step, [0, raw_time_step])), + any(np.isclose(next_timestamp % est_time_step, [0, est_time_step])), + ] + ) + + # just as a short-hand + orientation = self._config["rocket_orientation"] + + # finds acceleration from thrust curve data, if the timestamp is before the cutoff time + # of the motor + if next_timestamp <= self._thrust_data[0][-1]: + interpreted_thrust = np.interp( + next_timestamp, self._thrust_data[0], self._thrust_data[1] + ) + vertical_linear_accel = interpreted_thrust / self._config["rocket_mass"] + + new_packets = np.array([None, None]) + + # assembles the raw data packet + if packet_type_flag[0]: + # scaled acceleration is compensated, but divided by gravity. On launch pad, the + # vertical sacled acceleration will be -1.00 (if negative is vertical). + # this value should always be positive though (during motor burn) + vert_scaled_accel = 1 + vertical_linear_accel / GRAVITY + + # gets the last vertical scaled accel, flips depending on the vertical index + last_vert_scaled_accel = [ + self._last_raw_packet.scaledAccelX, + self._last_raw_packet.scaledAccelY, + self._last_raw_packet.scaledAccelZ, + ][self._vertical_index] + + # finds the vertical delta velocity + vert_delta_v = GRAVITY * (vert_scaled_accel - last_vert_scaled_accel) * raw_time_step + + raw_data_packet = RawDataPacket( + next_timestamp * 1e9, + scaledAccelX=vert_scaled_accel * orientation[0], + scaledAccelY=vert_scaled_accel * orientation[1], + scaledAccelZ=vert_scaled_accel * orientation[2], + scaledGyroX=0.0, + scaledGyroY=0.0, + scaledGyroZ=0.0, + deltaVelX=vert_delta_v * orientation[0], + deltaVelY=vert_delta_v * orientation[1], + deltaVelZ=vert_delta_v * orientation[2], + deltaThetaX=0.0, + deltaThetaY=0.0, + deltaThetaZ=0.0, + ) + # appends to array + new_packets[0] = raw_data_packet + + # updates last packet + self._last_raw_packet = raw_data_packet + + # Assembles the estimated data packet + if packet_type_flag[1]: + # default value is positive during motor burn + vert_comp_accel = vertical_linear_accel + GRAVITY + + # gets vertical velocity and finds updated altitude + vert_velocity = self._calculate_vertical_velocity(vert_comp_accel, est_time_step) + last_altitude = self._last_est_packet.estPressureAlt + new_altitude = last_altitude + vert_velocity * est_time_step + + # gets previous quaternion + last_quat = np.array( + [ + self._last_est_packet.estOrientQuaternionW, + self._last_est_packet.estOrientQuaternionX, + self._last_est_packet.estOrientQuaternionY, + self._last_est_packet.estOrientQuaternionZ, + ] + ) + + est_data_packet = EstimatedDataPacket( + next_timestamp * 1e9, + estOrientQuaternionW=last_quat[0], + estOrientQuaternionX=last_quat[1], + estOrientQuaternionY=last_quat[2], + estOrientQuaternionZ=last_quat[3], + estCompensatedAccelX=vert_comp_accel * orientation[0], + estCompensatedAccelY=vert_comp_accel * orientation[1], + estCompensatedAccelZ=vert_comp_accel * orientation[2], + estPressureAlt=new_altitude, + estGravityVectorX=-GRAVITY * orientation[0], + estGravityVectorY=-GRAVITY * orientation[1], + estGravityVectorZ=-GRAVITY * orientation[2], + estAngularRateX=0, + estAngularRateY=0, + estAngularRateZ=0, + ) + + # appends to array + new_packets[1] = est_data_packet + + # updates last packet + self._last_est_packet = est_data_packet + + # updates the timestamp + self._current_timestamp = next_timestamp + + return new_packets + + def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: + """ + Calculates the velocity of the rocket based on the compensated acceleration. + Integrates that acceleration to get the velocity. + + :return: the velocity of the rocket + """ + # deadbands the acceleration + acceleration = deadband(acceleration - GRAVITY, ACCELERATION_NOISE_THRESHOLD) + + # Integrate the acceleration to get the velocity + vertical_velocity = self._last_velocity + acceleration * time_diff + + # updates the last vertical velocity + self._last_velocity = vertical_velocity + + return vertical_velocity + + def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + """ + Updates the current timestamp of the data generator, based off time step defined in config. + Will also determine if the next timestamp will be a raw packet, estimated packet, or both. + + :param current_timestamp: the current timestamp of the simulation + + :return: the updated current timestamp, rounded to 3 decimals + """ + + # finding whether the raw or estimated data packets have a lower time_step + lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) + highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) + + # checks if current time is a multiple of the highest and/or lowest time step + at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) + at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) + + # If current timestamp is a multiple of both, the next timestamp will be the + # the current timestamp + the lower time steps + if all([at_low, at_high]): + return np.round(current_timestamp + lowest_dt, 3) + + # If timestamp is a multiple of just the lowest time step, the next will be + # either current + lowest, or the next timestamp that is divisible by the highest + if at_low and not at_high: + dt = min(lowest_dt, highest_dt - (current_timestamp % highest_dt)) + return np.round(current_timestamp + dt, 3) + + # If timestamp is a multiple of only the highest time step, the next will + # always be the next timestamp that is divisible by the lowest + if at_high and not at_low: + return np.round(current_timestamp + lowest_dt - (current_timestamp % lowest_dt), 3) + + # This would happen if the input current timestamp is not a multiple of the raw + # or estimated time steps, or if there is a rounding/floating point error. + raise ValueError("Could not update timestamp, time stamp is invalid") diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index c788f262..b9e1e438 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -1,6 +1,15 @@ # config file for simulator -sim_apogee: 1800 # The simulator will generate a dataset where the apogee will be around this value (in meters) -time_step: 0.002 # The time step between data points the simulator will generate (in seconds) -motor_thrust_curve: 0 # placeholder -drag_coefficient: 0.4 # coefficient of drag for the rocket \ No newline at end of file +sim_apogee: 1800 # Simulator will generate data with the apogee around this value (in meters) +raw_time_step: 0.001 # Time step between raw data packets the simulator will generate (in seconds) +est_time_step: 0.002 # Time step between estimated data packets the simulator will generate (in seconds) + +# selects the motor to use, this is the name of the csv file located in /simulator/thrust_curves +# with the ".csv" removed +motor: "AeroTech_L1940X" +drag_coefficient: 0.4 # coefficient of drag for the rocket +rocket_mass: 16 # mass of rocket (in kg) + +# vertical orientation of the rocket on the launch pad. only one value should be non-zero. +# Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) +rocket_orientation: [0, 0, -1] \ No newline at end of file diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 4c9b137f..7a47a3ed 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -1,6 +1,15 @@ """Module for simulating the IMU on the rocket using generated data.""" +import contextlib +import multiprocessing +import time + +from airbrakes.data_handling.imu_data_packet import ( + IMUDataPacket, +) from airbrakes.hardware.imu import IMU +from constants import MAX_QUEUE_SIZE +from simulator.data_gen import DataGenerator class SimIMU(IMU): @@ -8,8 +17,51 @@ class SimIMU(IMU): A mock implementation of the IMU for testing purposes. It doesn't interact with any hardware and returns randomly generated data. """ + + __slots__ = ( + "_data_fetch_process", + "_data_generator", + "_data_queue", + "_running", + ) + def __init__(self): """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. """ + # This limits the queue size to a very high limit, because the data generator will + # generate all of the data before the imu reads it + self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue( + MAX_QUEUE_SIZE + ) + + # Starts the process that fetches the generated data + self._data_fetch_process = multiprocessing.Process( + target=self._fetch_data_loop, + name="Sim IMU Process", + ) + + # Makes a boolean value that is shared between processes + self._running = multiprocessing.Value("b", False) + + self._data_generator = DataGenerator() + + def _fetch_data_loop(self) -> None: + """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated + data.""" + # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the + # function in a context manager to suppress the KeyboardInterrupt + with contextlib.suppress(KeyboardInterrupt): + for i in range(2000): + packets = self._data_generator.generate_data_packet() + timestamp = 0 + if packets[0] is not None: + self._data_queue.put(packets[0]) + timestamp = packets[0].timestamp / 1e9 + if packets[1] is not None: + self._data_queue.put(packets[1]) + timestamp = packets[1].timestamp / 1e9 + + dt = self._data_generator.update_timestamp(timestamp) - timestamp + time.sleep(dt) diff --git a/simulator/test.py b/simulator/test.py new file mode 100644 index 00000000..41a59605 --- /dev/null +++ b/simulator/test.py @@ -0,0 +1,34 @@ +import numpy as np +from scipy.spatial.transform import Rotation as R + +reference_vector = np.array([0, 0.001, -0.999]) + +orientation = [-1, 0, 0] +rotation = R.from_quat([0.61057544, -0.34405011, -0.60791582, -0.37318307], scalar_first=True) +rotation, _ = R.align_vectors( + [reference_vector], + [orientation], +) +print(R.as_quat(rotation, scalar_first=True)) +vec = rotation.apply(orientation) +print(vec) + +orientation = [1, 0, 0] +rotation = R.from_quat([0.37394506, -0.59507751, 0.41232952, 0.57968295], scalar_first=True) +rotation, _ = R.align_vectors( + [reference_vector], + [orientation], +) +print(R.as_quat(rotation, scalar_first=True)) +vec = rotation.apply(orientation) +print(vec) + +orientation = [0, 0, -1] +rotation = R.from_quat([0.85752511, 0.02006555, 0.00998313, -0.51395386], scalar_first=True) +rotation, _ = R.align_vectors( + [reference_vector], + [orientation], +) +print(R.as_quat(rotation, scalar_first=True)) +vec = rotation.apply(orientation) +print(vec) diff --git a/simulator/thrust_curves/AeroTech_L1940X.csv b/simulator/thrust_curves/AeroTech_L1940X.csv new file mode 100644 index 00000000..1328b5b1 --- /dev/null +++ b/simulator/thrust_curves/AeroTech_L1940X.csv @@ -0,0 +1,25 @@ +"motor:","AeroTech L1940X" +"contributor:","Mike Caplinger" +"details:","https://www.thrustcurve.org/simfiles/60ac84458dc4640004c24eb6/" + +"Time (s)","Thrust (N)" +0.02,48.137 +0.04,2107.333 +0.055,2289.184 +0.073,2150.122 +0.156,2101.985 +0.397,2150.122 +0.691,2155.47 +0.982,2118.03 +1.41,2000.362 +1.696,1898.739 +1.935,1839.905 +2.06,1818.511 +2.103,1652.705 +2.116,1374.58 +2.133,935.998 +2.163,534.856 +2.214,240.685 +2.244,112.32 +2.296,5.349 +2.309,0 diff --git a/utils.py b/utils.py index c5a89852..414c5585 100644 --- a/utils.py +++ b/utils.py @@ -111,14 +111,16 @@ def arg_parser() -> argparse.Namespace: # Check if the user has passed any options that are only available in simulation mode: if ( - any([ - args.real_servo, - args.keep_log_file, - args.fast_simulation, - args.debug, - args.path, - args.sim, - ]) + any( + [ + args.real_servo, + args.keep_log_file, + args.fast_simulation, + args.debug, + args.path, + args.sim, + ] + ) and not args.mock ): parser.error( From c2d7235270f13635e1717fad47996d42d2f4867b Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Tue, 5 Nov 2024 22:10:13 -0500 Subject: [PATCH 03/63] ruff --- simulator/sim_imu.py | 9 +++++---- simulator/test.py | 34 ---------------------------------- 2 files changed, 5 insertions(+), 38 deletions(-) delete mode 100644 simulator/test.py diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 7a47a3ed..2a2bef1b 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -3,10 +3,11 @@ import contextlib import multiprocessing import time +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from airbrakes.data_handling.imu_data_packet import IMUDataPacket -from airbrakes.data_handling.imu_data_packet import ( - IMUDataPacket, -) from airbrakes.hardware.imu import IMU from constants import MAX_QUEUE_SIZE from simulator.data_gen import DataGenerator @@ -53,7 +54,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - for i in range(2000): + for _ in range(2000): packets = self._data_generator.generate_data_packet() timestamp = 0 if packets[0] is not None: diff --git a/simulator/test.py b/simulator/test.py deleted file mode 100644 index 41a59605..00000000 --- a/simulator/test.py +++ /dev/null @@ -1,34 +0,0 @@ -import numpy as np -from scipy.spatial.transform import Rotation as R - -reference_vector = np.array([0, 0.001, -0.999]) - -orientation = [-1, 0, 0] -rotation = R.from_quat([0.61057544, -0.34405011, -0.60791582, -0.37318307], scalar_first=True) -rotation, _ = R.align_vectors( - [reference_vector], - [orientation], -) -print(R.as_quat(rotation, scalar_first=True)) -vec = rotation.apply(orientation) -print(vec) - -orientation = [1, 0, 0] -rotation = R.from_quat([0.37394506, -0.59507751, 0.41232952, 0.57968295], scalar_first=True) -rotation, _ = R.align_vectors( - [reference_vector], - [orientation], -) -print(R.as_quat(rotation, scalar_first=True)) -vec = rotation.apply(orientation) -print(vec) - -orientation = [0, 0, -1] -rotation = R.from_quat([0.85752511, 0.02006555, 0.00998313, -0.51395386], scalar_first=True) -rotation, _ = R.align_vectors( - [reference_vector], - [orientation], -) -print(R.as_quat(rotation, scalar_first=True)) -vec = rotation.apply(orientation) -print(vec) From 9f9c2ca9241f5f5f9f02e40d78eeadf36a992550 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Wed, 6 Nov 2024 00:42:32 -0500 Subject: [PATCH 04/63] added drag force and coast phase --- simulator/data_gen.py | 19 ++++++++++++++++--- simulator/sim_config.yaml | 3 ++- simulator/sim_imu.py | 2 +- 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/simulator/data_gen.py b/simulator/data_gen.py index 31fad97b..0875351f 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -165,13 +165,19 @@ def generate_data_packet(self) -> npt.NDArray: # just as a short-hand orientation = self._config["rocket_orientation"] - # finds acceleration from thrust curve data, if the timestamp is before the cutoff time + # initializes the net force, drag force and thrust (if applicable) will be added + net_force = 0 + # finds force from thrust curve data, if the timestamp is before the cutoff time # of the motor if next_timestamp <= self._thrust_data[0][-1]: - interpreted_thrust = np.interp( + net_force = np.interp( next_timestamp, self._thrust_data[0], self._thrust_data[1] ) - vertical_linear_accel = interpreted_thrust / self._config["rocket_mass"] + # subtracts drag force from net force + net_force -= self._calculate_drag_force(self._last_velocity) + # subtracts weight force from net force + net_force -= GRAVITY*self._config["rocket_mass"] + vertical_linear_accel = net_force / self._config["rocket_mass"] new_packets = np.array([None, None]) @@ -317,3 +323,10 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: # This would happen if the input current timestamp is not a multiple of the raw # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") + + def _calculate_drag_force(self,velocity): + # we could probably actually calculate this, maybe we can set temp as a constant? + roe = 1.1 + reference_area = self._config["reference_area"] + drag_coefficient = self._config["drag_coefficient"] + return 0.5 * roe * reference_area * drag_coefficient * velocity**2 diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index b9e1e438..89dad200 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -8,7 +8,8 @@ est_time_step: 0.002 # Time step between estimated data packets the simulator wi # with the ".csv" removed motor: "AeroTech_L1940X" drag_coefficient: 0.4 # coefficient of drag for the rocket -rocket_mass: 16 # mass of rocket (in kg) +rocket_mass: 14.5 # mass of rocket (in kg) +reference_area: 0.01929 # reference area (in meters squared) # vertical orientation of the rocket on the launch pad. only one value should be non-zero. # Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 2a2bef1b..17635f23 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -54,7 +54,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - for _ in range(2000): + for _ in range(21000): packets = self._data_generator.generate_data_packet() timestamp = 0 if packets[0] is not None: From 49760990b29ed3cdd6b1f01b4529488373c463a9 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Wed, 6 Nov 2024 21:50:29 -0500 Subject: [PATCH 05/63] reformatting --- simulator/data_gen.py | 373 ++++++++++++++++++++---------------------- simulator/sim_imu.py | 84 ++++++++-- simulator/test.py | 5 + 3 files changed, 256 insertions(+), 206 deletions(-) create mode 100644 simulator/test.py diff --git a/simulator/data_gen.py b/simulator/data_gen.py index 0875351f..13afb94b 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -10,6 +10,7 @@ from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, + IMUDataPacket, RawDataPacket, ) from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY @@ -24,7 +25,6 @@ class DataGenerator: __slots__ = ( "_config", - "_current_timestamp", "_last_est_packet", "_last_raw_packet", "_last_velocity", @@ -34,7 +34,6 @@ class DataGenerator: def __init__(self): """Initializes the data generator object""" - self._current_timestamp: np.float64 = np.float64(0.0) self._last_est_packet: EstimatedDataPacket | None = None self._last_raw_packet: RawDataPacket | None = None self._last_velocity: np.float64 = np.float64(0.0) @@ -86,169 +85,50 @@ def _load_thrust_curve(self) -> npt.NDArray: return np.array([motor_timestamps, motor_thrusts]) - def _first_update(self) -> npt.NDArray: + def _get_first_packet( + self,packet_type: RawDataPacket | EstimatedDataPacket + ) -> RawDataPacket | EstimatedDataPacket: """ Sets up the initial values for the estimated and raw data packets. This should only be called once, and all values will be approximate launch pad conditions. - :return: numpy array containing a raw data packet and an estimated data packet - respectively, immitating initial conditions on the launch pad. + :param packet_type: either RawDataPacket or EstimatedDataPacket class objects. + used as identifier. + + :return: data packet of the specified type with launch pad conditions. """ orientation = self._config["rocket_orientation"] initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) - return np.array( - [ - RawDataPacket( - 0, - scaledAccelX=orientation[0], - scaledAccelY=orientation[1], - scaledAccelZ=orientation[2], - scaledGyroX=0.0, - scaledGyroY=0.0, - scaledGyroZ=0.0, - deltaVelX=0.0, - deltaVelY=0.0, - deltaVelZ=0.0, - deltaThetaX=0.0, - deltaThetaY=0.0, - deltaThetaZ=0.0, - ), - EstimatedDataPacket( - 0, - estOrientQuaternionW=initial_quaternion[0], - estOrientQuaternionX=initial_quaternion[1], - estOrientQuaternionY=initial_quaternion[2], - estOrientQuaternionZ=initial_quaternion[3], - estCompensatedAccelX=GRAVITY * orientation[0], - estCompensatedAccelY=GRAVITY * orientation[1], - estCompensatedAccelZ=GRAVITY * orientation[2], - estPressureAlt=0, - estGravityVectorX=-GRAVITY * orientation[0], - estGravityVectorY=-GRAVITY * orientation[1], - estGravityVectorZ=-GRAVITY * orientation[2], - estAngularRateX=0, - estAngularRateY=0, - estAngularRateZ=0, - ), - ] - ) - - def generate_data_packet(self) -> npt.NDArray: - """ - Generates data points and sends to queue - - :return: Numpy list containing a raw data packet, an estimated data packet, or both. - """ - # gets the next timestamp - next_timestamp = self.update_timestamp(self._current_timestamp) - # If the simulation has just started, we want to just generate the initial data points - # and initialize "self._last_" variables - if any(packet is None for packet in [self._last_est_packet, self._last_raw_packet]): - self._last_raw_packet, self._last_est_packet = self._first_update() - self._current_timestamp = next_timestamp - return np.array([self._last_raw_packet, self._last_est_packet]) - - raw_time_step = self._config["raw_time_step"] - est_time_step = self._config["est_time_step"] - - # creates a boolean array to indicate whether the next timestamp will have a raw - # data packet, an estimated data packet, or both. - packet_type_flag = np.array( - [ - any(np.isclose(next_timestamp % raw_time_step, [0, raw_time_step])), - any(np.isclose(next_timestamp % est_time_step, [0, est_time_step])), - ] - ) - - # just as a short-hand - orientation = self._config["rocket_orientation"] - - # initializes the net force, drag force and thrust (if applicable) will be added - net_force = 0 - # finds force from thrust curve data, if the timestamp is before the cutoff time - # of the motor - if next_timestamp <= self._thrust_data[0][-1]: - net_force = np.interp( - next_timestamp, self._thrust_data[0], self._thrust_data[1] - ) - # subtracts drag force from net force - net_force -= self._calculate_drag_force(self._last_velocity) - # subtracts weight force from net force - net_force -= GRAVITY*self._config["rocket_mass"] - vertical_linear_accel = net_force / self._config["rocket_mass"] - - new_packets = np.array([None, None]) - - # assembles the raw data packet - if packet_type_flag[0]: - # scaled acceleration is compensated, but divided by gravity. On launch pad, the - # vertical sacled acceleration will be -1.00 (if negative is vertical). - # this value should always be positive though (during motor burn) - vert_scaled_accel = 1 + vertical_linear_accel / GRAVITY - - # gets the last vertical scaled accel, flips depending on the vertical index - last_vert_scaled_accel = [ - self._last_raw_packet.scaledAccelX, - self._last_raw_packet.scaledAccelY, - self._last_raw_packet.scaledAccelZ, - ][self._vertical_index] - - # finds the vertical delta velocity - vert_delta_v = GRAVITY * (vert_scaled_accel - last_vert_scaled_accel) * raw_time_step - - raw_data_packet = RawDataPacket( - next_timestamp * 1e9, - scaledAccelX=vert_scaled_accel * orientation[0], - scaledAccelY=vert_scaled_accel * orientation[1], - scaledAccelZ=vert_scaled_accel * orientation[2], + packet = None + if packet_type == RawDataPacket: + packet = RawDataPacket( + 0, + scaledAccelX=orientation[0], + scaledAccelY=orientation[1], + scaledAccelZ=orientation[2], scaledGyroX=0.0, scaledGyroY=0.0, scaledGyroZ=0.0, - deltaVelX=vert_delta_v * orientation[0], - deltaVelY=vert_delta_v * orientation[1], - deltaVelZ=vert_delta_v * orientation[2], + deltaVelX=0.0, + deltaVelY=0.0, + deltaVelZ=0.0, deltaThetaX=0.0, deltaThetaY=0.0, deltaThetaZ=0.0, ) - # appends to array - new_packets[0] = raw_data_packet - - # updates last packet - self._last_raw_packet = raw_data_packet - - # Assembles the estimated data packet - if packet_type_flag[1]: - # default value is positive during motor burn - vert_comp_accel = vertical_linear_accel + GRAVITY - - # gets vertical velocity and finds updated altitude - vert_velocity = self._calculate_vertical_velocity(vert_comp_accel, est_time_step) - last_altitude = self._last_est_packet.estPressureAlt - new_altitude = last_altitude + vert_velocity * est_time_step - - # gets previous quaternion - last_quat = np.array( - [ - self._last_est_packet.estOrientQuaternionW, - self._last_est_packet.estOrientQuaternionX, - self._last_est_packet.estOrientQuaternionY, - self._last_est_packet.estOrientQuaternionZ, - ] - ) - - est_data_packet = EstimatedDataPacket( - next_timestamp * 1e9, - estOrientQuaternionW=last_quat[0], - estOrientQuaternionX=last_quat[1], - estOrientQuaternionY=last_quat[2], - estOrientQuaternionZ=last_quat[3], - estCompensatedAccelX=vert_comp_accel * orientation[0], - estCompensatedAccelY=vert_comp_accel * orientation[1], - estCompensatedAccelZ=vert_comp_accel * orientation[2], - estPressureAlt=new_altitude, + else: + packet = EstimatedDataPacket( + 0, + estOrientQuaternionW=initial_quaternion[0], + estOrientQuaternionX=initial_quaternion[1], + estOrientQuaternionY=initial_quaternion[2], + estOrientQuaternionZ=initial_quaternion[3], + estCompensatedAccelX=GRAVITY * orientation[0], + estCompensatedAccelY=GRAVITY * orientation[1], + estCompensatedAccelZ=GRAVITY * orientation[2], + estPressureAlt=0, estGravityVectorX=-GRAVITY * orientation[0], estGravityVectorY=-GRAVITY * orientation[1], estGravityVectorZ=-GRAVITY * orientation[2], @@ -257,16 +137,115 @@ def generate_data_packet(self) -> npt.NDArray: estAngularRateZ=0, ) - # appends to array - new_packets[1] = est_data_packet + return packet + + def generate_raw_data_packet(self) -> RawDataPacket: + """ + Generates a raw data packet containing randomly generated data points - # updates last packet - self._last_est_packet = est_data_packet + :return: RawDataPacket + """ + # creating shorthand variables from config + time_step = self._config["raw_time_step"] + orientation = self._config["rocket_orientation"] - # updates the timestamp - self._current_timestamp = next_timestamp + # If the simulation has just started, we want to just generate the initial raw packet + # and initialize "self._last_" variables + if self._last_raw_packet is None: + self._last_raw_packet = self._get_first_packet(RawDataPacket) + return self._last_raw_packet + + # calculates the timestamp for this packet (in seconds) + next_timestamp = (self._last_raw_packet.timestamp/1e9) + time_step + + # calculates the net force and vertical scaled acceleration + net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + vertical_scaled_accel = net_force / (self._config["rocket_mass"] * GRAVITY) + + # calculates vertical delta velocity + last_vertical_scaled_accel = self._get_vertical_data_point("scaledAccel") + vert_delta_v = (vertical_scaled_accel - last_vertical_scaled_accel) * GRAVITY * time_step + + packet = RawDataPacket( + next_timestamp * 1e9, + scaledAccelX=vertical_scaled_accel * orientation[0], + scaledAccelY=vertical_scaled_accel * orientation[1], + scaledAccelZ=vertical_scaled_accel * orientation[2], + scaledGyroX=0.0, + scaledGyroY=0.0, + scaledGyroZ=0.0, + deltaVelX=vert_delta_v * orientation[0], + deltaVelY=vert_delta_v * orientation[1], + deltaVelZ=vert_delta_v * orientation[2], + deltaThetaX=0.0, + deltaThetaY=0.0, + deltaThetaZ=0.0, + ) + + # updates last raw data packet + self._last_raw_packet = packet - return new_packets + return packet + + def generate_estimated_data_packet(self) -> EstimatedDataPacket: + """ + Generates an estimated data packet containing randomly generated data points + + :return: EstimatedDataPacket + """ + # creating shorthand variables from config + time_step = self._config["est_time_step"] + orientation = self._config["rocket_orientation"] + + # If the simulation has just started, we want to just generate the initial estimated packet + # and initialize "self._last_" variables + if self._last_est_packet is None: + self._last_est_packet = self._get_first_packet(EstimatedDataPacket) + return self._last_raw_packet + + # calculates the timestamp for this packet (in seconds) + next_timestamp = (self._last_est_packet.timestamp/1e9) + time_step + + # calculates the net force and vertical accelerations + net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + vertical_linear_accel = net_force / self._config["rocket_mass"] + vertical_comp_accel = vertical_linear_accel + GRAVITY + + # gets vertical velocity and finds updated altitude + vert_velocity = self._calculate_vertical_velocity(vertical_comp_accel, time_step) + new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step + + # gets previous quaternion + last_quat = np.array( + [ + self._last_est_packet.estOrientQuaternionW, + self._last_est_packet.estOrientQuaternionX, + self._last_est_packet.estOrientQuaternionY, + self._last_est_packet.estOrientQuaternionZ, + ]) + + packet = EstimatedDataPacket( + next_timestamp * 1e9, + estOrientQuaternionW=last_quat[0], + estOrientQuaternionX=last_quat[1], + estOrientQuaternionY=last_quat[2], + estOrientQuaternionZ=last_quat[3], + estCompensatedAccelX=vertical_comp_accel * orientation[0], + estCompensatedAccelY=vertical_comp_accel * orientation[1], + estCompensatedAccelZ=vertical_comp_accel * orientation[2], + estPressureAlt=new_altitude, + estGravityVectorX=-GRAVITY * orientation[0], + estGravityVectorY=-GRAVITY * orientation[1], + estGravityVectorZ=-GRAVITY * orientation[2], + estAngularRateX=0, + estAngularRateY=0, + estAngularRateZ=0, + ) + + # updates last estimated packet + self._last_est_packet = packet + + return packet def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: """ @@ -286,47 +265,53 @@ def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: return vertical_velocity - def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + def _calculate_net_force(self, timestamp, velocity) -> np.float64: """ - Updates the current timestamp of the data generator, based off time step defined in config. - Will also determine if the next timestamp will be a raw packet, estimated packet, or both. + Calculates the drag force, thrust force, and weight, and sums them. - :param current_timestamp: the current timestamp of the simulation + :param timestamp: the timestamp of the rocket to calcuate the net forces at + :param velocity: the vertical velocity at the given instant - :return: the updated current timestamp, rounded to 3 decimals + :return: float containing the net force. Thrust is positive, drag and weight is negative. """ - - # finding whether the raw or estimated data packets have a lower time_step - lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) - highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) - - # checks if current time is a multiple of the highest and/or lowest time step - at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) - at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) - - # If current timestamp is a multiple of both, the next timestamp will be the - # the current timestamp + the lower time steps - if all([at_low, at_high]): - return np.round(current_timestamp + lowest_dt, 3) - - # If timestamp is a multiple of just the lowest time step, the next will be - # either current + lowest, or the next timestamp that is divisible by the highest - if at_low and not at_high: - dt = min(lowest_dt, highest_dt - (current_timestamp % highest_dt)) - return np.round(current_timestamp + dt, 3) - - # If timestamp is a multiple of only the highest time step, the next will - # always be the next timestamp that is divisible by the lowest - if at_high and not at_low: - return np.round(current_timestamp + lowest_dt - (current_timestamp % lowest_dt), 3) - - # This would happen if the input current timestamp is not a multiple of the raw - # or estimated time steps, or if there is a rounding/floating point error. - raise ValueError("Could not update timestamp, time stamp is invalid") - - def _calculate_drag_force(self,velocity): - # we could probably actually calculate this, maybe we can set temp as a constant? - roe = 1.1 + # we could probably actually calculate air density, maybe we set temperature as constant? + air_density = 1.1 reference_area = self._config["reference_area"] drag_coefficient = self._config["drag_coefficient"] - return 0.5 * roe * reference_area * drag_coefficient * velocity**2 + rocket_mass = self._config["rocket_mass"] + + drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 + weight_force = GRAVITY * rocket_mass + thrust_force = 0.0 + + # thrust force is non-zero if the timestamp is within the timeframe of + # the motor burn + if timestamp <= self._thrust_data[0][-1]: + thrust_force = np.interp( + timestamp, self._thrust_data[0], self._thrust_data[1] + ) + return thrust_force - weight_force - drag_force + + def _get_vertical_data_point(self,string_identifier: str) -> np.float64: + """ + gets the last vertical data point specified from a vector data attribute by using IMU + orientation in config + + :param string_identfier: a string representing the exact attribute name of the data packet + + :return: float containing the specified data point + """ + # Dynamically retrieve x, y, z components based on the identifier + values = [ + getattr(self._last_raw_packet, f"{string_identifier}X", None), + getattr(self._last_raw_packet, f"{string_identifier}Y", None), + getattr(self._last_raw_packet, f"{string_identifier}Z", None), + ] + + # Just an edge case, but I really hope this never happens + if any(value is None for value in values): + raise AttributeError( + f"Could not find all components for identifier '{string_identifier}'." + ) + + return values[self._vertical_index] diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 17635f23..4f7a84be 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -3,8 +3,12 @@ import contextlib import multiprocessing import time +from pathlib import Path from typing import TYPE_CHECKING +import numpy as np +import yaml + if TYPE_CHECKING: from airbrakes.data_handling.imu_data_packet import IMUDataPacket @@ -20,10 +24,12 @@ class SimIMU(IMU): """ __slots__ = ( + "_config", "_data_fetch_process", "_data_generator", "_data_queue", "_running", + "_timestamp", ) def __init__(self): @@ -31,6 +37,12 @@ def __init__(self): Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. """ + self._timestamp: np.float64 = np.float64(0.0) + # loads the sim_config.yaml file + config_path = Path("simulator/sim_config.yaml") + with config_path.open(mode="r", newline="") as file: + self._config: dict = yaml.safe_load(file) + # This limits the queue size to a very high limit, because the data generator will # generate all of the data before the imu reads it self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue( @@ -48,21 +60,69 @@ def __init__(self): self._data_generator = DataGenerator() + def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + """ + Updates the current timestamp of the data generator, based off time step defined in config. + Will also determine if the next timestamp will be a raw packet, estimated packet, or both. + + :param current_timestamp: the current timestamp of the simulation + + :return: the updated current timestamp, rounded to 3 decimals + """ + + # finding whether the raw or estimated data packets have a lower time_step + lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) + highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) + + # checks if current time is a multiple of the highest and/or lowest time step + at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) + at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) + + # If current timestamp is a multiple of both, the next timestamp will be the + # the current timestamp + the lower time steps + if all([at_low, at_high]): + return np.round(current_timestamp + lowest_dt, 3) + + # If timestamp is a multiple of just the lowest time step, the next will be + # either current + lowest, or the next timestamp that is divisible by the highest + if at_low and not at_high: + dt = min(lowest_dt, highest_dt - (current_timestamp % highest_dt)) + return np.round(current_timestamp + dt, 3) + + # If timestamp is a multiple of only the highest time step, the next will + # always be the next timestamp that is divisible by the lowest + if at_high and not at_low: + return np.round(current_timestamp + lowest_dt - (current_timestamp % lowest_dt), 3) + + # This would happen if the input current timestamp is not a multiple of the raw + # or estimated time steps, or if there is a rounding/floating point error. + raise ValueError("Could not update timestamp, time stamp is invalid") + def _fetch_data_loop(self) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" + + raw_dt = self._config["raw_time_step"] + est_dt = self._config["est_time_step"] + # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - for _ in range(21000): - packets = self._data_generator.generate_data_packet() - timestamp = 0 - if packets[0] is not None: - self._data_queue.put(packets[0]) - timestamp = packets[0].timestamp / 1e9 - if packets[1] is not None: - self._data_queue.put(packets[1]) - timestamp = packets[1].timestamp / 1e9 - - dt = self._data_generator.update_timestamp(timestamp) - timestamp - time.sleep(dt) + while self._data_generator._last_velocity > -1: + # starts timer + start_time = time.time() + + # if the timestamp is a multiple of the raw time step, generate a raw data packet. + if any(np.isclose(self._timestamp % raw_dt, [0, raw_dt])): + self._data_queue.put(self._data_generator.generate_raw_data_packet()) + + # if the timestamp is a multiple of the est time step, generate an est data packet. + if any(np.isclose(self._timestamp % est_dt, [0, est_dt])): + self._data_queue.put(self._data_generator.generate_estimated_data_packet()) + + # updates the timestamp and sleeps until next packet is ready in real-time + time_step = self.update_timestamp(self._timestamp) - self._timestamp + self._timestamp += time_step + end_time = time.time() + time.sleep(max(0.0, time_step - (end_time - start_time))) + diff --git a/simulator/test.py b/simulator/test.py new file mode 100644 index 00000000..1e648e4a --- /dev/null +++ b/simulator/test.py @@ -0,0 +1,5 @@ +import numpy as np + +values = [1,2,3] +if any(value is None for value in values): + raise AttributeError('test') From 35c7f08572c4245f6d3f3169e76bf2b6140bf22e Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Thu, 7 Nov 2024 14:52:35 -0500 Subject: [PATCH 06/63] starting rotation --- pyproject.toml | 2 +- simulator/data_gen.py | 42 +++++++++++++++++++++++++++++---- simulator/rotation_manager.py | 33 ++++++++++++++++++++++++++ simulator/sim_config.yaml | 10 +++++++- simulator/test.py | 44 ++++++++++++++++++++++++++++++++--- 5 files changed, 122 insertions(+), 9 deletions(-) create mode 100644 simulator/rotation_manager.py diff --git a/pyproject.toml b/pyproject.toml index 30d1fea7..fdda2ac9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -37,7 +37,7 @@ exclude = ["scripts"] [tool.ruff.lint] preview = true explicit-preview-rules = true # TODO: Drop this when RUF022 and RUF023 are out of preview -ignore = ["PLR2004", "PLR0911", "PLR0912", "PLR0913", "PLR0915", "PERF203", "ISC001"] +ignore = ["PLR2004", "PLR0911", "PLR0912", "PLR0913", "PLR0915", "PERF203", "ISC001", "S311"] select = ["E", "F", "I", "PL", "UP", "RUF", "PTH", "C4", "B", "PIE", "SIM", "RET", "RSE", "G", "ISC", "PT", "ASYNC", "TCH", "SLOT", "PERF", "PYI", "FLY", "AIR", "RUF022", "RUF023", "Q", "INP", "W", "YTT", "DTZ", "ARG", "T20", "FURB", "DOC", diff --git a/simulator/data_gen.py b/simulator/data_gen.py index 13afb94b..f4011b56 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -1,6 +1,7 @@ """Module that creates randomly generated data to sent to the simulator IMU""" import csv +import random from pathlib import Path import numpy as np @@ -10,10 +11,10 @@ from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, - IMUDataPacket, RawDataPacket, ) from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY +from simulator.rotation_manager import RotationManager from utils import deadband @@ -43,10 +44,43 @@ def __init__(self): with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) + # loads thrust curve + self._thrust_data: npt.NDArray = self._load_thrust_curve() + + # initializes the rotation manager with the launch pad conditions + self._rotation_manager = self._get_rotation_manager() # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] - self._thrust_data: npt.NDArray = self._load_thrust_curve() + + def _get_rotation_manager(self) -> RotationManager: + # gets angle of attack and wind direction for the rocket + rod_angle_of_attack = self._get_random("launch_rod_angle") + wind_direction = self._get_random("wind_direction") + # points the rocket into the wind, with some amount of error + rod_direction = self._get_random("rod_direction_error") + (wind_direction-180) + + # initializes the rotation manager + return RotationManager() + + def _get_random(self,identifier: str) -> np.float64: + """ + Gets a random value for the selected identifier, using the standard deviation if given. + + :param identifier: string that matches a config variable in sim_config.yaml + + :return: float containing a random value for the selected identfier, between + the bounds specified in the config + """ + parameters = self._config[identifier] + if len(parameters) == 3: + # uses standard deviation to get random number + mean = np.mean([parameters[0],parameters[1]]) + val = random.gauss(mean,parameters[2]) + # restricts the value to the bounds + return np.max(parameters[0], np.min(parameters[1], val)) + # if no standard deviation is given, just return a uniform distribution + return random.uniform(parameters[0],parameters[1]) def _load_thrust_curve(self) -> npt.NDArray: """ @@ -98,8 +132,6 @@ def _get_first_packet( :return: data packet of the specified type with launch pad conditions. """ orientation = self._config["rocket_orientation"] - initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) - initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) packet = None if packet_type == RawDataPacket: @@ -119,6 +151,8 @@ def _get_first_packet( deltaThetaZ=0.0, ) else: + initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) + initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) packet = EstimatedDataPacket( 0, estOrientQuaternionW=initial_quaternion[0], diff --git a/simulator/rotation_manager.py b/simulator/rotation_manager.py new file mode 100644 index 00000000..ce95b98c --- /dev/null +++ b/simulator/rotation_manager.py @@ -0,0 +1,33 @@ +"""Module that contains methods for rotation operations""" + +import numpy as np +import numpy.typing as npt + + +class RotationManager: + """ + Manages rotation calculations for rocket orientation. This class provides + methods to compute and apply various rotation operations, for the purpose + of generating accurate 6DOF data points for the simulation IMU + """ + + __slots__ = ( + "_vertical", + ) + + def __init__( + self, + orientation: npt.NDArray, + rod_angle_of_attack: np.float64, + + ) -> None: + """ + Initializes a RotationManager instance with a specified orientation. This orientation + serves as the reference direction for rotation operations. + + :param orientation: the vertical orientation of the rocket + """ + self._vertical = orientation + + + diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index 89dad200..a241ba2b 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -13,4 +13,12 @@ reference_area: 0.01929 # reference area (in meters squared) # vertical orientation of the rocket on the launch pad. only one value should be non-zero. # Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) -rocket_orientation: [0, 0, -1] \ No newline at end of file +rocket_orientation: [0, 0, -1] + +#--------------------- +# CONFIG FOR RANDOMNESS +#--------------------- +wind_direction: [0,360] # min and max for wind direction (in degrees) +wind_speed: [0.5,8.5] # min and max for wind speed (in meters per second) +launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) +rod_direction_error: [-10,10] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file diff --git a/simulator/test.py b/simulator/test.py index 1e648e4a..7f9784ca 100644 --- a/simulator/test.py +++ b/simulator/test.py @@ -1,5 +1,43 @@ +import timeit +import yaml +from pathlib import Path +import random import numpy as np -values = [1,2,3] -if any(value is None for value in values): - raise AttributeError('test') +config_path = Path("simulator/sim_config.yaml") +with config_path.open(mode="r", newline="") as file: + config: dict = yaml.safe_load(file) +rng = np.random.default_rng() +# Define the two functions to test +def function1(): + a = rng.random() + +def function2(): + a = random.random() + +# Number of executions for each function (increase for more precise timing) +num_executions = 10 + +# Time the execution of function1 +time_function1 = timeit.timeit("function1()", globals=globals(), number=num_executions) + +# Time the execution of function2 +time_function2 = timeit.timeit("function2()", globals=globals(), number=num_executions) + +# Print the results +print(f"Time taken by function1: {time_function1:.6f} seconds") +print(f"Time taken by function2: {time_function2:.6f} seconds") + +# Compare which function is faster +if time_function1 < time_function2: + print("function1 is faster") +elif time_function1 > time_function2: + print("function2 is faster") +else: + print("Both functions have the same execution time") + + + + + + From de2961099fdeb4588acf62f3ee05a6685e1deb51 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Thu, 7 Nov 2024 18:11:46 -0500 Subject: [PATCH 07/63] progress --- simulator/data_gen.py | 17 ++++++----------- simulator/rotation_manager.py | 18 ++++++++++++++++-- simulator/sim_config.yaml | 4 +--- simulator/sim_imu.py | 2 +- 4 files changed, 24 insertions(+), 17 deletions(-) diff --git a/simulator/data_gen.py b/simulator/data_gen.py index f4011b56..c0fd1faa 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -29,6 +29,7 @@ class DataGenerator: "_last_est_packet", "_last_raw_packet", "_last_velocity", + "_rotation_manager", "_thrust_data", "_vertical_index", ) @@ -48,21 +49,15 @@ def __init__(self): self._thrust_data: npt.NDArray = self._load_thrust_curve() # initializes the rotation manager with the launch pad conditions - self._rotation_manager = self._get_rotation_manager() + self._rotation_manager = RotationManager( + self._config["orientation"], + self._get_random("launch_rod_angle"), + self._get_random("launch_rod_direction"), + ) # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] - def _get_rotation_manager(self) -> RotationManager: - # gets angle of attack and wind direction for the rocket - rod_angle_of_attack = self._get_random("launch_rod_angle") - wind_direction = self._get_random("wind_direction") - # points the rocket into the wind, with some amount of error - rod_direction = self._get_random("rod_direction_error") + (wind_direction-180) - - # initializes the rotation manager - return RotationManager() - def _get_random(self,identifier: str) -> np.float64: """ Gets a random value for the selected identifier, using the standard deviation if given. diff --git a/simulator/rotation_manager.py b/simulator/rotation_manager.py index ce95b98c..f6ea0d4d 100644 --- a/simulator/rotation_manager.py +++ b/simulator/rotation_manager.py @@ -12,14 +12,16 @@ class RotationManager: """ __slots__ = ( + "_azimuth", "_vertical", + "_zenith", ) def __init__( self, orientation: npt.NDArray, rod_angle_of_attack: np.float64, - + rod_direction: np.float64 ) -> None: """ Initializes a RotationManager instance with a specified orientation. This orientation @@ -27,7 +29,19 @@ def __init__( :param orientation: the vertical orientation of the rocket """ - self._vertical = orientation + self._vertical: npt.NDArray = orientation + self._zenith: np.float64 = (rod_angle_of_attack * np.pi)/180 + self._azimuth: np.float64 = (rod_direction * np.pi)/180 + + def update_orientation(self,time_step: np.float64) -> None: + """ + Updates the baseline vehicle-frame orientation + + :param time_step: how much time has passed between updates + :param apogee_progress: current altitude divided by the target apogee + """ + + diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index a241ba2b..0dda9c29 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -18,7 +18,5 @@ rocket_orientation: [0, 0, -1] #--------------------- # CONFIG FOR RANDOMNESS #--------------------- -wind_direction: [0,360] # min and max for wind direction (in degrees) -wind_speed: [0.5,8.5] # min and max for wind speed (in meters per second) launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) -rod_direction_error: [-10,10] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file +rod_direction_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 4f7a84be..44be69da 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -32,7 +32,7 @@ class SimIMU(IMU): "_timestamp", ) - def __init__(self): + def __init__(self) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. From e2b7a17f94d3a0922e5c8ea7faa7c6a6b3a6c9ce Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 7 Nov 2024 22:49:45 -0500 Subject: [PATCH 08/63] Clean up --- airbrakes/data_handling/data_processor.py | 6 +- .../simulation}/__init__.py | 0 .../simulation/data_generator.py | 72 +++++++++---------- .../simulation}/rotation_manager.py | 18 ++--- .../simulation}/sim_config.yaml | 8 +-- .../simulation}/sim_imu.py | 13 ++-- {simulator => airbrakes/simulation}/test.py | 31 ++++---- .../thrust_curves/AeroTech_L1940X.csv | 0 main.py | 4 +- pyproject.toml | 1 + utils.py | 2 +- 11 files changed, 67 insertions(+), 88 deletions(-) rename {simulator => airbrakes/simulation}/__init__.py (100%) rename simulator/data_gen.py => airbrakes/simulation/data_generator.py (88%) rename {simulator => airbrakes/simulation}/rotation_manager.py (78%) rename {simulator => airbrakes/simulation}/sim_config.yaml (87%) rename {simulator => airbrakes/simulation}/sim_imu.py (94%) rename {simulator => airbrakes/simulation}/test.py (62%) rename {simulator => airbrakes/simulation}/thrust_curves/AeroTech_L1940X.csv (100%) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index f4a96311..89e2fe0f 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -204,7 +204,6 @@ def _calculate_rotated_accelerations(self) -> npt.NDArray[np.float64]: Calculates the rotated acceleration vector. Converts gyroscope data into a delta quaternion, and adds onto the last quaternion. Will most likely be replaced by IMU quaternion data in the future, this is a work-around due to bad datasets. - :return: numpy list of rotated acceleration vector [x,y,z] """ @@ -229,7 +228,8 @@ def _calculate_rotated_accelerations(self) -> npt.NDArray[np.float64]: if any(val is None for val in [x_accel, y_accel, z_accel, gyro_x, gyro_y, gyro_z]): return rotated_accelerations - # scipy docs for more info: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html + # scipy docs for more info: + # https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html # Calculate the delta quaternion from the angular rates delta_rotation = R.from_rotvec([gyro_x * dt, gyro_y * dt, gyro_z * dt]) @@ -282,7 +282,7 @@ def _calculate_time_differences(self) -> npt.NDArray[np.float64]: This cannot be called on the first update as _last_data_packet is None. Units are in seconds. :return: A numpy array of the time difference between each data packet and the previous - data packet. + data packet. """ # calculate the time differences between each data packet # We are converting from ns to s, since we don't want to have a velocity in m/ns^2 diff --git a/simulator/__init__.py b/airbrakes/simulation/__init__.py similarity index 100% rename from simulator/__init__.py rename to airbrakes/simulation/__init__.py diff --git a/simulator/data_gen.py b/airbrakes/simulation/data_generator.py similarity index 88% rename from simulator/data_gen.py rename to airbrakes/simulation/data_generator.py index c0fd1faa..4346997e 100644 --- a/simulator/data_gen.py +++ b/airbrakes/simulation/data_generator.py @@ -1,4 +1,4 @@ -"""Module that creates randomly generated data to sent to the simulator IMU""" +"""Module that creates randomly generated data to sent to the simulation IMU""" import csv import random @@ -13,8 +13,8 @@ EstimatedDataPacket, RawDataPacket, ) +from airbrakes.simulation.rotation_manager import RotationManager from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY -from simulator.rotation_manager import RotationManager from utils import deadband @@ -41,7 +41,7 @@ def __init__(self): self._last_velocity: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulator/sim_config.yaml") + config_path = Path("simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) @@ -58,41 +58,39 @@ def __init__(self): # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] - def _get_random(self,identifier: str) -> np.float64: + @property + def velocity(self) -> np.float64: + """Returns the last calculated velocity of the rocket""" + return self._last_velocity + + def _get_random(self, identifier: str) -> np.float64: """ Gets a random value for the selected identifier, using the standard deviation if given. - :param identifier: string that matches a config variable in sim_config.yaml - - :return: float containing a random value for the selected identfier, between - the bounds specified in the config + :return: float containing a random value for the selected identifier, between + the bounds specified in the config """ parameters = self._config[identifier] if len(parameters) == 3: # uses standard deviation to get random number - mean = np.mean([parameters[0],parameters[1]]) - val = random.gauss(mean,parameters[2]) + mean = float(np.mean([parameters[0], parameters[1]])) + val = random.gauss(mean, parameters[2]) # restricts the value to the bounds return np.max(parameters[0], np.min(parameters[1], val)) # if no standard deviation is given, just return a uniform distribution - return random.uniform(parameters[0],parameters[1]) + return random.uniform(parameters[0], parameters[1]) def _load_thrust_curve(self) -> npt.NDArray: """ Loads the thrust curve from the motor specified in the configs. - :return: numpy array containing tuples with the time and thrust at that time. """ # gets the path of the csv based on the config file - csv_path = Path(f"simulator/thrust_curves/{self._config["motor"]}.csv") + csv_path = Path(f"simulation/thrust_curves/{self._config["motor"]}.csv") # initializes the list for timestamps and thrust values - motor_timestamps = [ - 0, - ] - motor_thrusts = [ - 0, - ] + motor_timestamps = [0] + motor_thrusts = [0] start_flag = False # flag to identify when the metadata/header rows are skipped @@ -115,8 +113,8 @@ def _load_thrust_curve(self) -> npt.NDArray: return np.array([motor_timestamps, motor_thrusts]) def _get_first_packet( - self,packet_type: RawDataPacket | EstimatedDataPacket - ) -> RawDataPacket | EstimatedDataPacket: + self, packet_type: RawDataPacket | EstimatedDataPacket + ) -> RawDataPacket | EstimatedDataPacket: """ Sets up the initial values for the estimated and raw data packets. This should only be called once, and all values will be approximate launch pad conditions. @@ -130,7 +128,7 @@ def _get_first_packet( packet = None if packet_type == RawDataPacket: - packet = RawDataPacket( + packet = RawDataPacket( 0, scaledAccelX=orientation[0], scaledAccelY=orientation[1], @@ -148,7 +146,7 @@ def _get_first_packet( else: initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) - packet = EstimatedDataPacket( + packet = EstimatedDataPacket( 0, estOrientQuaternionW=initial_quaternion[0], estOrientQuaternionX=initial_quaternion[1], @@ -185,17 +183,17 @@ def generate_raw_data_packet(self) -> RawDataPacket: return self._last_raw_packet # calculates the timestamp for this packet (in seconds) - next_timestamp = (self._last_raw_packet.timestamp/1e9) + time_step + next_timestamp = (self._last_raw_packet.timestamp / 1e9) + time_step # calculates the net force and vertical scaled acceleration - net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + net_force = self._calculate_net_force(next_timestamp, self._last_velocity) vertical_scaled_accel = net_force / (self._config["rocket_mass"] * GRAVITY) # calculates vertical delta velocity last_vertical_scaled_accel = self._get_vertical_data_point("scaledAccel") vert_delta_v = (vertical_scaled_accel - last_vertical_scaled_accel) * GRAVITY * time_step - packet = RawDataPacket( + packet = RawDataPacket( next_timestamp * 1e9, scaledAccelX=vertical_scaled_accel * orientation[0], scaledAccelY=vertical_scaled_accel * orientation[1], @@ -233,10 +231,10 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: return self._last_raw_packet # calculates the timestamp for this packet (in seconds) - next_timestamp = (self._last_est_packet.timestamp/1e9) + time_step + next_timestamp = (self._last_est_packet.timestamp / 1e9) + time_step # calculates the net force and vertical accelerations - net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + net_force = self._calculate_net_force(next_timestamp, self._last_velocity) vertical_linear_accel = net_force / self._config["rocket_mass"] vertical_comp_accel = vertical_linear_accel + GRAVITY @@ -251,7 +249,8 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: self._last_est_packet.estOrientQuaternionX, self._last_est_packet.estOrientQuaternionY, self._last_est_packet.estOrientQuaternionZ, - ]) + ] + ) packet = EstimatedDataPacket( next_timestamp * 1e9, @@ -280,7 +279,6 @@ def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: """ Calculates the velocity of the rocket based on the compensated acceleration. Integrates that acceleration to get the velocity. - :return: the velocity of the rocket """ # deadbands the acceleration @@ -309,25 +307,21 @@ def _calculate_net_force(self, timestamp, velocity) -> np.float64: drag_coefficient = self._config["drag_coefficient"] rocket_mass = self._config["rocket_mass"] - drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 + drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 weight_force = GRAVITY * rocket_mass thrust_force = 0.0 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn if timestamp <= self._thrust_data[0][-1]: - thrust_force = np.interp( - timestamp, self._thrust_data[0], self._thrust_data[1] - ) + thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) return thrust_force - weight_force - drag_force - def _get_vertical_data_point(self,string_identifier: str) -> np.float64: + def _get_vertical_data_point(self, string_identifier: str) -> np.float64: """ gets the last vertical data point specified from a vector data attribute by using IMU orientation in config - - :param string_identfier: a string representing the exact attribute name of the data packet - + :param string_identifier: a string representing the exact attribute name of the data packet :return: float containing the specified data point """ # Dynamically retrieve x, y, z components based on the identifier @@ -341,6 +335,6 @@ def _get_vertical_data_point(self,string_identifier: str) -> np.float64: if any(value is None for value in values): raise AttributeError( f"Could not find all components for identifier '{string_identifier}'." - ) + ) return values[self._vertical_index] diff --git a/simulator/rotation_manager.py b/airbrakes/simulation/rotation_manager.py similarity index 78% rename from simulator/rotation_manager.py rename to airbrakes/simulation/rotation_manager.py index f6ea0d4d..7763dd7c 100644 --- a/simulator/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -18,11 +18,8 @@ class RotationManager: ) def __init__( - self, - orientation: npt.NDArray, - rod_angle_of_attack: np.float64, - rod_direction: np.float64 - ) -> None: + self, orientation: npt.NDArray, rod_angle_of_attack: np.float64, rod_direction: np.float64 + ) -> None: """ Initializes a RotationManager instance with a specified orientation. This orientation serves as the reference direction for rotation operations. @@ -30,18 +27,13 @@ def __init__( :param orientation: the vertical orientation of the rocket """ self._vertical: npt.NDArray = orientation - self._zenith: np.float64 = (rod_angle_of_attack * np.pi)/180 - self._azimuth: np.float64 = (rod_direction * np.pi)/180 + self._zenith: np.float64 = (rod_angle_of_attack * np.pi) / 180 + self._azimuth: np.float64 = (rod_direction * np.pi) / 180 - def update_orientation(self,time_step: np.float64) -> None: + def update_orientation(self, time_step: np.float64) -> None: """ Updates the baseline vehicle-frame orientation :param time_step: how much time has passed between updates :param apogee_progress: current altitude divided by the target apogee """ - - - - - diff --git a/simulator/sim_config.yaml b/airbrakes/simulation/sim_config.yaml similarity index 87% rename from simulator/sim_config.yaml rename to airbrakes/simulation/sim_config.yaml index 0dda9c29..99013ee3 100644 --- a/simulator/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -1,10 +1,10 @@ -# config file for simulator +# config file for simulation sim_apogee: 1800 # Simulator will generate data with the apogee around this value (in meters) -raw_time_step: 0.001 # Time step between raw data packets the simulator will generate (in seconds) -est_time_step: 0.002 # Time step between estimated data packets the simulator will generate (in seconds) +raw_time_step: 0.001 # Time step between raw data packets the simulation will generate (in seconds) +est_time_step: 0.002 # Time step between estimated data packets the simulation will generate (in seconds) -# selects the motor to use, this is the name of the csv file located in /simulator/thrust_curves +# selects the motor to use, this is the name of the csv file located in /simulation/thrust_curves # with the ".csv" removed motor: "AeroTech_L1940X" drag_coefficient: 0.4 # coefficient of drag for the rocket diff --git a/simulator/sim_imu.py b/airbrakes/simulation/sim_imu.py similarity index 94% rename from simulator/sim_imu.py rename to airbrakes/simulation/sim_imu.py index 44be69da..016e00e5 100644 --- a/simulator/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -13,8 +13,8 @@ from airbrakes.data_handling.imu_data_packet import IMUDataPacket from airbrakes.hardware.imu import IMU +from airbrakes.simulation.data_generator import DataGenerator from constants import MAX_QUEUE_SIZE -from simulator.data_gen import DataGenerator class SimIMU(IMU): @@ -39,12 +39,12 @@ def __init__(self) -> None: """ self._timestamp: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulator/sim_config.yaml") + config_path = Path("simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) # This limits the queue size to a very high limit, because the data generator will - # generate all of the data before the imu reads it + # generate all the data before the imu reads it self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue( MAX_QUEUE_SIZE ) @@ -64,9 +64,7 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: """ Updates the current timestamp of the data generator, based off time step defined in config. Will also determine if the next timestamp will be a raw packet, estimated packet, or both. - :param current_timestamp: the current timestamp of the simulation - :return: the updated current timestamp, rounded to 3 decimals """ @@ -79,7 +77,7 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) # If current timestamp is a multiple of both, the next timestamp will be the - # the current timestamp + the lower time steps + # current timestamp + the lower time steps if all([at_low, at_high]): return np.round(current_timestamp + lowest_dt, 3) @@ -108,7 +106,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - while self._data_generator._last_velocity > -1: + while self._data_generator.velocity > -1: # starts timer start_time = time.time() @@ -125,4 +123,3 @@ def _fetch_data_loop(self) -> None: self._timestamp += time_step end_time = time.time() time.sleep(max(0.0, time_step - (end_time - start_time))) - diff --git a/simulator/test.py b/airbrakes/simulation/test.py similarity index 62% rename from simulator/test.py rename to airbrakes/simulation/test.py index 7f9784ca..129f7ffa 100644 --- a/simulator/test.py +++ b/airbrakes/simulation/test.py @@ -1,19 +1,24 @@ +import random import timeit -import yaml from pathlib import Path -import random + import numpy as np +import yaml -config_path = Path("simulator/sim_config.yaml") +config_path = Path("simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: config: dict = yaml.safe_load(file) rng = np.random.default_rng() + + # Define the two functions to test def function1(): - a = rng.random() + rng.random() + def function2(): - a = random.random() + random.random() + # Number of executions for each function (increase for more precise timing) num_executions = 10 @@ -25,19 +30,9 @@ def function2(): time_function2 = timeit.timeit("function2()", globals=globals(), number=num_executions) # Print the results -print(f"Time taken by function1: {time_function1:.6f} seconds") -print(f"Time taken by function2: {time_function2:.6f} seconds") # Compare which function is faster -if time_function1 < time_function2: - print("function1 is faster") -elif time_function1 > time_function2: - print("function2 is faster") +if time_function1 < time_function2 or time_function1 > time_function2: + pass else: - print("Both functions have the same execution time") - - - - - - + pass diff --git a/simulator/thrust_curves/AeroTech_L1940X.csv b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv similarity index 100% rename from simulator/thrust_curves/AeroTech_L1940X.csv rename to airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv diff --git a/main.py b/main.py index e37e6ad6..3ccb992b 100644 --- a/main.py +++ b/main.py @@ -15,13 +15,13 @@ from airbrakes.mock.display import FlightDisplay from airbrakes.mock.mock_imu import MockIMU from airbrakes.mock.mock_logger import MockLogger +from airbrakes.simulation.sim_imu import SimIMU from constants import ( FREQUENCY, LOGS_PATH, PORT, SERVO_PIN, ) -from simulator.sim_imu import SimIMU from utils import arg_parser @@ -52,7 +52,7 @@ def main(args: argparse.Namespace) -> None: real_time_simulation=not args.fast_simulation, start_after_log_buffer=True, ) - # If we are running the simulator for generating datasets, we will replace our IMU object + # If we are running the simulation for generating datasets, we will replace our IMU object # with a sim variant, similar to running a mock simulation. else: imu = SimIMU() diff --git a/pyproject.toml b/pyproject.toml index fdda2ac9..16b0f187 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,6 +11,7 @@ dependencies = [ "colorama", "psutil", "scipy", + "PyYAML" # Installation instructions for the following dependencies can be found in the README: # "mscl" https://github.com/LORD-MicroStrain/MSCL/blob/master/BuildScripts/buildReadme_Linux.md ] diff --git a/utils.py b/utils.py index 414c5585..c262a70b 100644 --- a/utils.py +++ b/utils.py @@ -102,7 +102,7 @@ def arg_parser() -> argparse.Namespace: parser.add_argument( "-s", "--sim", - help="runs the data simulator alongside the mock simulator, to randomly generate a dataset", + help="runs the data simulation alongside the mock simulation, to randomly generate a dataset", action="store_true", default=False, ) From d18e4a8ca1c9ced23765604295c443e7ce628d47 Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 7 Nov 2024 22:57:13 -0500 Subject: [PATCH 09/63] Clean up --- airbrakes/mock/mock_imu.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/airbrakes/mock/mock_imu.py b/airbrakes/mock/mock_imu.py index 7ad1cb32..b59a7282 100644 --- a/airbrakes/mock/mock_imu.py +++ b/airbrakes/mock/mock_imu.py @@ -22,8 +22,6 @@ class MockIMU(IMU): and returns data read from a previous log file. """ - __slots__ = ("_log_file_path",) - def __init__( self, log_file_path: Path | bool, @@ -32,19 +30,17 @@ def __init__( ): """ Initializes the object that pretends to be an IMU for testing purposes by reading from a - log file. + log file. :param log_file_path: The path of the log file to read data from. :param real_time_simulation: Whether to simulate a real flight by sleeping for a set - period, or run at full - speed, e.g. for using it in the CI. + period, or run at full speed, e.g. for using it in the CI. :param start_after_log_buffer: Whether to send the data packets only after the log buffer - was filled for Standby state. + was filled for Standby state. """ - self._log_file_path = log_file_path # Check if the launch data file exists: if not log_file_path: # Just use the first file in the `launch_data` directory: - self._log_file_path = next(iter(Path("launch_data").glob("*.csv"))) + log_file_path = next(iter(Path("launch_data").glob("*.csv"))) # We don't call the parent constructor as the IMU class has different parameters, so we # manually start the process that fetches data from the log file @@ -61,7 +57,7 @@ def __init__( self._data_fetch_process = multiprocessing.Process( target=self._fetch_data_loop, args=( - self._log_file_path, + log_file_path, real_time_simulation, start_after_log_buffer, ), From ab355539a2bca9c7f1ffbd06bf8571e6f8d45cf3 Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 7 Nov 2024 23:11:10 -0500 Subject: [PATCH 10/63] Added simulation stuff to readme --- README.md | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b3d36001..f65ba572 100644 --- a/README.md +++ b/README.md @@ -114,12 +114,14 @@ We have put great effort into keeping the file structure of this project organiz ``` AirbrakesV2/ ├── airbrakes/ +| ├── data_handling/ +│ │ ├── [files related to the processing of data ...] | ├── hardware/ │ │ ├── [files related to the connection of the pi with hardware ...] | ├── mock/ │ │ ├── [files related to the connection of mock (or simulated) hardware ...] -| ├── data_handling/ -│ │ ├── [files related to the processing of data ...] +| ├── simulation/ +│ │ ├── [files related to our custom air brakes sim ...] │ ├── [files which control the airbrakes at a high level ...] ├── tests/ [used for testing all the code] │ ├── ... @@ -179,6 +181,10 @@ If you want to run a mock launch, but with the real servo running, run: ```bash python3 main.py -m -r ``` +To run a mock launch with our custom simulation: +```bash +python3 main.py -m -s +``` There are some additional options you can use when running a mock launch. To view them all, run: ```bash python3 main.py --help From 47c3adab2ecc364fa959ea3a12aa56b6c4bccc3f Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Fri, 8 Nov 2024 00:47:11 -0500 Subject: [PATCH 11/63] rotation wip --- airbrakes/simulation/data_generator.py | 6 ++-- airbrakes/simulation/rotation_manager.py | 24 ++++++++++----- airbrakes/simulation/sim_config.yaml | 3 +- airbrakes/simulation/sim_imu.py | 2 +- airbrakes/simulation/test.py | 38 ------------------------ utils.py | 3 +- 6 files changed, 24 insertions(+), 52 deletions(-) delete mode 100644 airbrakes/simulation/test.py diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 4346997e..e3fe8a39 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -41,7 +41,7 @@ def __init__(self): self._last_velocity: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulation/sim_config.yaml") + config_path = Path("airbrakes/simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) @@ -50,7 +50,7 @@ def __init__(self): # initializes the rotation manager with the launch pad conditions self._rotation_manager = RotationManager( - self._config["orientation"], + self._config["rocket_orientation"], self._get_random("launch_rod_angle"), self._get_random("launch_rod_direction"), ) @@ -86,7 +86,7 @@ def _load_thrust_curve(self) -> npt.NDArray: :return: numpy array containing tuples with the time and thrust at that time. """ # gets the path of the csv based on the config file - csv_path = Path(f"simulation/thrust_curves/{self._config["motor"]}.csv") + csv_path = Path(f"airbrakes/simulation/thrust_curves/{self._config["motor"]}.csv") # initializes the list for timestamps and thrust values motor_timestamps = [0] diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 7763dd7c..e1cdb73c 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -1,5 +1,7 @@ """Module that contains methods for rotation operations""" +from collections import deque + import numpy as np import numpy.typing as npt @@ -12,9 +14,9 @@ class RotationManager: """ __slots__ = ( - "_azimuth", + "_azimuths", "_vertical", - "_zenith", + "_zeniths", ) def __init__( @@ -27,13 +29,21 @@ def __init__( :param orientation: the vertical orientation of the rocket """ self._vertical: npt.NDArray = orientation - self._zenith: np.float64 = (rod_angle_of_attack * np.pi) / 180 - self._azimuth: np.float64 = (rod_direction * np.pi) / 180 + self._zeniths: deque = deque[(rod_angle_of_attack * np.pi) / 180] + self._azimuths: deque = deque[(rod_direction * np.pi) / 180] - def update_orientation(self, time_step: np.float64) -> None: + def update_orientation(self, time_step: np.float64, vertical_velocity: np.float64) -> None: """ - Updates the baseline vehicle-frame orientation + Updates the baseline vehicle-frame orientation, based on how close the velocity + is to zero. This accounts for the velocity being zero at the start of launch :param time_step: how much time has passed between updates - :param apogee_progress: current altitude divided by the target apogee + :param vertical_velocity: current vertical velocity of the rocket """ + # we dont want to change angle if in motor burn phase, for simplicity + if self._zeniths[-1] is not self._zeniths[0] or vertical_velocity >= 100: + self._zeniths.append( + (np.pi / 2 - self._zeniths[0]) * np.exp(-0.035 * vertical_velocity) + + self._zeniths[0] + ) + _ = time_step diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml index 99013ee3..00a50f82 100644 --- a/airbrakes/simulation/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -1,6 +1,5 @@ # config file for simulation -sim_apogee: 1800 # Simulator will generate data with the apogee around this value (in meters) raw_time_step: 0.001 # Time step between raw data packets the simulation will generate (in seconds) est_time_step: 0.002 # Time step between estimated data packets the simulation will generate (in seconds) @@ -19,4 +18,4 @@ rocket_orientation: [0, 0, -1] # CONFIG FOR RANDOMNESS #--------------------- launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) -rod_direction_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file +launch_rod_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 016e00e5..a4e69796 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -39,7 +39,7 @@ def __init__(self) -> None: """ self._timestamp: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulation/sim_config.yaml") + config_path = Path("airbrakes/simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) diff --git a/airbrakes/simulation/test.py b/airbrakes/simulation/test.py deleted file mode 100644 index 129f7ffa..00000000 --- a/airbrakes/simulation/test.py +++ /dev/null @@ -1,38 +0,0 @@ -import random -import timeit -from pathlib import Path - -import numpy as np -import yaml - -config_path = Path("simulation/sim_config.yaml") -with config_path.open(mode="r", newline="") as file: - config: dict = yaml.safe_load(file) -rng = np.random.default_rng() - - -# Define the two functions to test -def function1(): - rng.random() - - -def function2(): - random.random() - - -# Number of executions for each function (increase for more precise timing) -num_executions = 10 - -# Time the execution of function1 -time_function1 = timeit.timeit("function1()", globals=globals(), number=num_executions) - -# Time the execution of function2 -time_function2 = timeit.timeit("function2()", globals=globals(), number=num_executions) - -# Print the results - -# Compare which function is faster -if time_function1 < time_function2 or time_function1 > time_function2: - pass -else: - pass diff --git a/utils.py b/utils.py index c262a70b..ab03d666 100644 --- a/utils.py +++ b/utils.py @@ -102,7 +102,8 @@ def arg_parser() -> argparse.Namespace: parser.add_argument( "-s", "--sim", - help="runs the data simulation alongside the mock simulation, to randomly generate a dataset", + help="runs the data simulation alongside the mock simulation, to randomly generate " + "a dataset", action="store_true", default=False, ) From 0f53f3704e65ea6b85392b6cea1569141c790702 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Fri, 8 Nov 2024 21:24:14 -0500 Subject: [PATCH 12/63] rotations almost done, some bugs --- airbrakes/simulation/data_generator.py | 243 +++++++++++++++-------- airbrakes/simulation/rotation_manager.py | 141 +++++++++++-- airbrakes/simulation/sim_imu.py | 2 +- 3 files changed, 281 insertions(+), 105 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index e3fe8a39..5b3e8b41 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -7,14 +7,13 @@ import numpy as np import numpy.typing as npt import yaml -from scipy.spatial.transform import Rotation as R from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, RawDataPacket, ) from airbrakes.simulation.rotation_manager import RotationManager -from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY +from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY, MAX_VELOCITY_THRESHOLD from utils import deadband @@ -26,10 +25,12 @@ class DataGenerator: __slots__ = ( "_config", + "_est_rotation_manager", "_last_est_packet", "_last_raw_packet", - "_last_velocity", - "_rotation_manager", + "_last_velocities", + "_max_velocity", + "_raw_rotation_manager", "_thrust_data", "_vertical_index", ) @@ -38,7 +39,8 @@ def __init__(self): """Initializes the data generator object""" self._last_est_packet: EstimatedDataPacket | None = None self._last_raw_packet: RawDataPacket | None = None - self._last_velocity: np.float64 = np.float64(0.0) + self._last_velocities: npt.NDArray = np.array([0.0, 0.0, 0.0]) + self._max_velocity: np.float64 = np.float64(0.1) # loads the sim_config.yaml file config_path = Path("airbrakes/simulation/sim_config.yaml") @@ -49,19 +51,18 @@ def __init__(self): self._thrust_data: npt.NDArray = self._load_thrust_curve() # initializes the rotation manager with the launch pad conditions - self._rotation_manager = RotationManager( - self._config["rocket_orientation"], - self._get_random("launch_rod_angle"), - self._get_random("launch_rod_direction"), - ) + raw_manager, est_manager = self._get_rotation_managers() + self._raw_rotation_manager: RotationManager = raw_manager + self._est_rotation_manager: RotationManager = est_manager + # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] @property - def velocity(self) -> np.float64: + def velocities(self) -> np.float64: """Returns the last calculated velocity of the rocket""" - return self._last_velocity + return self._last_velocities def _get_random(self, identifier: str) -> np.float64: """ @@ -112,6 +113,28 @@ def _load_thrust_curve(self) -> npt.NDArray: return np.array([motor_timestamps, motor_thrusts]) + def _get_rotation_managers(self) -> npt.NDArray: + """ + Creates and initializes both rotation managers that will be used to contain any rotation + related math and methods for the raw and estimated data. + + :return: a 2 element array containing the raw rotation manager and the estimated rotation + manager, respectively. + """ + launch_rod_angle = self._get_random("launch_rod_angle") + launch_rod_direction = self._get_random("launch_rod_direction") + raw_manager = RotationManager( + self._config["rocket_orientation"], + launch_rod_angle, + launch_rod_direction, + ) + est_manager = RotationManager( + self._config["rocket_orientation"], + launch_rod_angle, + launch_rod_direction, + ) + return np.array([raw_manager, est_manager]) + def _get_first_packet( self, packet_type: RawDataPacket | EstimatedDataPacket ) -> RawDataPacket | EstimatedDataPacket: @@ -124,15 +147,17 @@ def _get_first_packet( :return: data packet of the specified type with launch pad conditions. """ - orientation = self._config["rocket_orientation"] packet = None if packet_type == RawDataPacket: + scaled_accel = ( + self._raw_rotation_manager.calculate_compensated_accel(0.0, 0.0) / GRAVITY + ) packet = RawDataPacket( 0, - scaledAccelX=orientation[0], - scaledAccelY=orientation[1], - scaledAccelZ=orientation[2], + scaledAccelX=scaled_accel[0], + scaledAccelY=scaled_accel[1], + scaledAccelZ=scaled_accel[2], scaledGyroX=0.0, scaledGyroY=0.0, scaledGyroZ=0.0, @@ -144,24 +169,29 @@ def _get_first_packet( deltaThetaZ=0.0, ) else: - initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) - initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) + initial_quaternion = self._est_rotation_manager.quaternion + compensated_accel = self._est_rotation_manager.calculate_compensated_accel(0.0, 0.0) + linear_accel = self._est_rotation_manager.calculate_linear_accel(0.0, 0.0) + gravity_vector = self._est_rotation_manager.gravity_vector packet = EstimatedDataPacket( 0, estOrientQuaternionW=initial_quaternion[0], estOrientQuaternionX=initial_quaternion[1], estOrientQuaternionY=initial_quaternion[2], estOrientQuaternionZ=initial_quaternion[3], - estCompensatedAccelX=GRAVITY * orientation[0], - estCompensatedAccelY=GRAVITY * orientation[1], - estCompensatedAccelZ=GRAVITY * orientation[2], estPressureAlt=0, - estGravityVectorX=-GRAVITY * orientation[0], - estGravityVectorY=-GRAVITY * orientation[1], - estGravityVectorZ=-GRAVITY * orientation[2], estAngularRateX=0, estAngularRateY=0, estAngularRateZ=0, + estCompensatedAccelX=compensated_accel[0], + estCompensatedAccelY=compensated_accel[1], + estCompensatedAccelZ=compensated_accel[2], + estLinearAccelX=linear_accel[0], + estLinearAccelY=linear_accel[1], + estLinearAccelZ=linear_accel[2], + estGravityVectorX=gravity_vector[0], + estGravityVectorY=gravity_vector[1], + estGravityVectorZ=gravity_vector[2], ) return packet @@ -174,7 +204,6 @@ def generate_raw_data_packet(self) -> RawDataPacket: """ # creating shorthand variables from config time_step = self._config["raw_time_step"] - orientation = self._config["rocket_orientation"] # If the simulation has just started, we want to just generate the initial raw packet # and initialize "self._last_" variables @@ -182,31 +211,53 @@ def generate_raw_data_packet(self) -> RawDataPacket: self._last_raw_packet = self._get_first_packet(RawDataPacket) return self._last_raw_packet + # updates the raw rotation manager, if we are after motor burn phase + if ( + self._last_velocities[2] + < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + ): + velocity_ratio = self._last_velocities[2] / self._max_velocity + self._raw_rotation_manager.update_orientation(time_step, velocity_ratio) + # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_raw_packet.timestamp / 1e9) + time_step - # calculates the net force and vertical scaled acceleration - net_force = self._calculate_net_force(next_timestamp, self._last_velocity) - vertical_scaled_accel = net_force / (self._config["rocket_mass"] * GRAVITY) + # calculates the forces and vertical scaled acceleration + forces = self._calculate_forces(next_timestamp, self._last_velocities) + force_accelerations = forces / self._config["rocket_mass"] + compensated_accel = self._raw_rotation_manager.calculate_compensated_accel( + force_accelerations[0], + force_accelerations[1], + ) + scaled_accel = compensated_accel / GRAVITY - # calculates vertical delta velocity - last_vertical_scaled_accel = self._get_vertical_data_point("scaledAccel") - vert_delta_v = (vertical_scaled_accel - last_vertical_scaled_accel) * GRAVITY * time_step + # calculates vertical delta velocity, and gyro + last_scaled_accel = np.array( + [ + self._last_raw_packet.scaledAccelX, + self._last_raw_packet.scaledAccelY, + self._last_raw_packet.scaledAccelZ, + ] + ) + delta_velocity = (scaled_accel - last_scaled_accel) * GRAVITY + delta_theta = self._raw_rotation_manager.calculate_delta_theta() + scaled_gyro_vector = delta_theta / time_step + # assembles the packet packet = RawDataPacket( next_timestamp * 1e9, - scaledAccelX=vertical_scaled_accel * orientation[0], - scaledAccelY=vertical_scaled_accel * orientation[1], - scaledAccelZ=vertical_scaled_accel * orientation[2], - scaledGyroX=0.0, - scaledGyroY=0.0, - scaledGyroZ=0.0, - deltaVelX=vert_delta_v * orientation[0], - deltaVelY=vert_delta_v * orientation[1], - deltaVelZ=vert_delta_v * orientation[2], - deltaThetaX=0.0, - deltaThetaY=0.0, - deltaThetaZ=0.0, + scaledAccelX=scaled_accel[0], + scaledAccelY=scaled_accel[1], + scaledAccelZ=scaled_accel[2], + scaledGyroX=scaled_gyro_vector[0], + scaledGyroY=scaled_gyro_vector[1], + scaledGyroZ=scaled_gyro_vector[2], + deltaVelX=delta_velocity[0], + deltaVelY=delta_velocity[1], + deltaVelZ=delta_velocity[2], + deltaThetaX=delta_theta[0], + deltaThetaY=delta_theta[1], + deltaThetaZ=delta_theta[2], ) # updates last raw data packet @@ -222,7 +273,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: """ # creating shorthand variables from config time_step = self._config["est_time_step"] - orientation = self._config["rocket_orientation"] # If the simulation has just started, we want to just generate the initial estimated packet # and initialize "self._last_" variables @@ -230,44 +280,55 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: self._last_est_packet = self._get_first_packet(EstimatedDataPacket) return self._last_raw_packet + # updates the estimated rotation manager, if we are after motor burn phase + if ( + self._last_velocities[2] + < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + ): + velocity_ratio = self._last_velocities[2] / self._max_velocity + self._est_rotation_manager.update_orientation(time_step, velocity_ratio) + # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_est_packet.timestamp / 1e9) + time_step - # calculates the net force and vertical accelerations - net_force = self._calculate_net_force(next_timestamp, self._last_velocity) - vertical_linear_accel = net_force / self._config["rocket_mass"] - vertical_comp_accel = vertical_linear_accel + GRAVITY + # calculates the forces and accelerations + forces = self._calculate_forces(next_timestamp, self._last_velocities) + force_accelerations = forces / self._config["rocket_mass"] + compensated_accel = self._est_rotation_manager.calculate_compensated_accel( + force_accelerations[0], + force_accelerations[1], + ) + linear_accel = self._est_rotation_manager.calculate_linear_accel( + force_accelerations[0], + force_accelerations[1], + ) # gets vertical velocity and finds updated altitude - vert_velocity = self._calculate_vertical_velocity(vertical_comp_accel, time_step) + vert_velocity = self._calculate_velocities(compensated_accel, time_step)[2] new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step - # gets previous quaternion - last_quat = np.array( - [ - self._last_est_packet.estOrientQuaternionW, - self._last_est_packet.estOrientQuaternionX, - self._last_est_packet.estOrientQuaternionY, - self._last_est_packet.estOrientQuaternionZ, - ] - ) + delta_theta = self._est_rotation_manager.calculate_delta_theta() + angular_rates = delta_theta / time_step packet = EstimatedDataPacket( next_timestamp * 1e9, - estOrientQuaternionW=last_quat[0], - estOrientQuaternionX=last_quat[1], - estOrientQuaternionY=last_quat[2], - estOrientQuaternionZ=last_quat[3], - estCompensatedAccelX=vertical_comp_accel * orientation[0], - estCompensatedAccelY=vertical_comp_accel * orientation[1], - estCompensatedAccelZ=vertical_comp_accel * orientation[2], + estOrientQuaternionW=self._est_rotation_manager.quaternion[0], + estOrientQuaternionX=self._est_rotation_manager.quaternion[1], + estOrientQuaternionY=self._est_rotation_manager.quaternion[2], + estOrientQuaternionZ=self._est_rotation_manager.quaternion[3], + estCompensatedAccelX=compensated_accel[0], + estCompensatedAccelY=compensated_accel[1], + estCompensatedAccelZ=compensated_accel[2], estPressureAlt=new_altitude, - estGravityVectorX=-GRAVITY * orientation[0], - estGravityVectorY=-GRAVITY * orientation[1], - estGravityVectorZ=-GRAVITY * orientation[2], - estAngularRateX=0, - estAngularRateY=0, - estAngularRateZ=0, + estGravityVectorX=self._est_rotation_manager.gravity_vector[0], + estGravityVectorY=self._est_rotation_manager.gravity_vector[1], + estGravityVectorZ=self._est_rotation_manager.gravity_vector[2], + estAngularRateX=angular_rates[0], + estAngularRateY=angular_rates[1], + estAngularRateZ=angular_rates[2], + estLinearAccelX=linear_accel[0], + estLinearAccelY=linear_accel[1], + estLinearAccelZ=linear_accel[2], ) # updates last estimated packet @@ -275,47 +336,57 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: return packet - def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: + def _calculate_velocities(self, comp_accel: npt.NDArray, time_diff: np.float64) -> np.float64: """ Calculates the velocity of the rocket based on the compensated acceleration. Integrates that acceleration to get the velocity. + + :param: numpy array containing the compensated acceleration vector + :return: the velocity of the rocket """ - # deadbands the acceleration - acceleration = deadband(acceleration - GRAVITY, ACCELERATION_NOISE_THRESHOLD) + # gets the rotated acceleration vector + accelerations = self._est_rotation_manager._calculate_rotated_accelerations(comp_accel) + # deadbands the vertical part of the acceleration + accelerations[2] = deadband(accelerations[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) - # Integrate the acceleration to get the velocity - vertical_velocity = self._last_velocity + acceleration * time_diff + # Integrate the accelerations to get the velocity + velocities = self._last_velocities + accelerations * time_diff # updates the last vertical velocity - self._last_velocity = vertical_velocity + self._last_velocities = velocities + + # updates max velocity + self._max_velocity = max(velocities[2], self._max_velocity) - return vertical_velocity + return velocities - def _calculate_net_force(self, timestamp, velocity) -> np.float64: + def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: """ - Calculates the drag force, thrust force, and weight, and sums them. + Calculates the thrust force and drag force, and returns them in an array :param timestamp: the timestamp of the rocket to calcuate the net forces at - :param velocity: the vertical velocity at the given instant + :param velocities: numpy array containing the x, y, and z velocities - :return: float containing the net force. Thrust is positive, drag and weight is negative. + :return: a 2 element numpy array containing thrust force and drag force, respectively. + Thrust is positive, drag is negative. """ + # calculate the magnitude of velocity + speed = np.linalg.norm(velocities) + # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 reference_area = self._config["reference_area"] drag_coefficient = self._config["drag_coefficient"] - rocket_mass = self._config["rocket_mass"] - drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 - weight_force = GRAVITY * rocket_mass thrust_force = 0.0 + drag_force = 0.5 * air_density * reference_area * drag_coefficient * speed**2 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn if timestamp <= self._thrust_data[0][-1]: thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) - return thrust_force - weight_force - drag_force + return np.array([thrust_force, drag_force]) def _get_vertical_data_point(self, string_identifier: str) -> np.float64: """ diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index e1cdb73c..f4933d4c 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -1,9 +1,10 @@ """Module that contains methods for rotation operations""" -from collections import deque - import numpy as np import numpy.typing as npt +from scipy.spatial.transform import Rotation as R + +from constants import GRAVITY class RotationManager: @@ -14,9 +15,11 @@ class RotationManager: """ __slots__ = ( - "_azimuths", + "_azimuth", + "_last_orientation", + "_orientation", "_vertical", - "_zeniths", + "_zenith", ) def __init__( @@ -29,21 +32,123 @@ def __init__( :param orientation: the vertical orientation of the rocket """ self._vertical: npt.NDArray = orientation - self._zeniths: deque = deque[(rod_angle_of_attack * np.pi) / 180] - self._azimuths: deque = deque[(rod_direction * np.pi) / 180] + self._zenith: np.float64 = np.float64((rod_angle_of_attack * np.pi) / 180) + self._azimuth: np.float64 = np.float64((rod_direction * np.pi) / 180) + self._last_orientation: R | None = None + self._orientation: R | None = None + self.update_orientation(0, 0) + + @property + def quaternion(self) -> npt.NDArray: + return self._orientation.as_quat(scalar_first=True) - def update_orientation(self, time_step: np.float64, vertical_velocity: np.float64) -> None: + @property + def gravity_vector(self) -> npt.NDArray: + return self._orientation.apply([0, 0, -GRAVITY]) + + def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) -> None: """ - Updates the baseline vehicle-frame orientation, based on how close the velocity - is to zero. This accounts for the velocity being zero at the start of launch + Updates the baseline vehicle-frame orientation, and all of the vehicle-frame + orientations :param time_step: how much time has passed between updates - :param vertical_velocity: current vertical velocity of the rocket - """ - # we dont want to change angle if in motor burn phase, for simplicity - if self._zeniths[-1] is not self._zeniths[0] or vertical_velocity >= 100: - self._zeniths.append( - (np.pi / 2 - self._zeniths[0]) * np.exp(-0.035 * vertical_velocity) - + self._zeniths[0] - ) - _ = time_step + :param velocity_ratio: current vertical velocity of the rocket divided by max velocity + """ + if velocity_ratio == 0.0: + scale_factor = 0.0 + else: + scale_factor = np.sin(self._zenith) * time_step / abs(velocity_ratio * 10) + self._zenith = self._zenith + scale_factor * (np.pi / 2 - self._zenith) + + # We want to convert the azimuth and zenith values into a scipy rotation object + + # Step 1: We have to declare that the orientation of the rocket is vertical + aligned_orientation = R.align_vectors([self._vertical], [[0, 0, 1]])[0] + + # Step 2: Apply azimuth and zenith rotations relative to the aligned orientation + # 'zy' is used because azimute is yaw (z), and zenith is pitch (y) + azimuth_zenith_rotation = R.from_euler("zy", [self._azimuth, self._zenith], degrees=False) + + # Step 3: Combined rotation: aligned orientation, followed by azimuth and zenith rotations + orientation = azimuth_zenith_rotation * aligned_orientation + + # updating last orientation + self._last_orientation = ( + orientation if self._last_orientation is None else self._orientation + ) + self._orientation = orientation + + def calculate_compensated_accel( + self, thrust_acceleration: np.float64, drag_acceleration: np.float64 + ) -> npt.NDArray: + """ + Uses the acceleration due to drag and thrust to find the compensated acceleration + + :param drag_acceleration: scalar value of acceleration due to drag + :param thrust_acceleration: scalar value of acceleration due to thrust + + :return: the compensated acceleration in the vehicle frame of reference + """ + thrust_drag_accel = thrust_acceleration - drag_acceleration + # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket + # is still on the ground. We have to include the acceleration due to the normal force of + # the ground. + compensated_accel = self._scalar_to_vector(thrust_drag_accel) + if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: + normal_force_accel_vector = self._orientation.apply([0, 0, GRAVITY]) + compensated_accel += normal_force_accel_vector + return compensated_accel + + def calculate_linear_accel( + self, thrust_acceleration: np.float64, drag_acceleration: np.float64 + ) -> npt.NDArray: + """ + Uses the compensated acceleration to find the linear acceleration + + :param drag_acceleration: scalar value of acceleration due to drag + :param thrust_acceleration: scalar value of acceleration due to thrust + + :return: the linear acceleration in the vehicle frame of reference + """ + comp_accel = self.calculate_compensated_accel(drag_acceleration, thrust_acceleration) + # apply gravity + gravity_accel_vector = self._orientation.apply([0, 0, -GRAVITY]) + return np.array(comp_accel - gravity_accel_vector) + + def calculate_delta_theta(self) -> npt.NDArray: + """ + Calculates the delta theta in the vehicle frame of reference, with units in + radians. + + :return: numpy array containing the delta thetas, in [x, y, z] format. + """ + # Calculate the relative rotation from last orientation to the current orientation + relative_rotation = self._last_orientation.inv() * self._orientation + + # Converts the relative rotation to a rotation vector + return relative_rotation.as_rotvec() + + def _calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> np.float64: + """ + Calculates the rotated acceleration vector with the compensated acceleration + + :param compensated_acceleration: compensated acceleration in the vehicle-frame reference. + + :return: float containing vertical acceleration with respect to Earth. + """ + return self._orientation.apply([compensated_acceleration])[0] + + def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: + """ + Converts a scalar value to a vector, where it will align with the vertical orientation. + + :param scalar: a float representing the scalar to be converted + + :return: a 3-element numpy array containing the converted vector. + """ + vector = np.zeros(3) + # Gets index of non-zero value in the vertical orientation + index = np.argmax(np.abs(self._vertical)) + # Place the scalar in the appropriate position, and with the correct sign + vector[index] = scalar * self._vertical[index] + return vector diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index a4e69796..a6f5d298 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -106,7 +106,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - while self._data_generator.velocity > -1: + while self._data_generator.velocities[2] > -100: # starts timer start_time = time.time() From b45df1c782806849f4fd7380712829b9f52beb62 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sat, 9 Nov 2024 00:07:05 -0500 Subject: [PATCH 13/63] bug fixes wip --- airbrakes/simulation/data_generator.py | 48 +++++++++--------------- airbrakes/simulation/rotation_manager.py | 45 ++++++++++++++++------ airbrakes/simulation/sim_config.yaml | 4 +- 3 files changed, 53 insertions(+), 44 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 5b3e8b41..f57bba4c 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -72,6 +72,8 @@ def _get_random(self, identifier: str) -> np.float64: the bounds specified in the config """ parameters = self._config[identifier] + if len(parameters) == 1: + return parameters[0] if len(parameters) == 3: # uses standard deviation to get random number mean = float(np.mean([parameters[0], parameters[1]])) @@ -169,7 +171,7 @@ def _get_first_packet( deltaThetaZ=0.0, ) else: - initial_quaternion = self._est_rotation_manager.quaternion + initial_quaternion = self._est_rotation_manager.calculate_imu_quaternions() compensated_accel = self._est_rotation_manager.calculate_compensated_accel(0.0, 0.0) linear_accel = self._est_rotation_manager.calculate_linear_accel(0.0, 0.0) gravity_vector = self._est_rotation_manager.gravity_vector @@ -212,9 +214,11 @@ def generate_raw_data_packet(self) -> RawDataPacket: return self._last_raw_packet # updates the raw rotation manager, if we are after motor burn phase + if ( self._last_velocities[2] < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity self._raw_rotation_manager.update_orientation(time_step, velocity_ratio) @@ -284,6 +288,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: if ( self._last_velocities[2] < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity self._est_rotation_manager.update_orientation(time_step, velocity_ratio) @@ -303,19 +308,23 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[1], ) - # gets vertical velocity and finds updated altitude + # gets velocity and finds updated altitude vert_velocity = self._calculate_velocities(compensated_accel, time_step)[2] new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step - + # print(f"sim imu altitude: {new_altitude}") delta_theta = self._est_rotation_manager.calculate_delta_theta() angular_rates = delta_theta / time_step + # print(f"sim imu linear accel: {np.round(linear_accel,3)}") + # print(f"sim imu comp accel: {np.round(compensated_accel,3)}") + # print(f"sim imu linear accel: {np.round(linear_accel,3)}") + quaternion = self._est_rotation_manager.calculate_imu_quaternions() packet = EstimatedDataPacket( next_timestamp * 1e9, - estOrientQuaternionW=self._est_rotation_manager.quaternion[0], - estOrientQuaternionX=self._est_rotation_manager.quaternion[1], - estOrientQuaternionY=self._est_rotation_manager.quaternion[2], - estOrientQuaternionZ=self._est_rotation_manager.quaternion[3], + estOrientQuaternionW=quaternion[0], + estOrientQuaternionX=quaternion[1], + estOrientQuaternionY=quaternion[2], + estOrientQuaternionZ=quaternion[3], estCompensatedAccelX=compensated_accel[0], estCompensatedAccelY=compensated_accel[1], estCompensatedAccelZ=compensated_accel[2], @@ -346,7 +355,8 @@ def _calculate_velocities(self, comp_accel: npt.NDArray, time_diff: np.float64) :return: the velocity of the rocket """ # gets the rotated acceleration vector - accelerations = self._est_rotation_manager._calculate_rotated_accelerations(comp_accel) + accelerations = self._est_rotation_manager.calculate_rotated_accelerations(comp_accel) + # deadbands the vertical part of the acceleration accelerations[2] = deadband(accelerations[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) @@ -387,25 +397,3 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: if timestamp <= self._thrust_data[0][-1]: thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) return np.array([thrust_force, drag_force]) - - def _get_vertical_data_point(self, string_identifier: str) -> np.float64: - """ - gets the last vertical data point specified from a vector data attribute by using IMU - orientation in config - :param string_identifier: a string representing the exact attribute name of the data packet - :return: float containing the specified data point - """ - # Dynamically retrieve x, y, z components based on the identifier - values = [ - getattr(self._last_raw_packet, f"{string_identifier}X", None), - getattr(self._last_raw_packet, f"{string_identifier}Y", None), - getattr(self._last_raw_packet, f"{string_identifier}Z", None), - ] - - # Just an edge case, but I really hope this never happens - if any(value is None for value in values): - raise AttributeError( - f"Could not find all components for identifier '{string_identifier}'." - ) - - return values[self._vertical_index] diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index f4933d4c..07f62264 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -38,10 +38,6 @@ def __init__( self._orientation: R | None = None self.update_orientation(0, 0) - @property - def quaternion(self) -> npt.NDArray: - return self._orientation.as_quat(scalar_first=True) - @property def gravity_vector(self) -> npt.NDArray: return self._orientation.apply([0, 0, -GRAVITY]) @@ -65,12 +61,14 @@ def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) # Step 1: We have to declare that the orientation of the rocket is vertical aligned_orientation = R.align_vectors([self._vertical], [[0, 0, 1]])[0] - # Step 2: Apply azimuth and zenith rotations relative to the aligned orientation - # 'zy' is used because azimute is yaw (z), and zenith is pitch (y) - azimuth_zenith_rotation = R.from_euler("zy", [self._azimuth, self._zenith], degrees=False) + # Step 2: Tilt by the zenith angle to adjust pitch away from vertical + tilt_rotation = R.from_rotvec(self._zenith * np.array([1, 0, 0])) + + # Step 3: Rotate around the vertical by azimuth to set the compass direction of the tilt + azimuth_rotation = R.from_rotvec(self._azimuth * np.array([0, 0, -1])) - # Step 3: Combined rotation: aligned orientation, followed by azimuth and zenith rotations - orientation = azimuth_zenith_rotation * aligned_orientation + # Step 4: Combined rotation: aligned orientation, followed by azimuth and zenith rotations + orientation = azimuth_rotation * tilt_rotation * aligned_orientation # updating last orientation self._last_orientation = ( @@ -110,7 +108,7 @@ def calculate_linear_accel( :return: the linear acceleration in the vehicle frame of reference """ - comp_accel = self.calculate_compensated_accel(drag_acceleration, thrust_acceleration) + comp_accel = self.calculate_compensated_accel(thrust_acceleration, drag_acceleration) # apply gravity gravity_accel_vector = self._orientation.apply([0, 0, -GRAVITY]) return np.array(comp_accel - gravity_accel_vector) @@ -128,16 +126,39 @@ def calculate_delta_theta(self) -> npt.NDArray: # Converts the relative rotation to a rotation vector return relative_rotation.as_rotvec() - def _calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> np.float64: + def calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> np.float64: """ - Calculates the rotated acceleration vector with the compensated acceleration + Calculates the rotated acceleration vector with the compensated acceleration. Rotates from + vehicle frame of reference to Earth frame of reference. :param compensated_acceleration: compensated acceleration in the vehicle-frame reference. :return: float containing vertical acceleration with respect to Earth. """ + # # Define the rotation needed to align the IMU + # imu_alignment_rotation = R.align_vectors([[0, 0, -1]], [[0, 0, 1]])[0] + # # Apply this alignment rotation to the given orientation + # imu_adjusted_orientation = imu_alignment_rotation * self._orientation + + # return imu_adjusted_orientation.apply([compensated_acceleration])[0] return self._orientation.apply([compensated_acceleration])[0] + def calculate_imu_quaternions(self) -> npt.NDArray: + """ + Adjusts the given orientation to align with the IMU's reference frame, where + [0, 0, -1] is vertical, and returns a quaternions. + + :param orientation: A scipy Rotation object representing the current orientation. + + :return: A quaternion adjusted to the IMU's frame. + """ + # rotation needed to align the IMU + imu_alignment_rotation = R.align_vectors([[0, 0, 1]], [[0, 0, -1]])[0] + imu_adjusted_orientation = imu_alignment_rotation * self._orientation + + # convert to quaternion + return imu_adjusted_orientation.as_quat(scalar_first=True) + def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: """ Converts a scalar value to a vector, where it will align with the vertical orientation. diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml index 00a50f82..8302284d 100644 --- a/airbrakes/simulation/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -17,5 +17,5 @@ rocket_orientation: [0, 0, -1] #--------------------- # CONFIG FOR RANDOMNESS #--------------------- -launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) -launch_rod_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file +launch_rod_angle: [10] # min and max values for the launch rod angle of attack (in degrees) +launch_rod_direction: [90] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file From 728420a12346a71d9142061924e3303b5594db8c Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sat, 9 Nov 2024 18:02:42 -0500 Subject: [PATCH 14/63] rotation fixes wip --- airbrakes/simulation/data_generator.py | 9 +++------ airbrakes/simulation/rotation_manager.py | 22 +++++++++++++--------- airbrakes/simulation/sim_config.yaml | 2 +- airbrakes/simulation/sim_imu.py | 1 + 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index f57bba4c..d4aa2441 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -311,12 +311,10 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: # gets velocity and finds updated altitude vert_velocity = self._calculate_velocities(compensated_accel, time_step)[2] new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step - # print(f"sim imu altitude: {new_altitude}") + delta_theta = self._est_rotation_manager.calculate_delta_theta() angular_rates = delta_theta / time_step - # print(f"sim imu linear accel: {np.round(linear_accel,3)}") - # print(f"sim imu comp accel: {np.round(compensated_accel,3)}") - # print(f"sim imu linear accel: {np.round(linear_accel,3)}") + quaternion = self._est_rotation_manager.calculate_imu_quaternions() packet = EstimatedDataPacket( @@ -356,8 +354,7 @@ def _calculate_velocities(self, comp_accel: npt.NDArray, time_diff: np.float64) """ # gets the rotated acceleration vector accelerations = self._est_rotation_manager.calculate_rotated_accelerations(comp_accel) - - # deadbands the vertical part of the acceleration + # gets vertical part of the gravity vector accelerations[2] = deadband(accelerations[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) # Integrate the accelerations to get the velocity diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 07f62264..0d9cc2c1 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -93,8 +93,10 @@ def calculate_compensated_accel( # the ground. compensated_accel = self._scalar_to_vector(thrust_drag_accel) if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: - normal_force_accel_vector = self._orientation.apply([0, 0, GRAVITY]) - compensated_accel += normal_force_accel_vector + normal_force_vector = np.array([0, 0, GRAVITY]) + rotated_normal_vector = self._orientation.apply(normal_force_vector) + + compensated_accel += rotated_normal_vector return compensated_accel def calculate_linear_accel( @@ -120,8 +122,13 @@ def calculate_delta_theta(self) -> npt.NDArray: :return: numpy array containing the delta thetas, in [x, y, z] format. """ + # first have to take the current orientation and the last orientation that is in + # Earth -> vehicle reference and convert to vehicle -> Earth reference + imu_adjusted_orientation = self._orientation.inv() + imu_adjusted_last_orientation = self._last_orientation.inv() + # Calculate the relative rotation from last orientation to the current orientation - relative_rotation = self._last_orientation.inv() * self._orientation + relative_rotation = imu_adjusted_last_orientation.inv() * imu_adjusted_orientation # Converts the relative rotation to a rotation vector return relative_rotation.as_rotvec() @@ -135,13 +142,10 @@ def calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) :return: float containing vertical acceleration with respect to Earth. """ - # # Define the rotation needed to align the IMU - # imu_alignment_rotation = R.align_vectors([[0, 0, -1]], [[0, 0, 1]])[0] - # # Apply this alignment rotation to the given orientation - # imu_adjusted_orientation = imu_alignment_rotation * self._orientation + # rotation needed to align the IMU + imu_adjusted_orientation = self._orientation.inv() - # return imu_adjusted_orientation.apply([compensated_acceleration])[0] - return self._orientation.apply([compensated_acceleration])[0] + return imu_adjusted_orientation.apply([compensated_acceleration])[0] def calculate_imu_quaternions(self) -> npt.NDArray: """ diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml index 8302284d..e76bc0a0 100644 --- a/airbrakes/simulation/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -18,4 +18,4 @@ rocket_orientation: [0, 0, -1] # CONFIG FOR RANDOMNESS #--------------------- launch_rod_angle: [10] # min and max values for the launch rod angle of attack (in degrees) -launch_rod_direction: [90] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file +launch_rod_direction: [0] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index a6f5d298..06ce6c6f 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -123,3 +123,4 @@ def _fetch_data_loop(self) -> None: self._timestamp += time_step end_time = time.time() time.sleep(max(0.0, time_step - (end_time - start_time))) + # if self._data_generator._last_est_packet.timestamp>1.02e9: From af2a007c9cf3bcf4e2e323755274f1289133ae19 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 19:13:39 -0500 Subject: [PATCH 15/63] Made sim config a class --- airbrakes/simulation/data_generator.py | 2 +- airbrakes/simulation/sim_config.py | 55 ++++++++++++++++++++++++++ airbrakes/simulation/sim_config.yaml | 21 ---------- airbrakes/simulation/sim_imu.py | 10 ++--- 4 files changed, 61 insertions(+), 27 deletions(-) create mode 100644 airbrakes/simulation/sim_config.py delete mode 100644 airbrakes/simulation/sim_config.yaml diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index d4aa2441..81759500 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -60,7 +60,7 @@ def __init__(self): self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] @property - def velocities(self) -> np.float64: + def velocities(self) -> npt.NDArray: """Returns the last calculated velocity of the rocket""" return self._last_velocities diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py new file mode 100644 index 00000000..7fffd3d5 --- /dev/null +++ b/airbrakes/simulation/sim_config.py @@ -0,0 +1,55 @@ +class SimulationConfig: + def __init__(self, + raw_time_step, + est_time_step, + motor, + drag_coefficient, + rocket_mass, + reference_area, + rocket_orientation, + launch_rod_angle, + launch_rod_direction): + # Time steps for data packet generation in the simulation + self.raw_time_step = raw_time_step + self.est_time_step = est_time_step + + # Motor selection (name of CSV file without extension) + self.motor = motor + + # Rocket properties + self.drag_coefficient = drag_coefficient + self.rocket_mass = rocket_mass + self.reference_area = reference_area + + # Rocket orientation on the launch pad + self.rocket_orientation = rocket_orientation + + # Config for randomness in the simulation + self.launch_rod_angle = launch_rod_angle + self.launch_rod_direction = launch_rod_direction + + +FULL_SCALE_CONFIG = SimulationConfig( + raw_time_step=0.001, + est_time_step=0.002, + motor="AeroTech_L1940X", + drag_coefficient=0.4, + rocket_mass=14.5, + reference_area=0.01929, + rocket_orientation=[0, 0, -1], + launch_rod_angle=[10], + launch_rod_direction=[0] +) + +# TODO: get actual values for sub scale +SUB_SCALE_CONFIG = SimulationConfig( + raw_time_step=0.001, + est_time_step=0.002, + motor="PLACEHOLDER", + drag_coefficient=0.4, + rocket_mass=0, + reference_area=0.001929, + rocket_orientation=[0, 0, -1], + launch_rod_angle=[10], + launch_rod_direction=[0] +) diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml deleted file mode 100644 index e76bc0a0..00000000 --- a/airbrakes/simulation/sim_config.yaml +++ /dev/null @@ -1,21 +0,0 @@ -# config file for simulation - -raw_time_step: 0.001 # Time step between raw data packets the simulation will generate (in seconds) -est_time_step: 0.002 # Time step between estimated data packets the simulation will generate (in seconds) - -# selects the motor to use, this is the name of the csv file located in /simulation/thrust_curves -# with the ".csv" removed -motor: "AeroTech_L1940X" -drag_coefficient: 0.4 # coefficient of drag for the rocket -rocket_mass: 14.5 # mass of rocket (in kg) -reference_area: 0.01929 # reference area (in meters squared) - -# vertical orientation of the rocket on the launch pad. only one value should be non-zero. -# Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) -rocket_orientation: [0, 0, -1] - -#--------------------- -# CONFIG FOR RANDOMNESS -#--------------------- -launch_rod_angle: [10] # min and max values for the launch rod angle of attack (in degrees) -launch_rod_direction: [0] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 06ce6c6f..22431e86 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -24,7 +24,6 @@ class SimIMU(IMU): """ __slots__ = ( - "_config", "_data_fetch_process", "_data_generator", "_data_queue", @@ -41,7 +40,7 @@ def __init__(self) -> None: # loads the sim_config.yaml file config_path = Path("airbrakes/simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: - self._config: dict = yaml.safe_load(file) + config: dict = yaml.safe_load(file) # This limits the queue size to a very high limit, because the data generator will # generate all the data before the imu reads it @@ -53,6 +52,7 @@ def __init__(self) -> None: self._data_fetch_process = multiprocessing.Process( target=self._fetch_data_loop, name="Sim IMU Process", + args=(config,), ) # Makes a boolean value that is shared between processes @@ -96,12 +96,12 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") - def _fetch_data_loop(self) -> None: + def _fetch_data_loop(self, config: dict) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" - raw_dt = self._config["raw_time_step"] - est_dt = self._config["est_time_step"] + raw_dt = config["raw_time_step"] + est_dt = config["est_time_step"] # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt From 7cf10cf6acdd5c2028c61471b4f5a539f710f618 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 21:21:16 -0500 Subject: [PATCH 16/63] Made it so you could pass in the sim config --- airbrakes/simulation/sim_config.py | 15 +++++++++++++++ airbrakes/simulation/sim_imu.py | 16 ++++++++-------- main.py | 2 +- pyproject.toml | 1 - utils.py | 3 ++- 5 files changed, 26 insertions(+), 11 deletions(-) diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 7fffd3d5..e4df8f65 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -53,3 +53,18 @@ def __init__(self, launch_rod_angle=[10], launch_rod_direction=[0] ) + + +def get_configuration(config_type: str) -> SimulationConfig: + """ + Gets the configuration for the simulation + :param config_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". + :return: The configuration for the simulation + """ + match config_type: + case "full-scale": + return FULL_SCALE_CONFIG + case "sub-scale": + return SUB_SCALE_CONFIG + case _: + raise ValueError(f"Invalid config type: {config_type}") diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 22431e86..25f25954 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -3,11 +3,11 @@ import contextlib import multiprocessing import time -from pathlib import Path from typing import TYPE_CHECKING import numpy as np -import yaml + +from airbrakes.simulation.sim_config import get_configuration, SimulationConfig if TYPE_CHECKING: from airbrakes.data_handling.imu_data_packet import IMUDataPacket @@ -31,16 +31,16 @@ class SimIMU(IMU): "_timestamp", ) - def __init__(self) -> None: + def __init__(self, sim_type: str) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. + :param sim_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". """ self._timestamp: np.float64 = np.float64(0.0) - # loads the sim_config.yaml file - config_path = Path("airbrakes/simulation/sim_config.yaml") - with config_path.open(mode="r", newline="") as file: - config: dict = yaml.safe_load(file) + + # Gets the configuration for the simulation + config = get_configuration(sim_type) # This limits the queue size to a very high limit, because the data generator will # generate all the data before the imu reads it @@ -96,7 +96,7 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") - def _fetch_data_loop(self, config: dict) -> None: + def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" diff --git a/main.py b/main.py index 3ccb992b..bf2d9c6c 100644 --- a/main.py +++ b/main.py @@ -55,7 +55,7 @@ def main(args: argparse.Namespace) -> None: # If we are running the simulation for generating datasets, we will replace our IMU object # with a sim variant, similar to running a mock simulation. else: - imu = SimIMU() + imu = SimIMU(sim_type=args.sim) # MockFactory is used to create a mock servo object that pretends to be the real servo servo = ( Servo(SERVO_PIN) diff --git a/pyproject.toml b/pyproject.toml index 16b0f187..fdda2ac9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,6 @@ dependencies = [ "colorama", "psutil", "scipy", - "PyYAML" # Installation instructions for the following dependencies can be found in the README: # "mscl" https://github.com/LORD-MicroStrain/MSCL/blob/master/BuildScripts/buildReadme_Linux.md ] diff --git a/utils.py b/utils.py index ab03d666..9a3662b1 100644 --- a/utils.py +++ b/utils.py @@ -105,7 +105,8 @@ def arg_parser() -> argparse.Namespace: help="runs the data simulation alongside the mock simulation, to randomly generate " "a dataset", action="store_true", - default=False, + choices=["full-scale", "sub-scale"], + default="full-scale", ) args = parser.parse_args() From d10ab9a1aacb13702a8c7133a6d4f6300e5cc8c8 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 21:41:33 -0500 Subject: [PATCH 17/63] Moved SimIMU variables into the process --- airbrakes/mock/mock_imu.py | 5 ++--- airbrakes/simulation/sim_imu.py | 40 +++++++++++++-------------------- 2 files changed, 18 insertions(+), 27 deletions(-) diff --git a/airbrakes/mock/mock_imu.py b/airbrakes/mock/mock_imu.py index b59a7282..faa80ce3 100644 --- a/airbrakes/mock/mock_imu.py +++ b/airbrakes/mock/mock_imu.py @@ -74,9 +74,9 @@ def _read_file( Reads the data from the log file and puts it into the shared queue. :param log_file_path: the name of the log file to read data from located in scripts/imu_data :param real_time_simulation: Whether to simulate a real flight by sleeping for a set period, - or run at full speed, e.g. for using it in the CI. + or run at full speed, e.g. for using it in the CI. :param start_after_log_buffer: Whether to send the data packets only after the log buffer - was filled for Standby state. + was filled for Standby state. """ start_index = 0 @@ -116,7 +116,6 @@ def _read_file( if not self._running.value: break - imu_data_packet = None fields_dict = {} scaled_accel_x = row.get("scaledAccelX") # raw data packet field diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 25f25954..992e4eee 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -23,22 +23,12 @@ class SimIMU(IMU): and returns randomly generated data. """ - __slots__ = ( - "_data_fetch_process", - "_data_generator", - "_data_queue", - "_running", - "_timestamp", - ) - def __init__(self, sim_type: str) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. :param sim_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". """ - self._timestamp: np.float64 = np.float64(0.0) - # Gets the configuration for the simulation config = get_configuration(sim_type) @@ -58,9 +48,8 @@ def __init__(self, sim_type: str) -> None: # Makes a boolean value that is shared between processes self._running = multiprocessing.Value("b", False) - self._data_generator = DataGenerator() - - def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + @staticmethod + def _update_timestamp(current_timestamp: np.float64, config: SimulationConfig) -> np.float64: """ Updates the current timestamp of the data generator, based off time step defined in config. Will also determine if the next timestamp will be a raw packet, estimated packet, or both. @@ -69,8 +58,8 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: """ # finding whether the raw or estimated data packets have a lower time_step - lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) - highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) + lowest_dt = min(config.raw_time_step, config.est_time_step) + highest_dt = max(config.raw_time_step, config.est_time_step) # checks if current time is a multiple of the highest and/or lowest time step at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) @@ -100,27 +89,30 @@ def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" - raw_dt = config["raw_time_step"] - est_dt = config["est_time_step"] + data_generator = DataGenerator() + timestamp: np.float64 = np.float64(0.0) + + raw_dt = config.raw_time_step + est_dt = config.est_time_step # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - while self._data_generator.velocities[2] > -100: + while data_generator.velocities[2] > -100: # starts timer start_time = time.time() # if the timestamp is a multiple of the raw time step, generate a raw data packet. - if any(np.isclose(self._timestamp % raw_dt, [0, raw_dt])): - self._data_queue.put(self._data_generator.generate_raw_data_packet()) + if any(np.isclose(timestamp % raw_dt, [0, raw_dt])): + self._data_queue.put(data_generator.generate_raw_data_packet()) # if the timestamp is a multiple of the est time step, generate an est data packet. - if any(np.isclose(self._timestamp % est_dt, [0, est_dt])): - self._data_queue.put(self._data_generator.generate_estimated_data_packet()) + if any(np.isclose(timestamp % est_dt, [0, est_dt])): + self._data_queue.put(data_generator.generate_estimated_data_packet()) # updates the timestamp and sleeps until next packet is ready in real-time - time_step = self.update_timestamp(self._timestamp) - self._timestamp - self._timestamp += time_step + time_step = self._update_timestamp(timestamp, config) - timestamp + timestamp += time_step end_time = time.time() time.sleep(max(0.0, time_step - (end_time - start_time))) # if self._data_generator._last_est_packet.timestamp>1.02e9: From 7014bfaddc28881a5c7026da8e1d616cc3642bea Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:09:53 -0500 Subject: [PATCH 18/63] Finished switching config to a class --- airbrakes/simulation/data_generator.py | 39 +++++++++++++------------- airbrakes/simulation/sim_config.py | 38 ++++++++++++++----------- airbrakes/simulation/sim_imu.py | 2 +- 3 files changed, 41 insertions(+), 38 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 81759500..b992796b 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -6,13 +6,13 @@ import numpy as np import numpy.typing as npt -import yaml from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, RawDataPacket, ) from airbrakes.simulation.rotation_manager import RotationManager +from airbrakes.simulation.sim_config import SimulationConfig from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY, MAX_VELOCITY_THRESHOLD from utils import deadband @@ -35,18 +35,19 @@ class DataGenerator: "_vertical_index", ) - def __init__(self): - """Initializes the data generator object""" + def __init__(self, config: SimulationConfig): + """ + Initializes the data generator object with the provided configuration + :param config: the configuration object for the simulation + """ + + self._config = config + self._last_est_packet: EstimatedDataPacket | None = None self._last_raw_packet: RawDataPacket | None = None self._last_velocities: npt.NDArray = np.array([0.0, 0.0, 0.0]) self._max_velocity: np.float64 = np.float64(0.1) - # loads the sim_config.yaml file - config_path = Path("airbrakes/simulation/sim_config.yaml") - with config_path.open(mode="r", newline="") as file: - self._config: dict = yaml.safe_load(file) - # loads thrust curve self._thrust_data: npt.NDArray = self._load_thrust_curve() @@ -57,7 +58,7 @@ def __init__(self): # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. - self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] + self._vertical_index: np.int64 = np.nonzero(self._config.rocket_orientation)[0][0] @property def velocities(self) -> npt.NDArray: @@ -71,7 +72,7 @@ def _get_random(self, identifier: str) -> np.float64: :return: float containing a random value for the selected identifier, between the bounds specified in the config """ - parameters = self._config[identifier] + parameters = getattr(self._config, identifier) if len(parameters) == 1: return parameters[0] if len(parameters) == 3: @@ -89,7 +90,7 @@ def _load_thrust_curve(self) -> npt.NDArray: :return: numpy array containing tuples with the time and thrust at that time. """ # gets the path of the csv based on the config file - csv_path = Path(f"airbrakes/simulation/thrust_curves/{self._config["motor"]}.csv") + csv_path = Path(f"airbrakes/simulation/thrust_curves/{self._config.motor}.csv") # initializes the list for timestamps and thrust values motor_timestamps = [0] @@ -126,12 +127,12 @@ def _get_rotation_managers(self) -> npt.NDArray: launch_rod_angle = self._get_random("launch_rod_angle") launch_rod_direction = self._get_random("launch_rod_direction") raw_manager = RotationManager( - self._config["rocket_orientation"], + self._config.rocket_orientation, launch_rod_angle, launch_rod_direction, ) est_manager = RotationManager( - self._config["rocket_orientation"], + self._config.rocket_orientation, launch_rod_angle, launch_rod_direction, ) @@ -205,7 +206,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: :return: RawDataPacket """ # creating shorthand variables from config - time_step = self._config["raw_time_step"] + time_step = self._config.raw_time_step # If the simulation has just started, we want to just generate the initial raw packet # and initialize "self._last_" variables @@ -228,7 +229,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: # calculates the forces and vertical scaled acceleration forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config["rocket_mass"] + force_accelerations = forces / self._config.rocket_mass compensated_accel = self._raw_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -276,7 +277,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: :return: EstimatedDataPacket """ # creating shorthand variables from config - time_step = self._config["est_time_step"] + time_step = self._config.est_time_step # If the simulation has just started, we want to just generate the initial estimated packet # and initialize "self._last_" variables @@ -298,7 +299,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: # calculates the forces and accelerations forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config["rocket_mass"] + force_accelerations = forces / self._config.rocket_mass compensated_accel = self._est_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -383,11 +384,9 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 - reference_area = self._config["reference_area"] - drag_coefficient = self._config["drag_coefficient"] thrust_force = 0.0 - drag_force = 0.5 * air_density * reference_area * drag_coefficient * speed**2 + drag_force = 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * speed**2 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index e4df8f65..da65a7ab 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -1,32 +1,36 @@ +import numpy as np +import numpy.typing as npt + + class SimulationConfig: def __init__(self, - raw_time_step, - est_time_step, - motor, - drag_coefficient, - rocket_mass, - reference_area, - rocket_orientation, - launch_rod_angle, - launch_rod_direction): + raw_time_step: np.float64, + est_time_step: np.float64, + motor: str, + drag_coefficient: np.float64, + rocket_mass: np.float64, + reference_area: np.float64, + rocket_orientation: npt.NDArray[np.float64], + launch_rod_angle: np.float64, + launch_rod_direction: np.float64): # Time steps for data packet generation in the simulation - self.raw_time_step = raw_time_step - self.est_time_step = est_time_step + self.raw_time_step = np.float64(raw_time_step) + self.est_time_step = np.float64(est_time_step) # Motor selection (name of CSV file without extension) self.motor = motor # Rocket properties - self.drag_coefficient = drag_coefficient - self.rocket_mass = rocket_mass - self.reference_area = reference_area + self.drag_coefficient = np.float64(drag_coefficient) + self.rocket_mass = np.float64(rocket_mass) + self.reference_area = np.float64(reference_area) # Rocket orientation on the launch pad - self.rocket_orientation = rocket_orientation + self.rocket_orientation = np.asarray(rocket_orientation, dtype=np.float64) # Config for randomness in the simulation - self.launch_rod_angle = launch_rod_angle - self.launch_rod_direction = launch_rod_direction + self.launch_rod_angle = np.float64(launch_rod_angle) + self.launch_rod_direction = np.float64(launch_rod_direction) FULL_SCALE_CONFIG = SimulationConfig( diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 992e4eee..5ce32ab5 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -89,7 +89,7 @@ def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" - data_generator = DataGenerator() + data_generator = DataGenerator(config) timestamp: np.float64 = np.float64(0.0) raw_dt = config.raw_time_step From c475013e7b50b133e1175478bccc2fbccd001b32 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:13:35 -0500 Subject: [PATCH 19/63] Fixed arg parser --- utils.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/utils.py b/utils.py index 9a3662b1..f72264e6 100644 --- a/utils.py +++ b/utils.py @@ -102,11 +102,11 @@ def arg_parser() -> argparse.Namespace: parser.add_argument( "-s", "--sim", - help="runs the data simulation alongside the mock simulation, to randomly generate " - "a dataset", - action="store_true", + help="Runs the data simulation alongside the mock simulation, with an optional scale", choices=["full-scale", "sub-scale"], - default="full-scale", + nargs="?", # Allows an optional argument + const="full-scale", # Default when `-s` is provided without a value + default=None, # Default when `-s` is not provided at all ) args = parser.parse_args() From 790c9b3d33a2f4a784033369fdedcba71d4d11d7 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:32:39 -0500 Subject: [PATCH 20/63] Cleaned up typing for SimulationConfig --- airbrakes/simulation/sim_config.py | 36 +++++++++++++++--------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index da65a7ab..7cc227b9 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -11,8 +11,8 @@ def __init__(self, rocket_mass: np.float64, reference_area: np.float64, rocket_orientation: npt.NDArray[np.float64], - launch_rod_angle: np.float64, - launch_rod_direction: np.float64): + launch_rod_angle: npt.NDArray[np.float64], + launch_rod_direction: npt.NDArray[np.float64]): # Time steps for data packet generation in the simulation self.raw_time_step = np.float64(raw_time_step) self.est_time_step = np.float64(est_time_step) @@ -34,28 +34,28 @@ def __init__(self, FULL_SCALE_CONFIG = SimulationConfig( - raw_time_step=0.001, - est_time_step=0.002, + raw_time_step=np.float64(0.001), + est_time_step=np.float64(0.002), motor="AeroTech_L1940X", - drag_coefficient=0.4, - rocket_mass=14.5, - reference_area=0.01929, - rocket_orientation=[0, 0, -1], - launch_rod_angle=[10], - launch_rod_direction=[0] + drag_coefficient=np.float64(0.4), + rocket_mass=np.float64(14.5), + reference_area=np.float64(0.01929), + rocket_orientation=np.array([0, 0, -1]), + launch_rod_angle=np.array([10]), + launch_rod_direction=np.array([0]) ) # TODO: get actual values for sub scale SUB_SCALE_CONFIG = SimulationConfig( - raw_time_step=0.001, - est_time_step=0.002, + raw_time_step=np.float64(0.001), + est_time_step=np.float64(0.002), motor="PLACEHOLDER", - drag_coefficient=0.4, - rocket_mass=0, - reference_area=0.001929, - rocket_orientation=[0, 0, -1], - launch_rod_angle=[10], - launch_rod_direction=[0] + drag_coefficient=np.float64(0.4), + rocket_mass=np.float64(0), + reference_area=np.float64(0.001929), + rocket_orientation=np.array([0, 0, -1]), + launch_rod_angle=np.array([10]), + launch_rod_direction=np.array([0]) ) From 2c234d3f2161df5c83471d96f0eb43808aea02eb Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:43:20 -0500 Subject: [PATCH 21/63] Fixed everything --- airbrakes/simulation/sim_config.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 7cc227b9..7a8ddf1a 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -14,23 +14,23 @@ def __init__(self, launch_rod_angle: npt.NDArray[np.float64], launch_rod_direction: npt.NDArray[np.float64]): # Time steps for data packet generation in the simulation - self.raw_time_step = np.float64(raw_time_step) - self.est_time_step = np.float64(est_time_step) + self.raw_time_step = raw_time_step + self.est_time_step = est_time_step # Motor selection (name of CSV file without extension) self.motor = motor # Rocket properties - self.drag_coefficient = np.float64(drag_coefficient) - self.rocket_mass = np.float64(rocket_mass) - self.reference_area = np.float64(reference_area) + self.drag_coefficient = drag_coefficient + self.rocket_mass = rocket_mass + self.reference_area = reference_area # Rocket orientation on the launch pad - self.rocket_orientation = np.asarray(rocket_orientation, dtype=np.float64) + self.rocket_orientation = rocket_orientation # Config for randomness in the simulation - self.launch_rod_angle = np.float64(launch_rod_angle) - self.launch_rod_direction = np.float64(launch_rod_direction) + self.launch_rod_angle = launch_rod_angle + self.launch_rod_direction = launch_rod_direction FULL_SCALE_CONFIG = SimulationConfig( From 648a35d1f726700c2283c5143e4df6e406ba6c11 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sat, 9 Nov 2024 23:26:42 -0500 Subject: [PATCH 22/63] rotation almost fixed --- airbrakes/simulation/README.md | 7 +++++ airbrakes/simulation/data_generator.py | 8 ++++- airbrakes/simulation/rotation_manager.py | 38 ++++++++++++++--------- airbrakes/simulation/sim_config.py | 39 +++++++++++++++--------- airbrakes/simulation/sim_imu.py | 5 +-- 5 files changed, 64 insertions(+), 33 deletions(-) create mode 100644 airbrakes/simulation/README.md diff --git a/airbrakes/simulation/README.md b/airbrakes/simulation/README.md new file mode 100644 index 00000000..dd5e0b4c --- /dev/null +++ b/airbrakes/simulation/README.md @@ -0,0 +1,7 @@ +## Rotation Guide for sim IMU + +Vertical orientation of the IMU is described in vector form. +- Example: If the -x axis of the IMU is facing the sky, the vertical orientation is [-1, 0, 0] + +Direction the rocket leans to on the pad is the launch_pad_direction. Ranges from 0 to 360 degrees. +Important to note that the IMU does not change position inside of the rocket when changing launch pad angle. \ No newline at end of file diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index b992796b..8e3457e9 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -318,6 +318,9 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: quaternion = self._est_rotation_manager.calculate_imu_quaternions() + # print(f"comp: {np.round(compensated_accel,3)}") + # print(f"vels: {np.round(vert_velocity,3)}") + packet = EstimatedDataPacket( next_timestamp * 1e9, estOrientQuaternionW=quaternion[0], @@ -381,12 +384,15 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: """ # calculate the magnitude of velocity speed = np.linalg.norm(velocities) + _ = speed # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 thrust_force = 0.0 - drag_force = 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * speed**2 + drag_force = ( + 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * 1**2 + ) # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 0d9cc2c1..f37d490d 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -61,14 +61,23 @@ def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) # Step 1: We have to declare that the orientation of the rocket is vertical aligned_orientation = R.align_vectors([self._vertical], [[0, 0, 1]])[0] - # Step 2: Tilt by the zenith angle to adjust pitch away from vertical - tilt_rotation = R.from_rotvec(self._zenith * np.array([1, 0, 0])) + # Step 2: Get the axis that the zenith rotates around, it changes depending on orientation. + zenith_rotation_axis = np.array( + [ + np.abs(self._vertical[2]), + np.abs(self._vertical[0]), + np.abs(self._vertical[1]), + ] + ) + + # Step 3: rotate by zenith + zenith_rotation = R.from_rotvec(self._zenith * zenith_rotation_axis) - # Step 3: Rotate around the vertical by azimuth to set the compass direction of the tilt - azimuth_rotation = R.from_rotvec(self._azimuth * np.array([0, 0, -1])) + # Step 4: rotate by azimuth + azimuth_rotation = R.from_rotvec(self._azimuth * np.array(self._vertical)) - # Step 4: Combined rotation: aligned orientation, followed by azimuth and zenith rotations - orientation = azimuth_rotation * tilt_rotation * aligned_orientation + # Step 5: Combined rotation: aligned orientation, azimuth rotation, and zenith rotation + orientation = azimuth_rotation * zenith_rotation * aligned_orientation # updating last orientation self._last_orientation = ( @@ -91,13 +100,12 @@ def calculate_compensated_accel( # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket # is still on the ground. We have to include the acceleration due to the normal force of # the ground. - compensated_accel = self._scalar_to_vector(thrust_drag_accel) - if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: - normal_force_vector = np.array([0, 0, GRAVITY]) - rotated_normal_vector = self._orientation.apply(normal_force_vector) + # if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: + # normal_force_vector = np.array([0, 0, GRAVITY]) + # rotated_normal_vector = self._orientation.apply(normal_force_vector) - compensated_accel += rotated_normal_vector - return compensated_accel + # compensated_accel += rotated_normal_vector + return self._scalar_to_vector(thrust_drag_accel) def calculate_linear_accel( self, thrust_acceleration: np.float64, drag_acceleration: np.float64 @@ -157,11 +165,11 @@ def calculate_imu_quaternions(self) -> npt.NDArray: :return: A quaternion adjusted to the IMU's frame. """ # rotation needed to align the IMU - imu_alignment_rotation = R.align_vectors([[0, 0, 1]], [[0, 0, -1]])[0] - imu_adjusted_orientation = imu_alignment_rotation * self._orientation + imu_alignment = R.align_vectors([[0, 0, 1]], [[0, 0, -1]])[0] + imu_quaternion_rotation = imu_alignment * self._orientation.inv() # convert to quaternion - return imu_adjusted_orientation.as_quat(scalar_first=True) + return imu_quaternion_rotation.as_quat(scalar_first=True) def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: """ diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 7a8ddf1a..b414f45f 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -1,18 +1,26 @@ +"""Module containing config settings for simulation""" + import numpy as np import numpy.typing as npt class SimulationConfig: - def __init__(self, - raw_time_step: np.float64, - est_time_step: np.float64, - motor: str, - drag_coefficient: np.float64, - rocket_mass: np.float64, - reference_area: np.float64, - rocket_orientation: npt.NDArray[np.float64], - launch_rod_angle: npt.NDArray[np.float64], - launch_rod_direction: npt.NDArray[np.float64]): + """ + config class for simulation + """ + + def __init__( + self, + raw_time_step: np.float64, + est_time_step: np.float64, + motor: str, + drag_coefficient: np.float64, + rocket_mass: np.float64, + reference_area: np.float64, + rocket_orientation: npt.NDArray[np.float64], + launch_rod_angle: npt.NDArray[np.float64], + launch_rod_direction: npt.NDArray[np.float64], + ): # Time steps for data packet generation in the simulation self.raw_time_step = raw_time_step self.est_time_step = est_time_step @@ -40,9 +48,9 @@ def __init__(self, drag_coefficient=np.float64(0.4), rocket_mass=np.float64(14.5), reference_area=np.float64(0.01929), - rocket_orientation=np.array([0, 0, -1]), + rocket_orientation=np.array([0, 0, 1]), launch_rod_angle=np.array([10]), - launch_rod_direction=np.array([0]) + launch_rod_direction=np.array([90]), ) # TODO: get actual values for sub scale @@ -53,16 +61,17 @@ def __init__(self, drag_coefficient=np.float64(0.4), rocket_mass=np.float64(0), reference_area=np.float64(0.001929), - rocket_orientation=np.array([0, 0, -1]), + rocket_orientation=np.array([-1, 0, 0]), launch_rod_angle=np.array([10]), - launch_rod_direction=np.array([0]) + launch_rod_direction=np.array([90]), ) def get_configuration(config_type: str) -> SimulationConfig: """ Gets the configuration for the simulation - :param config_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". + :param config_type: The type of simulation to run. This can be either "full-scale" + or "sub-scale". :return: The configuration for the simulation """ match config_type: diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 5ce32ab5..aa3c4596 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -7,7 +7,7 @@ import numpy as np -from airbrakes.simulation.sim_config import get_configuration, SimulationConfig +from airbrakes.simulation.sim_config import SimulationConfig, get_configuration if TYPE_CHECKING: from airbrakes.data_handling.imu_data_packet import IMUDataPacket @@ -27,7 +27,8 @@ def __init__(self, sim_type: str) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. - :param sim_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". + :param sim_type: The type of simulation to run. This can be either "full-scale" or + "sub-scale". """ # Gets the configuration for the simulation config = get_configuration(sim_type) From 4db8f69299b9b0325072e62d934fd3b60c593bf6 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 13:55:55 -0500 Subject: [PATCH 23/63] working --- airbrakes/simulation/data_generator.py | 9 ++++----- airbrakes/simulation/rotation_manager.py | 11 ++++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 8e3457e9..d4284bb2 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -384,18 +384,17 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: """ # calculate the magnitude of velocity speed = np.linalg.norm(velocities) - _ = speed # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 + drag_coefficient = self._config.drag_coefficient thrust_force = 0.0 - drag_force = ( - 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * 1**2 - ) - # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn if timestamp <= self._thrust_data[0][-1]: thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) + + drag_force = 0.5 * air_density * self._config.reference_area * drag_coefficient * speed**2 + return np.array([thrust_force, drag_force]) diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index f37d490d..d1bb4925 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -97,15 +97,16 @@ def calculate_compensated_accel( :return: the compensated acceleration in the vehicle frame of reference """ thrust_drag_accel = thrust_acceleration - drag_acceleration + compensated_accel = self._scalar_to_vector(thrust_drag_accel) # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket # is still on the ground. We have to include the acceleration due to the normal force of # the ground. - # if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: - # normal_force_vector = np.array([0, 0, GRAVITY]) - # rotated_normal_vector = self._orientation.apply(normal_force_vector) + if thrust_drag_accel <= GRAVITY and thrust_acceleration != 0.0: + normal_force_vector = np.array([0, 0, GRAVITY]) + rotated_normal_vector = self._orientation.apply(normal_force_vector) - # compensated_accel += rotated_normal_vector - return self._scalar_to_vector(thrust_drag_accel) + compensated_accel += rotated_normal_vector + return compensated_accel def calculate_linear_accel( self, thrust_acceleration: np.float64, drag_acceleration: np.float64 From 11addf76be4a27073aec12a4aa1c1a30a944a141 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 18:15:19 -0500 Subject: [PATCH 24/63] added variable mass --- airbrakes/simulation/data_generator.py | 48 ++++++++++++++----- airbrakes/simulation/sim_config.py | 4 +- .../thrust_curves/AeroTech_L1940X.csv | 2 +- 3 files changed, 39 insertions(+), 15 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index d4284bb2..8c7b83b1 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -25,6 +25,7 @@ class DataGenerator: __slots__ = ( "_config", + "_eff_exhaust_velocity", "_est_rotation_manager", "_last_est_packet", "_last_raw_packet", @@ -32,7 +33,6 @@ class DataGenerator: "_max_velocity", "_raw_rotation_manager", "_thrust_data", - "_vertical_index", ) def __init__(self, config: SimulationConfig): @@ -47,6 +47,7 @@ def __init__(self, config: SimulationConfig): self._last_raw_packet: RawDataPacket | None = None self._last_velocities: npt.NDArray = np.array([0.0, 0.0, 0.0]) self._max_velocity: np.float64 = np.float64(0.1) + self._eff_exhaust_velocity: np.float64 | None = None # loads thrust curve self._thrust_data: npt.NDArray = self._load_thrust_curve() @@ -56,13 +57,9 @@ def __init__(self, config: SimulationConfig): self._raw_rotation_manager: RotationManager = raw_manager self._est_rotation_manager: RotationManager = est_manager - # finds the vertical index of the orientation. For example, if -y is vertical, the index - # will be 1. - self._vertical_index: np.int64 = np.nonzero(self._config.rocket_orientation)[0][0] - @property def velocities(self) -> npt.NDArray: - """Returns the last calculated velocity of the rocket""" + """Returns the last calculated velocities of the rocket""" return self._last_velocities def _get_random(self, identifier: str) -> np.float64: @@ -99,9 +96,15 @@ def _load_thrust_curve(self) -> npt.NDArray: start_flag = False # flag to identify when the metadata/header rows are skipped with csv_path.open(mode="r", newline="") as thrust_csv: + propellant_weight = None reader = csv.reader(thrust_csv) for row in reader: + # We want to store the propellant weight + if row[0] == "propellant weight:": + propellant_weight = float(row[1]) / 1000 # we want in kg + continue + # Start appending data only after the header row if row == ["Time (s)", "Thrust (N)"]: start_flag = True @@ -114,6 +117,11 @@ def _load_thrust_curve(self) -> npt.NDArray: motor_timestamps.append(time) motor_thrusts.append(thrust) + # for future mass calculations, finding total impulse + total_impulse = np.trapezoid(motor_thrusts, motor_timestamps) + # calculating effective exhaust velocity (constant) + self._eff_exhaust_velocity = total_impulse / propellant_weight + return np.array([motor_timestamps, motor_thrusts]) def _get_rotation_managers(self) -> npt.NDArray: @@ -229,7 +237,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: # calculates the forces and vertical scaled acceleration forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config.rocket_mass + force_accelerations = forces / self._calculate_mass(next_timestamp) compensated_accel = self._raw_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -299,7 +307,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: # calculates the forces and accelerations forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config.rocket_mass + force_accelerations = forces / self._calculate_mass(next_timestamp) compensated_accel = self._est_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -318,9 +326,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: quaternion = self._est_rotation_manager.calculate_imu_quaternions() - # print(f"comp: {np.round(compensated_accel,3)}") - # print(f"vels: {np.round(vert_velocity,3)}") - packet = EstimatedDataPacket( next_timestamp * 1e9, estOrientQuaternionW=quaternion[0], @@ -386,7 +391,7 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: speed = np.linalg.norm(velocities) # we could probably actually calculate air density, maybe we set temperature as constant? - air_density = 1.1 + air_density = 1.2 drag_coefficient = self._config.drag_coefficient thrust_force = 0.0 @@ -398,3 +403,22 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: drag_force = 0.5 * air_density * self._config.reference_area * drag_coefficient * speed**2 return np.array([thrust_force, drag_force]) + + def _calculate_mass(self, timestamp: np.float64) -> np.float64: + """ + Calculates the weight of the rocket at any given timestamp. The weight loss is found + by calculating the mass flow rate using effective exhaust velocity. + :param timestamp: current timestamp of the rocket, in seconds + :return: the current mass of the rocket, in kilograms + """ + # find current thrust + current_thrust = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) + # getting thrust curve up to current timestamp + mask = self._thrust_data[0] <= timestamp + time_values = np.append(self._thrust_data[0][mask], [timestamp]) + thrust_values = np.append(self._thrust_data[1][mask], [current_thrust]) + # Calculate the total impulse up to this timestamp as the integral of thrust over time + current_total_impulse = np.trapezoid(thrust_values, time_values) + + # mass lost is current total impulse divided by effective exhaust velocity + return self._config.rocket_mass - current_total_impulse / self._eff_exhaust_velocity diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index b414f45f..51df4df3 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -30,7 +30,7 @@ def __init__( # Rocket properties self.drag_coefficient = drag_coefficient - self.rocket_mass = rocket_mass + self.rocket_mass = rocket_mass # This is wetted mass (including propellant weight) self.reference_area = reference_area # Rocket orientation on the launch pad @@ -46,7 +46,7 @@ def __init__( est_time_step=np.float64(0.002), motor="AeroTech_L1940X", drag_coefficient=np.float64(0.4), - rocket_mass=np.float64(14.5), + rocket_mass=np.float64(15.856), reference_area=np.float64(0.01929), rocket_orientation=np.array([0, 0, 1]), launch_rod_angle=np.array([10]), diff --git a/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv index 1328b5b1..3be8dcac 100644 --- a/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv +++ b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv @@ -1,7 +1,7 @@ "motor:","AeroTech L1940X" "contributor:","Mike Caplinger" "details:","https://www.thrustcurve.org/simfiles/60ac84458dc4640004c24eb6/" - +"propellant weight:",1825 "Time (s)","Thrust (N)" 0.02,48.137 0.04,2107.333 From d15a1195ea035b13b1017146782021638e68646e Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 19:30:49 -0500 Subject: [PATCH 25/63] atmospheric properties and variable drag --- airbrakes/simulation/data_generator.py | 25 ++++++++++++++++++++++--- airbrakes/simulation/sim_config.py | 16 +++++++++++----- 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 8c7b83b1..d59b76bd 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -387,13 +387,32 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: :return: a 2 element numpy array containing thrust force and drag force, respectively. Thrust is positive, drag is negative. """ + gradient = -6.5e-3 # troposphere gradient + gas_constant = 287 # J/kg*K + ratio_spec_heat = 1.4 + + # last altitude + altitude = self._last_est_packet.estPressureAlt # calculate the magnitude of velocity speed = np.linalg.norm(velocities) + # temperature using troposphere gradient + temp = self._config.air_temperature + 273.15 + gradient * altitude + + # air density formula for troposphere gradient + air_density = 1.225 * (temp / (self._config.air_temperature + 273.15)) ** ( + -GRAVITY / (gas_constant * gradient) - 1 + ) - # we could probably actually calculate air density, maybe we set temperature as constant? - air_density = 1.2 + # using speed of sound to find mach number + mach_number = speed / np.sqrt(ratio_spec_heat * gas_constant * temp) + + # getting the drag coefficient + drag_coefficient = np.interp( + mach_number, + self._config.drag_coefficient[0], + self._config.drag_coefficient[1], + ) - drag_coefficient = self._config.drag_coefficient thrust_force = 0.0 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 51df4df3..3ad960dc 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -14,12 +14,13 @@ def __init__( raw_time_step: np.float64, est_time_step: np.float64, motor: str, - drag_coefficient: np.float64, + drag_coefficient: npt.NDArray, rocket_mass: np.float64, reference_area: np.float64, rocket_orientation: npt.NDArray[np.float64], launch_rod_angle: npt.NDArray[np.float64], launch_rod_direction: npt.NDArray[np.float64], + air_temperature: np.float64, ): # Time steps for data packet generation in the simulation self.raw_time_step = raw_time_step @@ -29,10 +30,13 @@ def __init__( self.motor = motor # Rocket properties - self.drag_coefficient = drag_coefficient + self.drag_coefficient = drag_coefficient # coefficient of drag at mach numbers self.rocket_mass = rocket_mass # This is wetted mass (including propellant weight) self.reference_area = reference_area + # Atmospheric properties + self.air_temperature = air_temperature # ground temperature, in celcius + # Rocket orientation on the launch pad self.rocket_orientation = rocket_orientation @@ -41,13 +45,14 @@ def __init__( self.launch_rod_direction = launch_rod_direction -FULL_SCALE_CONFIG = SimulationConfig( +FULL_SCALE_CONFIG = SimulationConfig( # 2018.99 raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), motor="AeroTech_L1940X", - drag_coefficient=np.float64(0.4), + drag_coefficient=np.array([[0.1, 0.3, 0.5, 0.7], [0.3565, 0.3666, 0.3871, 0.41499]]), rocket_mass=np.float64(15.856), reference_area=np.float64(0.01929), + air_temperature=np.float64(25), rocket_orientation=np.array([0, 0, 1]), launch_rod_angle=np.array([10]), launch_rod_direction=np.array([90]), @@ -58,9 +63,10 @@ def __init__( raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), motor="PLACEHOLDER", - drag_coefficient=np.float64(0.4), + drag_coefficient=np.array([[0.3], [0.4]]), rocket_mass=np.float64(0), reference_area=np.float64(0.001929), + air_temperature=np.float64(25), rocket_orientation=np.array([-1, 0, 0]), launch_rod_angle=np.array([10]), launch_rod_direction=np.array([90]), From 46453f3ac5e94ea931fe34b69c56f791a54747b2 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 22:24:11 -0500 Subject: [PATCH 26/63] might undo later --- airbrakes/simulation/data_generator.py | 5 ++-- airbrakes/simulation/rotation_manager.py | 31 ++++++++++++++++++------ airbrakes/simulation/sim_config.py | 2 +- 3 files changed, 27 insertions(+), 11 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index d59b76bd..6b300bb7 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -230,7 +230,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity - self._raw_rotation_manager.update_orientation(time_step, velocity_ratio) + self._raw_rotation_manager.update_orientation(velocity_ratio) # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_raw_packet.timestamp / 1e9) + time_step @@ -300,7 +300,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity - self._est_rotation_manager.update_orientation(time_step, velocity_ratio) + self._est_rotation_manager.update_orientation(velocity_ratio) # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_est_packet.timestamp / 1e9) + time_step @@ -312,6 +312,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[0], force_accelerations[1], ) + compensated_accel += random.uniform(-4, 4) linear_accel = self._est_rotation_manager.calculate_linear_accel( force_accelerations[0], force_accelerations[1], diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index d1bb4925..6f0de7ca 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -16,6 +16,7 @@ class RotationManager: __slots__ = ( "_azimuth", + "_formula_consts", "_last_orientation", "_orientation", "_vertical", @@ -36,25 +37,39 @@ def __init__( self._azimuth: np.float64 = np.float64((rod_direction * np.pi) / 180) self._last_orientation: R | None = None self._orientation: R | None = None - self.update_orientation(0, 0) + self._formula_consts: npt.NDArray = self._initialize_rotation_formula() + self.update_orientation(1) @property def gravity_vector(self) -> npt.NDArray: return self._orientation.apply([0, 0, -GRAVITY]) - def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) -> None: + def _initialize_rotation_formula(self) -> np.float64: + """ + Initializes the formula for rotation. Returns the shift of the function. Is constant + for the rest of the flight. + :return: the shift and initial angle of the function. + """ + # initializes vector with small step values, the shift is always below 0.1 + # for inital rod angles below 45 degrees + dx = 0.00001 + xvec = np.arange(dx, 0.1, dx) + # this graph is zenith versus v/vmax + no_shift = (-xvec + 1) / (15 * xvec) + self._zenith + # finding the index where no_shift is closest to pi/2 + closest_index = np.argmin(np.abs(no_shift - np.pi / 2)) + return np.array([closest_index * dx, self._zenith]) # returns the shift and zenith + + def update_orientation(self, velocity_ratio: np.float64) -> None: """ Updates the baseline vehicle-frame orientation, and all of the vehicle-frame orientations - :param time_step: how much time has passed between updates :param velocity_ratio: current vertical velocity of the rocket divided by max velocity """ - if velocity_ratio == 0.0: - scale_factor = 0.0 - else: - scale_factor = np.sin(self._zenith) * time_step / abs(velocity_ratio * 10) - self._zenith = self._zenith + scale_factor * (np.pi / 2 - self._zenith) + # splitting up the function into two parts + devisor = 15 * (self._formula_consts[0] + 1) * (velocity_ratio + self._formula_consts[0]) + self._zenith = (-velocity_ratio + 1) / devisor + self._formula_consts[1] # We want to convert the azimuth and zenith values into a scipy rotation object diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 3ad960dc..afb050cc 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -53,7 +53,7 @@ def __init__( rocket_mass=np.float64(15.856), reference_area=np.float64(0.01929), air_temperature=np.float64(25), - rocket_orientation=np.array([0, 0, 1]), + rocket_orientation=np.array([0, 0, -1]), launch_rod_angle=np.array([10]), launch_rod_direction=np.array([90]), ) From 04a286fbfa704670ed0117ae535bacfffaaaaaac Mon Sep 17 00:00:00 2001 From: jgelia Date: Mon, 11 Nov 2024 01:22:03 -0500 Subject: [PATCH 27/63] Excluded simulation code from coverage report --- pyproject.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index fdda2ac9..e15dd941 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -61,7 +61,8 @@ source = ["airbrakes"] omit = [ "*/tests/*", "*/site-packages/*", - "airbrakes/mock/*" + "airbrakes/mock/*", + "airbrakes/simulation/*", ] [tool.coverage.report] From ad963f8898fd08e0dbc49e5e6b68de75ba758d39 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Mon, 11 Nov 2024 19:56:51 -0500 Subject: [PATCH 28/63] subscale values --- airbrakes/simulation/data_generator.py | 1 - airbrakes/simulation/rotation_manager.py | 4 ++-- airbrakes/simulation/sim_config.py | 17 +++++++-------- .../thrust_curves/AeroTech_J500G.csv | 21 +++++++++++++++++++ 4 files changed, 31 insertions(+), 12 deletions(-) create mode 100644 airbrakes/simulation/thrust_curves/AeroTech_J500G.csv diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 6b300bb7..01067965 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -312,7 +312,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[0], force_accelerations[1], ) - compensated_accel += random.uniform(-4, 4) linear_accel = self._est_rotation_manager.calculate_linear_accel( force_accelerations[0], force_accelerations[1], diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 6f0de7ca..99f7648a 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -116,7 +116,7 @@ def calculate_compensated_accel( # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket # is still on the ground. We have to include the acceleration due to the normal force of # the ground. - if thrust_drag_accel <= GRAVITY and thrust_acceleration != 0.0: + if thrust_drag_accel <= GRAVITY and thrust_acceleration != 0.0 and drag_acceleration < 1: normal_force_vector = np.array([0, 0, GRAVITY]) rotated_normal_vector = self._orientation.apply(normal_force_vector) @@ -136,7 +136,7 @@ def calculate_linear_accel( """ comp_accel = self.calculate_compensated_accel(thrust_acceleration, drag_acceleration) # apply gravity - gravity_accel_vector = self._orientation.apply([0, 0, -GRAVITY]) + gravity_accel_vector = self._orientation.apply([0, 0, GRAVITY]) return np.array(comp_accel - gravity_accel_vector) def calculate_delta_theta(self) -> npt.NDArray: diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index afb050cc..f9c24a87 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -54,22 +54,21 @@ def __init__( reference_area=np.float64(0.01929), air_temperature=np.float64(25), rocket_orientation=np.array([0, 0, -1]), - launch_rod_angle=np.array([10]), + launch_rod_angle=np.array([5]), launch_rod_direction=np.array([90]), ) -# TODO: get actual values for sub scale SUB_SCALE_CONFIG = SimulationConfig( raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), - motor="PLACEHOLDER", - drag_coefficient=np.array([[0.3], [0.4]]), - rocket_mass=np.float64(0), - reference_area=np.float64(0.001929), - air_temperature=np.float64(25), + motor="AeroTech_J500G", + drag_coefficient=np.array([[0.05, 0.2, 0.3], [0.41, 0.4175, 0.425]]), + rocket_mass=np.float64(5.954), + reference_area=np.float64(0.008205), + air_temperature=np.float64(15), rocket_orientation=np.array([-1, 0, 0]), - launch_rod_angle=np.array([10]), - launch_rod_direction=np.array([90]), + launch_rod_angle=np.array([5]), + launch_rod_direction=np.array([0]), ) diff --git a/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv b/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv new file mode 100644 index 00000000..3d151b3b --- /dev/null +++ b/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv @@ -0,0 +1,21 @@ +"motor:","AeroTech L1940X" +"contributor:","Will Sanderson" +"details:","https://www.thrustcurve.org/motors/AeroTech/J500G/" +"propellant weight:",363 +"Time (s)","Thrust (N)" +0.0134378, 40.2458 +0.0335946, 724.425 +0.0403135, 781.616 +0.0604703, 787.971 +0.0895857, 711.716 +0.134378, 686.297 +0.394177, 637.578 +0.575588, 588.86 +0.606943, 622.751 +0.633819, 620.633 +1.20045, 360.094 +1.24076, 345.267 +1.31019, 182.165 +1.38186, 65.6642 +1.43337, 23.3002 +1.45, 0 \ No newline at end of file From 018ba3d09354de70f15c8f4cbe887e76af4e4c32 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Mon, 4 Nov 2024 23:35:08 -0500 Subject: [PATCH 29/63] initial framework --- main.py | 16 ++++++++++++---- simulator/__init__.py | 0 simulator/sim_config.yaml | 6 ++++++ simulator/sim_imu.py | 15 +++++++++++++++ utils.py | 26 ++++++++++++++++++++++---- 5 files changed, 55 insertions(+), 8 deletions(-) create mode 100644 simulator/__init__.py create mode 100644 simulator/sim_config.yaml create mode 100644 simulator/sim_imu.py diff --git a/main.py b/main.py index aab94485..60171760 100644 --- a/main.py +++ b/main.py @@ -21,6 +21,7 @@ PORT, SERVO_PIN, ) +from simulator.sim_imu import SimIMU from utils import arg_parser @@ -41,13 +42,20 @@ def main(args: argparse.Namespace) -> None: sim_time_start = time.time() if args.mock: - # If we are running a simulation, then we will replace our hardware objects with mock + # If we are running a mock simulation, then we will replace our hardware objects with mock # objects that just pretend to be the real hardware. This is useful for testing the # software without having to fly the rocket. MockIMU pretends to be the imu by reading # previous flight data from a log file - imu = MockIMU( - args.path, real_time_simulation=not args.fast_simulation, start_after_log_buffer=True - ) + if not args.sim: + imu = MockIMU( + args.path, + real_time_simulation=not args.fast_simulation, + start_after_log_buffer=True + ) + # If we are running the simulator for generating datasets, we will replace our IMU object + # with a sim variant, similar to running a mock simulation. + else: + imu = SimIMU() # MockFactory is used to create a mock servo object that pretends to be the real servo servo = ( Servo(SERVO_PIN) diff --git a/simulator/__init__.py b/simulator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml new file mode 100644 index 00000000..c788f262 --- /dev/null +++ b/simulator/sim_config.yaml @@ -0,0 +1,6 @@ +# config file for simulator + +sim_apogee: 1800 # The simulator will generate a dataset where the apogee will be around this value (in meters) +time_step: 0.002 # The time step between data points the simulator will generate (in seconds) +motor_thrust_curve: 0 # placeholder +drag_coefficient: 0.4 # coefficient of drag for the rocket \ No newline at end of file diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py new file mode 100644 index 00000000..4c9b137f --- /dev/null +++ b/simulator/sim_imu.py @@ -0,0 +1,15 @@ +"""Module for simulating the IMU on the rocket using generated data.""" + +from airbrakes.hardware.imu import IMU + + +class SimIMU(IMU): + """ + A mock implementation of the IMU for testing purposes. It doesn't interact with any hardware + and returns randomly generated data. + """ + def __init__(self): + """ + Initializes the object that pretends to be an IMU for testing purposes by returning + randomly generated data. + """ diff --git a/utils.py b/utils.py index d26f7e65..c5a89852 100644 --- a/utils.py +++ b/utils.py @@ -99,17 +99,35 @@ def arg_parser() -> argparse.Namespace: default=False, ) + parser.add_argument( + "-s", + "--sim", + help="runs the data simulator alongside the mock simulator, to randomly generate a dataset", + action="store_true", + default=False, + ) + args = parser.parse_args() # Check if the user has passed any options that are only available in simulation mode: if ( - any([args.real_servo, args.keep_log_file, args.fast_simulation, args.debug, args.path]) + any([ + args.real_servo, + args.keep_log_file, + args.fast_simulation, + args.debug, + args.path, + args.sim, + ]) and not args.mock ): parser.error( - "The `--real-servo`, `--keep-log-file`, `--fast-simulation`, `--debug`, and `--path` " - "options are only available in simulation mode. Please pass `-m` or `--mock` " - "to run in simulation mode." + "The `--real-servo`, `--keep-log-file`, `--fast-simulation`, `--debug`, `--path`, " + "and `--sim` options are only available in simulation mode. Please pass `-m` or " + "`--mock` to run in simulation mode." ) + if args.sim and args.path: + parser.error("The `--path` option is not able to be used with the `--sim` option") + return args From c914a688265d8af11edd2159ceee460ee3c73e78 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Tue, 5 Nov 2024 21:47:47 -0500 Subject: [PATCH 30/63] simulating thrust curves --- main.py | 2 +- simulator/data_gen.py | 319 ++++++++++++++++++++ simulator/sim_config.yaml | 17 +- simulator/sim_imu.py | 52 ++++ simulator/test.py | 34 +++ simulator/thrust_curves/AeroTech_L1940X.csv | 25 ++ utils.py | 18 +- 7 files changed, 454 insertions(+), 13 deletions(-) create mode 100644 simulator/data_gen.py create mode 100644 simulator/test.py create mode 100644 simulator/thrust_curves/AeroTech_L1940X.csv diff --git a/main.py b/main.py index 60171760..e37e6ad6 100644 --- a/main.py +++ b/main.py @@ -50,7 +50,7 @@ def main(args: argparse.Namespace) -> None: imu = MockIMU( args.path, real_time_simulation=not args.fast_simulation, - start_after_log_buffer=True + start_after_log_buffer=True, ) # If we are running the simulator for generating datasets, we will replace our IMU object # with a sim variant, similar to running a mock simulation. diff --git a/simulator/data_gen.py b/simulator/data_gen.py new file mode 100644 index 00000000..31fad97b --- /dev/null +++ b/simulator/data_gen.py @@ -0,0 +1,319 @@ +"""Module that creates randomly generated data to sent to the simulator IMU""" + +import csv +from pathlib import Path + +import numpy as np +import numpy.typing as npt +import yaml +from scipy.spatial.transform import Rotation as R + +from airbrakes.data_handling.imu_data_packet import ( + EstimatedDataPacket, + RawDataPacket, +) +from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY +from utils import deadband + + +class DataGenerator: + """ + Uses config settings to generate realistic flight trajectories. Returns raw and + estimated data points for the sim IMU to combine into data packets + """ + + __slots__ = ( + "_config", + "_current_timestamp", + "_last_est_packet", + "_last_raw_packet", + "_last_velocity", + "_thrust_data", + "_vertical_index", + ) + + def __init__(self): + """Initializes the data generator object""" + self._current_timestamp: np.float64 = np.float64(0.0) + self._last_est_packet: EstimatedDataPacket | None = None + self._last_raw_packet: RawDataPacket | None = None + self._last_velocity: np.float64 = np.float64(0.0) + + # loads the sim_config.yaml file + config_path = Path("simulator/sim_config.yaml") + with config_path.open(mode="r", newline="") as file: + self._config: dict = yaml.safe_load(file) + + # finds the vertical index of the orientation. For example, if -y is vertical, the index + # will be 1. + self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] + self._thrust_data: npt.NDArray = self._load_thrust_curve() + + def _load_thrust_curve(self) -> npt.NDArray: + """ + Loads the thrust curve from the motor specified in the configs. + + :return: numpy array containing tuples with the time and thrust at that time. + """ + # gets the path of the csv based on the config file + csv_path = Path(f"simulator/thrust_curves/{self._config["motor"]}.csv") + + # initializes the list for timestamps and thrust values + motor_timestamps = [ + 0, + ] + motor_thrusts = [ + 0, + ] + + start_flag = False # flag to identify when the metadata/header rows are skipped + + with csv_path.open(mode="r", newline="") as thrust_csv: + reader = csv.reader(thrust_csv) + + for row in reader: + # Start appending data only after the header row + if row == ["Time (s)", "Thrust (N)"]: + start_flag = True + continue # Skip header row itself + + if start_flag: + # Convert time and thrust values to floats and append as a tuple + time = float(row[0]) + thrust = float(row[1]) + motor_timestamps.append(time) + motor_thrusts.append(thrust) + + return np.array([motor_timestamps, motor_thrusts]) + + def _first_update(self) -> npt.NDArray: + """ + Sets up the initial values for the estimated and raw data packets. This should + only be called once, and all values will be approximate launch pad conditions. + + :return: numpy array containing a raw data packet and an estimated data packet + respectively, immitating initial conditions on the launch pad. + """ + orientation = self._config["rocket_orientation"] + initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) + initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) + + return np.array( + [ + RawDataPacket( + 0, + scaledAccelX=orientation[0], + scaledAccelY=orientation[1], + scaledAccelZ=orientation[2], + scaledGyroX=0.0, + scaledGyroY=0.0, + scaledGyroZ=0.0, + deltaVelX=0.0, + deltaVelY=0.0, + deltaVelZ=0.0, + deltaThetaX=0.0, + deltaThetaY=0.0, + deltaThetaZ=0.0, + ), + EstimatedDataPacket( + 0, + estOrientQuaternionW=initial_quaternion[0], + estOrientQuaternionX=initial_quaternion[1], + estOrientQuaternionY=initial_quaternion[2], + estOrientQuaternionZ=initial_quaternion[3], + estCompensatedAccelX=GRAVITY * orientation[0], + estCompensatedAccelY=GRAVITY * orientation[1], + estCompensatedAccelZ=GRAVITY * orientation[2], + estPressureAlt=0, + estGravityVectorX=-GRAVITY * orientation[0], + estGravityVectorY=-GRAVITY * orientation[1], + estGravityVectorZ=-GRAVITY * orientation[2], + estAngularRateX=0, + estAngularRateY=0, + estAngularRateZ=0, + ), + ] + ) + + def generate_data_packet(self) -> npt.NDArray: + """ + Generates data points and sends to queue + + :return: Numpy list containing a raw data packet, an estimated data packet, or both. + """ + # gets the next timestamp + next_timestamp = self.update_timestamp(self._current_timestamp) + # If the simulation has just started, we want to just generate the initial data points + # and initialize "self._last_" variables + if any(packet is None for packet in [self._last_est_packet, self._last_raw_packet]): + self._last_raw_packet, self._last_est_packet = self._first_update() + self._current_timestamp = next_timestamp + return np.array([self._last_raw_packet, self._last_est_packet]) + + raw_time_step = self._config["raw_time_step"] + est_time_step = self._config["est_time_step"] + + # creates a boolean array to indicate whether the next timestamp will have a raw + # data packet, an estimated data packet, or both. + packet_type_flag = np.array( + [ + any(np.isclose(next_timestamp % raw_time_step, [0, raw_time_step])), + any(np.isclose(next_timestamp % est_time_step, [0, est_time_step])), + ] + ) + + # just as a short-hand + orientation = self._config["rocket_orientation"] + + # finds acceleration from thrust curve data, if the timestamp is before the cutoff time + # of the motor + if next_timestamp <= self._thrust_data[0][-1]: + interpreted_thrust = np.interp( + next_timestamp, self._thrust_data[0], self._thrust_data[1] + ) + vertical_linear_accel = interpreted_thrust / self._config["rocket_mass"] + + new_packets = np.array([None, None]) + + # assembles the raw data packet + if packet_type_flag[0]: + # scaled acceleration is compensated, but divided by gravity. On launch pad, the + # vertical sacled acceleration will be -1.00 (if negative is vertical). + # this value should always be positive though (during motor burn) + vert_scaled_accel = 1 + vertical_linear_accel / GRAVITY + + # gets the last vertical scaled accel, flips depending on the vertical index + last_vert_scaled_accel = [ + self._last_raw_packet.scaledAccelX, + self._last_raw_packet.scaledAccelY, + self._last_raw_packet.scaledAccelZ, + ][self._vertical_index] + + # finds the vertical delta velocity + vert_delta_v = GRAVITY * (vert_scaled_accel - last_vert_scaled_accel) * raw_time_step + + raw_data_packet = RawDataPacket( + next_timestamp * 1e9, + scaledAccelX=vert_scaled_accel * orientation[0], + scaledAccelY=vert_scaled_accel * orientation[1], + scaledAccelZ=vert_scaled_accel * orientation[2], + scaledGyroX=0.0, + scaledGyroY=0.0, + scaledGyroZ=0.0, + deltaVelX=vert_delta_v * orientation[0], + deltaVelY=vert_delta_v * orientation[1], + deltaVelZ=vert_delta_v * orientation[2], + deltaThetaX=0.0, + deltaThetaY=0.0, + deltaThetaZ=0.0, + ) + # appends to array + new_packets[0] = raw_data_packet + + # updates last packet + self._last_raw_packet = raw_data_packet + + # Assembles the estimated data packet + if packet_type_flag[1]: + # default value is positive during motor burn + vert_comp_accel = vertical_linear_accel + GRAVITY + + # gets vertical velocity and finds updated altitude + vert_velocity = self._calculate_vertical_velocity(vert_comp_accel, est_time_step) + last_altitude = self._last_est_packet.estPressureAlt + new_altitude = last_altitude + vert_velocity * est_time_step + + # gets previous quaternion + last_quat = np.array( + [ + self._last_est_packet.estOrientQuaternionW, + self._last_est_packet.estOrientQuaternionX, + self._last_est_packet.estOrientQuaternionY, + self._last_est_packet.estOrientQuaternionZ, + ] + ) + + est_data_packet = EstimatedDataPacket( + next_timestamp * 1e9, + estOrientQuaternionW=last_quat[0], + estOrientQuaternionX=last_quat[1], + estOrientQuaternionY=last_quat[2], + estOrientQuaternionZ=last_quat[3], + estCompensatedAccelX=vert_comp_accel * orientation[0], + estCompensatedAccelY=vert_comp_accel * orientation[1], + estCompensatedAccelZ=vert_comp_accel * orientation[2], + estPressureAlt=new_altitude, + estGravityVectorX=-GRAVITY * orientation[0], + estGravityVectorY=-GRAVITY * orientation[1], + estGravityVectorZ=-GRAVITY * orientation[2], + estAngularRateX=0, + estAngularRateY=0, + estAngularRateZ=0, + ) + + # appends to array + new_packets[1] = est_data_packet + + # updates last packet + self._last_est_packet = est_data_packet + + # updates the timestamp + self._current_timestamp = next_timestamp + + return new_packets + + def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: + """ + Calculates the velocity of the rocket based on the compensated acceleration. + Integrates that acceleration to get the velocity. + + :return: the velocity of the rocket + """ + # deadbands the acceleration + acceleration = deadband(acceleration - GRAVITY, ACCELERATION_NOISE_THRESHOLD) + + # Integrate the acceleration to get the velocity + vertical_velocity = self._last_velocity + acceleration * time_diff + + # updates the last vertical velocity + self._last_velocity = vertical_velocity + + return vertical_velocity + + def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + """ + Updates the current timestamp of the data generator, based off time step defined in config. + Will also determine if the next timestamp will be a raw packet, estimated packet, or both. + + :param current_timestamp: the current timestamp of the simulation + + :return: the updated current timestamp, rounded to 3 decimals + """ + + # finding whether the raw or estimated data packets have a lower time_step + lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) + highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) + + # checks if current time is a multiple of the highest and/or lowest time step + at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) + at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) + + # If current timestamp is a multiple of both, the next timestamp will be the + # the current timestamp + the lower time steps + if all([at_low, at_high]): + return np.round(current_timestamp + lowest_dt, 3) + + # If timestamp is a multiple of just the lowest time step, the next will be + # either current + lowest, or the next timestamp that is divisible by the highest + if at_low and not at_high: + dt = min(lowest_dt, highest_dt - (current_timestamp % highest_dt)) + return np.round(current_timestamp + dt, 3) + + # If timestamp is a multiple of only the highest time step, the next will + # always be the next timestamp that is divisible by the lowest + if at_high and not at_low: + return np.round(current_timestamp + lowest_dt - (current_timestamp % lowest_dt), 3) + + # This would happen if the input current timestamp is not a multiple of the raw + # or estimated time steps, or if there is a rounding/floating point error. + raise ValueError("Could not update timestamp, time stamp is invalid") diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index c788f262..b9e1e438 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -1,6 +1,15 @@ # config file for simulator -sim_apogee: 1800 # The simulator will generate a dataset where the apogee will be around this value (in meters) -time_step: 0.002 # The time step between data points the simulator will generate (in seconds) -motor_thrust_curve: 0 # placeholder -drag_coefficient: 0.4 # coefficient of drag for the rocket \ No newline at end of file +sim_apogee: 1800 # Simulator will generate data with the apogee around this value (in meters) +raw_time_step: 0.001 # Time step between raw data packets the simulator will generate (in seconds) +est_time_step: 0.002 # Time step between estimated data packets the simulator will generate (in seconds) + +# selects the motor to use, this is the name of the csv file located in /simulator/thrust_curves +# with the ".csv" removed +motor: "AeroTech_L1940X" +drag_coefficient: 0.4 # coefficient of drag for the rocket +rocket_mass: 16 # mass of rocket (in kg) + +# vertical orientation of the rocket on the launch pad. only one value should be non-zero. +# Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) +rocket_orientation: [0, 0, -1] \ No newline at end of file diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 4c9b137f..7a47a3ed 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -1,6 +1,15 @@ """Module for simulating the IMU on the rocket using generated data.""" +import contextlib +import multiprocessing +import time + +from airbrakes.data_handling.imu_data_packet import ( + IMUDataPacket, +) from airbrakes.hardware.imu import IMU +from constants import MAX_QUEUE_SIZE +from simulator.data_gen import DataGenerator class SimIMU(IMU): @@ -8,8 +17,51 @@ class SimIMU(IMU): A mock implementation of the IMU for testing purposes. It doesn't interact with any hardware and returns randomly generated data. """ + + __slots__ = ( + "_data_fetch_process", + "_data_generator", + "_data_queue", + "_running", + ) + def __init__(self): """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. """ + # This limits the queue size to a very high limit, because the data generator will + # generate all of the data before the imu reads it + self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue( + MAX_QUEUE_SIZE + ) + + # Starts the process that fetches the generated data + self._data_fetch_process = multiprocessing.Process( + target=self._fetch_data_loop, + name="Sim IMU Process", + ) + + # Makes a boolean value that is shared between processes + self._running = multiprocessing.Value("b", False) + + self._data_generator = DataGenerator() + + def _fetch_data_loop(self) -> None: + """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated + data.""" + # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the + # function in a context manager to suppress the KeyboardInterrupt + with contextlib.suppress(KeyboardInterrupt): + for i in range(2000): + packets = self._data_generator.generate_data_packet() + timestamp = 0 + if packets[0] is not None: + self._data_queue.put(packets[0]) + timestamp = packets[0].timestamp / 1e9 + if packets[1] is not None: + self._data_queue.put(packets[1]) + timestamp = packets[1].timestamp / 1e9 + + dt = self._data_generator.update_timestamp(timestamp) - timestamp + time.sleep(dt) diff --git a/simulator/test.py b/simulator/test.py new file mode 100644 index 00000000..41a59605 --- /dev/null +++ b/simulator/test.py @@ -0,0 +1,34 @@ +import numpy as np +from scipy.spatial.transform import Rotation as R + +reference_vector = np.array([0, 0.001, -0.999]) + +orientation = [-1, 0, 0] +rotation = R.from_quat([0.61057544, -0.34405011, -0.60791582, -0.37318307], scalar_first=True) +rotation, _ = R.align_vectors( + [reference_vector], + [orientation], +) +print(R.as_quat(rotation, scalar_first=True)) +vec = rotation.apply(orientation) +print(vec) + +orientation = [1, 0, 0] +rotation = R.from_quat([0.37394506, -0.59507751, 0.41232952, 0.57968295], scalar_first=True) +rotation, _ = R.align_vectors( + [reference_vector], + [orientation], +) +print(R.as_quat(rotation, scalar_first=True)) +vec = rotation.apply(orientation) +print(vec) + +orientation = [0, 0, -1] +rotation = R.from_quat([0.85752511, 0.02006555, 0.00998313, -0.51395386], scalar_first=True) +rotation, _ = R.align_vectors( + [reference_vector], + [orientation], +) +print(R.as_quat(rotation, scalar_first=True)) +vec = rotation.apply(orientation) +print(vec) diff --git a/simulator/thrust_curves/AeroTech_L1940X.csv b/simulator/thrust_curves/AeroTech_L1940X.csv new file mode 100644 index 00000000..1328b5b1 --- /dev/null +++ b/simulator/thrust_curves/AeroTech_L1940X.csv @@ -0,0 +1,25 @@ +"motor:","AeroTech L1940X" +"contributor:","Mike Caplinger" +"details:","https://www.thrustcurve.org/simfiles/60ac84458dc4640004c24eb6/" + +"Time (s)","Thrust (N)" +0.02,48.137 +0.04,2107.333 +0.055,2289.184 +0.073,2150.122 +0.156,2101.985 +0.397,2150.122 +0.691,2155.47 +0.982,2118.03 +1.41,2000.362 +1.696,1898.739 +1.935,1839.905 +2.06,1818.511 +2.103,1652.705 +2.116,1374.58 +2.133,935.998 +2.163,534.856 +2.214,240.685 +2.244,112.32 +2.296,5.349 +2.309,0 diff --git a/utils.py b/utils.py index c5a89852..414c5585 100644 --- a/utils.py +++ b/utils.py @@ -111,14 +111,16 @@ def arg_parser() -> argparse.Namespace: # Check if the user has passed any options that are only available in simulation mode: if ( - any([ - args.real_servo, - args.keep_log_file, - args.fast_simulation, - args.debug, - args.path, - args.sim, - ]) + any( + [ + args.real_servo, + args.keep_log_file, + args.fast_simulation, + args.debug, + args.path, + args.sim, + ] + ) and not args.mock ): parser.error( From 1aebae792a3aff70b699fb8029aad020f2225af5 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Tue, 5 Nov 2024 22:10:13 -0500 Subject: [PATCH 31/63] ruff --- simulator/sim_imu.py | 9 +++++---- simulator/test.py | 34 ---------------------------------- 2 files changed, 5 insertions(+), 38 deletions(-) delete mode 100644 simulator/test.py diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 7a47a3ed..2a2bef1b 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -3,10 +3,11 @@ import contextlib import multiprocessing import time +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from airbrakes.data_handling.imu_data_packet import IMUDataPacket -from airbrakes.data_handling.imu_data_packet import ( - IMUDataPacket, -) from airbrakes.hardware.imu import IMU from constants import MAX_QUEUE_SIZE from simulator.data_gen import DataGenerator @@ -53,7 +54,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - for i in range(2000): + for _ in range(2000): packets = self._data_generator.generate_data_packet() timestamp = 0 if packets[0] is not None: diff --git a/simulator/test.py b/simulator/test.py deleted file mode 100644 index 41a59605..00000000 --- a/simulator/test.py +++ /dev/null @@ -1,34 +0,0 @@ -import numpy as np -from scipy.spatial.transform import Rotation as R - -reference_vector = np.array([0, 0.001, -0.999]) - -orientation = [-1, 0, 0] -rotation = R.from_quat([0.61057544, -0.34405011, -0.60791582, -0.37318307], scalar_first=True) -rotation, _ = R.align_vectors( - [reference_vector], - [orientation], -) -print(R.as_quat(rotation, scalar_first=True)) -vec = rotation.apply(orientation) -print(vec) - -orientation = [1, 0, 0] -rotation = R.from_quat([0.37394506, -0.59507751, 0.41232952, 0.57968295], scalar_first=True) -rotation, _ = R.align_vectors( - [reference_vector], - [orientation], -) -print(R.as_quat(rotation, scalar_first=True)) -vec = rotation.apply(orientation) -print(vec) - -orientation = [0, 0, -1] -rotation = R.from_quat([0.85752511, 0.02006555, 0.00998313, -0.51395386], scalar_first=True) -rotation, _ = R.align_vectors( - [reference_vector], - [orientation], -) -print(R.as_quat(rotation, scalar_first=True)) -vec = rotation.apply(orientation) -print(vec) From 6a99579aa5327fa6014762271e9fdb5687edbe13 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Wed, 6 Nov 2024 00:42:32 -0500 Subject: [PATCH 32/63] added drag force and coast phase --- simulator/data_gen.py | 19 ++++++++++++++++--- simulator/sim_config.yaml | 3 ++- simulator/sim_imu.py | 2 +- 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/simulator/data_gen.py b/simulator/data_gen.py index 31fad97b..0875351f 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -165,13 +165,19 @@ def generate_data_packet(self) -> npt.NDArray: # just as a short-hand orientation = self._config["rocket_orientation"] - # finds acceleration from thrust curve data, if the timestamp is before the cutoff time + # initializes the net force, drag force and thrust (if applicable) will be added + net_force = 0 + # finds force from thrust curve data, if the timestamp is before the cutoff time # of the motor if next_timestamp <= self._thrust_data[0][-1]: - interpreted_thrust = np.interp( + net_force = np.interp( next_timestamp, self._thrust_data[0], self._thrust_data[1] ) - vertical_linear_accel = interpreted_thrust / self._config["rocket_mass"] + # subtracts drag force from net force + net_force -= self._calculate_drag_force(self._last_velocity) + # subtracts weight force from net force + net_force -= GRAVITY*self._config["rocket_mass"] + vertical_linear_accel = net_force / self._config["rocket_mass"] new_packets = np.array([None, None]) @@ -317,3 +323,10 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: # This would happen if the input current timestamp is not a multiple of the raw # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") + + def _calculate_drag_force(self,velocity): + # we could probably actually calculate this, maybe we can set temp as a constant? + roe = 1.1 + reference_area = self._config["reference_area"] + drag_coefficient = self._config["drag_coefficient"] + return 0.5 * roe * reference_area * drag_coefficient * velocity**2 diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index b9e1e438..89dad200 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -8,7 +8,8 @@ est_time_step: 0.002 # Time step between estimated data packets the simulator wi # with the ".csv" removed motor: "AeroTech_L1940X" drag_coefficient: 0.4 # coefficient of drag for the rocket -rocket_mass: 16 # mass of rocket (in kg) +rocket_mass: 14.5 # mass of rocket (in kg) +reference_area: 0.01929 # reference area (in meters squared) # vertical orientation of the rocket on the launch pad. only one value should be non-zero. # Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 2a2bef1b..17635f23 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -54,7 +54,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - for _ in range(2000): + for _ in range(21000): packets = self._data_generator.generate_data_packet() timestamp = 0 if packets[0] is not None: From 91e89b39ec37c7828fcb4efadcdbbf729829deda Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Wed, 6 Nov 2024 21:50:29 -0500 Subject: [PATCH 33/63] reformatting --- simulator/data_gen.py | 373 ++++++++++++++++++++---------------------- simulator/sim_imu.py | 84 ++++++++-- simulator/test.py | 5 + 3 files changed, 256 insertions(+), 206 deletions(-) create mode 100644 simulator/test.py diff --git a/simulator/data_gen.py b/simulator/data_gen.py index 0875351f..13afb94b 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -10,6 +10,7 @@ from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, + IMUDataPacket, RawDataPacket, ) from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY @@ -24,7 +25,6 @@ class DataGenerator: __slots__ = ( "_config", - "_current_timestamp", "_last_est_packet", "_last_raw_packet", "_last_velocity", @@ -34,7 +34,6 @@ class DataGenerator: def __init__(self): """Initializes the data generator object""" - self._current_timestamp: np.float64 = np.float64(0.0) self._last_est_packet: EstimatedDataPacket | None = None self._last_raw_packet: RawDataPacket | None = None self._last_velocity: np.float64 = np.float64(0.0) @@ -86,169 +85,50 @@ def _load_thrust_curve(self) -> npt.NDArray: return np.array([motor_timestamps, motor_thrusts]) - def _first_update(self) -> npt.NDArray: + def _get_first_packet( + self,packet_type: RawDataPacket | EstimatedDataPacket + ) -> RawDataPacket | EstimatedDataPacket: """ Sets up the initial values for the estimated and raw data packets. This should only be called once, and all values will be approximate launch pad conditions. - :return: numpy array containing a raw data packet and an estimated data packet - respectively, immitating initial conditions on the launch pad. + :param packet_type: either RawDataPacket or EstimatedDataPacket class objects. + used as identifier. + + :return: data packet of the specified type with launch pad conditions. """ orientation = self._config["rocket_orientation"] initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) - return np.array( - [ - RawDataPacket( - 0, - scaledAccelX=orientation[0], - scaledAccelY=orientation[1], - scaledAccelZ=orientation[2], - scaledGyroX=0.0, - scaledGyroY=0.0, - scaledGyroZ=0.0, - deltaVelX=0.0, - deltaVelY=0.0, - deltaVelZ=0.0, - deltaThetaX=0.0, - deltaThetaY=0.0, - deltaThetaZ=0.0, - ), - EstimatedDataPacket( - 0, - estOrientQuaternionW=initial_quaternion[0], - estOrientQuaternionX=initial_quaternion[1], - estOrientQuaternionY=initial_quaternion[2], - estOrientQuaternionZ=initial_quaternion[3], - estCompensatedAccelX=GRAVITY * orientation[0], - estCompensatedAccelY=GRAVITY * orientation[1], - estCompensatedAccelZ=GRAVITY * orientation[2], - estPressureAlt=0, - estGravityVectorX=-GRAVITY * orientation[0], - estGravityVectorY=-GRAVITY * orientation[1], - estGravityVectorZ=-GRAVITY * orientation[2], - estAngularRateX=0, - estAngularRateY=0, - estAngularRateZ=0, - ), - ] - ) - - def generate_data_packet(self) -> npt.NDArray: - """ - Generates data points and sends to queue - - :return: Numpy list containing a raw data packet, an estimated data packet, or both. - """ - # gets the next timestamp - next_timestamp = self.update_timestamp(self._current_timestamp) - # If the simulation has just started, we want to just generate the initial data points - # and initialize "self._last_" variables - if any(packet is None for packet in [self._last_est_packet, self._last_raw_packet]): - self._last_raw_packet, self._last_est_packet = self._first_update() - self._current_timestamp = next_timestamp - return np.array([self._last_raw_packet, self._last_est_packet]) - - raw_time_step = self._config["raw_time_step"] - est_time_step = self._config["est_time_step"] - - # creates a boolean array to indicate whether the next timestamp will have a raw - # data packet, an estimated data packet, or both. - packet_type_flag = np.array( - [ - any(np.isclose(next_timestamp % raw_time_step, [0, raw_time_step])), - any(np.isclose(next_timestamp % est_time_step, [0, est_time_step])), - ] - ) - - # just as a short-hand - orientation = self._config["rocket_orientation"] - - # initializes the net force, drag force and thrust (if applicable) will be added - net_force = 0 - # finds force from thrust curve data, if the timestamp is before the cutoff time - # of the motor - if next_timestamp <= self._thrust_data[0][-1]: - net_force = np.interp( - next_timestamp, self._thrust_data[0], self._thrust_data[1] - ) - # subtracts drag force from net force - net_force -= self._calculate_drag_force(self._last_velocity) - # subtracts weight force from net force - net_force -= GRAVITY*self._config["rocket_mass"] - vertical_linear_accel = net_force / self._config["rocket_mass"] - - new_packets = np.array([None, None]) - - # assembles the raw data packet - if packet_type_flag[0]: - # scaled acceleration is compensated, but divided by gravity. On launch pad, the - # vertical sacled acceleration will be -1.00 (if negative is vertical). - # this value should always be positive though (during motor burn) - vert_scaled_accel = 1 + vertical_linear_accel / GRAVITY - - # gets the last vertical scaled accel, flips depending on the vertical index - last_vert_scaled_accel = [ - self._last_raw_packet.scaledAccelX, - self._last_raw_packet.scaledAccelY, - self._last_raw_packet.scaledAccelZ, - ][self._vertical_index] - - # finds the vertical delta velocity - vert_delta_v = GRAVITY * (vert_scaled_accel - last_vert_scaled_accel) * raw_time_step - - raw_data_packet = RawDataPacket( - next_timestamp * 1e9, - scaledAccelX=vert_scaled_accel * orientation[0], - scaledAccelY=vert_scaled_accel * orientation[1], - scaledAccelZ=vert_scaled_accel * orientation[2], + packet = None + if packet_type == RawDataPacket: + packet = RawDataPacket( + 0, + scaledAccelX=orientation[0], + scaledAccelY=orientation[1], + scaledAccelZ=orientation[2], scaledGyroX=0.0, scaledGyroY=0.0, scaledGyroZ=0.0, - deltaVelX=vert_delta_v * orientation[0], - deltaVelY=vert_delta_v * orientation[1], - deltaVelZ=vert_delta_v * orientation[2], + deltaVelX=0.0, + deltaVelY=0.0, + deltaVelZ=0.0, deltaThetaX=0.0, deltaThetaY=0.0, deltaThetaZ=0.0, ) - # appends to array - new_packets[0] = raw_data_packet - - # updates last packet - self._last_raw_packet = raw_data_packet - - # Assembles the estimated data packet - if packet_type_flag[1]: - # default value is positive during motor burn - vert_comp_accel = vertical_linear_accel + GRAVITY - - # gets vertical velocity and finds updated altitude - vert_velocity = self._calculate_vertical_velocity(vert_comp_accel, est_time_step) - last_altitude = self._last_est_packet.estPressureAlt - new_altitude = last_altitude + vert_velocity * est_time_step - - # gets previous quaternion - last_quat = np.array( - [ - self._last_est_packet.estOrientQuaternionW, - self._last_est_packet.estOrientQuaternionX, - self._last_est_packet.estOrientQuaternionY, - self._last_est_packet.estOrientQuaternionZ, - ] - ) - - est_data_packet = EstimatedDataPacket( - next_timestamp * 1e9, - estOrientQuaternionW=last_quat[0], - estOrientQuaternionX=last_quat[1], - estOrientQuaternionY=last_quat[2], - estOrientQuaternionZ=last_quat[3], - estCompensatedAccelX=vert_comp_accel * orientation[0], - estCompensatedAccelY=vert_comp_accel * orientation[1], - estCompensatedAccelZ=vert_comp_accel * orientation[2], - estPressureAlt=new_altitude, + else: + packet = EstimatedDataPacket( + 0, + estOrientQuaternionW=initial_quaternion[0], + estOrientQuaternionX=initial_quaternion[1], + estOrientQuaternionY=initial_quaternion[2], + estOrientQuaternionZ=initial_quaternion[3], + estCompensatedAccelX=GRAVITY * orientation[0], + estCompensatedAccelY=GRAVITY * orientation[1], + estCompensatedAccelZ=GRAVITY * orientation[2], + estPressureAlt=0, estGravityVectorX=-GRAVITY * orientation[0], estGravityVectorY=-GRAVITY * orientation[1], estGravityVectorZ=-GRAVITY * orientation[2], @@ -257,16 +137,115 @@ def generate_data_packet(self) -> npt.NDArray: estAngularRateZ=0, ) - # appends to array - new_packets[1] = est_data_packet + return packet + + def generate_raw_data_packet(self) -> RawDataPacket: + """ + Generates a raw data packet containing randomly generated data points - # updates last packet - self._last_est_packet = est_data_packet + :return: RawDataPacket + """ + # creating shorthand variables from config + time_step = self._config["raw_time_step"] + orientation = self._config["rocket_orientation"] - # updates the timestamp - self._current_timestamp = next_timestamp + # If the simulation has just started, we want to just generate the initial raw packet + # and initialize "self._last_" variables + if self._last_raw_packet is None: + self._last_raw_packet = self._get_first_packet(RawDataPacket) + return self._last_raw_packet + + # calculates the timestamp for this packet (in seconds) + next_timestamp = (self._last_raw_packet.timestamp/1e9) + time_step + + # calculates the net force and vertical scaled acceleration + net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + vertical_scaled_accel = net_force / (self._config["rocket_mass"] * GRAVITY) + + # calculates vertical delta velocity + last_vertical_scaled_accel = self._get_vertical_data_point("scaledAccel") + vert_delta_v = (vertical_scaled_accel - last_vertical_scaled_accel) * GRAVITY * time_step + + packet = RawDataPacket( + next_timestamp * 1e9, + scaledAccelX=vertical_scaled_accel * orientation[0], + scaledAccelY=vertical_scaled_accel * orientation[1], + scaledAccelZ=vertical_scaled_accel * orientation[2], + scaledGyroX=0.0, + scaledGyroY=0.0, + scaledGyroZ=0.0, + deltaVelX=vert_delta_v * orientation[0], + deltaVelY=vert_delta_v * orientation[1], + deltaVelZ=vert_delta_v * orientation[2], + deltaThetaX=0.0, + deltaThetaY=0.0, + deltaThetaZ=0.0, + ) + + # updates last raw data packet + self._last_raw_packet = packet - return new_packets + return packet + + def generate_estimated_data_packet(self) -> EstimatedDataPacket: + """ + Generates an estimated data packet containing randomly generated data points + + :return: EstimatedDataPacket + """ + # creating shorthand variables from config + time_step = self._config["est_time_step"] + orientation = self._config["rocket_orientation"] + + # If the simulation has just started, we want to just generate the initial estimated packet + # and initialize "self._last_" variables + if self._last_est_packet is None: + self._last_est_packet = self._get_first_packet(EstimatedDataPacket) + return self._last_raw_packet + + # calculates the timestamp for this packet (in seconds) + next_timestamp = (self._last_est_packet.timestamp/1e9) + time_step + + # calculates the net force and vertical accelerations + net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + vertical_linear_accel = net_force / self._config["rocket_mass"] + vertical_comp_accel = vertical_linear_accel + GRAVITY + + # gets vertical velocity and finds updated altitude + vert_velocity = self._calculate_vertical_velocity(vertical_comp_accel, time_step) + new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step + + # gets previous quaternion + last_quat = np.array( + [ + self._last_est_packet.estOrientQuaternionW, + self._last_est_packet.estOrientQuaternionX, + self._last_est_packet.estOrientQuaternionY, + self._last_est_packet.estOrientQuaternionZ, + ]) + + packet = EstimatedDataPacket( + next_timestamp * 1e9, + estOrientQuaternionW=last_quat[0], + estOrientQuaternionX=last_quat[1], + estOrientQuaternionY=last_quat[2], + estOrientQuaternionZ=last_quat[3], + estCompensatedAccelX=vertical_comp_accel * orientation[0], + estCompensatedAccelY=vertical_comp_accel * orientation[1], + estCompensatedAccelZ=vertical_comp_accel * orientation[2], + estPressureAlt=new_altitude, + estGravityVectorX=-GRAVITY * orientation[0], + estGravityVectorY=-GRAVITY * orientation[1], + estGravityVectorZ=-GRAVITY * orientation[2], + estAngularRateX=0, + estAngularRateY=0, + estAngularRateZ=0, + ) + + # updates last estimated packet + self._last_est_packet = packet + + return packet def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: """ @@ -286,47 +265,53 @@ def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: return vertical_velocity - def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + def _calculate_net_force(self, timestamp, velocity) -> np.float64: """ - Updates the current timestamp of the data generator, based off time step defined in config. - Will also determine if the next timestamp will be a raw packet, estimated packet, or both. + Calculates the drag force, thrust force, and weight, and sums them. - :param current_timestamp: the current timestamp of the simulation + :param timestamp: the timestamp of the rocket to calcuate the net forces at + :param velocity: the vertical velocity at the given instant - :return: the updated current timestamp, rounded to 3 decimals + :return: float containing the net force. Thrust is positive, drag and weight is negative. """ - - # finding whether the raw or estimated data packets have a lower time_step - lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) - highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) - - # checks if current time is a multiple of the highest and/or lowest time step - at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) - at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) - - # If current timestamp is a multiple of both, the next timestamp will be the - # the current timestamp + the lower time steps - if all([at_low, at_high]): - return np.round(current_timestamp + lowest_dt, 3) - - # If timestamp is a multiple of just the lowest time step, the next will be - # either current + lowest, or the next timestamp that is divisible by the highest - if at_low and not at_high: - dt = min(lowest_dt, highest_dt - (current_timestamp % highest_dt)) - return np.round(current_timestamp + dt, 3) - - # If timestamp is a multiple of only the highest time step, the next will - # always be the next timestamp that is divisible by the lowest - if at_high and not at_low: - return np.round(current_timestamp + lowest_dt - (current_timestamp % lowest_dt), 3) - - # This would happen if the input current timestamp is not a multiple of the raw - # or estimated time steps, or if there is a rounding/floating point error. - raise ValueError("Could not update timestamp, time stamp is invalid") - - def _calculate_drag_force(self,velocity): - # we could probably actually calculate this, maybe we can set temp as a constant? - roe = 1.1 + # we could probably actually calculate air density, maybe we set temperature as constant? + air_density = 1.1 reference_area = self._config["reference_area"] drag_coefficient = self._config["drag_coefficient"] - return 0.5 * roe * reference_area * drag_coefficient * velocity**2 + rocket_mass = self._config["rocket_mass"] + + drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 + weight_force = GRAVITY * rocket_mass + thrust_force = 0.0 + + # thrust force is non-zero if the timestamp is within the timeframe of + # the motor burn + if timestamp <= self._thrust_data[0][-1]: + thrust_force = np.interp( + timestamp, self._thrust_data[0], self._thrust_data[1] + ) + return thrust_force - weight_force - drag_force + + def _get_vertical_data_point(self,string_identifier: str) -> np.float64: + """ + gets the last vertical data point specified from a vector data attribute by using IMU + orientation in config + + :param string_identfier: a string representing the exact attribute name of the data packet + + :return: float containing the specified data point + """ + # Dynamically retrieve x, y, z components based on the identifier + values = [ + getattr(self._last_raw_packet, f"{string_identifier}X", None), + getattr(self._last_raw_packet, f"{string_identifier}Y", None), + getattr(self._last_raw_packet, f"{string_identifier}Z", None), + ] + + # Just an edge case, but I really hope this never happens + if any(value is None for value in values): + raise AttributeError( + f"Could not find all components for identifier '{string_identifier}'." + ) + + return values[self._vertical_index] diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 17635f23..4f7a84be 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -3,8 +3,12 @@ import contextlib import multiprocessing import time +from pathlib import Path from typing import TYPE_CHECKING +import numpy as np +import yaml + if TYPE_CHECKING: from airbrakes.data_handling.imu_data_packet import IMUDataPacket @@ -20,10 +24,12 @@ class SimIMU(IMU): """ __slots__ = ( + "_config", "_data_fetch_process", "_data_generator", "_data_queue", "_running", + "_timestamp", ) def __init__(self): @@ -31,6 +37,12 @@ def __init__(self): Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. """ + self._timestamp: np.float64 = np.float64(0.0) + # loads the sim_config.yaml file + config_path = Path("simulator/sim_config.yaml") + with config_path.open(mode="r", newline="") as file: + self._config: dict = yaml.safe_load(file) + # This limits the queue size to a very high limit, because the data generator will # generate all of the data before the imu reads it self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue( @@ -48,21 +60,69 @@ def __init__(self): self._data_generator = DataGenerator() + def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + """ + Updates the current timestamp of the data generator, based off time step defined in config. + Will also determine if the next timestamp will be a raw packet, estimated packet, or both. + + :param current_timestamp: the current timestamp of the simulation + + :return: the updated current timestamp, rounded to 3 decimals + """ + + # finding whether the raw or estimated data packets have a lower time_step + lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) + highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) + + # checks if current time is a multiple of the highest and/or lowest time step + at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) + at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) + + # If current timestamp is a multiple of both, the next timestamp will be the + # the current timestamp + the lower time steps + if all([at_low, at_high]): + return np.round(current_timestamp + lowest_dt, 3) + + # If timestamp is a multiple of just the lowest time step, the next will be + # either current + lowest, or the next timestamp that is divisible by the highest + if at_low and not at_high: + dt = min(lowest_dt, highest_dt - (current_timestamp % highest_dt)) + return np.round(current_timestamp + dt, 3) + + # If timestamp is a multiple of only the highest time step, the next will + # always be the next timestamp that is divisible by the lowest + if at_high and not at_low: + return np.round(current_timestamp + lowest_dt - (current_timestamp % lowest_dt), 3) + + # This would happen if the input current timestamp is not a multiple of the raw + # or estimated time steps, or if there is a rounding/floating point error. + raise ValueError("Could not update timestamp, time stamp is invalid") + def _fetch_data_loop(self) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" + + raw_dt = self._config["raw_time_step"] + est_dt = self._config["est_time_step"] + # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - for _ in range(21000): - packets = self._data_generator.generate_data_packet() - timestamp = 0 - if packets[0] is not None: - self._data_queue.put(packets[0]) - timestamp = packets[0].timestamp / 1e9 - if packets[1] is not None: - self._data_queue.put(packets[1]) - timestamp = packets[1].timestamp / 1e9 - - dt = self._data_generator.update_timestamp(timestamp) - timestamp - time.sleep(dt) + while self._data_generator._last_velocity > -1: + # starts timer + start_time = time.time() + + # if the timestamp is a multiple of the raw time step, generate a raw data packet. + if any(np.isclose(self._timestamp % raw_dt, [0, raw_dt])): + self._data_queue.put(self._data_generator.generate_raw_data_packet()) + + # if the timestamp is a multiple of the est time step, generate an est data packet. + if any(np.isclose(self._timestamp % est_dt, [0, est_dt])): + self._data_queue.put(self._data_generator.generate_estimated_data_packet()) + + # updates the timestamp and sleeps until next packet is ready in real-time + time_step = self.update_timestamp(self._timestamp) - self._timestamp + self._timestamp += time_step + end_time = time.time() + time.sleep(max(0.0, time_step - (end_time - start_time))) + diff --git a/simulator/test.py b/simulator/test.py new file mode 100644 index 00000000..1e648e4a --- /dev/null +++ b/simulator/test.py @@ -0,0 +1,5 @@ +import numpy as np + +values = [1,2,3] +if any(value is None for value in values): + raise AttributeError('test') From bb4d5586399ff60fe3dd22df8ad685075bc9deaa Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Thu, 7 Nov 2024 14:52:35 -0500 Subject: [PATCH 34/63] starting rotation --- pyproject.toml | 2 +- simulator/data_gen.py | 42 +++++++++++++++++++++++++++++---- simulator/rotation_manager.py | 33 ++++++++++++++++++++++++++ simulator/sim_config.yaml | 10 +++++++- simulator/test.py | 44 ++++++++++++++++++++++++++++++++--- 5 files changed, 122 insertions(+), 9 deletions(-) create mode 100644 simulator/rotation_manager.py diff --git a/pyproject.toml b/pyproject.toml index 30d1fea7..fdda2ac9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -37,7 +37,7 @@ exclude = ["scripts"] [tool.ruff.lint] preview = true explicit-preview-rules = true # TODO: Drop this when RUF022 and RUF023 are out of preview -ignore = ["PLR2004", "PLR0911", "PLR0912", "PLR0913", "PLR0915", "PERF203", "ISC001"] +ignore = ["PLR2004", "PLR0911", "PLR0912", "PLR0913", "PLR0915", "PERF203", "ISC001", "S311"] select = ["E", "F", "I", "PL", "UP", "RUF", "PTH", "C4", "B", "PIE", "SIM", "RET", "RSE", "G", "ISC", "PT", "ASYNC", "TCH", "SLOT", "PERF", "PYI", "FLY", "AIR", "RUF022", "RUF023", "Q", "INP", "W", "YTT", "DTZ", "ARG", "T20", "FURB", "DOC", diff --git a/simulator/data_gen.py b/simulator/data_gen.py index 13afb94b..f4011b56 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -1,6 +1,7 @@ """Module that creates randomly generated data to sent to the simulator IMU""" import csv +import random from pathlib import Path import numpy as np @@ -10,10 +11,10 @@ from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, - IMUDataPacket, RawDataPacket, ) from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY +from simulator.rotation_manager import RotationManager from utils import deadband @@ -43,10 +44,43 @@ def __init__(self): with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) + # loads thrust curve + self._thrust_data: npt.NDArray = self._load_thrust_curve() + + # initializes the rotation manager with the launch pad conditions + self._rotation_manager = self._get_rotation_manager() # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] - self._thrust_data: npt.NDArray = self._load_thrust_curve() + + def _get_rotation_manager(self) -> RotationManager: + # gets angle of attack and wind direction for the rocket + rod_angle_of_attack = self._get_random("launch_rod_angle") + wind_direction = self._get_random("wind_direction") + # points the rocket into the wind, with some amount of error + rod_direction = self._get_random("rod_direction_error") + (wind_direction-180) + + # initializes the rotation manager + return RotationManager() + + def _get_random(self,identifier: str) -> np.float64: + """ + Gets a random value for the selected identifier, using the standard deviation if given. + + :param identifier: string that matches a config variable in sim_config.yaml + + :return: float containing a random value for the selected identfier, between + the bounds specified in the config + """ + parameters = self._config[identifier] + if len(parameters) == 3: + # uses standard deviation to get random number + mean = np.mean([parameters[0],parameters[1]]) + val = random.gauss(mean,parameters[2]) + # restricts the value to the bounds + return np.max(parameters[0], np.min(parameters[1], val)) + # if no standard deviation is given, just return a uniform distribution + return random.uniform(parameters[0],parameters[1]) def _load_thrust_curve(self) -> npt.NDArray: """ @@ -98,8 +132,6 @@ def _get_first_packet( :return: data packet of the specified type with launch pad conditions. """ orientation = self._config["rocket_orientation"] - initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) - initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) packet = None if packet_type == RawDataPacket: @@ -119,6 +151,8 @@ def _get_first_packet( deltaThetaZ=0.0, ) else: + initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) + initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) packet = EstimatedDataPacket( 0, estOrientQuaternionW=initial_quaternion[0], diff --git a/simulator/rotation_manager.py b/simulator/rotation_manager.py new file mode 100644 index 00000000..ce95b98c --- /dev/null +++ b/simulator/rotation_manager.py @@ -0,0 +1,33 @@ +"""Module that contains methods for rotation operations""" + +import numpy as np +import numpy.typing as npt + + +class RotationManager: + """ + Manages rotation calculations for rocket orientation. This class provides + methods to compute and apply various rotation operations, for the purpose + of generating accurate 6DOF data points for the simulation IMU + """ + + __slots__ = ( + "_vertical", + ) + + def __init__( + self, + orientation: npt.NDArray, + rod_angle_of_attack: np.float64, + + ) -> None: + """ + Initializes a RotationManager instance with a specified orientation. This orientation + serves as the reference direction for rotation operations. + + :param orientation: the vertical orientation of the rocket + """ + self._vertical = orientation + + + diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index 89dad200..a241ba2b 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -13,4 +13,12 @@ reference_area: 0.01929 # reference area (in meters squared) # vertical orientation of the rocket on the launch pad. only one value should be non-zero. # Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) -rocket_orientation: [0, 0, -1] \ No newline at end of file +rocket_orientation: [0, 0, -1] + +#--------------------- +# CONFIG FOR RANDOMNESS +#--------------------- +wind_direction: [0,360] # min and max for wind direction (in degrees) +wind_speed: [0.5,8.5] # min and max for wind speed (in meters per second) +launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) +rod_direction_error: [-10,10] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file diff --git a/simulator/test.py b/simulator/test.py index 1e648e4a..7f9784ca 100644 --- a/simulator/test.py +++ b/simulator/test.py @@ -1,5 +1,43 @@ +import timeit +import yaml +from pathlib import Path +import random import numpy as np -values = [1,2,3] -if any(value is None for value in values): - raise AttributeError('test') +config_path = Path("simulator/sim_config.yaml") +with config_path.open(mode="r", newline="") as file: + config: dict = yaml.safe_load(file) +rng = np.random.default_rng() +# Define the two functions to test +def function1(): + a = rng.random() + +def function2(): + a = random.random() + +# Number of executions for each function (increase for more precise timing) +num_executions = 10 + +# Time the execution of function1 +time_function1 = timeit.timeit("function1()", globals=globals(), number=num_executions) + +# Time the execution of function2 +time_function2 = timeit.timeit("function2()", globals=globals(), number=num_executions) + +# Print the results +print(f"Time taken by function1: {time_function1:.6f} seconds") +print(f"Time taken by function2: {time_function2:.6f} seconds") + +# Compare which function is faster +if time_function1 < time_function2: + print("function1 is faster") +elif time_function1 > time_function2: + print("function2 is faster") +else: + print("Both functions have the same execution time") + + + + + + From 4b7923af1d8fb2819d5cbcce308c3e71c7583e17 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Thu, 7 Nov 2024 18:11:46 -0500 Subject: [PATCH 35/63] progress --- simulator/data_gen.py | 17 ++++++----------- simulator/rotation_manager.py | 18 ++++++++++++++++-- simulator/sim_config.yaml | 4 +--- simulator/sim_imu.py | 2 +- 4 files changed, 24 insertions(+), 17 deletions(-) diff --git a/simulator/data_gen.py b/simulator/data_gen.py index f4011b56..c0fd1faa 100644 --- a/simulator/data_gen.py +++ b/simulator/data_gen.py @@ -29,6 +29,7 @@ class DataGenerator: "_last_est_packet", "_last_raw_packet", "_last_velocity", + "_rotation_manager", "_thrust_data", "_vertical_index", ) @@ -48,21 +49,15 @@ def __init__(self): self._thrust_data: npt.NDArray = self._load_thrust_curve() # initializes the rotation manager with the launch pad conditions - self._rotation_manager = self._get_rotation_manager() + self._rotation_manager = RotationManager( + self._config["orientation"], + self._get_random("launch_rod_angle"), + self._get_random("launch_rod_direction"), + ) # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] - def _get_rotation_manager(self) -> RotationManager: - # gets angle of attack and wind direction for the rocket - rod_angle_of_attack = self._get_random("launch_rod_angle") - wind_direction = self._get_random("wind_direction") - # points the rocket into the wind, with some amount of error - rod_direction = self._get_random("rod_direction_error") + (wind_direction-180) - - # initializes the rotation manager - return RotationManager() - def _get_random(self,identifier: str) -> np.float64: """ Gets a random value for the selected identifier, using the standard deviation if given. diff --git a/simulator/rotation_manager.py b/simulator/rotation_manager.py index ce95b98c..f6ea0d4d 100644 --- a/simulator/rotation_manager.py +++ b/simulator/rotation_manager.py @@ -12,14 +12,16 @@ class RotationManager: """ __slots__ = ( + "_azimuth", "_vertical", + "_zenith", ) def __init__( self, orientation: npt.NDArray, rod_angle_of_attack: np.float64, - + rod_direction: np.float64 ) -> None: """ Initializes a RotationManager instance with a specified orientation. This orientation @@ -27,7 +29,19 @@ def __init__( :param orientation: the vertical orientation of the rocket """ - self._vertical = orientation + self._vertical: npt.NDArray = orientation + self._zenith: np.float64 = (rod_angle_of_attack * np.pi)/180 + self._azimuth: np.float64 = (rod_direction * np.pi)/180 + + def update_orientation(self,time_step: np.float64) -> None: + """ + Updates the baseline vehicle-frame orientation + + :param time_step: how much time has passed between updates + :param apogee_progress: current altitude divided by the target apogee + """ + + diff --git a/simulator/sim_config.yaml b/simulator/sim_config.yaml index a241ba2b..0dda9c29 100644 --- a/simulator/sim_config.yaml +++ b/simulator/sim_config.yaml @@ -18,7 +18,5 @@ rocket_orientation: [0, 0, -1] #--------------------- # CONFIG FOR RANDOMNESS #--------------------- -wind_direction: [0,360] # min and max for wind direction (in degrees) -wind_speed: [0.5,8.5] # min and max for wind speed (in meters per second) launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) -rod_direction_error: [-10,10] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file +rod_direction_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file diff --git a/simulator/sim_imu.py b/simulator/sim_imu.py index 4f7a84be..44be69da 100644 --- a/simulator/sim_imu.py +++ b/simulator/sim_imu.py @@ -32,7 +32,7 @@ class SimIMU(IMU): "_timestamp", ) - def __init__(self): + def __init__(self) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. From cbabad7ddf64ffe557fa2bd23d390fbb1768da14 Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 7 Nov 2024 22:49:45 -0500 Subject: [PATCH 36/63] Clean up --- airbrakes/data_handling/data_processor.py | 6 +- .../simulation}/__init__.py | 0 .../simulation/data_generator.py | 72 +++++++++---------- .../simulation}/rotation_manager.py | 18 ++--- .../simulation}/sim_config.yaml | 8 +-- .../simulation}/sim_imu.py | 13 ++-- {simulator => airbrakes/simulation}/test.py | 31 ++++---- .../thrust_curves/AeroTech_L1940X.csv | 0 main.py | 4 +- pyproject.toml | 1 + utils.py | 2 +- 11 files changed, 67 insertions(+), 88 deletions(-) rename {simulator => airbrakes/simulation}/__init__.py (100%) rename simulator/data_gen.py => airbrakes/simulation/data_generator.py (88%) rename {simulator => airbrakes/simulation}/rotation_manager.py (78%) rename {simulator => airbrakes/simulation}/sim_config.yaml (87%) rename {simulator => airbrakes/simulation}/sim_imu.py (94%) rename {simulator => airbrakes/simulation}/test.py (62%) rename {simulator => airbrakes/simulation}/thrust_curves/AeroTech_L1940X.csv (100%) diff --git a/airbrakes/data_handling/data_processor.py b/airbrakes/data_handling/data_processor.py index f4a96311..89e2fe0f 100644 --- a/airbrakes/data_handling/data_processor.py +++ b/airbrakes/data_handling/data_processor.py @@ -204,7 +204,6 @@ def _calculate_rotated_accelerations(self) -> npt.NDArray[np.float64]: Calculates the rotated acceleration vector. Converts gyroscope data into a delta quaternion, and adds onto the last quaternion. Will most likely be replaced by IMU quaternion data in the future, this is a work-around due to bad datasets. - :return: numpy list of rotated acceleration vector [x,y,z] """ @@ -229,7 +228,8 @@ def _calculate_rotated_accelerations(self) -> npt.NDArray[np.float64]: if any(val is None for val in [x_accel, y_accel, z_accel, gyro_x, gyro_y, gyro_z]): return rotated_accelerations - # scipy docs for more info: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html + # scipy docs for more info: + # https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html # Calculate the delta quaternion from the angular rates delta_rotation = R.from_rotvec([gyro_x * dt, gyro_y * dt, gyro_z * dt]) @@ -282,7 +282,7 @@ def _calculate_time_differences(self) -> npt.NDArray[np.float64]: This cannot be called on the first update as _last_data_packet is None. Units are in seconds. :return: A numpy array of the time difference between each data packet and the previous - data packet. + data packet. """ # calculate the time differences between each data packet # We are converting from ns to s, since we don't want to have a velocity in m/ns^2 diff --git a/simulator/__init__.py b/airbrakes/simulation/__init__.py similarity index 100% rename from simulator/__init__.py rename to airbrakes/simulation/__init__.py diff --git a/simulator/data_gen.py b/airbrakes/simulation/data_generator.py similarity index 88% rename from simulator/data_gen.py rename to airbrakes/simulation/data_generator.py index c0fd1faa..4346997e 100644 --- a/simulator/data_gen.py +++ b/airbrakes/simulation/data_generator.py @@ -1,4 +1,4 @@ -"""Module that creates randomly generated data to sent to the simulator IMU""" +"""Module that creates randomly generated data to sent to the simulation IMU""" import csv import random @@ -13,8 +13,8 @@ EstimatedDataPacket, RawDataPacket, ) +from airbrakes.simulation.rotation_manager import RotationManager from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY -from simulator.rotation_manager import RotationManager from utils import deadband @@ -41,7 +41,7 @@ def __init__(self): self._last_velocity: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulator/sim_config.yaml") + config_path = Path("simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) @@ -58,41 +58,39 @@ def __init__(self): # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] - def _get_random(self,identifier: str) -> np.float64: + @property + def velocity(self) -> np.float64: + """Returns the last calculated velocity of the rocket""" + return self._last_velocity + + def _get_random(self, identifier: str) -> np.float64: """ Gets a random value for the selected identifier, using the standard deviation if given. - :param identifier: string that matches a config variable in sim_config.yaml - - :return: float containing a random value for the selected identfier, between - the bounds specified in the config + :return: float containing a random value for the selected identifier, between + the bounds specified in the config """ parameters = self._config[identifier] if len(parameters) == 3: # uses standard deviation to get random number - mean = np.mean([parameters[0],parameters[1]]) - val = random.gauss(mean,parameters[2]) + mean = float(np.mean([parameters[0], parameters[1]])) + val = random.gauss(mean, parameters[2]) # restricts the value to the bounds return np.max(parameters[0], np.min(parameters[1], val)) # if no standard deviation is given, just return a uniform distribution - return random.uniform(parameters[0],parameters[1]) + return random.uniform(parameters[0], parameters[1]) def _load_thrust_curve(self) -> npt.NDArray: """ Loads the thrust curve from the motor specified in the configs. - :return: numpy array containing tuples with the time and thrust at that time. """ # gets the path of the csv based on the config file - csv_path = Path(f"simulator/thrust_curves/{self._config["motor"]}.csv") + csv_path = Path(f"simulation/thrust_curves/{self._config["motor"]}.csv") # initializes the list for timestamps and thrust values - motor_timestamps = [ - 0, - ] - motor_thrusts = [ - 0, - ] + motor_timestamps = [0] + motor_thrusts = [0] start_flag = False # flag to identify when the metadata/header rows are skipped @@ -115,8 +113,8 @@ def _load_thrust_curve(self) -> npt.NDArray: return np.array([motor_timestamps, motor_thrusts]) def _get_first_packet( - self,packet_type: RawDataPacket | EstimatedDataPacket - ) -> RawDataPacket | EstimatedDataPacket: + self, packet_type: RawDataPacket | EstimatedDataPacket + ) -> RawDataPacket | EstimatedDataPacket: """ Sets up the initial values for the estimated and raw data packets. This should only be called once, and all values will be approximate launch pad conditions. @@ -130,7 +128,7 @@ def _get_first_packet( packet = None if packet_type == RawDataPacket: - packet = RawDataPacket( + packet = RawDataPacket( 0, scaledAccelX=orientation[0], scaledAccelY=orientation[1], @@ -148,7 +146,7 @@ def _get_first_packet( else: initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) - packet = EstimatedDataPacket( + packet = EstimatedDataPacket( 0, estOrientQuaternionW=initial_quaternion[0], estOrientQuaternionX=initial_quaternion[1], @@ -185,17 +183,17 @@ def generate_raw_data_packet(self) -> RawDataPacket: return self._last_raw_packet # calculates the timestamp for this packet (in seconds) - next_timestamp = (self._last_raw_packet.timestamp/1e9) + time_step + next_timestamp = (self._last_raw_packet.timestamp / 1e9) + time_step # calculates the net force and vertical scaled acceleration - net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + net_force = self._calculate_net_force(next_timestamp, self._last_velocity) vertical_scaled_accel = net_force / (self._config["rocket_mass"] * GRAVITY) # calculates vertical delta velocity last_vertical_scaled_accel = self._get_vertical_data_point("scaledAccel") vert_delta_v = (vertical_scaled_accel - last_vertical_scaled_accel) * GRAVITY * time_step - packet = RawDataPacket( + packet = RawDataPacket( next_timestamp * 1e9, scaledAccelX=vertical_scaled_accel * orientation[0], scaledAccelY=vertical_scaled_accel * orientation[1], @@ -233,10 +231,10 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: return self._last_raw_packet # calculates the timestamp for this packet (in seconds) - next_timestamp = (self._last_est_packet.timestamp/1e9) + time_step + next_timestamp = (self._last_est_packet.timestamp / 1e9) + time_step # calculates the net force and vertical accelerations - net_force = self._calculate_net_force(next_timestamp,self._last_velocity) + net_force = self._calculate_net_force(next_timestamp, self._last_velocity) vertical_linear_accel = net_force / self._config["rocket_mass"] vertical_comp_accel = vertical_linear_accel + GRAVITY @@ -251,7 +249,8 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: self._last_est_packet.estOrientQuaternionX, self._last_est_packet.estOrientQuaternionY, self._last_est_packet.estOrientQuaternionZ, - ]) + ] + ) packet = EstimatedDataPacket( next_timestamp * 1e9, @@ -280,7 +279,6 @@ def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: """ Calculates the velocity of the rocket based on the compensated acceleration. Integrates that acceleration to get the velocity. - :return: the velocity of the rocket """ # deadbands the acceleration @@ -309,25 +307,21 @@ def _calculate_net_force(self, timestamp, velocity) -> np.float64: drag_coefficient = self._config["drag_coefficient"] rocket_mass = self._config["rocket_mass"] - drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 + drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 weight_force = GRAVITY * rocket_mass thrust_force = 0.0 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn if timestamp <= self._thrust_data[0][-1]: - thrust_force = np.interp( - timestamp, self._thrust_data[0], self._thrust_data[1] - ) + thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) return thrust_force - weight_force - drag_force - def _get_vertical_data_point(self,string_identifier: str) -> np.float64: + def _get_vertical_data_point(self, string_identifier: str) -> np.float64: """ gets the last vertical data point specified from a vector data attribute by using IMU orientation in config - - :param string_identfier: a string representing the exact attribute name of the data packet - + :param string_identifier: a string representing the exact attribute name of the data packet :return: float containing the specified data point """ # Dynamically retrieve x, y, z components based on the identifier @@ -341,6 +335,6 @@ def _get_vertical_data_point(self,string_identifier: str) -> np.float64: if any(value is None for value in values): raise AttributeError( f"Could not find all components for identifier '{string_identifier}'." - ) + ) return values[self._vertical_index] diff --git a/simulator/rotation_manager.py b/airbrakes/simulation/rotation_manager.py similarity index 78% rename from simulator/rotation_manager.py rename to airbrakes/simulation/rotation_manager.py index f6ea0d4d..7763dd7c 100644 --- a/simulator/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -18,11 +18,8 @@ class RotationManager: ) def __init__( - self, - orientation: npt.NDArray, - rod_angle_of_attack: np.float64, - rod_direction: np.float64 - ) -> None: + self, orientation: npt.NDArray, rod_angle_of_attack: np.float64, rod_direction: np.float64 + ) -> None: """ Initializes a RotationManager instance with a specified orientation. This orientation serves as the reference direction for rotation operations. @@ -30,18 +27,13 @@ def __init__( :param orientation: the vertical orientation of the rocket """ self._vertical: npt.NDArray = orientation - self._zenith: np.float64 = (rod_angle_of_attack * np.pi)/180 - self._azimuth: np.float64 = (rod_direction * np.pi)/180 + self._zenith: np.float64 = (rod_angle_of_attack * np.pi) / 180 + self._azimuth: np.float64 = (rod_direction * np.pi) / 180 - def update_orientation(self,time_step: np.float64) -> None: + def update_orientation(self, time_step: np.float64) -> None: """ Updates the baseline vehicle-frame orientation :param time_step: how much time has passed between updates :param apogee_progress: current altitude divided by the target apogee """ - - - - - diff --git a/simulator/sim_config.yaml b/airbrakes/simulation/sim_config.yaml similarity index 87% rename from simulator/sim_config.yaml rename to airbrakes/simulation/sim_config.yaml index 0dda9c29..99013ee3 100644 --- a/simulator/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -1,10 +1,10 @@ -# config file for simulator +# config file for simulation sim_apogee: 1800 # Simulator will generate data with the apogee around this value (in meters) -raw_time_step: 0.001 # Time step between raw data packets the simulator will generate (in seconds) -est_time_step: 0.002 # Time step between estimated data packets the simulator will generate (in seconds) +raw_time_step: 0.001 # Time step between raw data packets the simulation will generate (in seconds) +est_time_step: 0.002 # Time step between estimated data packets the simulation will generate (in seconds) -# selects the motor to use, this is the name of the csv file located in /simulator/thrust_curves +# selects the motor to use, this is the name of the csv file located in /simulation/thrust_curves # with the ".csv" removed motor: "AeroTech_L1940X" drag_coefficient: 0.4 # coefficient of drag for the rocket diff --git a/simulator/sim_imu.py b/airbrakes/simulation/sim_imu.py similarity index 94% rename from simulator/sim_imu.py rename to airbrakes/simulation/sim_imu.py index 44be69da..016e00e5 100644 --- a/simulator/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -13,8 +13,8 @@ from airbrakes.data_handling.imu_data_packet import IMUDataPacket from airbrakes.hardware.imu import IMU +from airbrakes.simulation.data_generator import DataGenerator from constants import MAX_QUEUE_SIZE -from simulator.data_gen import DataGenerator class SimIMU(IMU): @@ -39,12 +39,12 @@ def __init__(self) -> None: """ self._timestamp: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulator/sim_config.yaml") + config_path = Path("simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) # This limits the queue size to a very high limit, because the data generator will - # generate all of the data before the imu reads it + # generate all the data before the imu reads it self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue( MAX_QUEUE_SIZE ) @@ -64,9 +64,7 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: """ Updates the current timestamp of the data generator, based off time step defined in config. Will also determine if the next timestamp will be a raw packet, estimated packet, or both. - :param current_timestamp: the current timestamp of the simulation - :return: the updated current timestamp, rounded to 3 decimals """ @@ -79,7 +77,7 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: at_high = any(np.isclose(current_timestamp % highest_dt, [0, highest_dt])) # If current timestamp is a multiple of both, the next timestamp will be the - # the current timestamp + the lower time steps + # current timestamp + the lower time steps if all([at_low, at_high]): return np.round(current_timestamp + lowest_dt, 3) @@ -108,7 +106,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - while self._data_generator._last_velocity > -1: + while self._data_generator.velocity > -1: # starts timer start_time = time.time() @@ -125,4 +123,3 @@ def _fetch_data_loop(self) -> None: self._timestamp += time_step end_time = time.time() time.sleep(max(0.0, time_step - (end_time - start_time))) - diff --git a/simulator/test.py b/airbrakes/simulation/test.py similarity index 62% rename from simulator/test.py rename to airbrakes/simulation/test.py index 7f9784ca..129f7ffa 100644 --- a/simulator/test.py +++ b/airbrakes/simulation/test.py @@ -1,19 +1,24 @@ +import random import timeit -import yaml from pathlib import Path -import random + import numpy as np +import yaml -config_path = Path("simulator/sim_config.yaml") +config_path = Path("simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: config: dict = yaml.safe_load(file) rng = np.random.default_rng() + + # Define the two functions to test def function1(): - a = rng.random() + rng.random() + def function2(): - a = random.random() + random.random() + # Number of executions for each function (increase for more precise timing) num_executions = 10 @@ -25,19 +30,9 @@ def function2(): time_function2 = timeit.timeit("function2()", globals=globals(), number=num_executions) # Print the results -print(f"Time taken by function1: {time_function1:.6f} seconds") -print(f"Time taken by function2: {time_function2:.6f} seconds") # Compare which function is faster -if time_function1 < time_function2: - print("function1 is faster") -elif time_function1 > time_function2: - print("function2 is faster") +if time_function1 < time_function2 or time_function1 > time_function2: + pass else: - print("Both functions have the same execution time") - - - - - - + pass diff --git a/simulator/thrust_curves/AeroTech_L1940X.csv b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv similarity index 100% rename from simulator/thrust_curves/AeroTech_L1940X.csv rename to airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv diff --git a/main.py b/main.py index e37e6ad6..3ccb992b 100644 --- a/main.py +++ b/main.py @@ -15,13 +15,13 @@ from airbrakes.mock.display import FlightDisplay from airbrakes.mock.mock_imu import MockIMU from airbrakes.mock.mock_logger import MockLogger +from airbrakes.simulation.sim_imu import SimIMU from constants import ( FREQUENCY, LOGS_PATH, PORT, SERVO_PIN, ) -from simulator.sim_imu import SimIMU from utils import arg_parser @@ -52,7 +52,7 @@ def main(args: argparse.Namespace) -> None: real_time_simulation=not args.fast_simulation, start_after_log_buffer=True, ) - # If we are running the simulator for generating datasets, we will replace our IMU object + # If we are running the simulation for generating datasets, we will replace our IMU object # with a sim variant, similar to running a mock simulation. else: imu = SimIMU() diff --git a/pyproject.toml b/pyproject.toml index fdda2ac9..16b0f187 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,6 +11,7 @@ dependencies = [ "colorama", "psutil", "scipy", + "PyYAML" # Installation instructions for the following dependencies can be found in the README: # "mscl" https://github.com/LORD-MicroStrain/MSCL/blob/master/BuildScripts/buildReadme_Linux.md ] diff --git a/utils.py b/utils.py index 414c5585..c262a70b 100644 --- a/utils.py +++ b/utils.py @@ -102,7 +102,7 @@ def arg_parser() -> argparse.Namespace: parser.add_argument( "-s", "--sim", - help="runs the data simulator alongside the mock simulator, to randomly generate a dataset", + help="runs the data simulation alongside the mock simulation, to randomly generate a dataset", action="store_true", default=False, ) From 6e08a5e03561f2b394c6c70886a1659450a89f46 Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 7 Nov 2024 22:57:13 -0500 Subject: [PATCH 37/63] Clean up --- airbrakes/mock/mock_imu.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/airbrakes/mock/mock_imu.py b/airbrakes/mock/mock_imu.py index 7ad1cb32..b59a7282 100644 --- a/airbrakes/mock/mock_imu.py +++ b/airbrakes/mock/mock_imu.py @@ -22,8 +22,6 @@ class MockIMU(IMU): and returns data read from a previous log file. """ - __slots__ = ("_log_file_path",) - def __init__( self, log_file_path: Path | bool, @@ -32,19 +30,17 @@ def __init__( ): """ Initializes the object that pretends to be an IMU for testing purposes by reading from a - log file. + log file. :param log_file_path: The path of the log file to read data from. :param real_time_simulation: Whether to simulate a real flight by sleeping for a set - period, or run at full - speed, e.g. for using it in the CI. + period, or run at full speed, e.g. for using it in the CI. :param start_after_log_buffer: Whether to send the data packets only after the log buffer - was filled for Standby state. + was filled for Standby state. """ - self._log_file_path = log_file_path # Check if the launch data file exists: if not log_file_path: # Just use the first file in the `launch_data` directory: - self._log_file_path = next(iter(Path("launch_data").glob("*.csv"))) + log_file_path = next(iter(Path("launch_data").glob("*.csv"))) # We don't call the parent constructor as the IMU class has different parameters, so we # manually start the process that fetches data from the log file @@ -61,7 +57,7 @@ def __init__( self._data_fetch_process = multiprocessing.Process( target=self._fetch_data_loop, args=( - self._log_file_path, + log_file_path, real_time_simulation, start_after_log_buffer, ), From d6a427ece2e92e1e42a0c03dc7bd643393e67d30 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Fri, 8 Nov 2024 00:47:11 -0500 Subject: [PATCH 38/63] rotation wip --- airbrakes/simulation/data_generator.py | 6 ++-- airbrakes/simulation/rotation_manager.py | 24 ++++++++++----- airbrakes/simulation/sim_config.yaml | 3 +- airbrakes/simulation/sim_imu.py | 2 +- airbrakes/simulation/test.py | 38 ------------------------ utils.py | 3 +- 6 files changed, 24 insertions(+), 52 deletions(-) delete mode 100644 airbrakes/simulation/test.py diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 4346997e..e3fe8a39 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -41,7 +41,7 @@ def __init__(self): self._last_velocity: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulation/sim_config.yaml") + config_path = Path("airbrakes/simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) @@ -50,7 +50,7 @@ def __init__(self): # initializes the rotation manager with the launch pad conditions self._rotation_manager = RotationManager( - self._config["orientation"], + self._config["rocket_orientation"], self._get_random("launch_rod_angle"), self._get_random("launch_rod_direction"), ) @@ -86,7 +86,7 @@ def _load_thrust_curve(self) -> npt.NDArray: :return: numpy array containing tuples with the time and thrust at that time. """ # gets the path of the csv based on the config file - csv_path = Path(f"simulation/thrust_curves/{self._config["motor"]}.csv") + csv_path = Path(f"airbrakes/simulation/thrust_curves/{self._config["motor"]}.csv") # initializes the list for timestamps and thrust values motor_timestamps = [0] diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 7763dd7c..e1cdb73c 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -1,5 +1,7 @@ """Module that contains methods for rotation operations""" +from collections import deque + import numpy as np import numpy.typing as npt @@ -12,9 +14,9 @@ class RotationManager: """ __slots__ = ( - "_azimuth", + "_azimuths", "_vertical", - "_zenith", + "_zeniths", ) def __init__( @@ -27,13 +29,21 @@ def __init__( :param orientation: the vertical orientation of the rocket """ self._vertical: npt.NDArray = orientation - self._zenith: np.float64 = (rod_angle_of_attack * np.pi) / 180 - self._azimuth: np.float64 = (rod_direction * np.pi) / 180 + self._zeniths: deque = deque[(rod_angle_of_attack * np.pi) / 180] + self._azimuths: deque = deque[(rod_direction * np.pi) / 180] - def update_orientation(self, time_step: np.float64) -> None: + def update_orientation(self, time_step: np.float64, vertical_velocity: np.float64) -> None: """ - Updates the baseline vehicle-frame orientation + Updates the baseline vehicle-frame orientation, based on how close the velocity + is to zero. This accounts for the velocity being zero at the start of launch :param time_step: how much time has passed between updates - :param apogee_progress: current altitude divided by the target apogee + :param vertical_velocity: current vertical velocity of the rocket """ + # we dont want to change angle if in motor burn phase, for simplicity + if self._zeniths[-1] is not self._zeniths[0] or vertical_velocity >= 100: + self._zeniths.append( + (np.pi / 2 - self._zeniths[0]) * np.exp(-0.035 * vertical_velocity) + + self._zeniths[0] + ) + _ = time_step diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml index 99013ee3..00a50f82 100644 --- a/airbrakes/simulation/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -1,6 +1,5 @@ # config file for simulation -sim_apogee: 1800 # Simulator will generate data with the apogee around this value (in meters) raw_time_step: 0.001 # Time step between raw data packets the simulation will generate (in seconds) est_time_step: 0.002 # Time step between estimated data packets the simulation will generate (in seconds) @@ -19,4 +18,4 @@ rocket_orientation: [0, 0, -1] # CONFIG FOR RANDOMNESS #--------------------- launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) -rod_direction_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file +launch_rod_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 016e00e5..a4e69796 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -39,7 +39,7 @@ def __init__(self) -> None: """ self._timestamp: np.float64 = np.float64(0.0) # loads the sim_config.yaml file - config_path = Path("simulation/sim_config.yaml") + config_path = Path("airbrakes/simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: self._config: dict = yaml.safe_load(file) diff --git a/airbrakes/simulation/test.py b/airbrakes/simulation/test.py deleted file mode 100644 index 129f7ffa..00000000 --- a/airbrakes/simulation/test.py +++ /dev/null @@ -1,38 +0,0 @@ -import random -import timeit -from pathlib import Path - -import numpy as np -import yaml - -config_path = Path("simulation/sim_config.yaml") -with config_path.open(mode="r", newline="") as file: - config: dict = yaml.safe_load(file) -rng = np.random.default_rng() - - -# Define the two functions to test -def function1(): - rng.random() - - -def function2(): - random.random() - - -# Number of executions for each function (increase for more precise timing) -num_executions = 10 - -# Time the execution of function1 -time_function1 = timeit.timeit("function1()", globals=globals(), number=num_executions) - -# Time the execution of function2 -time_function2 = timeit.timeit("function2()", globals=globals(), number=num_executions) - -# Print the results - -# Compare which function is faster -if time_function1 < time_function2 or time_function1 > time_function2: - pass -else: - pass diff --git a/utils.py b/utils.py index c262a70b..ab03d666 100644 --- a/utils.py +++ b/utils.py @@ -102,7 +102,8 @@ def arg_parser() -> argparse.Namespace: parser.add_argument( "-s", "--sim", - help="runs the data simulation alongside the mock simulation, to randomly generate a dataset", + help="runs the data simulation alongside the mock simulation, to randomly generate " + "a dataset", action="store_true", default=False, ) From 651c46ab512a0659f72749cbfd180fa1e98e13ac Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Fri, 8 Nov 2024 21:24:14 -0500 Subject: [PATCH 39/63] rotations almost done, some bugs --- airbrakes/simulation/data_generator.py | 243 +++++++++++++++-------- airbrakes/simulation/rotation_manager.py | 141 +++++++++++-- airbrakes/simulation/sim_imu.py | 2 +- 3 files changed, 281 insertions(+), 105 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index e3fe8a39..5b3e8b41 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -7,14 +7,13 @@ import numpy as np import numpy.typing as npt import yaml -from scipy.spatial.transform import Rotation as R from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, RawDataPacket, ) from airbrakes.simulation.rotation_manager import RotationManager -from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY +from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY, MAX_VELOCITY_THRESHOLD from utils import deadband @@ -26,10 +25,12 @@ class DataGenerator: __slots__ = ( "_config", + "_est_rotation_manager", "_last_est_packet", "_last_raw_packet", - "_last_velocity", - "_rotation_manager", + "_last_velocities", + "_max_velocity", + "_raw_rotation_manager", "_thrust_data", "_vertical_index", ) @@ -38,7 +39,8 @@ def __init__(self): """Initializes the data generator object""" self._last_est_packet: EstimatedDataPacket | None = None self._last_raw_packet: RawDataPacket | None = None - self._last_velocity: np.float64 = np.float64(0.0) + self._last_velocities: npt.NDArray = np.array([0.0, 0.0, 0.0]) + self._max_velocity: np.float64 = np.float64(0.1) # loads the sim_config.yaml file config_path = Path("airbrakes/simulation/sim_config.yaml") @@ -49,19 +51,18 @@ def __init__(self): self._thrust_data: npt.NDArray = self._load_thrust_curve() # initializes the rotation manager with the launch pad conditions - self._rotation_manager = RotationManager( - self._config["rocket_orientation"], - self._get_random("launch_rod_angle"), - self._get_random("launch_rod_direction"), - ) + raw_manager, est_manager = self._get_rotation_managers() + self._raw_rotation_manager: RotationManager = raw_manager + self._est_rotation_manager: RotationManager = est_manager + # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] @property - def velocity(self) -> np.float64: + def velocities(self) -> np.float64: """Returns the last calculated velocity of the rocket""" - return self._last_velocity + return self._last_velocities def _get_random(self, identifier: str) -> np.float64: """ @@ -112,6 +113,28 @@ def _load_thrust_curve(self) -> npt.NDArray: return np.array([motor_timestamps, motor_thrusts]) + def _get_rotation_managers(self) -> npt.NDArray: + """ + Creates and initializes both rotation managers that will be used to contain any rotation + related math and methods for the raw and estimated data. + + :return: a 2 element array containing the raw rotation manager and the estimated rotation + manager, respectively. + """ + launch_rod_angle = self._get_random("launch_rod_angle") + launch_rod_direction = self._get_random("launch_rod_direction") + raw_manager = RotationManager( + self._config["rocket_orientation"], + launch_rod_angle, + launch_rod_direction, + ) + est_manager = RotationManager( + self._config["rocket_orientation"], + launch_rod_angle, + launch_rod_direction, + ) + return np.array([raw_manager, est_manager]) + def _get_first_packet( self, packet_type: RawDataPacket | EstimatedDataPacket ) -> RawDataPacket | EstimatedDataPacket: @@ -124,15 +147,17 @@ def _get_first_packet( :return: data packet of the specified type with launch pad conditions. """ - orientation = self._config["rocket_orientation"] packet = None if packet_type == RawDataPacket: + scaled_accel = ( + self._raw_rotation_manager.calculate_compensated_accel(0.0, 0.0) / GRAVITY + ) packet = RawDataPacket( 0, - scaledAccelX=orientation[0], - scaledAccelY=orientation[1], - scaledAccelZ=orientation[2], + scaledAccelX=scaled_accel[0], + scaledAccelY=scaled_accel[1], + scaledAccelZ=scaled_accel[2], scaledGyroX=0.0, scaledGyroY=0.0, scaledGyroZ=0.0, @@ -144,24 +169,29 @@ def _get_first_packet( deltaThetaZ=0.0, ) else: - initial_quaternion, _ = R.align_vectors([0, 0, -1], [orientation]) - initial_quaternion = R.as_quat(initial_quaternion, scalar_first=True) + initial_quaternion = self._est_rotation_manager.quaternion + compensated_accel = self._est_rotation_manager.calculate_compensated_accel(0.0, 0.0) + linear_accel = self._est_rotation_manager.calculate_linear_accel(0.0, 0.0) + gravity_vector = self._est_rotation_manager.gravity_vector packet = EstimatedDataPacket( 0, estOrientQuaternionW=initial_quaternion[0], estOrientQuaternionX=initial_quaternion[1], estOrientQuaternionY=initial_quaternion[2], estOrientQuaternionZ=initial_quaternion[3], - estCompensatedAccelX=GRAVITY * orientation[0], - estCompensatedAccelY=GRAVITY * orientation[1], - estCompensatedAccelZ=GRAVITY * orientation[2], estPressureAlt=0, - estGravityVectorX=-GRAVITY * orientation[0], - estGravityVectorY=-GRAVITY * orientation[1], - estGravityVectorZ=-GRAVITY * orientation[2], estAngularRateX=0, estAngularRateY=0, estAngularRateZ=0, + estCompensatedAccelX=compensated_accel[0], + estCompensatedAccelY=compensated_accel[1], + estCompensatedAccelZ=compensated_accel[2], + estLinearAccelX=linear_accel[0], + estLinearAccelY=linear_accel[1], + estLinearAccelZ=linear_accel[2], + estGravityVectorX=gravity_vector[0], + estGravityVectorY=gravity_vector[1], + estGravityVectorZ=gravity_vector[2], ) return packet @@ -174,7 +204,6 @@ def generate_raw_data_packet(self) -> RawDataPacket: """ # creating shorthand variables from config time_step = self._config["raw_time_step"] - orientation = self._config["rocket_orientation"] # If the simulation has just started, we want to just generate the initial raw packet # and initialize "self._last_" variables @@ -182,31 +211,53 @@ def generate_raw_data_packet(self) -> RawDataPacket: self._last_raw_packet = self._get_first_packet(RawDataPacket) return self._last_raw_packet + # updates the raw rotation manager, if we are after motor burn phase + if ( + self._last_velocities[2] + < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + ): + velocity_ratio = self._last_velocities[2] / self._max_velocity + self._raw_rotation_manager.update_orientation(time_step, velocity_ratio) + # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_raw_packet.timestamp / 1e9) + time_step - # calculates the net force and vertical scaled acceleration - net_force = self._calculate_net_force(next_timestamp, self._last_velocity) - vertical_scaled_accel = net_force / (self._config["rocket_mass"] * GRAVITY) + # calculates the forces and vertical scaled acceleration + forces = self._calculate_forces(next_timestamp, self._last_velocities) + force_accelerations = forces / self._config["rocket_mass"] + compensated_accel = self._raw_rotation_manager.calculate_compensated_accel( + force_accelerations[0], + force_accelerations[1], + ) + scaled_accel = compensated_accel / GRAVITY - # calculates vertical delta velocity - last_vertical_scaled_accel = self._get_vertical_data_point("scaledAccel") - vert_delta_v = (vertical_scaled_accel - last_vertical_scaled_accel) * GRAVITY * time_step + # calculates vertical delta velocity, and gyro + last_scaled_accel = np.array( + [ + self._last_raw_packet.scaledAccelX, + self._last_raw_packet.scaledAccelY, + self._last_raw_packet.scaledAccelZ, + ] + ) + delta_velocity = (scaled_accel - last_scaled_accel) * GRAVITY + delta_theta = self._raw_rotation_manager.calculate_delta_theta() + scaled_gyro_vector = delta_theta / time_step + # assembles the packet packet = RawDataPacket( next_timestamp * 1e9, - scaledAccelX=vertical_scaled_accel * orientation[0], - scaledAccelY=vertical_scaled_accel * orientation[1], - scaledAccelZ=vertical_scaled_accel * orientation[2], - scaledGyroX=0.0, - scaledGyroY=0.0, - scaledGyroZ=0.0, - deltaVelX=vert_delta_v * orientation[0], - deltaVelY=vert_delta_v * orientation[1], - deltaVelZ=vert_delta_v * orientation[2], - deltaThetaX=0.0, - deltaThetaY=0.0, - deltaThetaZ=0.0, + scaledAccelX=scaled_accel[0], + scaledAccelY=scaled_accel[1], + scaledAccelZ=scaled_accel[2], + scaledGyroX=scaled_gyro_vector[0], + scaledGyroY=scaled_gyro_vector[1], + scaledGyroZ=scaled_gyro_vector[2], + deltaVelX=delta_velocity[0], + deltaVelY=delta_velocity[1], + deltaVelZ=delta_velocity[2], + deltaThetaX=delta_theta[0], + deltaThetaY=delta_theta[1], + deltaThetaZ=delta_theta[2], ) # updates last raw data packet @@ -222,7 +273,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: """ # creating shorthand variables from config time_step = self._config["est_time_step"] - orientation = self._config["rocket_orientation"] # If the simulation has just started, we want to just generate the initial estimated packet # and initialize "self._last_" variables @@ -230,44 +280,55 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: self._last_est_packet = self._get_first_packet(EstimatedDataPacket) return self._last_raw_packet + # updates the estimated rotation manager, if we are after motor burn phase + if ( + self._last_velocities[2] + < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + ): + velocity_ratio = self._last_velocities[2] / self._max_velocity + self._est_rotation_manager.update_orientation(time_step, velocity_ratio) + # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_est_packet.timestamp / 1e9) + time_step - # calculates the net force and vertical accelerations - net_force = self._calculate_net_force(next_timestamp, self._last_velocity) - vertical_linear_accel = net_force / self._config["rocket_mass"] - vertical_comp_accel = vertical_linear_accel + GRAVITY + # calculates the forces and accelerations + forces = self._calculate_forces(next_timestamp, self._last_velocities) + force_accelerations = forces / self._config["rocket_mass"] + compensated_accel = self._est_rotation_manager.calculate_compensated_accel( + force_accelerations[0], + force_accelerations[1], + ) + linear_accel = self._est_rotation_manager.calculate_linear_accel( + force_accelerations[0], + force_accelerations[1], + ) # gets vertical velocity and finds updated altitude - vert_velocity = self._calculate_vertical_velocity(vertical_comp_accel, time_step) + vert_velocity = self._calculate_velocities(compensated_accel, time_step)[2] new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step - # gets previous quaternion - last_quat = np.array( - [ - self._last_est_packet.estOrientQuaternionW, - self._last_est_packet.estOrientQuaternionX, - self._last_est_packet.estOrientQuaternionY, - self._last_est_packet.estOrientQuaternionZ, - ] - ) + delta_theta = self._est_rotation_manager.calculate_delta_theta() + angular_rates = delta_theta / time_step packet = EstimatedDataPacket( next_timestamp * 1e9, - estOrientQuaternionW=last_quat[0], - estOrientQuaternionX=last_quat[1], - estOrientQuaternionY=last_quat[2], - estOrientQuaternionZ=last_quat[3], - estCompensatedAccelX=vertical_comp_accel * orientation[0], - estCompensatedAccelY=vertical_comp_accel * orientation[1], - estCompensatedAccelZ=vertical_comp_accel * orientation[2], + estOrientQuaternionW=self._est_rotation_manager.quaternion[0], + estOrientQuaternionX=self._est_rotation_manager.quaternion[1], + estOrientQuaternionY=self._est_rotation_manager.quaternion[2], + estOrientQuaternionZ=self._est_rotation_manager.quaternion[3], + estCompensatedAccelX=compensated_accel[0], + estCompensatedAccelY=compensated_accel[1], + estCompensatedAccelZ=compensated_accel[2], estPressureAlt=new_altitude, - estGravityVectorX=-GRAVITY * orientation[0], - estGravityVectorY=-GRAVITY * orientation[1], - estGravityVectorZ=-GRAVITY * orientation[2], - estAngularRateX=0, - estAngularRateY=0, - estAngularRateZ=0, + estGravityVectorX=self._est_rotation_manager.gravity_vector[0], + estGravityVectorY=self._est_rotation_manager.gravity_vector[1], + estGravityVectorZ=self._est_rotation_manager.gravity_vector[2], + estAngularRateX=angular_rates[0], + estAngularRateY=angular_rates[1], + estAngularRateZ=angular_rates[2], + estLinearAccelX=linear_accel[0], + estLinearAccelY=linear_accel[1], + estLinearAccelZ=linear_accel[2], ) # updates last estimated packet @@ -275,47 +336,57 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: return packet - def _calculate_vertical_velocity(self, acceleration, time_diff) -> np.float64: + def _calculate_velocities(self, comp_accel: npt.NDArray, time_diff: np.float64) -> np.float64: """ Calculates the velocity of the rocket based on the compensated acceleration. Integrates that acceleration to get the velocity. + + :param: numpy array containing the compensated acceleration vector + :return: the velocity of the rocket """ - # deadbands the acceleration - acceleration = deadband(acceleration - GRAVITY, ACCELERATION_NOISE_THRESHOLD) + # gets the rotated acceleration vector + accelerations = self._est_rotation_manager._calculate_rotated_accelerations(comp_accel) + # deadbands the vertical part of the acceleration + accelerations[2] = deadband(accelerations[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) - # Integrate the acceleration to get the velocity - vertical_velocity = self._last_velocity + acceleration * time_diff + # Integrate the accelerations to get the velocity + velocities = self._last_velocities + accelerations * time_diff # updates the last vertical velocity - self._last_velocity = vertical_velocity + self._last_velocities = velocities + + # updates max velocity + self._max_velocity = max(velocities[2], self._max_velocity) - return vertical_velocity + return velocities - def _calculate_net_force(self, timestamp, velocity) -> np.float64: + def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: """ - Calculates the drag force, thrust force, and weight, and sums them. + Calculates the thrust force and drag force, and returns them in an array :param timestamp: the timestamp of the rocket to calcuate the net forces at - :param velocity: the vertical velocity at the given instant + :param velocities: numpy array containing the x, y, and z velocities - :return: float containing the net force. Thrust is positive, drag and weight is negative. + :return: a 2 element numpy array containing thrust force and drag force, respectively. + Thrust is positive, drag is negative. """ + # calculate the magnitude of velocity + speed = np.linalg.norm(velocities) + # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 reference_area = self._config["reference_area"] drag_coefficient = self._config["drag_coefficient"] - rocket_mass = self._config["rocket_mass"] - drag_force = 0.5 * air_density * reference_area * drag_coefficient * velocity**2 - weight_force = GRAVITY * rocket_mass thrust_force = 0.0 + drag_force = 0.5 * air_density * reference_area * drag_coefficient * speed**2 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn if timestamp <= self._thrust_data[0][-1]: thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) - return thrust_force - weight_force - drag_force + return np.array([thrust_force, drag_force]) def _get_vertical_data_point(self, string_identifier: str) -> np.float64: """ diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index e1cdb73c..f4933d4c 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -1,9 +1,10 @@ """Module that contains methods for rotation operations""" -from collections import deque - import numpy as np import numpy.typing as npt +from scipy.spatial.transform import Rotation as R + +from constants import GRAVITY class RotationManager: @@ -14,9 +15,11 @@ class RotationManager: """ __slots__ = ( - "_azimuths", + "_azimuth", + "_last_orientation", + "_orientation", "_vertical", - "_zeniths", + "_zenith", ) def __init__( @@ -29,21 +32,123 @@ def __init__( :param orientation: the vertical orientation of the rocket """ self._vertical: npt.NDArray = orientation - self._zeniths: deque = deque[(rod_angle_of_attack * np.pi) / 180] - self._azimuths: deque = deque[(rod_direction * np.pi) / 180] + self._zenith: np.float64 = np.float64((rod_angle_of_attack * np.pi) / 180) + self._azimuth: np.float64 = np.float64((rod_direction * np.pi) / 180) + self._last_orientation: R | None = None + self._orientation: R | None = None + self.update_orientation(0, 0) + + @property + def quaternion(self) -> npt.NDArray: + return self._orientation.as_quat(scalar_first=True) - def update_orientation(self, time_step: np.float64, vertical_velocity: np.float64) -> None: + @property + def gravity_vector(self) -> npt.NDArray: + return self._orientation.apply([0, 0, -GRAVITY]) + + def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) -> None: """ - Updates the baseline vehicle-frame orientation, based on how close the velocity - is to zero. This accounts for the velocity being zero at the start of launch + Updates the baseline vehicle-frame orientation, and all of the vehicle-frame + orientations :param time_step: how much time has passed between updates - :param vertical_velocity: current vertical velocity of the rocket - """ - # we dont want to change angle if in motor burn phase, for simplicity - if self._zeniths[-1] is not self._zeniths[0] or vertical_velocity >= 100: - self._zeniths.append( - (np.pi / 2 - self._zeniths[0]) * np.exp(-0.035 * vertical_velocity) - + self._zeniths[0] - ) - _ = time_step + :param velocity_ratio: current vertical velocity of the rocket divided by max velocity + """ + if velocity_ratio == 0.0: + scale_factor = 0.0 + else: + scale_factor = np.sin(self._zenith) * time_step / abs(velocity_ratio * 10) + self._zenith = self._zenith + scale_factor * (np.pi / 2 - self._zenith) + + # We want to convert the azimuth and zenith values into a scipy rotation object + + # Step 1: We have to declare that the orientation of the rocket is vertical + aligned_orientation = R.align_vectors([self._vertical], [[0, 0, 1]])[0] + + # Step 2: Apply azimuth and zenith rotations relative to the aligned orientation + # 'zy' is used because azimute is yaw (z), and zenith is pitch (y) + azimuth_zenith_rotation = R.from_euler("zy", [self._azimuth, self._zenith], degrees=False) + + # Step 3: Combined rotation: aligned orientation, followed by azimuth and zenith rotations + orientation = azimuth_zenith_rotation * aligned_orientation + + # updating last orientation + self._last_orientation = ( + orientation if self._last_orientation is None else self._orientation + ) + self._orientation = orientation + + def calculate_compensated_accel( + self, thrust_acceleration: np.float64, drag_acceleration: np.float64 + ) -> npt.NDArray: + """ + Uses the acceleration due to drag and thrust to find the compensated acceleration + + :param drag_acceleration: scalar value of acceleration due to drag + :param thrust_acceleration: scalar value of acceleration due to thrust + + :return: the compensated acceleration in the vehicle frame of reference + """ + thrust_drag_accel = thrust_acceleration - drag_acceleration + # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket + # is still on the ground. We have to include the acceleration due to the normal force of + # the ground. + compensated_accel = self._scalar_to_vector(thrust_drag_accel) + if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: + normal_force_accel_vector = self._orientation.apply([0, 0, GRAVITY]) + compensated_accel += normal_force_accel_vector + return compensated_accel + + def calculate_linear_accel( + self, thrust_acceleration: np.float64, drag_acceleration: np.float64 + ) -> npt.NDArray: + """ + Uses the compensated acceleration to find the linear acceleration + + :param drag_acceleration: scalar value of acceleration due to drag + :param thrust_acceleration: scalar value of acceleration due to thrust + + :return: the linear acceleration in the vehicle frame of reference + """ + comp_accel = self.calculate_compensated_accel(drag_acceleration, thrust_acceleration) + # apply gravity + gravity_accel_vector = self._orientation.apply([0, 0, -GRAVITY]) + return np.array(comp_accel - gravity_accel_vector) + + def calculate_delta_theta(self) -> npt.NDArray: + """ + Calculates the delta theta in the vehicle frame of reference, with units in + radians. + + :return: numpy array containing the delta thetas, in [x, y, z] format. + """ + # Calculate the relative rotation from last orientation to the current orientation + relative_rotation = self._last_orientation.inv() * self._orientation + + # Converts the relative rotation to a rotation vector + return relative_rotation.as_rotvec() + + def _calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> np.float64: + """ + Calculates the rotated acceleration vector with the compensated acceleration + + :param compensated_acceleration: compensated acceleration in the vehicle-frame reference. + + :return: float containing vertical acceleration with respect to Earth. + """ + return self._orientation.apply([compensated_acceleration])[0] + + def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: + """ + Converts a scalar value to a vector, where it will align with the vertical orientation. + + :param scalar: a float representing the scalar to be converted + + :return: a 3-element numpy array containing the converted vector. + """ + vector = np.zeros(3) + # Gets index of non-zero value in the vertical orientation + index = np.argmax(np.abs(self._vertical)) + # Place the scalar in the appropriate position, and with the correct sign + vector[index] = scalar * self._vertical[index] + return vector diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index a4e69796..a6f5d298 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -106,7 +106,7 @@ def _fetch_data_loop(self) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - while self._data_generator.velocity > -1: + while self._data_generator.velocities[2] > -100: # starts timer start_time = time.time() From 7096498f91d446d405047071f4e36a16656a3370 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sat, 9 Nov 2024 00:07:05 -0500 Subject: [PATCH 40/63] bug fixes wip --- airbrakes/simulation/data_generator.py | 48 +++++++++--------------- airbrakes/simulation/rotation_manager.py | 45 ++++++++++++++++------ airbrakes/simulation/sim_config.yaml | 4 +- 3 files changed, 53 insertions(+), 44 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 5b3e8b41..f57bba4c 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -72,6 +72,8 @@ def _get_random(self, identifier: str) -> np.float64: the bounds specified in the config """ parameters = self._config[identifier] + if len(parameters) == 1: + return parameters[0] if len(parameters) == 3: # uses standard deviation to get random number mean = float(np.mean([parameters[0], parameters[1]])) @@ -169,7 +171,7 @@ def _get_first_packet( deltaThetaZ=0.0, ) else: - initial_quaternion = self._est_rotation_manager.quaternion + initial_quaternion = self._est_rotation_manager.calculate_imu_quaternions() compensated_accel = self._est_rotation_manager.calculate_compensated_accel(0.0, 0.0) linear_accel = self._est_rotation_manager.calculate_linear_accel(0.0, 0.0) gravity_vector = self._est_rotation_manager.gravity_vector @@ -212,9 +214,11 @@ def generate_raw_data_packet(self) -> RawDataPacket: return self._last_raw_packet # updates the raw rotation manager, if we are after motor burn phase + if ( self._last_velocities[2] < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity self._raw_rotation_manager.update_orientation(time_step, velocity_ratio) @@ -284,6 +288,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: if ( self._last_velocities[2] < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity self._est_rotation_manager.update_orientation(time_step, velocity_ratio) @@ -303,19 +308,23 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[1], ) - # gets vertical velocity and finds updated altitude + # gets velocity and finds updated altitude vert_velocity = self._calculate_velocities(compensated_accel, time_step)[2] new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step - + # print(f"sim imu altitude: {new_altitude}") delta_theta = self._est_rotation_manager.calculate_delta_theta() angular_rates = delta_theta / time_step + # print(f"sim imu linear accel: {np.round(linear_accel,3)}") + # print(f"sim imu comp accel: {np.round(compensated_accel,3)}") + # print(f"sim imu linear accel: {np.round(linear_accel,3)}") + quaternion = self._est_rotation_manager.calculate_imu_quaternions() packet = EstimatedDataPacket( next_timestamp * 1e9, - estOrientQuaternionW=self._est_rotation_manager.quaternion[0], - estOrientQuaternionX=self._est_rotation_manager.quaternion[1], - estOrientQuaternionY=self._est_rotation_manager.quaternion[2], - estOrientQuaternionZ=self._est_rotation_manager.quaternion[3], + estOrientQuaternionW=quaternion[0], + estOrientQuaternionX=quaternion[1], + estOrientQuaternionY=quaternion[2], + estOrientQuaternionZ=quaternion[3], estCompensatedAccelX=compensated_accel[0], estCompensatedAccelY=compensated_accel[1], estCompensatedAccelZ=compensated_accel[2], @@ -346,7 +355,8 @@ def _calculate_velocities(self, comp_accel: npt.NDArray, time_diff: np.float64) :return: the velocity of the rocket """ # gets the rotated acceleration vector - accelerations = self._est_rotation_manager._calculate_rotated_accelerations(comp_accel) + accelerations = self._est_rotation_manager.calculate_rotated_accelerations(comp_accel) + # deadbands the vertical part of the acceleration accelerations[2] = deadband(accelerations[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) @@ -387,25 +397,3 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: if timestamp <= self._thrust_data[0][-1]: thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) return np.array([thrust_force, drag_force]) - - def _get_vertical_data_point(self, string_identifier: str) -> np.float64: - """ - gets the last vertical data point specified from a vector data attribute by using IMU - orientation in config - :param string_identifier: a string representing the exact attribute name of the data packet - :return: float containing the specified data point - """ - # Dynamically retrieve x, y, z components based on the identifier - values = [ - getattr(self._last_raw_packet, f"{string_identifier}X", None), - getattr(self._last_raw_packet, f"{string_identifier}Y", None), - getattr(self._last_raw_packet, f"{string_identifier}Z", None), - ] - - # Just an edge case, but I really hope this never happens - if any(value is None for value in values): - raise AttributeError( - f"Could not find all components for identifier '{string_identifier}'." - ) - - return values[self._vertical_index] diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index f4933d4c..07f62264 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -38,10 +38,6 @@ def __init__( self._orientation: R | None = None self.update_orientation(0, 0) - @property - def quaternion(self) -> npt.NDArray: - return self._orientation.as_quat(scalar_first=True) - @property def gravity_vector(self) -> npt.NDArray: return self._orientation.apply([0, 0, -GRAVITY]) @@ -65,12 +61,14 @@ def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) # Step 1: We have to declare that the orientation of the rocket is vertical aligned_orientation = R.align_vectors([self._vertical], [[0, 0, 1]])[0] - # Step 2: Apply azimuth and zenith rotations relative to the aligned orientation - # 'zy' is used because azimute is yaw (z), and zenith is pitch (y) - azimuth_zenith_rotation = R.from_euler("zy", [self._azimuth, self._zenith], degrees=False) + # Step 2: Tilt by the zenith angle to adjust pitch away from vertical + tilt_rotation = R.from_rotvec(self._zenith * np.array([1, 0, 0])) + + # Step 3: Rotate around the vertical by azimuth to set the compass direction of the tilt + azimuth_rotation = R.from_rotvec(self._azimuth * np.array([0, 0, -1])) - # Step 3: Combined rotation: aligned orientation, followed by azimuth and zenith rotations - orientation = azimuth_zenith_rotation * aligned_orientation + # Step 4: Combined rotation: aligned orientation, followed by azimuth and zenith rotations + orientation = azimuth_rotation * tilt_rotation * aligned_orientation # updating last orientation self._last_orientation = ( @@ -110,7 +108,7 @@ def calculate_linear_accel( :return: the linear acceleration in the vehicle frame of reference """ - comp_accel = self.calculate_compensated_accel(drag_acceleration, thrust_acceleration) + comp_accel = self.calculate_compensated_accel(thrust_acceleration, drag_acceleration) # apply gravity gravity_accel_vector = self._orientation.apply([0, 0, -GRAVITY]) return np.array(comp_accel - gravity_accel_vector) @@ -128,16 +126,39 @@ def calculate_delta_theta(self) -> npt.NDArray: # Converts the relative rotation to a rotation vector return relative_rotation.as_rotvec() - def _calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> np.float64: + def calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> np.float64: """ - Calculates the rotated acceleration vector with the compensated acceleration + Calculates the rotated acceleration vector with the compensated acceleration. Rotates from + vehicle frame of reference to Earth frame of reference. :param compensated_acceleration: compensated acceleration in the vehicle-frame reference. :return: float containing vertical acceleration with respect to Earth. """ + # # Define the rotation needed to align the IMU + # imu_alignment_rotation = R.align_vectors([[0, 0, -1]], [[0, 0, 1]])[0] + # # Apply this alignment rotation to the given orientation + # imu_adjusted_orientation = imu_alignment_rotation * self._orientation + + # return imu_adjusted_orientation.apply([compensated_acceleration])[0] return self._orientation.apply([compensated_acceleration])[0] + def calculate_imu_quaternions(self) -> npt.NDArray: + """ + Adjusts the given orientation to align with the IMU's reference frame, where + [0, 0, -1] is vertical, and returns a quaternions. + + :param orientation: A scipy Rotation object representing the current orientation. + + :return: A quaternion adjusted to the IMU's frame. + """ + # rotation needed to align the IMU + imu_alignment_rotation = R.align_vectors([[0, 0, 1]], [[0, 0, -1]])[0] + imu_adjusted_orientation = imu_alignment_rotation * self._orientation + + # convert to quaternion + return imu_adjusted_orientation.as_quat(scalar_first=True) + def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: """ Converts a scalar value to a vector, where it will align with the vertical orientation. diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml index 00a50f82..8302284d 100644 --- a/airbrakes/simulation/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -17,5 +17,5 @@ rocket_orientation: [0, 0, -1] #--------------------- # CONFIG FOR RANDOMNESS #--------------------- -launch_rod_angle: [2,10] # min and max values for the launch rod angle of attack (in degrees) -launch_rod_direction: [0,360] # how far off (in degrees) the launch rod will be pointed into wind \ No newline at end of file +launch_rod_angle: [10] # min and max values for the launch rod angle of attack (in degrees) +launch_rod_direction: [90] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file From e6283d090a7e360d56d2ddc5188534be35139d9e Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sat, 9 Nov 2024 18:02:42 -0500 Subject: [PATCH 41/63] rotation fixes wip --- airbrakes/simulation/data_generator.py | 9 +++------ airbrakes/simulation/rotation_manager.py | 22 +++++++++++++--------- airbrakes/simulation/sim_config.yaml | 2 +- airbrakes/simulation/sim_imu.py | 1 + 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index f57bba4c..d4aa2441 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -311,12 +311,10 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: # gets velocity and finds updated altitude vert_velocity = self._calculate_velocities(compensated_accel, time_step)[2] new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step - # print(f"sim imu altitude: {new_altitude}") + delta_theta = self._est_rotation_manager.calculate_delta_theta() angular_rates = delta_theta / time_step - # print(f"sim imu linear accel: {np.round(linear_accel,3)}") - # print(f"sim imu comp accel: {np.round(compensated_accel,3)}") - # print(f"sim imu linear accel: {np.round(linear_accel,3)}") + quaternion = self._est_rotation_manager.calculate_imu_quaternions() packet = EstimatedDataPacket( @@ -356,8 +354,7 @@ def _calculate_velocities(self, comp_accel: npt.NDArray, time_diff: np.float64) """ # gets the rotated acceleration vector accelerations = self._est_rotation_manager.calculate_rotated_accelerations(comp_accel) - - # deadbands the vertical part of the acceleration + # gets vertical part of the gravity vector accelerations[2] = deadband(accelerations[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) # Integrate the accelerations to get the velocity diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 07f62264..0d9cc2c1 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -93,8 +93,10 @@ def calculate_compensated_accel( # the ground. compensated_accel = self._scalar_to_vector(thrust_drag_accel) if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: - normal_force_accel_vector = self._orientation.apply([0, 0, GRAVITY]) - compensated_accel += normal_force_accel_vector + normal_force_vector = np.array([0, 0, GRAVITY]) + rotated_normal_vector = self._orientation.apply(normal_force_vector) + + compensated_accel += rotated_normal_vector return compensated_accel def calculate_linear_accel( @@ -120,8 +122,13 @@ def calculate_delta_theta(self) -> npt.NDArray: :return: numpy array containing the delta thetas, in [x, y, z] format. """ + # first have to take the current orientation and the last orientation that is in + # Earth -> vehicle reference and convert to vehicle -> Earth reference + imu_adjusted_orientation = self._orientation.inv() + imu_adjusted_last_orientation = self._last_orientation.inv() + # Calculate the relative rotation from last orientation to the current orientation - relative_rotation = self._last_orientation.inv() * self._orientation + relative_rotation = imu_adjusted_last_orientation.inv() * imu_adjusted_orientation # Converts the relative rotation to a rotation vector return relative_rotation.as_rotvec() @@ -135,13 +142,10 @@ def calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) :return: float containing vertical acceleration with respect to Earth. """ - # # Define the rotation needed to align the IMU - # imu_alignment_rotation = R.align_vectors([[0, 0, -1]], [[0, 0, 1]])[0] - # # Apply this alignment rotation to the given orientation - # imu_adjusted_orientation = imu_alignment_rotation * self._orientation + # rotation needed to align the IMU + imu_adjusted_orientation = self._orientation.inv() - # return imu_adjusted_orientation.apply([compensated_acceleration])[0] - return self._orientation.apply([compensated_acceleration])[0] + return imu_adjusted_orientation.apply([compensated_acceleration])[0] def calculate_imu_quaternions(self) -> npt.NDArray: """ diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml index 8302284d..e76bc0a0 100644 --- a/airbrakes/simulation/sim_config.yaml +++ b/airbrakes/simulation/sim_config.yaml @@ -18,4 +18,4 @@ rocket_orientation: [0, 0, -1] # CONFIG FOR RANDOMNESS #--------------------- launch_rod_angle: [10] # min and max values for the launch rod angle of attack (in degrees) -launch_rod_direction: [90] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file +launch_rod_direction: [0] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index a6f5d298..06ce6c6f 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -123,3 +123,4 @@ def _fetch_data_loop(self) -> None: self._timestamp += time_step end_time = time.time() time.sleep(max(0.0, time_step - (end_time - start_time))) + # if self._data_generator._last_est_packet.timestamp>1.02e9: From 084e8ce91db77ad0dabc9f4d7dda67ac64f2ab3a Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 19:13:39 -0500 Subject: [PATCH 42/63] Made sim config a class --- airbrakes/simulation/data_generator.py | 2 +- airbrakes/simulation/sim_config.py | 55 ++++++++++++++++++++++++++ airbrakes/simulation/sim_config.yaml | 21 ---------- airbrakes/simulation/sim_imu.py | 10 ++--- 4 files changed, 61 insertions(+), 27 deletions(-) create mode 100644 airbrakes/simulation/sim_config.py delete mode 100644 airbrakes/simulation/sim_config.yaml diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index d4aa2441..81759500 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -60,7 +60,7 @@ def __init__(self): self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] @property - def velocities(self) -> np.float64: + def velocities(self) -> npt.NDArray: """Returns the last calculated velocity of the rocket""" return self._last_velocities diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py new file mode 100644 index 00000000..7fffd3d5 --- /dev/null +++ b/airbrakes/simulation/sim_config.py @@ -0,0 +1,55 @@ +class SimulationConfig: + def __init__(self, + raw_time_step, + est_time_step, + motor, + drag_coefficient, + rocket_mass, + reference_area, + rocket_orientation, + launch_rod_angle, + launch_rod_direction): + # Time steps for data packet generation in the simulation + self.raw_time_step = raw_time_step + self.est_time_step = est_time_step + + # Motor selection (name of CSV file without extension) + self.motor = motor + + # Rocket properties + self.drag_coefficient = drag_coefficient + self.rocket_mass = rocket_mass + self.reference_area = reference_area + + # Rocket orientation on the launch pad + self.rocket_orientation = rocket_orientation + + # Config for randomness in the simulation + self.launch_rod_angle = launch_rod_angle + self.launch_rod_direction = launch_rod_direction + + +FULL_SCALE_CONFIG = SimulationConfig( + raw_time_step=0.001, + est_time_step=0.002, + motor="AeroTech_L1940X", + drag_coefficient=0.4, + rocket_mass=14.5, + reference_area=0.01929, + rocket_orientation=[0, 0, -1], + launch_rod_angle=[10], + launch_rod_direction=[0] +) + +# TODO: get actual values for sub scale +SUB_SCALE_CONFIG = SimulationConfig( + raw_time_step=0.001, + est_time_step=0.002, + motor="PLACEHOLDER", + drag_coefficient=0.4, + rocket_mass=0, + reference_area=0.001929, + rocket_orientation=[0, 0, -1], + launch_rod_angle=[10], + launch_rod_direction=[0] +) diff --git a/airbrakes/simulation/sim_config.yaml b/airbrakes/simulation/sim_config.yaml deleted file mode 100644 index e76bc0a0..00000000 --- a/airbrakes/simulation/sim_config.yaml +++ /dev/null @@ -1,21 +0,0 @@ -# config file for simulation - -raw_time_step: 0.001 # Time step between raw data packets the simulation will generate (in seconds) -est_time_step: 0.002 # Time step between estimated data packets the simulation will generate (in seconds) - -# selects the motor to use, this is the name of the csv file located in /simulation/thrust_curves -# with the ".csv" removed -motor: "AeroTech_L1940X" -drag_coefficient: 0.4 # coefficient of drag for the rocket -rocket_mass: 14.5 # mass of rocket (in kg) -reference_area: 0.01929 # reference area (in meters squared) - -# vertical orientation of the rocket on the launch pad. only one value should be non-zero. -# Non-zero values should either be -1 (negative axis is vertical) or 1 (positive axis is vertical) -rocket_orientation: [0, 0, -1] - -#--------------------- -# CONFIG FOR RANDOMNESS -#--------------------- -launch_rod_angle: [10] # min and max values for the launch rod angle of attack (in degrees) -launch_rod_direction: [0] # min and max values for what direction the launch rod will point in (in degrees) \ No newline at end of file diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 06ce6c6f..22431e86 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -24,7 +24,6 @@ class SimIMU(IMU): """ __slots__ = ( - "_config", "_data_fetch_process", "_data_generator", "_data_queue", @@ -41,7 +40,7 @@ def __init__(self) -> None: # loads the sim_config.yaml file config_path = Path("airbrakes/simulation/sim_config.yaml") with config_path.open(mode="r", newline="") as file: - self._config: dict = yaml.safe_load(file) + config: dict = yaml.safe_load(file) # This limits the queue size to a very high limit, because the data generator will # generate all the data before the imu reads it @@ -53,6 +52,7 @@ def __init__(self) -> None: self._data_fetch_process = multiprocessing.Process( target=self._fetch_data_loop, name="Sim IMU Process", + args=(config,), ) # Makes a boolean value that is shared between processes @@ -96,12 +96,12 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") - def _fetch_data_loop(self) -> None: + def _fetch_data_loop(self, config: dict) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" - raw_dt = self._config["raw_time_step"] - est_dt = self._config["est_time_step"] + raw_dt = config["raw_time_step"] + est_dt = config["est_time_step"] # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt From 75e5d9fdefba5fa619084e1a43611247bf2d51ed Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 21:21:16 -0500 Subject: [PATCH 43/63] Made it so you could pass in the sim config --- airbrakes/simulation/sim_config.py | 15 +++++++++++++++ airbrakes/simulation/sim_imu.py | 16 ++++++++-------- main.py | 2 +- pyproject.toml | 1 - utils.py | 3 ++- 5 files changed, 26 insertions(+), 11 deletions(-) diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 7fffd3d5..e4df8f65 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -53,3 +53,18 @@ def __init__(self, launch_rod_angle=[10], launch_rod_direction=[0] ) + + +def get_configuration(config_type: str) -> SimulationConfig: + """ + Gets the configuration for the simulation + :param config_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". + :return: The configuration for the simulation + """ + match config_type: + case "full-scale": + return FULL_SCALE_CONFIG + case "sub-scale": + return SUB_SCALE_CONFIG + case _: + raise ValueError(f"Invalid config type: {config_type}") diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 22431e86..25f25954 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -3,11 +3,11 @@ import contextlib import multiprocessing import time -from pathlib import Path from typing import TYPE_CHECKING import numpy as np -import yaml + +from airbrakes.simulation.sim_config import get_configuration, SimulationConfig if TYPE_CHECKING: from airbrakes.data_handling.imu_data_packet import IMUDataPacket @@ -31,16 +31,16 @@ class SimIMU(IMU): "_timestamp", ) - def __init__(self) -> None: + def __init__(self, sim_type: str) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. + :param sim_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". """ self._timestamp: np.float64 = np.float64(0.0) - # loads the sim_config.yaml file - config_path = Path("airbrakes/simulation/sim_config.yaml") - with config_path.open(mode="r", newline="") as file: - config: dict = yaml.safe_load(file) + + # Gets the configuration for the simulation + config = get_configuration(sim_type) # This limits the queue size to a very high limit, because the data generator will # generate all the data before the imu reads it @@ -96,7 +96,7 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") - def _fetch_data_loop(self, config: dict) -> None: + def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" diff --git a/main.py b/main.py index 3ccb992b..bf2d9c6c 100644 --- a/main.py +++ b/main.py @@ -55,7 +55,7 @@ def main(args: argparse.Namespace) -> None: # If we are running the simulation for generating datasets, we will replace our IMU object # with a sim variant, similar to running a mock simulation. else: - imu = SimIMU() + imu = SimIMU(sim_type=args.sim) # MockFactory is used to create a mock servo object that pretends to be the real servo servo = ( Servo(SERVO_PIN) diff --git a/pyproject.toml b/pyproject.toml index 16b0f187..fdda2ac9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,6 @@ dependencies = [ "colorama", "psutil", "scipy", - "PyYAML" # Installation instructions for the following dependencies can be found in the README: # "mscl" https://github.com/LORD-MicroStrain/MSCL/blob/master/BuildScripts/buildReadme_Linux.md ] diff --git a/utils.py b/utils.py index ab03d666..9a3662b1 100644 --- a/utils.py +++ b/utils.py @@ -105,7 +105,8 @@ def arg_parser() -> argparse.Namespace: help="runs the data simulation alongside the mock simulation, to randomly generate " "a dataset", action="store_true", - default=False, + choices=["full-scale", "sub-scale"], + default="full-scale", ) args = parser.parse_args() From 3fd399fe0e6c10e69a25b576a01e9da314805cc6 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 21:41:33 -0500 Subject: [PATCH 44/63] Moved SimIMU variables into the process --- airbrakes/mock/mock_imu.py | 5 ++--- airbrakes/simulation/sim_imu.py | 40 +++++++++++++-------------------- 2 files changed, 18 insertions(+), 27 deletions(-) diff --git a/airbrakes/mock/mock_imu.py b/airbrakes/mock/mock_imu.py index b59a7282..faa80ce3 100644 --- a/airbrakes/mock/mock_imu.py +++ b/airbrakes/mock/mock_imu.py @@ -74,9 +74,9 @@ def _read_file( Reads the data from the log file and puts it into the shared queue. :param log_file_path: the name of the log file to read data from located in scripts/imu_data :param real_time_simulation: Whether to simulate a real flight by sleeping for a set period, - or run at full speed, e.g. for using it in the CI. + or run at full speed, e.g. for using it in the CI. :param start_after_log_buffer: Whether to send the data packets only after the log buffer - was filled for Standby state. + was filled for Standby state. """ start_index = 0 @@ -116,7 +116,6 @@ def _read_file( if not self._running.value: break - imu_data_packet = None fields_dict = {} scaled_accel_x = row.get("scaledAccelX") # raw data packet field diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 25f25954..992e4eee 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -23,22 +23,12 @@ class SimIMU(IMU): and returns randomly generated data. """ - __slots__ = ( - "_data_fetch_process", - "_data_generator", - "_data_queue", - "_running", - "_timestamp", - ) - def __init__(self, sim_type: str) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. :param sim_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". """ - self._timestamp: np.float64 = np.float64(0.0) - # Gets the configuration for the simulation config = get_configuration(sim_type) @@ -58,9 +48,8 @@ def __init__(self, sim_type: str) -> None: # Makes a boolean value that is shared between processes self._running = multiprocessing.Value("b", False) - self._data_generator = DataGenerator() - - def update_timestamp(self, current_timestamp: np.float64) -> np.float64: + @staticmethod + def _update_timestamp(current_timestamp: np.float64, config: SimulationConfig) -> np.float64: """ Updates the current timestamp of the data generator, based off time step defined in config. Will also determine if the next timestamp will be a raw packet, estimated packet, or both. @@ -69,8 +58,8 @@ def update_timestamp(self, current_timestamp: np.float64) -> np.float64: """ # finding whether the raw or estimated data packets have a lower time_step - lowest_dt = min(self._config["raw_time_step"], self._config["est_time_step"]) - highest_dt = max(self._config["raw_time_step"], self._config["est_time_step"]) + lowest_dt = min(config.raw_time_step, config.est_time_step) + highest_dt = max(config.raw_time_step, config.est_time_step) # checks if current time is a multiple of the highest and/or lowest time step at_low = any(np.isclose(current_timestamp % lowest_dt, [0, lowest_dt])) @@ -100,27 +89,30 @@ def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" - raw_dt = config["raw_time_step"] - est_dt = config["est_time_step"] + data_generator = DataGenerator() + timestamp: np.float64 = np.float64(0.0) + + raw_dt = config.raw_time_step + est_dt = config.est_time_step # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - while self._data_generator.velocities[2] > -100: + while data_generator.velocities[2] > -100: # starts timer start_time = time.time() # if the timestamp is a multiple of the raw time step, generate a raw data packet. - if any(np.isclose(self._timestamp % raw_dt, [0, raw_dt])): - self._data_queue.put(self._data_generator.generate_raw_data_packet()) + if any(np.isclose(timestamp % raw_dt, [0, raw_dt])): + self._data_queue.put(data_generator.generate_raw_data_packet()) # if the timestamp is a multiple of the est time step, generate an est data packet. - if any(np.isclose(self._timestamp % est_dt, [0, est_dt])): - self._data_queue.put(self._data_generator.generate_estimated_data_packet()) + if any(np.isclose(timestamp % est_dt, [0, est_dt])): + self._data_queue.put(data_generator.generate_estimated_data_packet()) # updates the timestamp and sleeps until next packet is ready in real-time - time_step = self.update_timestamp(self._timestamp) - self._timestamp - self._timestamp += time_step + time_step = self._update_timestamp(timestamp, config) - timestamp + timestamp += time_step end_time = time.time() time.sleep(max(0.0, time_step - (end_time - start_time))) # if self._data_generator._last_est_packet.timestamp>1.02e9: From 5a0bab1f2031b3857281436125a4de4366f71834 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:09:53 -0500 Subject: [PATCH 45/63] Finished switching config to a class --- airbrakes/simulation/data_generator.py | 39 +++++++++++++------------- airbrakes/simulation/sim_config.py | 38 ++++++++++++++----------- airbrakes/simulation/sim_imu.py | 2 +- 3 files changed, 41 insertions(+), 38 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 81759500..b992796b 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -6,13 +6,13 @@ import numpy as np import numpy.typing as npt -import yaml from airbrakes.data_handling.imu_data_packet import ( EstimatedDataPacket, RawDataPacket, ) from airbrakes.simulation.rotation_manager import RotationManager +from airbrakes.simulation.sim_config import SimulationConfig from constants import ACCELERATION_NOISE_THRESHOLD, GRAVITY, MAX_VELOCITY_THRESHOLD from utils import deadband @@ -35,18 +35,19 @@ class DataGenerator: "_vertical_index", ) - def __init__(self): - """Initializes the data generator object""" + def __init__(self, config: SimulationConfig): + """ + Initializes the data generator object with the provided configuration + :param config: the configuration object for the simulation + """ + + self._config = config + self._last_est_packet: EstimatedDataPacket | None = None self._last_raw_packet: RawDataPacket | None = None self._last_velocities: npt.NDArray = np.array([0.0, 0.0, 0.0]) self._max_velocity: np.float64 = np.float64(0.1) - # loads the sim_config.yaml file - config_path = Path("airbrakes/simulation/sim_config.yaml") - with config_path.open(mode="r", newline="") as file: - self._config: dict = yaml.safe_load(file) - # loads thrust curve self._thrust_data: npt.NDArray = self._load_thrust_curve() @@ -57,7 +58,7 @@ def __init__(self): # finds the vertical index of the orientation. For example, if -y is vertical, the index # will be 1. - self._vertical_index: np.int64 = np.nonzero(self._config["rocket_orientation"])[0][0] + self._vertical_index: np.int64 = np.nonzero(self._config.rocket_orientation)[0][0] @property def velocities(self) -> npt.NDArray: @@ -71,7 +72,7 @@ def _get_random(self, identifier: str) -> np.float64: :return: float containing a random value for the selected identifier, between the bounds specified in the config """ - parameters = self._config[identifier] + parameters = getattr(self._config, identifier) if len(parameters) == 1: return parameters[0] if len(parameters) == 3: @@ -89,7 +90,7 @@ def _load_thrust_curve(self) -> npt.NDArray: :return: numpy array containing tuples with the time and thrust at that time. """ # gets the path of the csv based on the config file - csv_path = Path(f"airbrakes/simulation/thrust_curves/{self._config["motor"]}.csv") + csv_path = Path(f"airbrakes/simulation/thrust_curves/{self._config.motor}.csv") # initializes the list for timestamps and thrust values motor_timestamps = [0] @@ -126,12 +127,12 @@ def _get_rotation_managers(self) -> npt.NDArray: launch_rod_angle = self._get_random("launch_rod_angle") launch_rod_direction = self._get_random("launch_rod_direction") raw_manager = RotationManager( - self._config["rocket_orientation"], + self._config.rocket_orientation, launch_rod_angle, launch_rod_direction, ) est_manager = RotationManager( - self._config["rocket_orientation"], + self._config.rocket_orientation, launch_rod_angle, launch_rod_direction, ) @@ -205,7 +206,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: :return: RawDataPacket """ # creating shorthand variables from config - time_step = self._config["raw_time_step"] + time_step = self._config.raw_time_step # If the simulation has just started, we want to just generate the initial raw packet # and initialize "self._last_" variables @@ -228,7 +229,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: # calculates the forces and vertical scaled acceleration forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config["rocket_mass"] + force_accelerations = forces / self._config.rocket_mass compensated_accel = self._raw_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -276,7 +277,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: :return: EstimatedDataPacket """ # creating shorthand variables from config - time_step = self._config["est_time_step"] + time_step = self._config.est_time_step # If the simulation has just started, we want to just generate the initial estimated packet # and initialize "self._last_" variables @@ -298,7 +299,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: # calculates the forces and accelerations forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config["rocket_mass"] + force_accelerations = forces / self._config.rocket_mass compensated_accel = self._est_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -383,11 +384,9 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 - reference_area = self._config["reference_area"] - drag_coefficient = self._config["drag_coefficient"] thrust_force = 0.0 - drag_force = 0.5 * air_density * reference_area * drag_coefficient * speed**2 + drag_force = 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * speed**2 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index e4df8f65..da65a7ab 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -1,32 +1,36 @@ +import numpy as np +import numpy.typing as npt + + class SimulationConfig: def __init__(self, - raw_time_step, - est_time_step, - motor, - drag_coefficient, - rocket_mass, - reference_area, - rocket_orientation, - launch_rod_angle, - launch_rod_direction): + raw_time_step: np.float64, + est_time_step: np.float64, + motor: str, + drag_coefficient: np.float64, + rocket_mass: np.float64, + reference_area: np.float64, + rocket_orientation: npt.NDArray[np.float64], + launch_rod_angle: np.float64, + launch_rod_direction: np.float64): # Time steps for data packet generation in the simulation - self.raw_time_step = raw_time_step - self.est_time_step = est_time_step + self.raw_time_step = np.float64(raw_time_step) + self.est_time_step = np.float64(est_time_step) # Motor selection (name of CSV file without extension) self.motor = motor # Rocket properties - self.drag_coefficient = drag_coefficient - self.rocket_mass = rocket_mass - self.reference_area = reference_area + self.drag_coefficient = np.float64(drag_coefficient) + self.rocket_mass = np.float64(rocket_mass) + self.reference_area = np.float64(reference_area) # Rocket orientation on the launch pad - self.rocket_orientation = rocket_orientation + self.rocket_orientation = np.asarray(rocket_orientation, dtype=np.float64) # Config for randomness in the simulation - self.launch_rod_angle = launch_rod_angle - self.launch_rod_direction = launch_rod_direction + self.launch_rod_angle = np.float64(launch_rod_angle) + self.launch_rod_direction = np.float64(launch_rod_direction) FULL_SCALE_CONFIG = SimulationConfig( diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 992e4eee..5ce32ab5 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -89,7 +89,7 @@ def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" - data_generator = DataGenerator() + data_generator = DataGenerator(config) timestamp: np.float64 = np.float64(0.0) raw_dt = config.raw_time_step From afa2eb8b3c22c855cfbd646ab6369506b0e7801c Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:13:35 -0500 Subject: [PATCH 46/63] Fixed arg parser --- utils.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/utils.py b/utils.py index 9a3662b1..f72264e6 100644 --- a/utils.py +++ b/utils.py @@ -102,11 +102,11 @@ def arg_parser() -> argparse.Namespace: parser.add_argument( "-s", "--sim", - help="runs the data simulation alongside the mock simulation, to randomly generate " - "a dataset", - action="store_true", + help="Runs the data simulation alongside the mock simulation, with an optional scale", choices=["full-scale", "sub-scale"], - default="full-scale", + nargs="?", # Allows an optional argument + const="full-scale", # Default when `-s` is provided without a value + default=None, # Default when `-s` is not provided at all ) args = parser.parse_args() From 1bca6dbd27a721f89e7a674d875c4fa095a66301 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:32:39 -0500 Subject: [PATCH 47/63] Cleaned up typing for SimulationConfig --- airbrakes/simulation/sim_config.py | 36 +++++++++++++++--------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index da65a7ab..7cc227b9 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -11,8 +11,8 @@ def __init__(self, rocket_mass: np.float64, reference_area: np.float64, rocket_orientation: npt.NDArray[np.float64], - launch_rod_angle: np.float64, - launch_rod_direction: np.float64): + launch_rod_angle: npt.NDArray[np.float64], + launch_rod_direction: npt.NDArray[np.float64]): # Time steps for data packet generation in the simulation self.raw_time_step = np.float64(raw_time_step) self.est_time_step = np.float64(est_time_step) @@ -34,28 +34,28 @@ def __init__(self, FULL_SCALE_CONFIG = SimulationConfig( - raw_time_step=0.001, - est_time_step=0.002, + raw_time_step=np.float64(0.001), + est_time_step=np.float64(0.002), motor="AeroTech_L1940X", - drag_coefficient=0.4, - rocket_mass=14.5, - reference_area=0.01929, - rocket_orientation=[0, 0, -1], - launch_rod_angle=[10], - launch_rod_direction=[0] + drag_coefficient=np.float64(0.4), + rocket_mass=np.float64(14.5), + reference_area=np.float64(0.01929), + rocket_orientation=np.array([0, 0, -1]), + launch_rod_angle=np.array([10]), + launch_rod_direction=np.array([0]) ) # TODO: get actual values for sub scale SUB_SCALE_CONFIG = SimulationConfig( - raw_time_step=0.001, - est_time_step=0.002, + raw_time_step=np.float64(0.001), + est_time_step=np.float64(0.002), motor="PLACEHOLDER", - drag_coefficient=0.4, - rocket_mass=0, - reference_area=0.001929, - rocket_orientation=[0, 0, -1], - launch_rod_angle=[10], - launch_rod_direction=[0] + drag_coefficient=np.float64(0.4), + rocket_mass=np.float64(0), + reference_area=np.float64(0.001929), + rocket_orientation=np.array([0, 0, -1]), + launch_rod_angle=np.array([10]), + launch_rod_direction=np.array([0]) ) From db0e6edad6d450ce438f3ed2ceb0454d2f6cc437 Mon Sep 17 00:00:00 2001 From: jgelia Date: Sat, 9 Nov 2024 22:43:20 -0500 Subject: [PATCH 48/63] Fixed everything --- airbrakes/simulation/sim_config.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 7cc227b9..7a8ddf1a 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -14,23 +14,23 @@ def __init__(self, launch_rod_angle: npt.NDArray[np.float64], launch_rod_direction: npt.NDArray[np.float64]): # Time steps for data packet generation in the simulation - self.raw_time_step = np.float64(raw_time_step) - self.est_time_step = np.float64(est_time_step) + self.raw_time_step = raw_time_step + self.est_time_step = est_time_step # Motor selection (name of CSV file without extension) self.motor = motor # Rocket properties - self.drag_coefficient = np.float64(drag_coefficient) - self.rocket_mass = np.float64(rocket_mass) - self.reference_area = np.float64(reference_area) + self.drag_coefficient = drag_coefficient + self.rocket_mass = rocket_mass + self.reference_area = reference_area # Rocket orientation on the launch pad - self.rocket_orientation = np.asarray(rocket_orientation, dtype=np.float64) + self.rocket_orientation = rocket_orientation # Config for randomness in the simulation - self.launch_rod_angle = np.float64(launch_rod_angle) - self.launch_rod_direction = np.float64(launch_rod_direction) + self.launch_rod_angle = launch_rod_angle + self.launch_rod_direction = launch_rod_direction FULL_SCALE_CONFIG = SimulationConfig( From dc98b7a0734a913cceb2882503d2f8296824786d Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sat, 9 Nov 2024 23:26:42 -0500 Subject: [PATCH 49/63] rotation almost fixed --- airbrakes/simulation/README.md | 7 +++++ airbrakes/simulation/data_generator.py | 8 ++++- airbrakes/simulation/rotation_manager.py | 38 ++++++++++++++--------- airbrakes/simulation/sim_config.py | 39 +++++++++++++++--------- airbrakes/simulation/sim_imu.py | 5 +-- 5 files changed, 64 insertions(+), 33 deletions(-) create mode 100644 airbrakes/simulation/README.md diff --git a/airbrakes/simulation/README.md b/airbrakes/simulation/README.md new file mode 100644 index 00000000..dd5e0b4c --- /dev/null +++ b/airbrakes/simulation/README.md @@ -0,0 +1,7 @@ +## Rotation Guide for sim IMU + +Vertical orientation of the IMU is described in vector form. +- Example: If the -x axis of the IMU is facing the sky, the vertical orientation is [-1, 0, 0] + +Direction the rocket leans to on the pad is the launch_pad_direction. Ranges from 0 to 360 degrees. +Important to note that the IMU does not change position inside of the rocket when changing launch pad angle. \ No newline at end of file diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index b992796b..8e3457e9 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -318,6 +318,9 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: quaternion = self._est_rotation_manager.calculate_imu_quaternions() + # print(f"comp: {np.round(compensated_accel,3)}") + # print(f"vels: {np.round(vert_velocity,3)}") + packet = EstimatedDataPacket( next_timestamp * 1e9, estOrientQuaternionW=quaternion[0], @@ -381,12 +384,15 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: """ # calculate the magnitude of velocity speed = np.linalg.norm(velocities) + _ = speed # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 thrust_force = 0.0 - drag_force = 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * speed**2 + drag_force = ( + 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * 1**2 + ) # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 0d9cc2c1..f37d490d 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -61,14 +61,23 @@ def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) # Step 1: We have to declare that the orientation of the rocket is vertical aligned_orientation = R.align_vectors([self._vertical], [[0, 0, 1]])[0] - # Step 2: Tilt by the zenith angle to adjust pitch away from vertical - tilt_rotation = R.from_rotvec(self._zenith * np.array([1, 0, 0])) + # Step 2: Get the axis that the zenith rotates around, it changes depending on orientation. + zenith_rotation_axis = np.array( + [ + np.abs(self._vertical[2]), + np.abs(self._vertical[0]), + np.abs(self._vertical[1]), + ] + ) + + # Step 3: rotate by zenith + zenith_rotation = R.from_rotvec(self._zenith * zenith_rotation_axis) - # Step 3: Rotate around the vertical by azimuth to set the compass direction of the tilt - azimuth_rotation = R.from_rotvec(self._azimuth * np.array([0, 0, -1])) + # Step 4: rotate by azimuth + azimuth_rotation = R.from_rotvec(self._azimuth * np.array(self._vertical)) - # Step 4: Combined rotation: aligned orientation, followed by azimuth and zenith rotations - orientation = azimuth_rotation * tilt_rotation * aligned_orientation + # Step 5: Combined rotation: aligned orientation, azimuth rotation, and zenith rotation + orientation = azimuth_rotation * zenith_rotation * aligned_orientation # updating last orientation self._last_orientation = ( @@ -91,13 +100,12 @@ def calculate_compensated_accel( # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket # is still on the ground. We have to include the acceleration due to the normal force of # the ground. - compensated_accel = self._scalar_to_vector(thrust_drag_accel) - if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: - normal_force_vector = np.array([0, 0, GRAVITY]) - rotated_normal_vector = self._orientation.apply(normal_force_vector) + # if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: + # normal_force_vector = np.array([0, 0, GRAVITY]) + # rotated_normal_vector = self._orientation.apply(normal_force_vector) - compensated_accel += rotated_normal_vector - return compensated_accel + # compensated_accel += rotated_normal_vector + return self._scalar_to_vector(thrust_drag_accel) def calculate_linear_accel( self, thrust_acceleration: np.float64, drag_acceleration: np.float64 @@ -157,11 +165,11 @@ def calculate_imu_quaternions(self) -> npt.NDArray: :return: A quaternion adjusted to the IMU's frame. """ # rotation needed to align the IMU - imu_alignment_rotation = R.align_vectors([[0, 0, 1]], [[0, 0, -1]])[0] - imu_adjusted_orientation = imu_alignment_rotation * self._orientation + imu_alignment = R.align_vectors([[0, 0, 1]], [[0, 0, -1]])[0] + imu_quaternion_rotation = imu_alignment * self._orientation.inv() # convert to quaternion - return imu_adjusted_orientation.as_quat(scalar_first=True) + return imu_quaternion_rotation.as_quat(scalar_first=True) def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: """ diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 7a8ddf1a..b414f45f 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -1,18 +1,26 @@ +"""Module containing config settings for simulation""" + import numpy as np import numpy.typing as npt class SimulationConfig: - def __init__(self, - raw_time_step: np.float64, - est_time_step: np.float64, - motor: str, - drag_coefficient: np.float64, - rocket_mass: np.float64, - reference_area: np.float64, - rocket_orientation: npt.NDArray[np.float64], - launch_rod_angle: npt.NDArray[np.float64], - launch_rod_direction: npt.NDArray[np.float64]): + """ + config class for simulation + """ + + def __init__( + self, + raw_time_step: np.float64, + est_time_step: np.float64, + motor: str, + drag_coefficient: np.float64, + rocket_mass: np.float64, + reference_area: np.float64, + rocket_orientation: npt.NDArray[np.float64], + launch_rod_angle: npt.NDArray[np.float64], + launch_rod_direction: npt.NDArray[np.float64], + ): # Time steps for data packet generation in the simulation self.raw_time_step = raw_time_step self.est_time_step = est_time_step @@ -40,9 +48,9 @@ def __init__(self, drag_coefficient=np.float64(0.4), rocket_mass=np.float64(14.5), reference_area=np.float64(0.01929), - rocket_orientation=np.array([0, 0, -1]), + rocket_orientation=np.array([0, 0, 1]), launch_rod_angle=np.array([10]), - launch_rod_direction=np.array([0]) + launch_rod_direction=np.array([90]), ) # TODO: get actual values for sub scale @@ -53,16 +61,17 @@ def __init__(self, drag_coefficient=np.float64(0.4), rocket_mass=np.float64(0), reference_area=np.float64(0.001929), - rocket_orientation=np.array([0, 0, -1]), + rocket_orientation=np.array([-1, 0, 0]), launch_rod_angle=np.array([10]), - launch_rod_direction=np.array([0]) + launch_rod_direction=np.array([90]), ) def get_configuration(config_type: str) -> SimulationConfig: """ Gets the configuration for the simulation - :param config_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". + :param config_type: The type of simulation to run. This can be either "full-scale" + or "sub-scale". :return: The configuration for the simulation """ match config_type: diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 5ce32ab5..aa3c4596 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -7,7 +7,7 @@ import numpy as np -from airbrakes.simulation.sim_config import get_configuration, SimulationConfig +from airbrakes.simulation.sim_config import SimulationConfig, get_configuration if TYPE_CHECKING: from airbrakes.data_handling.imu_data_packet import IMUDataPacket @@ -27,7 +27,8 @@ def __init__(self, sim_type: str) -> None: """ Initializes the object that pretends to be an IMU for testing purposes by returning randomly generated data. - :param sim_type: The type of simulation to run. This can be either "full-scale" or "sub-scale". + :param sim_type: The type of simulation to run. This can be either "full-scale" or + "sub-scale". """ # Gets the configuration for the simulation config = get_configuration(sim_type) From a97643abecb522c5adebe407ac891248dc0f3d80 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 13:55:55 -0500 Subject: [PATCH 50/63] working --- airbrakes/simulation/data_generator.py | 9 ++++----- airbrakes/simulation/rotation_manager.py | 11 ++++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 8e3457e9..d4284bb2 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -384,18 +384,17 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: """ # calculate the magnitude of velocity speed = np.linalg.norm(velocities) - _ = speed # we could probably actually calculate air density, maybe we set temperature as constant? air_density = 1.1 + drag_coefficient = self._config.drag_coefficient thrust_force = 0.0 - drag_force = ( - 0.5 * air_density * self._config.reference_area * self._config.drag_coefficient * 1**2 - ) - # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn if timestamp <= self._thrust_data[0][-1]: thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) + + drag_force = 0.5 * air_density * self._config.reference_area * drag_coefficient * speed**2 + return np.array([thrust_force, drag_force]) diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index f37d490d..d1bb4925 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -97,15 +97,16 @@ def calculate_compensated_accel( :return: the compensated acceleration in the vehicle frame of reference """ thrust_drag_accel = thrust_acceleration - drag_acceleration + compensated_accel = self._scalar_to_vector(thrust_drag_accel) # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket # is still on the ground. We have to include the acceleration due to the normal force of # the ground. - # if thrust_drag_accel <= GRAVITY and thrust_drag_accel != 0.0: - # normal_force_vector = np.array([0, 0, GRAVITY]) - # rotated_normal_vector = self._orientation.apply(normal_force_vector) + if thrust_drag_accel <= GRAVITY and thrust_acceleration != 0.0: + normal_force_vector = np.array([0, 0, GRAVITY]) + rotated_normal_vector = self._orientation.apply(normal_force_vector) - # compensated_accel += rotated_normal_vector - return self._scalar_to_vector(thrust_drag_accel) + compensated_accel += rotated_normal_vector + return compensated_accel def calculate_linear_accel( self, thrust_acceleration: np.float64, drag_acceleration: np.float64 From b7e2789b998f5ddbef8bb0f0c0363ae54e822230 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 18:15:19 -0500 Subject: [PATCH 51/63] added variable mass --- airbrakes/simulation/data_generator.py | 48 ++++++++++++++----- airbrakes/simulation/sim_config.py | 4 +- .../thrust_curves/AeroTech_L1940X.csv | 2 +- 3 files changed, 39 insertions(+), 15 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index d4284bb2..8c7b83b1 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -25,6 +25,7 @@ class DataGenerator: __slots__ = ( "_config", + "_eff_exhaust_velocity", "_est_rotation_manager", "_last_est_packet", "_last_raw_packet", @@ -32,7 +33,6 @@ class DataGenerator: "_max_velocity", "_raw_rotation_manager", "_thrust_data", - "_vertical_index", ) def __init__(self, config: SimulationConfig): @@ -47,6 +47,7 @@ def __init__(self, config: SimulationConfig): self._last_raw_packet: RawDataPacket | None = None self._last_velocities: npt.NDArray = np.array([0.0, 0.0, 0.0]) self._max_velocity: np.float64 = np.float64(0.1) + self._eff_exhaust_velocity: np.float64 | None = None # loads thrust curve self._thrust_data: npt.NDArray = self._load_thrust_curve() @@ -56,13 +57,9 @@ def __init__(self, config: SimulationConfig): self._raw_rotation_manager: RotationManager = raw_manager self._est_rotation_manager: RotationManager = est_manager - # finds the vertical index of the orientation. For example, if -y is vertical, the index - # will be 1. - self._vertical_index: np.int64 = np.nonzero(self._config.rocket_orientation)[0][0] - @property def velocities(self) -> npt.NDArray: - """Returns the last calculated velocity of the rocket""" + """Returns the last calculated velocities of the rocket""" return self._last_velocities def _get_random(self, identifier: str) -> np.float64: @@ -99,9 +96,15 @@ def _load_thrust_curve(self) -> npt.NDArray: start_flag = False # flag to identify when the metadata/header rows are skipped with csv_path.open(mode="r", newline="") as thrust_csv: + propellant_weight = None reader = csv.reader(thrust_csv) for row in reader: + # We want to store the propellant weight + if row[0] == "propellant weight:": + propellant_weight = float(row[1]) / 1000 # we want in kg + continue + # Start appending data only after the header row if row == ["Time (s)", "Thrust (N)"]: start_flag = True @@ -114,6 +117,11 @@ def _load_thrust_curve(self) -> npt.NDArray: motor_timestamps.append(time) motor_thrusts.append(thrust) + # for future mass calculations, finding total impulse + total_impulse = np.trapezoid(motor_thrusts, motor_timestamps) + # calculating effective exhaust velocity (constant) + self._eff_exhaust_velocity = total_impulse / propellant_weight + return np.array([motor_timestamps, motor_thrusts]) def _get_rotation_managers(self) -> npt.NDArray: @@ -229,7 +237,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: # calculates the forces and vertical scaled acceleration forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config.rocket_mass + force_accelerations = forces / self._calculate_mass(next_timestamp) compensated_accel = self._raw_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -299,7 +307,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: # calculates the forces and accelerations forces = self._calculate_forces(next_timestamp, self._last_velocities) - force_accelerations = forces / self._config.rocket_mass + force_accelerations = forces / self._calculate_mass(next_timestamp) compensated_accel = self._est_rotation_manager.calculate_compensated_accel( force_accelerations[0], force_accelerations[1], @@ -318,9 +326,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: quaternion = self._est_rotation_manager.calculate_imu_quaternions() - # print(f"comp: {np.round(compensated_accel,3)}") - # print(f"vels: {np.round(vert_velocity,3)}") - packet = EstimatedDataPacket( next_timestamp * 1e9, estOrientQuaternionW=quaternion[0], @@ -386,7 +391,7 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: speed = np.linalg.norm(velocities) # we could probably actually calculate air density, maybe we set temperature as constant? - air_density = 1.1 + air_density = 1.2 drag_coefficient = self._config.drag_coefficient thrust_force = 0.0 @@ -398,3 +403,22 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: drag_force = 0.5 * air_density * self._config.reference_area * drag_coefficient * speed**2 return np.array([thrust_force, drag_force]) + + def _calculate_mass(self, timestamp: np.float64) -> np.float64: + """ + Calculates the weight of the rocket at any given timestamp. The weight loss is found + by calculating the mass flow rate using effective exhaust velocity. + :param timestamp: current timestamp of the rocket, in seconds + :return: the current mass of the rocket, in kilograms + """ + # find current thrust + current_thrust = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) + # getting thrust curve up to current timestamp + mask = self._thrust_data[0] <= timestamp + time_values = np.append(self._thrust_data[0][mask], [timestamp]) + thrust_values = np.append(self._thrust_data[1][mask], [current_thrust]) + # Calculate the total impulse up to this timestamp as the integral of thrust over time + current_total_impulse = np.trapezoid(thrust_values, time_values) + + # mass lost is current total impulse divided by effective exhaust velocity + return self._config.rocket_mass - current_total_impulse / self._eff_exhaust_velocity diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index b414f45f..51df4df3 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -30,7 +30,7 @@ def __init__( # Rocket properties self.drag_coefficient = drag_coefficient - self.rocket_mass = rocket_mass + self.rocket_mass = rocket_mass # This is wetted mass (including propellant weight) self.reference_area = reference_area # Rocket orientation on the launch pad @@ -46,7 +46,7 @@ def __init__( est_time_step=np.float64(0.002), motor="AeroTech_L1940X", drag_coefficient=np.float64(0.4), - rocket_mass=np.float64(14.5), + rocket_mass=np.float64(15.856), reference_area=np.float64(0.01929), rocket_orientation=np.array([0, 0, 1]), launch_rod_angle=np.array([10]), diff --git a/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv index 1328b5b1..3be8dcac 100644 --- a/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv +++ b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv @@ -1,7 +1,7 @@ "motor:","AeroTech L1940X" "contributor:","Mike Caplinger" "details:","https://www.thrustcurve.org/simfiles/60ac84458dc4640004c24eb6/" - +"propellant weight:",1825 "Time (s)","Thrust (N)" 0.02,48.137 0.04,2107.333 From b5596d0cda6fa826931ec5abbd62c1f7f408d1d1 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 19:30:49 -0500 Subject: [PATCH 52/63] atmospheric properties and variable drag --- airbrakes/simulation/data_generator.py | 25 ++++++++++++++++++++++--- airbrakes/simulation/sim_config.py | 16 +++++++++++----- 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 8c7b83b1..d59b76bd 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -387,13 +387,32 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: :return: a 2 element numpy array containing thrust force and drag force, respectively. Thrust is positive, drag is negative. """ + gradient = -6.5e-3 # troposphere gradient + gas_constant = 287 # J/kg*K + ratio_spec_heat = 1.4 + + # last altitude + altitude = self._last_est_packet.estPressureAlt # calculate the magnitude of velocity speed = np.linalg.norm(velocities) + # temperature using troposphere gradient + temp = self._config.air_temperature + 273.15 + gradient * altitude + + # air density formula for troposphere gradient + air_density = 1.225 * (temp / (self._config.air_temperature + 273.15)) ** ( + -GRAVITY / (gas_constant * gradient) - 1 + ) - # we could probably actually calculate air density, maybe we set temperature as constant? - air_density = 1.2 + # using speed of sound to find mach number + mach_number = speed / np.sqrt(ratio_spec_heat * gas_constant * temp) + + # getting the drag coefficient + drag_coefficient = np.interp( + mach_number, + self._config.drag_coefficient[0], + self._config.drag_coefficient[1], + ) - drag_coefficient = self._config.drag_coefficient thrust_force = 0.0 # thrust force is non-zero if the timestamp is within the timeframe of # the motor burn diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 51df4df3..3ad960dc 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -14,12 +14,13 @@ def __init__( raw_time_step: np.float64, est_time_step: np.float64, motor: str, - drag_coefficient: np.float64, + drag_coefficient: npt.NDArray, rocket_mass: np.float64, reference_area: np.float64, rocket_orientation: npt.NDArray[np.float64], launch_rod_angle: npt.NDArray[np.float64], launch_rod_direction: npt.NDArray[np.float64], + air_temperature: np.float64, ): # Time steps for data packet generation in the simulation self.raw_time_step = raw_time_step @@ -29,10 +30,13 @@ def __init__( self.motor = motor # Rocket properties - self.drag_coefficient = drag_coefficient + self.drag_coefficient = drag_coefficient # coefficient of drag at mach numbers self.rocket_mass = rocket_mass # This is wetted mass (including propellant weight) self.reference_area = reference_area + # Atmospheric properties + self.air_temperature = air_temperature # ground temperature, in celcius + # Rocket orientation on the launch pad self.rocket_orientation = rocket_orientation @@ -41,13 +45,14 @@ def __init__( self.launch_rod_direction = launch_rod_direction -FULL_SCALE_CONFIG = SimulationConfig( +FULL_SCALE_CONFIG = SimulationConfig( # 2018.99 raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), motor="AeroTech_L1940X", - drag_coefficient=np.float64(0.4), + drag_coefficient=np.array([[0.1, 0.3, 0.5, 0.7], [0.3565, 0.3666, 0.3871, 0.41499]]), rocket_mass=np.float64(15.856), reference_area=np.float64(0.01929), + air_temperature=np.float64(25), rocket_orientation=np.array([0, 0, 1]), launch_rod_angle=np.array([10]), launch_rod_direction=np.array([90]), @@ -58,9 +63,10 @@ def __init__( raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), motor="PLACEHOLDER", - drag_coefficient=np.float64(0.4), + drag_coefficient=np.array([[0.3], [0.4]]), rocket_mass=np.float64(0), reference_area=np.float64(0.001929), + air_temperature=np.float64(25), rocket_orientation=np.array([-1, 0, 0]), launch_rod_angle=np.array([10]), launch_rod_direction=np.array([90]), From d0560f762254c821c50a5f96847943774d51fa78 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Sun, 10 Nov 2024 22:24:11 -0500 Subject: [PATCH 53/63] might undo later --- airbrakes/simulation/data_generator.py | 5 ++-- airbrakes/simulation/rotation_manager.py | 31 ++++++++++++++++++------ airbrakes/simulation/sim_config.py | 2 +- 3 files changed, 27 insertions(+), 11 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index d59b76bd..6b300bb7 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -230,7 +230,7 @@ def generate_raw_data_packet(self) -> RawDataPacket: and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity - self._raw_rotation_manager.update_orientation(time_step, velocity_ratio) + self._raw_rotation_manager.update_orientation(velocity_ratio) # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_raw_packet.timestamp / 1e9) + time_step @@ -300,7 +300,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: and self._last_est_packet.timestamp > 1e9 ): velocity_ratio = self._last_velocities[2] / self._max_velocity - self._est_rotation_manager.update_orientation(time_step, velocity_ratio) + self._est_rotation_manager.update_orientation(velocity_ratio) # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_est_packet.timestamp / 1e9) + time_step @@ -312,6 +312,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[0], force_accelerations[1], ) + compensated_accel += random.uniform(-4, 4) linear_accel = self._est_rotation_manager.calculate_linear_accel( force_accelerations[0], force_accelerations[1], diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index d1bb4925..6f0de7ca 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -16,6 +16,7 @@ class RotationManager: __slots__ = ( "_azimuth", + "_formula_consts", "_last_orientation", "_orientation", "_vertical", @@ -36,25 +37,39 @@ def __init__( self._azimuth: np.float64 = np.float64((rod_direction * np.pi) / 180) self._last_orientation: R | None = None self._orientation: R | None = None - self.update_orientation(0, 0) + self._formula_consts: npt.NDArray = self._initialize_rotation_formula() + self.update_orientation(1) @property def gravity_vector(self) -> npt.NDArray: return self._orientation.apply([0, 0, -GRAVITY]) - def update_orientation(self, time_step: np.float64, velocity_ratio: np.float64) -> None: + def _initialize_rotation_formula(self) -> np.float64: + """ + Initializes the formula for rotation. Returns the shift of the function. Is constant + for the rest of the flight. + :return: the shift and initial angle of the function. + """ + # initializes vector with small step values, the shift is always below 0.1 + # for inital rod angles below 45 degrees + dx = 0.00001 + xvec = np.arange(dx, 0.1, dx) + # this graph is zenith versus v/vmax + no_shift = (-xvec + 1) / (15 * xvec) + self._zenith + # finding the index where no_shift is closest to pi/2 + closest_index = np.argmin(np.abs(no_shift - np.pi / 2)) + return np.array([closest_index * dx, self._zenith]) # returns the shift and zenith + + def update_orientation(self, velocity_ratio: np.float64) -> None: """ Updates the baseline vehicle-frame orientation, and all of the vehicle-frame orientations - :param time_step: how much time has passed between updates :param velocity_ratio: current vertical velocity of the rocket divided by max velocity """ - if velocity_ratio == 0.0: - scale_factor = 0.0 - else: - scale_factor = np.sin(self._zenith) * time_step / abs(velocity_ratio * 10) - self._zenith = self._zenith + scale_factor * (np.pi / 2 - self._zenith) + # splitting up the function into two parts + devisor = 15 * (self._formula_consts[0] + 1) * (velocity_ratio + self._formula_consts[0]) + self._zenith = (-velocity_ratio + 1) / devisor + self._formula_consts[1] # We want to convert the azimuth and zenith values into a scipy rotation object diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 3ad960dc..afb050cc 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -53,7 +53,7 @@ def __init__( rocket_mass=np.float64(15.856), reference_area=np.float64(0.01929), air_temperature=np.float64(25), - rocket_orientation=np.array([0, 0, 1]), + rocket_orientation=np.array([0, 0, -1]), launch_rod_angle=np.array([10]), launch_rod_direction=np.array([90]), ) From 145b306cb4789c8ea5cd33b3a50458738b0560ec Mon Sep 17 00:00:00 2001 From: jgelia Date: Mon, 11 Nov 2024 01:22:03 -0500 Subject: [PATCH 54/63] Excluded simulation code from coverage report --- pyproject.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index fdda2ac9..e15dd941 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -61,7 +61,8 @@ source = ["airbrakes"] omit = [ "*/tests/*", "*/site-packages/*", - "airbrakes/mock/*" + "airbrakes/mock/*", + "airbrakes/simulation/*", ] [tool.coverage.report] From d6cbabfcf07285f869dd7608e7fb4687afb5c0fe Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Mon, 11 Nov 2024 19:56:51 -0500 Subject: [PATCH 55/63] subscale values --- airbrakes/simulation/data_generator.py | 1 - airbrakes/simulation/rotation_manager.py | 4 ++-- airbrakes/simulation/sim_config.py | 17 +++++++-------- .../thrust_curves/AeroTech_J500G.csv | 21 +++++++++++++++++++ 4 files changed, 31 insertions(+), 12 deletions(-) create mode 100644 airbrakes/simulation/thrust_curves/AeroTech_J500G.csv diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 6b300bb7..01067965 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -312,7 +312,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[0], force_accelerations[1], ) - compensated_accel += random.uniform(-4, 4) linear_accel = self._est_rotation_manager.calculate_linear_accel( force_accelerations[0], force_accelerations[1], diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 6f0de7ca..99f7648a 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -116,7 +116,7 @@ def calculate_compensated_accel( # if the thrust is non-zero, and this returns a value smaller than gravity, than the rocket # is still on the ground. We have to include the acceleration due to the normal force of # the ground. - if thrust_drag_accel <= GRAVITY and thrust_acceleration != 0.0: + if thrust_drag_accel <= GRAVITY and thrust_acceleration != 0.0 and drag_acceleration < 1: normal_force_vector = np.array([0, 0, GRAVITY]) rotated_normal_vector = self._orientation.apply(normal_force_vector) @@ -136,7 +136,7 @@ def calculate_linear_accel( """ comp_accel = self.calculate_compensated_accel(thrust_acceleration, drag_acceleration) # apply gravity - gravity_accel_vector = self._orientation.apply([0, 0, -GRAVITY]) + gravity_accel_vector = self._orientation.apply([0, 0, GRAVITY]) return np.array(comp_accel - gravity_accel_vector) def calculate_delta_theta(self) -> npt.NDArray: diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index afb050cc..f9c24a87 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -54,22 +54,21 @@ def __init__( reference_area=np.float64(0.01929), air_temperature=np.float64(25), rocket_orientation=np.array([0, 0, -1]), - launch_rod_angle=np.array([10]), + launch_rod_angle=np.array([5]), launch_rod_direction=np.array([90]), ) -# TODO: get actual values for sub scale SUB_SCALE_CONFIG = SimulationConfig( raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), - motor="PLACEHOLDER", - drag_coefficient=np.array([[0.3], [0.4]]), - rocket_mass=np.float64(0), - reference_area=np.float64(0.001929), - air_temperature=np.float64(25), + motor="AeroTech_J500G", + drag_coefficient=np.array([[0.05, 0.2, 0.3], [0.41, 0.4175, 0.425]]), + rocket_mass=np.float64(5.954), + reference_area=np.float64(0.008205), + air_temperature=np.float64(15), rocket_orientation=np.array([-1, 0, 0]), - launch_rod_angle=np.array([10]), - launch_rod_direction=np.array([90]), + launch_rod_angle=np.array([5]), + launch_rod_direction=np.array([0]), ) diff --git a/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv b/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv new file mode 100644 index 00000000..3d151b3b --- /dev/null +++ b/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv @@ -0,0 +1,21 @@ +"motor:","AeroTech L1940X" +"contributor:","Will Sanderson" +"details:","https://www.thrustcurve.org/motors/AeroTech/J500G/" +"propellant weight:",363 +"Time (s)","Thrust (N)" +0.0134378, 40.2458 +0.0335946, 724.425 +0.0403135, 781.616 +0.0604703, 787.971 +0.0895857, 711.716 +0.134378, 686.297 +0.394177, 637.578 +0.575588, 588.86 +0.606943, 622.751 +0.633819, 620.633 +1.20045, 360.094 +1.24076, 345.267 +1.31019, 182.165 +1.38186, 65.6642 +1.43337, 23.3002 +1.45, 0 \ No newline at end of file From 24bc98de2e4e9026ade0a1d93a9301ff5bd1537e Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Wed, 13 Nov 2024 21:36:07 -0500 Subject: [PATCH 56/63] drag changes --- airbrakes/simulation/data_generator.py | 6 +++++- airbrakes/simulation/sim_config.py | 7 ++++--- airbrakes/simulation/sim_imu.py | 4 ++-- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 01067965..635ff01e 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -294,12 +294,12 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: return self._last_raw_packet # updates the estimated rotation manager, if we are after motor burn phase + velocity_ratio = self._last_velocities[2] / self._max_velocity if ( self._last_velocities[2] < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD and self._last_est_packet.timestamp > 1e9 ): - velocity_ratio = self._last_velocities[2] / self._max_velocity self._est_rotation_manager.update_orientation(velocity_ratio) # calculates the timestamp for this packet (in seconds) @@ -312,6 +312,10 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[0], force_accelerations[1], ) + # adding randomness to compensated acceleration + comp_rand = (0.3 + velocity_ratio * 2) * random.uniform(-1, 1) + compensated_accel += comp_rand + linear_accel = self._est_rotation_manager.calculate_linear_accel( force_accelerations[0], force_accelerations[1], diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index f9c24a87..27131dcb 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -45,12 +45,13 @@ def __init__( self.launch_rod_direction = launch_rod_direction -FULL_SCALE_CONFIG = SimulationConfig( # 2018.99 +FULL_SCALE_CONFIG = SimulationConfig( raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), motor="AeroTech_L1940X", - drag_coefficient=np.array([[0.1, 0.3, 0.5, 0.7], [0.3565, 0.3666, 0.3871, 0.41499]]), - rocket_mass=np.float64(15.856), + # drag_coefficient=np.array([[0.1, 0.3, 0.5, 0.7], [0.3565, 0.3666, 0.3871, 0.41499]]), + drag_coefficient=np.array([[0.1, 1], [0.29, 0.29]]), + rocket_mass=np.float64(17.6), reference_area=np.float64(0.01929), air_temperature=np.float64(25), rocket_orientation=np.array([0, 0, -1]), diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index aa3c4596..1d62ccb9 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -33,8 +33,8 @@ def __init__(self, sim_type: str) -> None: # Gets the configuration for the simulation config = get_configuration(sim_type) - # This limits the queue size to a very high limit, because the data generator will - # generate all the data before the imu reads it + # This limits the queue size to a very high limit + # TODO: should be smaller limit self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue( MAX_QUEUE_SIZE ) From 79c6f6dcb0b341a43d14d87d9839a9d09287e828 Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 14 Nov 2024 14:48:14 -0500 Subject: [PATCH 57/63] my changes before merge --- airbrakes/simulation/sim_imu.py | 12 ++++++++++-- main.py | 4 ++++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 1d62ccb9..cf6fa45c 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -14,7 +14,7 @@ from airbrakes.hardware.imu import IMU from airbrakes.simulation.data_generator import DataGenerator -from constants import MAX_QUEUE_SIZE +from constants import MAX_QUEUE_SIZE, ServoExtension class SimIMU(IMU): @@ -46,8 +46,9 @@ def __init__(self, sim_type: str) -> None: args=(config,), ) - # Makes a boolean value that is shared between processes + # Makes boolean values that are shared between processes self._running = multiprocessing.Value("b", False) + self._airbrakes_extended = multiprocessing.Value("b", False) @staticmethod def _update_timestamp(current_timestamp: np.float64, config: SimulationConfig) -> np.float64: @@ -86,6 +87,13 @@ def _update_timestamp(current_timestamp: np.float64, config: SimulationConfig) - # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") + # def set_airbrakes_status(self, servo_extension: ServoExtension) -> None: + # """ + # Sets the value of the shared boolean that indicates whether the airbrakes are extended. + # :param extended: The value to set the boolean to. + # """ + # self._airbrakes_extended.value = servo_extension == ServoExtension.MAX_EXTENSION or servo_extension == ServoExtension. + def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated data.""" diff --git a/main.py b/main.py index 5e6286f8..f9d9a130 100644 --- a/main.py +++ b/main.py @@ -95,6 +95,10 @@ def main(args: argparse.Namespace) -> None: # Update the airbrakes finite state machine airbrakes.update() + # if args.sim: + # imu: SimIMU + # imu.set_airbrakes_status(airbrakes.airbrakes_extended.value) + # Stop the sim when the data is exhausted: if args.mock and not airbrakes.imu._data_fetch_process.is_alive(): break From a150dcd1694739d3273f337c0f111e4e78daa125 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Thu, 14 Nov 2024 15:03:00 -0500 Subject: [PATCH 58/63] logger fix --- airbrakes/data_handling/logger.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/airbrakes/data_handling/logger.py b/airbrakes/data_handling/logger.py index 27703490..ce53fff9 100644 --- a/airbrakes/data_handling/logger.py +++ b/airbrakes/data_handling/logger.py @@ -197,9 +197,12 @@ def _prepare_log_dict( "state": state, "extension": extension, } + # Convert the imu data packet to a dictionary # Using to_builtins() is much faster than asdict() for some reason - imu_data_packet_dict: dict[str, int | float | list[str]] = to_builtins(imu_data_packet) + imu_data_packet_dict: dict[str, int | float | list[str]] = to_builtins( + imu_data_packet, enc_hook=self._convert_unknown_type + ) logged_fields.update(imu_data_packet_dict) # Get the processed data packet fields: From 6a35c4264b66726c19330dc9107eaeac87d9e454 Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 14 Nov 2024 15:23:54 -0500 Subject: [PATCH 59/63] Added differing cds to the sim --- airbrakes/simulation/data_generator.py | 18 ++++++++++++++++-- airbrakes/simulation/sim_config.py | 12 ++++++++---- airbrakes/simulation/sim_imu.py | 18 ++++++++++++------ constants.py | 1 - main.py | 9 ++++++--- 5 files changed, 42 insertions(+), 16 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 635ff01e..c4541b9b 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -33,6 +33,7 @@ class DataGenerator: "_max_velocity", "_raw_rotation_manager", "_thrust_data", + "is_airbrakes_extended", ) def __init__(self, config: SimulationConfig): @@ -57,6 +58,8 @@ def __init__(self, config: SimulationConfig): self._raw_rotation_manager: RotationManager = raw_manager self._est_rotation_manager: RotationManager = est_manager + self.is_airbrakes_extended = False + @property def velocities(self) -> npt.NDArray: """Returns the last calculated velocities of the rocket""" @@ -410,11 +413,22 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: # using speed of sound to find mach number mach_number = speed / np.sqrt(ratio_spec_heat * gas_constant * temp) + mach_numbers = ( + self._config.airbrakes_retracted_cd[0] + if not self.is_airbrakes_extended + else self._config.airbrakes_extended_cd[0] + ) + drag_coefficients = ( + self._config.airbrakes_retracted_cd[1] + if not self.is_airbrakes_extended + else self._config.airbrakes_extended_cd[1] + ) + # getting the drag coefficient drag_coefficient = np.interp( mach_number, - self._config.drag_coefficient[0], - self._config.drag_coefficient[1], + mach_numbers, + drag_coefficients, ) thrust_force = 0.0 diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 27131dcb..4d65bf2d 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -14,7 +14,8 @@ def __init__( raw_time_step: np.float64, est_time_step: np.float64, motor: str, - drag_coefficient: npt.NDArray, + airbrake_retracted_cd: npt.NDArray, + airbrake_extended_cd: npt.NDArray, rocket_mass: np.float64, reference_area: np.float64, rocket_orientation: npt.NDArray[np.float64], @@ -30,7 +31,8 @@ def __init__( self.motor = motor # Rocket properties - self.drag_coefficient = drag_coefficient # coefficient of drag at mach numbers + self.airbrakes_retracted_cd = airbrake_retracted_cd # coefficient of drag at mach numbers + self.airbrakes_extended_cd = airbrake_extended_cd self.rocket_mass = rocket_mass # This is wetted mass (including propellant weight) self.reference_area = reference_area @@ -50,7 +52,8 @@ def __init__( est_time_step=np.float64(0.002), motor="AeroTech_L1940X", # drag_coefficient=np.array([[0.1, 0.3, 0.5, 0.7], [0.3565, 0.3666, 0.3871, 0.41499]]), - drag_coefficient=np.array([[0.1, 1], [0.29, 0.29]]), + airbrake_retracted_cd=np.array([[0.1, 1], [0.29, 0.29]]), + airbrake_extended_cd=np.array([[0.1, 1], [0.49, 0.49]]), rocket_mass=np.float64(17.6), reference_area=np.float64(0.01929), air_temperature=np.float64(25), @@ -63,7 +66,8 @@ def __init__( raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), motor="AeroTech_J500G", - drag_coefficient=np.array([[0.05, 0.2, 0.3], [0.41, 0.4175, 0.425]]), + airbrake_retracted_cd=np.array([[0.05, 0.2, 0.3], [0.41, 0.4175, 0.425]]), + airbrake_extended_cd=np.array([[0.05, 0.2, 0.3], [0.51, 0.5175, 0.525]]), rocket_mass=np.float64(5.954), reference_area=np.float64(0.008205), air_temperature=np.float64(15), diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index cf6fa45c..0be59a27 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -87,12 +87,16 @@ def _update_timestamp(current_timestamp: np.float64, config: SimulationConfig) - # or estimated time steps, or if there is a rounding/floating point error. raise ValueError("Could not update timestamp, time stamp is invalid") - # def set_airbrakes_status(self, servo_extension: ServoExtension) -> None: - # """ - # Sets the value of the shared boolean that indicates whether the airbrakes are extended. - # :param extended: The value to set the boolean to. - # """ - # self._airbrakes_extended.value = servo_extension == ServoExtension.MAX_EXTENSION or servo_extension == ServoExtension. + def set_airbrakes_status(self, servo_extension: ServoExtension) -> None: + """ + Sets the value of the shared boolean that indicates whether the airbrakes are extended. + :param servo_extension: The extension of the airbrakes servo. + """ + # Sets the shared boolean to True if the servo extension is at max extension or max no buzz + self._airbrakes_extended.value = ( + servo_extension == ServoExtension.MAX_EXTENSION + or servo_extension == ServoExtension.MAX_NO_BUZZ + ) def _fetch_data_loop(self, config: SimulationConfig) -> None: """A wrapper function to suppress KeyboardInterrupt exceptions when obtaining generated @@ -111,6 +115,8 @@ def _fetch_data_loop(self, config: SimulationConfig) -> None: # starts timer start_time = time.time() + data_generator.is_airbrakes_extended = self._airbrakes_extended.value + # if the timestamp is a multiple of the raw time step, generate a raw data packet. if any(np.isclose(timestamp % raw_dt, [0, raw_dt])): self._data_queue.put(data_generator.generate_raw_data_packet()) diff --git a/constants.py b/constants.py index 7b5d65df..1552ec00 100644 --- a/constants.py +++ b/constants.py @@ -22,7 +22,6 @@ class ServoExtension(Enum): straining past the physical bounds of the air brakes. """ - MIN_EXTENSION = -0.685 MAX_EXTENSION = 0.1 MIN_NO_BUZZ = -0.05 diff --git a/main.py b/main.py index f9d9a130..3bbcc243 100644 --- a/main.py +++ b/main.py @@ -95,9 +95,12 @@ def main(args: argparse.Namespace) -> None: # Update the airbrakes finite state machine airbrakes.update() - # if args.sim: - # imu: SimIMU - # imu.set_airbrakes_status(airbrakes.airbrakes_extended.value) + # If we are running a simulation we need to tell the data generator/sim imu what the current + # airbrakes extension is so that it can change the Cd based on that + # It is a bit of a hack, but it is better we do it this way rather than changing the entire + # structure of the program + if args.sim: + imu.set_airbrakes_status(airbrakes.current_extension) # Stop the sim when the data is exhausted: if args.mock and not airbrakes.imu._data_fetch_process.is_alive(): From e2704a443d335bc773d765f8bc9ffbf1be9da969 Mon Sep 17 00:00:00 2001 From: jgelia Date: Thu, 14 Nov 2024 15:26:36 -0500 Subject: [PATCH 60/63] Ruff --- airbrakes/simulation/sim_imu.py | 3 +-- main.py | 8 ++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index 0be59a27..dba56717 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -94,8 +94,7 @@ def set_airbrakes_status(self, servo_extension: ServoExtension) -> None: """ # Sets the shared boolean to True if the servo extension is at max extension or max no buzz self._airbrakes_extended.value = ( - servo_extension == ServoExtension.MAX_EXTENSION - or servo_extension == ServoExtension.MAX_NO_BUZZ + servo_extension in (ServoExtension.MAX_EXTENSION, ServoExtension.MAX_NO_BUZZ) ) def _fetch_data_loop(self, config: SimulationConfig) -> None: diff --git a/main.py b/main.py index 3bbcc243..98ffc348 100644 --- a/main.py +++ b/main.py @@ -95,10 +95,10 @@ def main(args: argparse.Namespace) -> None: # Update the airbrakes finite state machine airbrakes.update() - # If we are running a simulation we need to tell the data generator/sim imu what the current - # airbrakes extension is so that it can change the Cd based on that - # It is a bit of a hack, but it is better we do it this way rather than changing the entire - # structure of the program + # If we are running a simulation we need to tell the data generator/sim imu what + # the current airbrakes extension is so that it can change the Cd based on that + # It is a bit of a hack, but it is better we do it this way rather than changing + # the entire structure of the program if args.sim: imu.set_airbrakes_status(airbrakes.current_extension) From 9ec3abd90e89d3747bedda164aa015644215a66b Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Thu, 14 Nov 2024 16:11:50 -0500 Subject: [PATCH 61/63] airbrakes ref area --- airbrakes/simulation/data_generator.py | 9 ++++++++- airbrakes/simulation/sim_config.py | 4 ++++ airbrakes/simulation/sim_imu.py | 5 +++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index c4541b9b..62d018b2 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -437,7 +437,14 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: if timestamp <= self._thrust_data[0][-1]: thrust_force = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) - drag_force = 0.5 * air_density * self._config.reference_area * drag_coefficient * speed**2 + # reference area based on if airbrakes is extended or not + reference_area = ( + self._config.reference_area + if not self.is_airbrakes_extended + else self._config.airbrakes_reference_area + self._config.reference_area + ) + + drag_force = 0.5 * air_density * reference_area * drag_coefficient * speed**2 return np.array([thrust_force, drag_force]) diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 4d65bf2d..a2402a66 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -22,6 +22,7 @@ def __init__( launch_rod_angle: npt.NDArray[np.float64], launch_rod_direction: npt.NDArray[np.float64], air_temperature: np.float64, + airbrakes_reference_area: np.float64, ): # Time steps for data packet generation in the simulation self.raw_time_step = raw_time_step @@ -35,6 +36,7 @@ def __init__( self.airbrakes_extended_cd = airbrake_extended_cd self.rocket_mass = rocket_mass # This is wetted mass (including propellant weight) self.reference_area = reference_area + self.airbrakes_reference_area = airbrakes_reference_area # Atmospheric properties self.air_temperature = air_temperature # ground temperature, in celcius @@ -56,6 +58,7 @@ def __init__( airbrake_extended_cd=np.array([[0.1, 1], [0.49, 0.49]]), rocket_mass=np.float64(17.6), reference_area=np.float64(0.01929), + airbrakes_reference_area=np.float64(0.01), air_temperature=np.float64(25), rocket_orientation=np.array([0, 0, -1]), launch_rod_angle=np.array([5]), @@ -70,6 +73,7 @@ def __init__( airbrake_extended_cd=np.array([[0.05, 0.2, 0.3], [0.51, 0.5175, 0.525]]), rocket_mass=np.float64(5.954), reference_area=np.float64(0.008205), + airbrakes_reference_area=np.float64(0.01), air_temperature=np.float64(15), rocket_orientation=np.array([-1, 0, 0]), launch_rod_angle=np.array([5]), diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index dba56717..fef465a3 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -93,8 +93,9 @@ def set_airbrakes_status(self, servo_extension: ServoExtension) -> None: :param servo_extension: The extension of the airbrakes servo. """ # Sets the shared boolean to True if the servo extension is at max extension or max no buzz - self._airbrakes_extended.value = ( - servo_extension in (ServoExtension.MAX_EXTENSION, ServoExtension.MAX_NO_BUZZ) + self._airbrakes_extended.value = servo_extension in ( + ServoExtension.MAX_EXTENSION, + ServoExtension.MAX_NO_BUZZ, ) def _fetch_data_loop(self, config: SimulationConfig) -> None: From 0c28f2cf4f88e416090b1a955c141e1e2d091829 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Fri, 15 Nov 2024 15:34:28 -0500 Subject: [PATCH 62/63] updated readme, standardized variable names --- airbrakes/simulation/README.md | 33 ++++- airbrakes/simulation/data_generator.py | 116 +++++++++--------- airbrakes/simulation/rotation_manager.py | 84 +++++++------ airbrakes/simulation/sim_config.py | 30 ++--- airbrakes/simulation/sim_imu.py | 2 +- .../thrust_curves/AeroTech_J500G.csv | 2 +- .../thrust_curves/AeroTech_L1940X.csv | 2 +- 7 files changed, 150 insertions(+), 119 deletions(-) diff --git a/airbrakes/simulation/README.md b/airbrakes/simulation/README.md index dd5e0b4c..5a0d38b2 100644 --- a/airbrakes/simulation/README.md +++ b/airbrakes/simulation/README.md @@ -1,7 +1,30 @@ -## Rotation Guide for sim IMU +# Rotation Guide for sim IMU -Vertical orientation of the IMU is described in vector form. -- Example: If the -x axis of the IMU is facing the sky, the vertical orientation is [-1, 0, 0] +### Axes and Direction Definitions: +#### Vehicle Frame: +- The coordinate system with respect to the IMU inside of the rocket. +#### Earth Frame: +- The coordinate system with respect to Earth frame of reference. +#### WGS Vertical: +- The upwards orientation of the rocket, direction opposite of gravitational force +#### Rocket Body Axis: +- The direction that the rocket is facing; roll axis, longitudinal axis of the rocket +#### Flight Path: +- Velocity vector direction of the rocket; which way the rocket is travelling +#### Azimuth: +- $\phi$, phi, the horizontal cardinal direction of the flight path, relative to Earth's surface +- An azimuth of zero degrees is facing North. -Direction the rocket leans to on the pad is the launch_pad_direction. Ranges from 0 to 360 degrees. -Important to note that the IMU does not change position inside of the rocket when changing launch pad angle. \ No newline at end of file +### Angle Vector Definitions: +#### Pitch Angle: +- $\theta$, theta, angle from WGS vertical to the rocket body axis +#### Flight Path Angle: +- $\gamma$, gamma, angle from WGS vertical to the flight path +#### Angle of Attack: +- $\alpha$, alpha, angle from the flight path to the rocket body axis + + +WGS vertical of the IMU is described in vector form. +- Example: If the -x axis of the IMU is facing the sky, the WGS vertical is [-1, 0, 0] + +Important to note that the IMU does not change orientation inside of the rocket when changing launch pad angle. \ No newline at end of file diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 62d018b2..2d8facde 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -30,7 +30,7 @@ class DataGenerator: "_last_est_packet", "_last_raw_packet", "_last_velocities", - "_max_velocity", + "_max_vertical_velocity", "_raw_rotation_manager", "_thrust_data", "is_airbrakes_extended", @@ -47,7 +47,7 @@ def __init__(self, config: SimulationConfig): self._last_est_packet: EstimatedDataPacket | None = None self._last_raw_packet: RawDataPacket | None = None self._last_velocities: npt.NDArray = np.array([0.0, 0.0, 0.0]) - self._max_velocity: np.float64 = np.float64(0.1) + self._max_vertical_velocity: np.float64 = np.float64(0.1) self._eff_exhaust_velocity: np.float64 | None = None # loads thrust curve @@ -61,16 +61,16 @@ def __init__(self, config: SimulationConfig): self.is_airbrakes_extended = False @property - def velocities(self) -> npt.NDArray: - """Returns the last calculated velocities of the rocket""" + def velocity_vector(self) -> npt.NDArray: + """Returns the last calculated velocity vectors of the rocket""" return self._last_velocities def _get_random(self, identifier: str) -> np.float64: """ Gets a random value for the selected identifier, using the standard deviation if given. - :param identifier: string that matches a config variable in sim_config.yaml + :param identifier: string that matches a config variable in sim_config.py :return: float containing a random value for the selected identifier, between - the bounds specified in the config + the bounds specified in the config. """ parameters = getattr(self._config, identifier) if len(parameters) == 1: @@ -87,7 +87,7 @@ def _get_random(self, identifier: str) -> np.float64: def _load_thrust_curve(self) -> npt.NDArray: """ Loads the thrust curve from the motor specified in the configs. - :return: numpy array containing tuples with the time and thrust at that time. + :return: numpy array containing lists with the time, and thrust at that time. """ # gets the path of the csv based on the config file csv_path = Path(f"airbrakes/simulation/thrust_curves/{self._config.motor}.csv") @@ -99,13 +99,13 @@ def _load_thrust_curve(self) -> npt.NDArray: start_flag = False # flag to identify when the metadata/header rows are skipped with csv_path.open(mode="r", newline="") as thrust_csv: - propellant_weight = None + propellant_mass = None reader = csv.reader(thrust_csv) for row in reader: - # We want to store the propellant weight - if row[0] == "propellant weight:": - propellant_weight = float(row[1]) / 1000 # we want in kg + # We want to store the propellant mass + if row[0] == "propellant mass:": + propellant_mass = float(row[1]) / 1000 # we want in kg continue # Start appending data only after the header row @@ -114,7 +114,7 @@ def _load_thrust_curve(self) -> npt.NDArray: continue # Skip header row itself if start_flag: - # Convert time and thrust values to floats and append as a tuple + # Convert time and thrust values to floats and append to list time = float(row[0]) thrust = float(row[1]) motor_timestamps.append(time) @@ -123,29 +123,29 @@ def _load_thrust_curve(self) -> npt.NDArray: # for future mass calculations, finding total impulse total_impulse = np.trapezoid(motor_thrusts, motor_timestamps) # calculating effective exhaust velocity (constant) - self._eff_exhaust_velocity = total_impulse / propellant_weight + self._eff_exhaust_velocity = total_impulse / propellant_mass return np.array([motor_timestamps, motor_thrusts]) def _get_rotation_managers(self) -> npt.NDArray: """ Creates and initializes both rotation managers that will be used to contain any rotation - related math and methods for the raw and estimated data. + related math, and methods for the raw and estimated data. :return: a 2 element array containing the raw rotation manager and the estimated rotation manager, respectively. """ - launch_rod_angle = self._get_random("launch_rod_angle") - launch_rod_direction = self._get_random("launch_rod_direction") + launch_rod_pitch = self._get_random("launch_rod_pitch") + launch_rod_azimuth = self._get_random("launch_rod_azimuth") raw_manager = RotationManager( - self._config.rocket_orientation, - launch_rod_angle, - launch_rod_direction, + self._config.wgs_vertical, + launch_rod_pitch, + launch_rod_azimuth, ) est_manager = RotationManager( - self._config.rocket_orientation, - launch_rod_angle, - launch_rod_direction, + self._config.wgs_vertical, + launch_rod_pitch, + launch_rod_azimuth, ) return np.array([raw_manager, est_manager]) @@ -229,11 +229,11 @@ def generate_raw_data_packet(self) -> RawDataPacket: if ( self._last_velocities[2] - < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + < self._max_vertical_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD and self._last_est_packet.timestamp > 1e9 ): - velocity_ratio = self._last_velocities[2] / self._max_velocity - self._raw_rotation_manager.update_orientation(velocity_ratio) + vertical_velocity_ratio = self._last_velocities[2] / self._max_vertical_velocity + self._raw_rotation_manager.update_orientation(vertical_velocity_ratio) # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_raw_packet.timestamp / 1e9) + time_step @@ -297,13 +297,13 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: return self._last_raw_packet # updates the estimated rotation manager, if we are after motor burn phase - velocity_ratio = self._last_velocities[2] / self._max_velocity + vertical_velocity_ratio = self._last_velocities[2] / self._max_vertical_velocity if ( self._last_velocities[2] - < self._max_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + < self._max_vertical_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD and self._last_est_packet.timestamp > 1e9 ): - self._est_rotation_manager.update_orientation(velocity_ratio) + self._est_rotation_manager.update_orientation(vertical_velocity_ratio) # calculates the timestamp for this packet (in seconds) next_timestamp = (self._last_est_packet.timestamp / 1e9) + time_step @@ -316,7 +316,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[1], ) # adding randomness to compensated acceleration - comp_rand = (0.3 + velocity_ratio * 2) * random.uniform(-1, 1) + comp_rand = (0.3 + vertical_velocity_ratio * 2) * random.uniform(-1, 1) compensated_accel += comp_rand linear_accel = self._est_rotation_manager.calculate_linear_accel( @@ -325,8 +325,8 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: ) # gets velocity and finds updated altitude - vert_velocity = self._calculate_velocities(compensated_accel, time_step)[2] - new_altitude = self._last_est_packet.estPressureAlt + vert_velocity * time_step + vertical_velocity = self._calculate_velocities(compensated_accel, time_step)[2] + new_altitude = self._last_est_packet.estPressureAlt + vertical_velocity * time_step delta_theta = self._est_rotation_manager.calculate_delta_theta() angular_rates = delta_theta / time_step @@ -361,35 +361,35 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: def _calculate_velocities(self, comp_accel: npt.NDArray, time_diff: np.float64) -> np.float64: """ - Calculates the velocity of the rocket based on the compensated acceleration. - Integrates that acceleration to get the velocity. + Calculates the velocity vector of the rocket based on the compensated acceleration vector. + Integrates that acceleration vector to get the velocity vector. :param: numpy array containing the compensated acceleration vector - :return: the velocity of the rocket + :return: the velocity vector of the rocket """ # gets the rotated acceleration vector - accelerations = self._est_rotation_manager.calculate_rotated_accelerations(comp_accel) - # gets vertical part of the gravity vector - accelerations[2] = deadband(accelerations[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) + rotated_accel = self._est_rotation_manager.calculate_rotated_accelerations(comp_accel) + # deadbands the wgs vertical part of the rotated acceleration + rotated_accel[2] = deadband(rotated_accel[2] - GRAVITY, ACCELERATION_NOISE_THRESHOLD) - # Integrate the accelerations to get the velocity - velocities = self._last_velocities + accelerations * time_diff + # Integrate the rotated accelerations to get the velocity vector + velocity_vector = self._last_velocities + rotated_accel * time_diff - # updates the last vertical velocity - self._last_velocities = velocities + # updates the last velocity vector + self._last_velocities = velocity_vector # updates max velocity - self._max_velocity = max(velocities[2], self._max_velocity) + self._max_vertical_velocity = max(velocity_vector[2], self._max_vertical_velocity) - return velocities + return velocity_vector - def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: + def _calculate_forces(self, timestamp, velocity_vector) -> npt.NDArray: """ Calculates the thrust force and drag force, and returns them in an array :param timestamp: the timestamp of the rocket to calcuate the net forces at - :param velocities: numpy array containing the x, y, and z velocities + :param velocity_vector: numpy array containing velocity vector :return: a 2 element numpy array containing thrust force and drag force, respectively. Thrust is positive, drag is negative. @@ -401,34 +401,36 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: # last altitude altitude = self._last_est_packet.estPressureAlt # calculate the magnitude of velocity - speed = np.linalg.norm(velocities) - # temperature using troposphere gradient + speed = np.linalg.norm(velocity_vector) + # temperature using temperature gradient temp = self._config.air_temperature + 273.15 + gradient * altitude - # air density formula for troposphere gradient + # air density formula, derived from ideal gas law air_density = 1.225 * (temp / (self._config.air_temperature + 273.15)) ** ( -GRAVITY / (gas_constant * gradient) - 1 ) # using speed of sound to find mach number - mach_number = speed / np.sqrt(ratio_spec_heat * gas_constant * temp) + current_mach_number = speed / np.sqrt(ratio_spec_heat * gas_constant * temp) - mach_numbers = ( + # gets the mach number vs drag coefficient lookup table in the config, depending on if + # airbrakes are currently extended or not. + mach_number_indices = ( self._config.airbrakes_retracted_cd[0] if not self.is_airbrakes_extended else self._config.airbrakes_extended_cd[0] ) - drag_coefficients = ( + drag_coefficient_lookup_table = ( self._config.airbrakes_retracted_cd[1] if not self.is_airbrakes_extended else self._config.airbrakes_extended_cd[1] ) - # getting the drag coefficient + # getting the current drag coefficient drag_coefficient = np.interp( - mach_number, - mach_numbers, - drag_coefficients, + current_mach_number, + mach_number_indices, + drag_coefficient_lookup_table, ) thrust_force = 0.0 @@ -450,14 +452,14 @@ def _calculate_forces(self, timestamp, velocities) -> npt.NDArray: def _calculate_mass(self, timestamp: np.float64) -> np.float64: """ - Calculates the weight of the rocket at any given timestamp. The weight loss is found + Calculates the mass of the rocket at any given timestamp. The mass loss is found by calculating the mass flow rate using effective exhaust velocity. :param timestamp: current timestamp of the rocket, in seconds :return: the current mass of the rocket, in kilograms """ # find current thrust current_thrust = np.interp(timestamp, self._thrust_data[0], self._thrust_data[1]) - # getting thrust curve up to current timestamp + # getting the thrust curve of the motor, up to current timestamp mask = self._thrust_data[0] <= timestamp time_values = np.append(self._thrust_data[0][mask], [timestamp]) thrust_values = np.append(self._thrust_data[1][mask], [current_thrust]) diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 99f7648a..ab51f627 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -19,22 +19,27 @@ class RotationManager: "_formula_consts", "_last_orientation", "_orientation", - "_vertical", + "_wgs_vertical", "_zenith", ) def __init__( - self, orientation: npt.NDArray, rod_angle_of_attack: np.float64, rod_direction: np.float64 + self, + wgs_vertical: npt.NDArray, + launch_rod_pitch: np.float64, + launch_rod_azimuth: np.float64, ) -> None: """ - Initializes a RotationManager instance with a specified orientation. This orientation + Initializes a RotationManager instance with a specified WGS vertical direction vector. This serves as the reference direction for rotation operations. - :param orientation: the vertical orientation of the rocket + :param wgs_vertical: the WGS vertical direction vector of the rocket. + :param launch_rod_pitch: pitch angle of the launch rod, in degrees. + :param launch_rod_azimuth: the azimuth rotation of the launch rod, in degrees. """ - self._vertical: npt.NDArray = orientation - self._zenith: np.float64 = np.float64((rod_angle_of_attack * np.pi) / 180) - self._azimuth: np.float64 = np.float64((rod_direction * np.pi) / 180) + self._wgs_vertical: npt.NDArray = wgs_vertical + self._zenith: np.float64 = np.float64((launch_rod_pitch * np.pi) / 180) + self._azimuth: np.float64 = np.float64((launch_rod_azimuth * np.pi) / 180) self._last_orientation: R | None = None self._orientation: R | None = None self._formula_consts: npt.NDArray = self._initialize_rotation_formula() @@ -74,14 +79,14 @@ def update_orientation(self, velocity_ratio: np.float64) -> None: # We want to convert the azimuth and zenith values into a scipy rotation object # Step 1: We have to declare that the orientation of the rocket is vertical - aligned_orientation = R.align_vectors([self._vertical], [[0, 0, 1]])[0] + aligned_orientation = R.align_vectors([self._wgs_vertical], [[0, 0, 1]])[0] # Step 2: Get the axis that the zenith rotates around, it changes depending on orientation. zenith_rotation_axis = np.array( [ - np.abs(self._vertical[2]), - np.abs(self._vertical[0]), - np.abs(self._vertical[1]), + np.abs(self._wgs_vertical[2]), + np.abs(self._wgs_vertical[0]), + np.abs(self._wgs_vertical[1]), ] ) @@ -89,7 +94,7 @@ def update_orientation(self, velocity_ratio: np.float64) -> None: zenith_rotation = R.from_rotvec(self._zenith * zenith_rotation_axis) # Step 4: rotate by azimuth - azimuth_rotation = R.from_rotvec(self._azimuth * np.array(self._vertical)) + azimuth_rotation = R.from_rotvec(self._azimuth * np.array(self._wgs_vertical)) # Step 5: Combined rotation: aligned orientation, azimuth rotation, and zenith rotation orientation = azimuth_rotation * zenith_rotation * aligned_orientation @@ -104,12 +109,13 @@ def calculate_compensated_accel( self, thrust_acceleration: np.float64, drag_acceleration: np.float64 ) -> npt.NDArray: """ - Uses the acceleration due to drag and thrust to find the compensated acceleration + Uses the acceleration due to drag and thrust in Earth frame to find the + compensated acceleration :param drag_acceleration: scalar value of acceleration due to drag :param thrust_acceleration: scalar value of acceleration due to thrust - :return: the compensated acceleration in the vehicle frame of reference + :return: the compensated acceleration in the vehicle frame. """ thrust_drag_accel = thrust_acceleration - drag_acceleration compensated_accel = self._scalar_to_vector(thrust_drag_accel) @@ -127,13 +133,15 @@ def calculate_linear_accel( self, thrust_acceleration: np.float64, drag_acceleration: np.float64 ) -> npt.NDArray: """ - Uses the compensated acceleration to find the linear acceleration + Uses the acceleration due to drag and thrust in Earth frame to find the + linear acceleration :param drag_acceleration: scalar value of acceleration due to drag :param thrust_acceleration: scalar value of acceleration due to thrust - :return: the linear acceleration in the vehicle frame of reference + :return: the linear acceleration in vehicle frame """ + # gets compensated acceleration comp_accel = self.calculate_compensated_accel(thrust_acceleration, drag_acceleration) # apply gravity gravity_accel_vector = self._orientation.apply([0, 0, GRAVITY]) @@ -141,55 +149,53 @@ def calculate_linear_accel( def calculate_delta_theta(self) -> npt.NDArray: """ - Calculates the delta theta in the vehicle frame of reference, with units in - radians. + Calculates the delta theta in the vehicle frame, with units in radians. - :return: numpy array containing the delta thetas, in [x, y, z] format. + :return: numpy array containing the delta thetas, in [x, y, z] format, in vehicle frame. """ - # first have to take the current orientation and the last orientation that is in - # Earth -> vehicle reference and convert to vehicle -> Earth reference + # first have to take the current orientation and the last orientation, which rotates + # from Earth frame to vehicle frame, and convert to rotations from vehicle frame to + # Earth frame. imu_adjusted_orientation = self._orientation.inv() imu_adjusted_last_orientation = self._last_orientation.inv() - # Calculate the relative rotation from last orientation to the current orientation + # Calculate the relative rotation from the new inverted rotations relative_rotation = imu_adjusted_last_orientation.inv() * imu_adjusted_orientation - # Converts the relative rotation to a rotation vector + # Expresses as a rotation vector return relative_rotation.as_rotvec() - def calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> np.float64: + def calculate_rotated_accelerations(self, compensated_acceleration: npt.NDArray) -> npt.NDArray: """ Calculates the rotated acceleration vector with the compensated acceleration. Rotates from - vehicle frame of reference to Earth frame of reference. + vehicle frame to Earth frame. - :param compensated_acceleration: compensated acceleration in the vehicle-frame reference. + :param compensated_acceleration: compensated acceleration in the vehicle frame. - :return: float containing vertical acceleration with respect to Earth. + :return: numpy array containing the acceleration vector in Earth frame. """ - # rotation needed to align the IMU + # inverts the rotation, so it rotates from vehicle frame to Earth frame. imu_adjusted_orientation = self._orientation.inv() return imu_adjusted_orientation.apply([compensated_acceleration])[0] def calculate_imu_quaternions(self) -> npt.NDArray: """ - Adjusts the given orientation to align with the IMU's reference frame, where - [0, 0, -1] is vertical, and returns a quaternions. - - :param orientation: A scipy Rotation object representing the current orientation. - - :return: A quaternion adjusted to the IMU's frame. + Calculates the quaternion orientation of the rocket. + :return: numpy array containing the quaternion, in [w, x, y, z] format. """ - # rotation needed to align the IMU + # flips and inverts the rotation, so that the quaternions are represented + # accurately, reflecting what the IMU would output. imu_alignment = R.align_vectors([[0, 0, 1]], [[0, 0, -1]])[0] imu_quaternion_rotation = imu_alignment * self._orientation.inv() - # convert to quaternion + # returns the rotation as a quaternion return imu_quaternion_rotation.as_quat(scalar_first=True) def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: """ - Converts a scalar value to a vector, where it will align with the vertical orientation. + Converts a scalar value to a vector, where it will align with the designated + wgs vertical vector. :param scalar: a float representing the scalar to be converted @@ -197,7 +203,7 @@ def _scalar_to_vector(self, scalar: np.float64) -> npt.NDArray: """ vector = np.zeros(3) # Gets index of non-zero value in the vertical orientation - index = np.argmax(np.abs(self._vertical)) + index = np.argmax(np.abs(self._wgs_vertical)) # Place the scalar in the appropriate position, and with the correct sign - vector[index] = scalar * self._vertical[index] + vector[index] = scalar * self._wgs_vertical[index] return vector diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index a2402a66..93ae6688 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -6,7 +6,7 @@ class SimulationConfig: """ - config class for simulation + Configuration settings for the simulation. Includes presets of full-scale and sub-scale flights. """ def __init__( @@ -18,9 +18,9 @@ def __init__( airbrake_extended_cd: npt.NDArray, rocket_mass: np.float64, reference_area: np.float64, - rocket_orientation: npt.NDArray[np.float64], - launch_rod_angle: npt.NDArray[np.float64], - launch_rod_direction: npt.NDArray[np.float64], + wgs_vertical: npt.NDArray[np.float64], + launch_rod_pitch: npt.NDArray[np.float64], + launch_rod_azimuth: npt.NDArray[np.float64], air_temperature: np.float64, airbrakes_reference_area: np.float64, ): @@ -42,11 +42,11 @@ def __init__( self.air_temperature = air_temperature # ground temperature, in celcius # Rocket orientation on the launch pad - self.rocket_orientation = rocket_orientation + self.wgs_vertical = wgs_vertical # Config for randomness in the simulation - self.launch_rod_angle = launch_rod_angle - self.launch_rod_direction = launch_rod_direction + self.launch_rod_pitch = launch_rod_pitch + self.launch_rod_azimuth = launch_rod_azimuth FULL_SCALE_CONFIG = SimulationConfig( @@ -60,9 +60,9 @@ def __init__( reference_area=np.float64(0.01929), airbrakes_reference_area=np.float64(0.01), air_temperature=np.float64(25), - rocket_orientation=np.array([0, 0, -1]), - launch_rod_angle=np.array([5]), - launch_rod_direction=np.array([90]), + wgs_vertical=np.array([0, 0, -1]), + launch_rod_pitch=np.array([5]), + launch_rod_azimuth=np.array([90]), ) SUB_SCALE_CONFIG = SimulationConfig( @@ -71,13 +71,13 @@ def __init__( motor="AeroTech_J500G", airbrake_retracted_cd=np.array([[0.05, 0.2, 0.3], [0.41, 0.4175, 0.425]]), airbrake_extended_cd=np.array([[0.05, 0.2, 0.3], [0.51, 0.5175, 0.525]]), - rocket_mass=np.float64(5.954), + rocket_mass=np.float64(6.391), reference_area=np.float64(0.008205), - airbrakes_reference_area=np.float64(0.01), + airbrakes_reference_area=np.float64(0.00487741), air_temperature=np.float64(15), - rocket_orientation=np.array([-1, 0, 0]), - launch_rod_angle=np.array([5]), - launch_rod_direction=np.array([0]), + wgs_vertical=np.array([-1, 0, 0]), + launch_rod_pitch=np.array([5]), + launch_rod_azimuth=np.array([0]), ) diff --git a/airbrakes/simulation/sim_imu.py b/airbrakes/simulation/sim_imu.py index fef465a3..f2f771ce 100644 --- a/airbrakes/simulation/sim_imu.py +++ b/airbrakes/simulation/sim_imu.py @@ -111,7 +111,7 @@ def _fetch_data_loop(self, config: SimulationConfig) -> None: # unfortunately, doing the signal handling isn't always reliable, so we need to wrap the # function in a context manager to suppress the KeyboardInterrupt with contextlib.suppress(KeyboardInterrupt): - while data_generator.velocities[2] > -100: + while data_generator.velocity_vector[2] > -100: # starts timer start_time = time.time() diff --git a/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv b/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv index 3d151b3b..ff165bb3 100644 --- a/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv +++ b/airbrakes/simulation/thrust_curves/AeroTech_J500G.csv @@ -1,7 +1,7 @@ "motor:","AeroTech L1940X" "contributor:","Will Sanderson" "details:","https://www.thrustcurve.org/motors/AeroTech/J500G/" -"propellant weight:",363 +"propellant mass:",363 "Time (s)","Thrust (N)" 0.0134378, 40.2458 0.0335946, 724.425 diff --git a/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv index 3be8dcac..24866a9c 100644 --- a/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv +++ b/airbrakes/simulation/thrust_curves/AeroTech_L1940X.csv @@ -1,7 +1,7 @@ "motor:","AeroTech L1940X" "contributor:","Mike Caplinger" "details:","https://www.thrustcurve.org/simfiles/60ac84458dc4640004c24eb6/" -"propellant weight:",1825 +"propellant mass:",1825 "Time (s)","Thrust (N)" 0.02,48.137 0.04,2107.333 From 382f2ebdd7242fe23db29f87553a3e7e964a3b60 Mon Sep 17 00:00:00 2001 From: wlsanderson Date: Wed, 4 Dec 2024 23:05:22 -0500 Subject: [PATCH 63/63] wip, fixing apogee convergence time --- airbrakes/data_handling/apogee_predictor.py | 1 - airbrakes/simulation/data_generator.py | 11 ++------ airbrakes/simulation/rotation_manager.py | 30 ++++++--------------- airbrakes/simulation/sim_config.py | 12 ++++----- constants.py | 5 ++-- 5 files changed, 19 insertions(+), 40 deletions(-) diff --git a/airbrakes/data_handling/apogee_predictor.py b/airbrakes/data_handling/apogee_predictor.py index 1a362510..cb795d6a 100644 --- a/airbrakes/data_handling/apogee_predictor.py +++ b/airbrakes/data_handling/apogee_predictor.py @@ -152,7 +152,6 @@ def _curve_fit(self) -> CurveCoefficients: uncertainties = np.sqrt(np.diag(pcov)) if np.all(uncertainties < UNCERTAINTY_THRESHOLD): self._has_apogee_converged = True - a, b = popt return CurveCoefficients(A=a, B=b) diff --git a/airbrakes/simulation/data_generator.py b/airbrakes/simulation/data_generator.py index 9091d20f..00865590 100644 --- a/airbrakes/simulation/data_generator.py +++ b/airbrakes/simulation/data_generator.py @@ -231,10 +231,8 @@ def generate_raw_data_packet(self) -> RawDataPacket: return self._last_raw_packet # updates the raw rotation manager, if we are after motor burn phase - if ( - self._last_velocities[2] - < self._max_vertical_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + self._last_velocities[2] < self._max_vertical_velocity * MAX_VELOCITY_THRESHOLD and self._last_est_packet.timestamp > 1e9 ): vertical_velocity_ratio = self._last_velocities[2] / self._max_vertical_velocity @@ -304,8 +302,7 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: # updates the estimated rotation manager, if we are after motor burn phase vertical_velocity_ratio = self._last_velocities[2] / self._max_vertical_velocity if ( - self._last_velocities[2] - < self._max_vertical_velocity - self._last_velocities[2] * MAX_VELOCITY_THRESHOLD + self._last_velocities[2] < self._max_vertical_velocity * MAX_VELOCITY_THRESHOLD and self._last_est_packet.timestamp > 1e9 ): self._est_rotation_manager.update_orientation(vertical_velocity_ratio) @@ -320,9 +317,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: force_accelerations[0], force_accelerations[1], ) - # adding randomness to compensated acceleration - comp_rand = (0.3 + vertical_velocity_ratio * 2) * random.uniform(-1, 1) - compensated_accel += comp_rand linear_accel = self._est_rotation_manager.calculate_linear_accel( force_accelerations[0], @@ -337,7 +331,6 @@ def generate_estimated_data_packet(self) -> EstimatedDataPacket: angular_rates = delta_theta / time_step quaternion = self._est_rotation_manager.calculate_imu_quaternions() - packet = EstimatedDataPacket( timestamp=int(next_timestamp * 1e9), estOrientQuaternionW=float(quaternion[0]), diff --git a/airbrakes/simulation/rotation_manager.py b/airbrakes/simulation/rotation_manager.py index 139d4ee4..e6ffe1d0 100644 --- a/airbrakes/simulation/rotation_manager.py +++ b/airbrakes/simulation/rotation_manager.py @@ -16,7 +16,6 @@ class RotationManager: __slots__ = ( "_azimuth", - "_formula_consts", "_last_orientation", "_orientation", "_wgs_vertical", @@ -42,29 +41,12 @@ def __init__( self._azimuth: np.float64 = np.float64((launch_rod_azimuth * np.pi) / 180) self._last_orientation: R | None = None self._orientation: R | None = None - self._formula_consts: npt.NDArray = self._initialize_rotation_formula() self.update_orientation(1) @property def gravity_vector(self) -> npt.NDArray: return self._orientation.apply([0, 0, -GRAVITY_METERS_PER_SECOND_SQUARED]) - def _initialize_rotation_formula(self) -> np.float64: - """ - Initializes the formula for rotation. Returns the shift of the function. Is constant - for the rest of the flight. - :return: the shift and initial angle of the function. - """ - # initializes vector with small step values, the shift is always below 0.1 - # for inital rod angles below 45 degrees - dx = 0.00001 - xvec = np.arange(dx, 0.1, dx) - # this graph is zenith versus v/vmax - no_shift = (-xvec + 1) / (15 * xvec) + self._zenith - # finding the index where no_shift is closest to pi/2 - closest_index = np.argmin(np.abs(no_shift - np.pi / 2)) - return np.array([closest_index * dx, self._zenith]) # returns the shift and zenith - def update_orientation(self, velocity_ratio: np.float64) -> None: """ Updates the baseline vehicle-frame orientation, and all of the vehicle-frame @@ -72,9 +54,13 @@ def update_orientation(self, velocity_ratio: np.float64) -> None: :param velocity_ratio: current vertical velocity of the rocket divided by max velocity """ - # splitting up the function into two parts - devisor = 15 * (self._formula_consts[0] + 1) * (velocity_ratio + self._formula_consts[0]) - self._zenith = (-velocity_ratio + 1) / devisor + self._formula_consts[1] + # ignore these parameters, they are not intended to be permanent. + a = 0.06 + b = -0.69 + c = 2.1 + d = 0.4 + + self._zenith = d / (b + np.exp(c * velocity_ratio)) + a # We want to convert the azimuth and zenith values into a scipy rotation object @@ -164,7 +150,7 @@ def calculate_delta_theta(self) -> npt.NDArray: imu_adjusted_last_orientation = self._last_orientation.inv() # Calculate the relative rotation from the new inverted rotations - relative_rotation = imu_adjusted_last_orientation.inv() * imu_adjusted_orientation + relative_rotation = imu_adjusted_orientation * imu_adjusted_last_orientation.inv() # Expresses as a rotation vector return relative_rotation.as_rotvec() diff --git a/airbrakes/simulation/sim_config.py b/airbrakes/simulation/sim_config.py index 93ae6688..aa6d5087 100644 --- a/airbrakes/simulation/sim_config.py +++ b/airbrakes/simulation/sim_config.py @@ -54,24 +54,24 @@ def __init__( est_time_step=np.float64(0.002), motor="AeroTech_L1940X", # drag_coefficient=np.array([[0.1, 0.3, 0.5, 0.7], [0.3565, 0.3666, 0.3871, 0.41499]]), - airbrake_retracted_cd=np.array([[0.1, 1], [0.29, 0.29]]), - airbrake_extended_cd=np.array([[0.1, 1], [0.49, 0.49]]), + airbrake_retracted_cd=np.array([[0.1, 0.3, 0.5, 0.7], [0.3565, 0.3666, 0.3871, 0.41499]]), + airbrake_extended_cd=np.array([[0.1, 0.3, 0.5, 0.7], [0.4565, 0.4666, 0.4871, 0.51499]]), rocket_mass=np.float64(17.6), reference_area=np.float64(0.01929), airbrakes_reference_area=np.float64(0.01), air_temperature=np.float64(25), wgs_vertical=np.array([0, 0, -1]), launch_rod_pitch=np.array([5]), - launch_rod_azimuth=np.array([90]), + launch_rod_azimuth=np.array([0]), ) SUB_SCALE_CONFIG = SimulationConfig( raw_time_step=np.float64(0.001), est_time_step=np.float64(0.002), motor="AeroTech_J500G", - airbrake_retracted_cd=np.array([[0.05, 0.2, 0.3], [0.41, 0.4175, 0.425]]), - airbrake_extended_cd=np.array([[0.05, 0.2, 0.3], [0.51, 0.5175, 0.525]]), - rocket_mass=np.float64(6.391), + airbrake_retracted_cd=np.array([[0.05, 0.2, 0.3], [0.5145, 0.5207, 0.5283]]), + airbrake_extended_cd=np.array([[0.05, 0.2, 0.3], [0.6145, 0.6207, 0.6283]]), + rocket_mass=np.float64(6.535), reference_area=np.float64(0.008205), airbrakes_reference_area=np.float64(0.00487741), air_temperature=np.float64(15), diff --git a/constants.py b/constants.py index 5c6766e3..45a3b6b9 100644 --- a/constants.py +++ b/constants.py @@ -128,7 +128,7 @@ class DisplayEndingType(StrEnum): motor has stopped burning if the current velocity is less than a percentage of the max velocity.""" # ----------------- Coasting to Freefall ----------------- -TARGET_ALTITUDE_METERS = 1000 +TARGET_ALTITUDE_METERS = 10000 """The target altitude in meters that we want the rocket to reach. This is used with our bang-bang controller to determine when to extend and retract the airbrakes.""" @@ -169,6 +169,7 @@ class DisplayEndingType(StrEnum): APOGEE_PREDICTION_MIN_PACKETS = 10 """The minimum number of data packets required to predict the apogee.""" -UNCERTAINTY_THRESHOLD = [0.0359, 0.00075] # For near quick convergence times, use: [0.1, 0.75] +# UNCERTAINTY_THRESHOLD = [0.0359, 0.00075] # For near quick convergence times, use: [0.1, 0.75] +UNCERTAINTY_THRESHOLD = [0.0359, 0.00075] """The uncertainty from the curve fit, below which we will say that our apogee has converged. This uncertainty corresponds to being off by +/- 5m."""