From cbb0cf0452b603f3b404e9971f8d140953b8a1af Mon Sep 17 00:00:00 2001
From: Cal Wing <20716204+calw20@users.noreply.github.com>
Date: Thu, 2 Nov 2023 20:51:26 +1000
Subject: [PATCH] Task 3 Data?

---
 main.py | 41 ++++++++++++++++++++++++++++++++++-------
 1 file changed, 34 insertions(+), 7 deletions(-)

diff --git a/main.py b/main.py
index d69a04d..7be6ff6 100644
--- a/main.py
+++ b/main.py
@@ -96,15 +96,20 @@ def importIMUData(mission, imu):
     return data
 
 # Load Truth Data
-TRUTH_N_POS_HEADER= ["Time [s]", "True North Position [m]"]
+TRUTH_N_POS_HEADER = ["Time [s]", "True North Position [m]"]
+TRUTH_FSP_X_HEADER = ["Time [s]", "True FSP_X [m/s^2]"]
 m1TruthData = pd.read_csv(
    f"./data/N_truth_IMU_M1L.txt", 
     header=None, skiprows=1,
     names=TRUTH_N_POS_HEADER,
-),  pd.read_csv(
+), pd.read_csv(
    f"./data/N_truth_IMU_M1H.txt", 
     header=None, skiprows=1,
     names=TRUTH_N_POS_HEADER,
+), pd.read_csv(
+   f"./data/FSPB1_truth_IMU_M1.txt", 
+    header=None, skiprows=1,
+    names=TRUTH_FSP_X_HEADER,
 )
 # Load the Mission Data
 m1_IMUData = importIMUData(1, 0), importIMUData(1, 1) #(L, H) Data
@@ -112,9 +117,9 @@ m2_IMUData = importIMUData(2, 0), importIMUData(2, 1)
 
 # NED Translation & Force Functions
 INIT_EULER_ANGLES = (0, 0, 0)
-def attitude_rate_2NED(angles, euler_angles):
+def attitude_rate_2NED(attitude, euler_angles):
     phi, theta, psi = euler_angles
-    p, q, r = angles
+    p, q, r = attitude
     
     transMat = np.array([ [1, sin(phi)*tan(theta), cos(phi)*tan(theta) ],
                           [0, cos(phi),            -sin(phi)           ], 
@@ -631,15 +636,17 @@ def generateTruthErrorGraphs(missionData):
 
 
 if __name__ == '__main__':
+    # Task 1 - b, c, d
     #Load, Process & Graph Mission Data for Mission 1
     print("Loading / Calculating to Mission 1 Data")
     missionData = cacheData("./tmp/m1_L_transData.dfz", calculateVelocityPosition, (m1_IMUData[0], INIT_FLIGHT_PRAMS), forceCalc=False), \
         cacheData("./tmp/m1_H_transData.dfz", calculateVelocityPosition, (m1_IMUData[1], INIT_FLIGHT_PRAMS), forceCalc=False)
     missionData = doTruthComparison(missionData, m1TruthData)
     print("Starting to Graph Mission 1")
-    errorPoly = generateTruthErrorGraphs(missionData)[0]
-    generateGraphs(missionData, 1)
+    #generateGraphs(missionData, 1)
 
+    # Task 2 - b
+    errorPoly = generateTruthErrorGraphs(missionData)[0]
     for i, poly in enumerate(errorPoly):
         string = ""
         for n, c_n in enumerate(poly):
@@ -647,11 +654,31 @@ if __name__ == '__main__':
         
         print(f"{'High' if i else 'Low'} Grade Error Poly:" + string[2:])
 
+    # Task 3
+    t_15_data = m1TruthData[0][IMU_TIME_HEADER + [TRUTH_N_POS_HEADER[1]]].iloc[(m1TruthData[0][IMU_TIME_HEADER[0]]-15).abs().argsort()[:1]].values, \
+        m1TruthData[2][IMU_TIME_HEADER + [TRUTH_FSP_X_HEADER[1]]].iloc[(m1TruthData[2][IMU_TIME_HEADER[0]]-15).abs().argsort()[:1]].values
+    
+    if t_15_data[0][0][0] == t_15_data[1][0][0]:
+        t_15_data = pd.DataFrame({
+           IMU_TIME_HEADER[0]     : [t_15_data[0][0][0]],
+           TRUTH_N_POS_HEADER[1]  : [t_15_data[0][0][1]],
+           TRUTH_FSP_X_HEADER[1]  : [t_15_data[1][0][1]]
+        })
+    else:
+        print(t_15_data)
+        raise ValueError("Something broke :(")
+
+    print(t_15_data)
+    
+    # Task 4
     #Load, Process & Graph Mission Data for Mission 2
-    print("Loading / Calculating to Mission 2 Data")
+    print("\nLoading / Calculating to Mission 2 Data")
     missionData = cacheData("./tmp/m2_L_transData.dfz", calculateVelocityPosition, (m2_IMUData[0], INIT_FLIGHT_PRAMS), forceCalc=False), \
         cacheData("./tmp/m2_H_transData.dfz", calculateVelocityPosition, (m2_IMUData[1], INIT_FLIGHT_PRAMS), forceCalc=False)
     print("Starting to Graph Mission 2")
     generateGraphs(missionData, 2)
 
+    
+
+
     print("Complete")
\ No newline at end of file