Added Detumbling, Magnetic field, attitude, actuate mtorquers

src/pfl_modes/detumble.py

@@ -6,6 +6,7 @@
 from pfl_types.cmd_types import ADCSCmd, PowerCmd
 from pfl_servers.fast_socket import FastSocket
 from pfl_modes.base_mode import PFLMode
+import numpy as np
 
 SOCKET_PATH = '/tmp/mode/detumble'
 COMMS_SOCKET_PATH = '/tmp/comms'
@@ -33,6 +34,8 @@ def start(self):
             RequestType.COMMAND, 
             ADCSCmd.IS_TUMBLING
         ).send_and_recv(SOCKET_PATH, ADCS_SOCKET_PATH)
+
+
 
         if not is_tumbling:
             self.finish_detumble()
@@ -55,11 +58,35 @@ def start(self):
 
 
     def finish_detumble(self):
-        '''
-        Disable mag coils, record telem, and go to safe mode
-        after. Mag coils should already be disabled (ie only running when
-        awaiting detumble conn).
-        '''
+        get_attitude = Msg(
+        RequestType.COMMAND, 
+            ADCSCmd.GET_ATTITUDE
+        ).send_and_recv(SOCKET_PATH, ADCS_SOCKET_PATH)
+
+        get_mag_field = Msg(
+        RequestType.COMMAND, 
+            ADCSCmd.GET_MAG_Field
+        ).send_and_recv(SOCKET_PATH, ADCS_SOCKET_PATH)
+
+
+        B = get_mag_field
+        q = get_attitude[0:4]
+        qt = q[1:]
+        qtb = np.cross(B,qt)
+        w = get_attitude[4:]
+        wb = np.cross(B,w)
+        Kp = 0.02e-5;
+        Kd = 0.02e-3;
+        M = -(Kp*qtb + Kd*wb) 
+
+        Msg(
+        RequestType.COMMAND, 
+            ADCSCmd.ACTUATE_MTORQUE
+        ).send(SOCKET_PATH, ADCS_SOCKET_PATH)
+
+
+
+
         pass
 
 

src/pfl_servers/adcs_server.py

@@ -10,6 +10,7 @@
 from pfl_types.cmd_types import ADCSCmd
 from pfl_servers.fast_socket import FastSocket
 from pfl_servers import base_server
+import numpy as np
 
 SOCKET_PATH = '/tmp/adcs'
 logging.basicConfig(level=logging.INFO)
@@ -31,12 +32,40 @@ def handle(self):
                 self.request.sendall(
                     pickle.dumps(Msg(RequestType.DATA, result))
                 )
+            elif msg.data ==ADCScmd.GET_ATTITUDE:
+                result = self.get_attitude()
+                self.request.sendall(
+                    pickle.dumps(Msg(RequestType.DATA, result)))
+
+            elif msg.data ==ADCScmd.GET_MAG_FIELD:
+                result = self.get_mag_field()
+                self.request.sendall(
+                    pickle.dumps(Msg(RequestType.DATA, result)))
+
+            elif msg.data ==ADCScmd.ACTUATE_MTORQUE:
+                result = self.actuate_mtorque()
+                self.request.sendall(
+                    pickle.dumps(Msg(RequestType.DATA, result)))
             else:
                 logging.error('Msg {} could not be executed'.format(self.msg))
         except Exception as e:
             print(e)
 
+    def get_attitude(self):
+        q0, q1, q2, q3 = [1, 0, 0, 0]
+        wx, wy, wz = [1,2,3]
+        yield np.array([q0, q1, q2, q3, q4, wx, wy, wz])
+        yield np.array([q0, q1, q2, q3, q4, wx-0.1, wy-0.1, wz-0.1])
+
+    def get_mag_field(self):
+        b1, b2, b3 = [1, 0, 0]
+        return np.array([b1,b2,b3])
+
+
+    def actuate_mtorquer(self,M):
+        pass
 
+
     def is_tumbling(self):
         return False