Sorta Finish Task 1
This commit is contained in:
Cal Wing
parent
2fb63c7309
commit
faa2400d4f
252
main.py
252
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")
|
||||
Loading…
x
Reference in New Issue
Block a user