From 7c84f4983066adcd7d6e6905b8914be008cce6be Mon Sep 17 00:00:00 2001
From: Jakob Meier <meierj@irs.uni-stuttgart.de>
Date: Wed, 17 Aug 2022 15:24:39 +0200
Subject: [PATCH] mgm calib fixes

---
 tmtc/acs_ctrl.py | 48 ++++++++++++++++++++++++++++++++----------------
 1 file changed, 32 insertions(+), 16 deletions(-)

diff --git a/tmtc/acs_ctrl.py b/tmtc/acs_ctrl.py
index e794461..22b8113 100644
--- a/tmtc/acs_ctrl.py
+++ b/tmtc/acs_ctrl.py
@@ -2,7 +2,7 @@ import enum
 import socket
 import struct
 from socket import AF_INET
-from typing import Tuple
+from typing import Tuple, Optional
 
 from config.definitions import CustomServiceList
 from config.object_ids import ACS_CONTROLLER
@@ -41,11 +41,17 @@ class Info:
     DISABLE_MGM_HK = "Disable MGM HK data generation"
 
 
-PERFORM_MGM_CALIBRATION = False
+PERFORM_MGM_CALIBRATION = True
 CALIBRATION_SOCKET_HOST = "localhost"
 CALIBRATION_SOCKET_PORT = 6677
 CALIBRATION_ADDR = (CALIBRATION_SOCKET_HOST, CALIBRATION_SOCKET_PORT)
 
+if PERFORM_MGM_CALIBRATION:
+    CALIBR_SOCKET = socket.socket(AF_INET, socket.SOCK_STREAM)
+    CALIBR_SOCKET.setblocking(False)
+    CALIBR_SOCKET.settimeout(0.2)
+    CALIBR_SOCKET.connect(CALIBRATION_ADDR)
+
 
 @tmtc_definitions_provider
 def acs_cmd_defs(defs: TmtcDefinitionWrapper):
@@ -141,29 +147,39 @@ def handle_acs_ctrl_mgm_data(printer: FsfwTmTcPrinter, hk_data: bytes):
 
 
 def perform_mgm_calibration(pw: PrintWrapper, mgm_tuple: Tuple):
-    calibr_socket = socket.socket(AF_INET, socket.SOCK_STREAM)
+    global CALIBR_SOCKET, CALIBRATION_ADDR
     try:
         declare_api_cmd = "declare_api_version 2"
-        calibr_socket.connect(CALIBRATION_ADDR)
-        calibr_socket.sendall(f"{declare_api_cmd}\n".encode())
-        calibr_socket.settimeout(0.2)
-        calibr_socket.setblocking(False)
-        reply = calibr_socket.recv(1024)
-        pw.dlog(
-            f"Received reply {reply} from Helmholtz Socket for command {declare_api_cmd}"
-        )
+        CALIBR_SOCKET.sendall(f"{declare_api_cmd}\n".encode())
+        reply = CALIBR_SOCKET.recv(1024)
+        if len(reply) != 2:
+            pw.dlog(f"MGM calibration: Reply received command {declare_api_cmd} has invalid length {len(reply)}")
+            return
+        else:
+            if str(reply[0]) == "0":
+                pw.dlog(f"MGM calibration: API version 2 was not accepted")
+                return
         if len(mgm_tuple) != 3:
             pw.dlog(f"MGM tuple has invalid length {len(mgm_tuple)}")
         mgm_list = [mgm / 10e6 for mgm in mgm_tuple]
         command = (
             f"magnetometer_field {mgm_list[0]} {mgm_list[1]} {mgm_list[2]}\n".encode()
         )
-        calibr_socket.sendall(command)
-        reply = calibr_socket.recv(1024)
-        pw.dlog(
-            f"Received reply {reply} from Helmholtz Socket for command magnetometer_field"
-        )
+        CALIBR_SOCKET.sendall(command)
+        reply = CALIBR_SOCKET.recv(1024)
+        if len(reply) != 2:
+            pw.dlog(f"MGM calibration: Reply received command magnetometer_field has invalid length {len(reply)}")
+            return
+        else:
+            if str(reply[0]) == "0":
+                pw.dlog(f"MGM calibration: magnetmeter field format was not accepted")
+                return
+        pw.dlog(f"Sent data {mgm_list} to Helmholtz Testbench successfully")
     except socket.timeout:
         pw.dlog("Socket timeout")
+    except BlockingIOError as e:
+        pw.dlog(f"Error {e}")
+    except ConnectionResetError as e:
+        pw.dlog("Socket was closed")
     except ConnectionRefusedError or OSError:
         pw.dlog("Connecting to Calibration Socket on addrss {} failed")