diff --git a/main.py b/main.py index c944c67..a0aef49 100644 --- a/main.py +++ b/main.py @@ -20,14 +20,14 @@ for folder in folders: # This is a cacheing function, it checks if a cache file exists and loads it or it calcs & saves it. # TL;DR This takes a file path & either a single variable (defArg) or some sort of callable and either # loads it or dumps it to disk. Note: It does not validate the data inside! -def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tuple = (), kargs: dict = {}, defArg=None): +def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tuple = (), kargs: dict = {}, defArg=None, forceCalc=False, doFileDelete=False): if len(args) == 0 and defArg is not None: args = (defArg, ) data = None dataFileExt = dataFilePath.rsplit(".")[-1] # Check if file exists - if os.path.isfile(dataFilePath): + if os.path.isfile(dataFilePath) and not forceCalc: print(f"Found data file \"{dataFilePath}\", loading data.") # Check if file is a compressed numpy file if dataFileExt == "npz": @@ -41,6 +41,10 @@ def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tup else: raise TypeError(f"Cannot determine file type of: {dataFilePath}") else: + if doFileDelete and os.path.isfile(dataFilePath): + print(f"Found data file \"{dataFilePath}\", deleting data.") + os.remove(dataFilePath) + print(f"Could not find data file \"{dataFilePath}\", generating & saving data.") # Calculate Value data = calcFunction(*args, **kargs) @@ -62,7 +66,7 @@ def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tup if data is None: raise ValueError("Could not import or generate data requested") return data - +GRAVITY_VEC = np.array([0, 0, 9.81]) # IMU Data Loading # I map in to a heading to add units / make things make more sense @@ -96,7 +100,7 @@ m2_IMUData = importIMUData(2, 0), importIMUData(2, 1) # NED Translation & Force Functions INIT_EULER_ANGLES = (0, 0, 0) -def attidude_rate_2NED(angles, euler_angles): +def attitude_rate_2NED(angles, euler_angles): phi, theta, psi = euler_angles p, q, r = angles @@ -110,61 +114,225 @@ def attidude_rate_2NED(angles, euler_angles): [r] ]) - return np.matmul(transMat, angleMat) + return np.matmul(transMat, angleMat).flatten() -def getNEDForces(NEDPos): - phi, theta, psi = NEDPos # Roll, Pitch, Yaw +def calc_NED_acceleration(NEDAttitude, specificForces): + phi, theta, psi = NEDAttitude # Roll, Pitch, Yaw R41 = np.array([ [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi), cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi)], [sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi), sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi)], [-sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi) ] ]) - return R41 + return np.matmul(R41, specificForces) + GRAVITY_VEC # Calculate Mission Translation & Force Data -TRANS_DATA_HEADER = IMU_TIME_HEADER + IMU_WBE_HEADERS + ["Forces", "ForceSum", "ForceCumSum"] -def calculateTranslatedData(missionWBEData) -> pd.DataFrame: +NED_ACCELERATION_HEADER = ["Acceleration N [m/s^2]", "Acceleration E [m/s^2]", "Acceleration D [m/s^2]"] +NED_VELOCITY_HEADER = ["Velocity N [m/s]", "Velocity E [m/s]", "Velocity D [m/s]"] +NED_POSITION_HEADER = ["Position N [m]", "Position E [m]", "Position D [m]"] +TRANS_DATA_HEADER = IMU_TIME_HEADER + IMU_WBE_HEADERS + NED_ACCELERATION_HEADER + NED_VELOCITY_HEADER + NED_POSITION_HEADER +def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame: print("Translating Motion & Calculating Resulting Forces") + + offset = initial_values + translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER) - for i in tqdm(range(len(missionWBEData))): - dataPoint = missionWBEData[IMU_WBE_HEADERS].iloc[i] # Get the time point data - trans = attidude_rate_2NED(dataPoint.values, INIT_EULER_ANGLES).flatten() # Translate to Net - forces = getNEDForces(trans) - forceSum = np.array([np.sum(forces[:,0].flatten()), np.sum(forces[:,1].flatten()), np.sum(forces[:,2].flatten())]) - privForce = translatedData[TRANS_DATA_HEADER[5]].iloc[i-1] if i > 0 else [0, 0, 0] - forceCumSum = np.array([ forceSum[ii] + value for ii, value in enumerate(privForce) ]) - translatedData.loc[i] = { - TRANS_DATA_HEADER[0]: missionWBEData[IMU_TIME_HEADER].iloc[i], - TRANS_DATA_HEADER[1]: trans[0], - TRANS_DATA_HEADER[2]: trans[1], - TRANS_DATA_HEADER[3]: trans[2], - TRANS_DATA_HEADER[4]: forces, - TRANS_DATA_HEADER[5]: forceSum, - TRANS_DATA_HEADER[6]: forceCumSum, - } + for i in tqdm(range(len(missionData))): + mData = lambda header, j=i: missionData[header].iloc[j].values + dt = mData(IMU_TIME_HEADER, i-1)[0] - mData(IMU_TIME_HEADER, i)[0] if i > 0 else mData(IMU_TIME_HEADER, i)[0] - mData(IMU_TIME_HEADER, i+1)[0] + + # Get the time point data + dataPoint = missionData[IMU_WBE_HEADERS].iloc[i] + + # Translate to NED Frame + NED_attitude_rate = attitude_rate_2NED(dataPoint.values, INIT_EULER_ANGLES) + NED_attitude = NED_attitude_rate * dt + offset["attitude"] + NED_acceleration = calc_NED_acceleration(NED_attitude, mData(IMU_FSP_HEADERS)) + NED_velocity = NED_acceleration * dt + offset["velocity"] + NED_position = NED_velocity * dt + offset["position"] + + offset["attitude"] = NED_acceleration + offset["velocity"] = NED_velocity + offset["position"] = NED_position + + translatedData.loc[i, IMU_TIME_HEADER] = mData(IMU_TIME_HEADER) + translatedData.loc[i, IMU_WBE_HEADERS] = NED_attitude_rate + translatedData.loc[i, NED_ACCELERATION_HEADER] = NED_acceleration + translatedData.loc[i, NED_VELOCITY_HEADER] = NED_velocity + translatedData.loc[i, NED_POSITION_HEADER] = NED_position return translatedData + +INIT_FLIGHT_PRAMS = { + "position": np.array([10, 10, -50]), + "velocity": np.array([0, 0, 0]), + "attitude": np.array([0, 0, 0]) +} + +def generateGraphs(missionData, mission): + pBar = tqdm(total=4) + graphData = { + "figTitle": f"Mission {mission} Flight Characteristics", + "figTitleFontSize": 16, + "figSize": (15,20), #Yay America, this is in inches :/ # Note: cm = 1/2.54 + "yLabel": "Time [s]", + "plotDim": (3,1), + "grid": True, + "subPlots":[ + { + "title": "Roll Rate", + "yLabel": "Roll Rate [rad/s]", + "plots": [ + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[0]], "label":"Low Grade Data", "colour":"uq:purple"}, + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[0]], "label":"High Grade Data", "colour":"uq:blue"}, + ] + }, + { + "title": "Pitch Rate", + "yLabel": "Pitch Rate [rad/s]", + "plots": [ + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[1]], "label":"Low Grade Data", "colour":"uq:purple"}, + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[1]], "label":"High Grade Data", "colour":"uq:blue"}, + ] + }, + { + "title": "Yaw Rate", + "yLabel": "Yaw Rate [rad/s]", + "plots": [ + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[2]], "label":"Low Grade Data", "colour":"uq:purple"}, + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[2]], "label":"High Grade Data", "colour":"uq:blue"},\ + ] + }, + ] + } + makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_rpy.png") + pBar.update(1) + + graphData = { + "figTitle": f"Mission {mission} Flight Position", + "figTitleFontSize": 16, + "figSize": (15,15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 + "yLabel": "Time [s]", + "plotDim": (3,1), + "grid": True, + "subPlots":[ + { + "title": "North Position", + "yLabel": "Position [m]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + { + "title": "East Position", + "yLabel": "Position [m]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + { + "title": "Down Position", + "yLabel": "Position [m]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + ] + } + makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_pos.png") + pBar.update(1) + + graphData = { + "figTitle": f"Mission {mission} Flight Velocity", + "figTitleFontSize": 16, + "figSize": (15,15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 + "yLabel": "Time [s]", + "plotDim": (3,1), + "grid": True, + "subPlots":[ + { + "title": "North Velocity", + "yLabel": "Velocity [m/s]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + { + "title": "East Velocity", + "yLabel": "Velocity [m/s]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + { + "title": "Down Velocity", + "yLabel": "Velocity [m/s]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + ] + } + makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_velocity.png") + pBar.update(1) + + graphData = { + "figTitle": f"Mission {mission} Flight Acceleration", + "figTitleFontSize": 16, + "figSize": (15,15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 + "yLabel": "Time [s]", + "plotDim": (3,1), + "grid": True, + "subPlots":[ + { + "title": "North Acceleration", + "yLabel": "Acceleration [m/s$^2$]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + { + "title": "East Acceleration", + "yLabel": "Acceleration [m/s$^2$]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + { + "title": "Down Acceleration", + "yLabel": "Acceleration [m/s$^2$]", + "plots": [ + {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, + {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, + ] + }, + ] + } + makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_accell.png") + pBar.update(1) if __name__ == '__main__': - missionWBEData = m1_IMUData[0][IMU_TIME_HEADER + IMU_WBE_HEADERS] - translatedData = cacheData("./tmp/m1_transData.dfz", calculateTranslatedData, (missionWBEData,)) - - x = np.cumsum(translatedData[IMU_WBE_HEADERS[0]]) - y = np.cumsum(translatedData[IMU_WBE_HEADERS[1]]) - z = np.cumsum(translatedData[IMU_WBE_HEADERS[2]]) + 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) - import matplotlib.pyplot as plt - fig = plt.figure() - ax = plt.axes(projection='3d') - plots = translatedData[TRANS_DATA_HEADER[6]].values - #x = [p[0] for p in plots] - #y = [p[1] for p in plots] - #z = [p[2] for p in plots] - ax.plot3D(x, y, z) - ax.plot3D(x+5, y-2, z+1) + generateGraphs(missionData, 1) - plt.show() + #import matplotlib.pyplot as plt + #fig = plt.figure() + #ax = plt.axes(projection='3d') + #x, y, z = missionData[0][NED_POSITION_HEADER[0]], missionData[0][NED_POSITION_HEADER[1]], missionData[0][NED_POSITION_HEADER[2]] + #ax.plot3D(x, y, z) + #x, y, z = missionData[1][NED_POSITION_HEADER[0]], missionData[1][NED_POSITION_HEADER[1]], missionData[1][NED_POSITION_HEADER[2]] + #ax.plot3D(x, y, z) + #plt.show() print("Complete") \ No newline at end of file