cal/AERO4200-Ass3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Potentially Fix Graphs? (dt calcs where inverted)

Browse Source
This commit is contained in:
Cal Wing 2023-11-01 19:34:28 +10:00
parent faa2400d4f
commit 188737b05a
1 changed files with 14 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
main.py
View File

@ -120,9 +120,9 @@ 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) ]
])
[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 np.matmul(R41, specificForces) + GRAVITY_VEC
@ -134,15 +134,15 @@ TRANS_DATA_HEADER = IMU_TIME_HEADER + IMU_WBE_HEADERS + NED_ACCELERATION_HEADER
def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame:
print("Translating Motion & Calculating Resulting Forces")
offset = initial_values
offset = initial_values.copy()
translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER)
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]
mData = lambda header, j=i: missionData.loc[j, header].values
dt = mData(IMU_TIME_HEADER, i)[0] - mData(IMU_TIME_HEADER, i-1)[0] if i > 0 else mData(IMU_TIME_HEADER, i+1)[0] - mData(IMU_TIME_HEADER, i)[0]
# Get the time point data
dataPoint = missionData[IMU_WBE_HEADERS].iloc[i]
dataPoint = missionData.loc[i, IMU_WBE_HEADERS]
# Translate to NED Frame
NED_attitude_rate = attitude_rate_2NED(dataPoint.values, INIT_EULER_ANGLES)
@ -151,7 +151,7 @@ def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame:
NED_velocity = NED_acceleration * dt + offset["velocity"]
NED_position = NED_velocity * dt + offset["position"]
offset["attitude"] = NED_acceleration
offset["attitude"] = NED_attitude
offset["velocity"] = NED_velocity
offset["position"] = NED_position
@ -242,7 +242,7 @@ def generateGraphs(missionData, mission):
},
]
}
makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_pos.png")
makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos.png")
pBar.update(1)
graphData = {
@ -279,7 +279,7 @@ def generateGraphs(missionData, mission):
},
]
}
makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_velocity.png")
makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_velocity.png")
pBar.update(1)
graphData = {
@ -294,45 +294,34 @@ def generateGraphs(missionData, mission):
"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"},
{"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"},
]
},
{
"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"},
{"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"},
]
},
{
"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"},
{"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"},
]
},
]
}
makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_accell.png")
makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_accell.png")
pBar.update(1)
if __name__ == '__main__':
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)
generateGraphs(missionData, 1)
#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")