diff --git a/app/rocket/kinetics.py b/app/rocket/kinetics.py index 9fc3828..f0e4850 100644 --- a/app/rocket/kinetics.py +++ b/app/rocket/kinetics.py @@ -6,6 +6,7 @@ WINDOW_SIZE = 50 + class Kinetics(Thread): def __init__(self, device_factory): self.imu = device_factory.imu @@ -33,7 +34,25 @@ def deactivate(self): raise Exception('Failed to deactivate Kinetics Model') def predicted_apogee(self): - return 0 + accel = self.dict_to_matrix(self.acceleration()) + m = 20 # empty rocket mass + accel_force = m*accel + + c = self.drag_cofficient() + area = self.compute_area() + rho = 2 # mass density of fluid + b = (1/2)*rho*area*c # nice variable to work with + velocity = self.dict_to_matrix(self.velocity()) + try: + drag_force = -1*b*np.square(velocity) + + n = np.sqrt(b/accel_force) # another nice variable + p_burnout = self.position() # TODO: make this return the position at start of TimeWindow + v_burnout = self.velocity() # TODO: make this return the velocity at start of TimeWindow + + p_apogee = p_burnout - np.dot((1/((np.square(accel))*(np.square(n)))), np.log(np.cos(np.arctan(2*np.dot(n, v_burnout))))) + + return p_apogee def acceleration(self): return self.acceleration_window.last() @@ -47,6 +66,17 @@ def position(self): def compute_brakes_percentage(self): return 1.0 + def drag_cofficient(self): + return 1.0 + + def compute_area(self): + return 1.0 + + def dict_to_matrix(self, dict): + return np.array([[dict['x']], + [dict['y']], + [dict['z']]]) + def run(self): while self.active: measurement = self.imu.read_accel_filtered() @@ -60,7 +90,6 @@ def run(self): y=prev_velocity['y'] + delta_velocity[1], z=prev_velocity['z'] + delta_velocity[2]) - prev_position = self.position_window.last() delta_position = self.velocity_window.integrate_last(self.time_series[-2], self.time_series[-1]) self.position_window.append( @@ -75,6 +104,7 @@ def run(self): position = self.position() logging.debug("Position x: {}, y: {}, z: {}".format(position['x'], position['y'], position['z'])) + class TimeWindow(object): def __init__(self, size=WINDOW_SIZE): self.x = deque(np.zeros(size), maxlen=size) @@ -95,6 +125,6 @@ def integrate_last(self, t0, t1): def last(self, count=1): if count == 1: - return {'x': self.x[-1], 'y': self.y[-1], 'z': self.z[-1] } + return {'x': self.x[-1], 'y': self.y[-1], 'z': self.z[-1]} else: - return {'x': self.x[-1:-1*count], 'y': self.y[-1:-1*count], 'z': self.z[-1:-1*count] } + return {'x': self.x[-1:-1*count], 'y': self.y[-1:-1*count], 'z': self.z[-1:-1*count]} diff --git a/test/rocket/test_kinetics.py b/test/rocket/test_kinetics.py index f52debe..8aaed40 100644 --- a/test/rocket/test_kinetics.py +++ b/test/rocket/test_kinetics.py @@ -1,15 +1,17 @@ import pytest import time -import numpy +import numpy as np from app.rocket.kinetics import Kinetics from test.fixtures.dummy_device_factory import DummyDeviceFactory + @pytest.fixture def kinetics(): device_factory = DummyDeviceFactory() return Kinetics(DummyDeviceFactory()) + def test_start_asynchonously_updates_acceleration(kinetics): kinetics.activate() initial_acceleration = kinetics.acceleration() @@ -18,6 +20,7 @@ def test_start_asynchonously_updates_acceleration(kinetics): kinetics.deactivate() assert initial_acceleration != final_acceleration + def test_start_asynchonously_updates_velocity(kinetics): kinetics.activate() initial_velocity = kinetics.velocity() @@ -26,6 +29,7 @@ def test_start_asynchonously_updates_velocity(kinetics): kinetics.deactivate() assert initial_velocity != final_velocity + def test_start_asynchonously_updates_position(kinetics): kinetics.activate() initial_position = kinetics.position() @@ -33,3 +37,13 @@ def test_start_asynchonously_updates_position(kinetics): final_position = kinetics.position() kinetics.deactivate() assert initial_position != final_position + +def test_matrix_conversion(kinetics): + dict = {'x': 1, 'y': 2, 'z': 3 } + matrix = kinetics.dict_to_matrix(dict) + assert np.array_equal(matrix, np.array([[1], + [2], + [3]])) + +def test_apogee_prediction_zero_acceleration(kinetics): + kinetics.predicted_apogee() # TODO: finish me