Apogee approx

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]}

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,10 +29,21 @@ 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()
     time.sleep(1)
     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