def trim_point_test():
flightLimits = FlightLimits()
ctrlLimits = CtrlLimits()
llc = LowLevelController(ctrlLimits)
ap = GcasAutopilot(llc.xequil, llc.uequil, flightLimits, ctrlLimits)
pass_fail = FlightLimitsPFA(flightLimits)
pass_fail.break_on_error = False
# ### Initial Conditions ###
# initialState = [Vt, alpha, beta, phi, theta, psi, 0, 0, 0, 0, 0, alt, power]
initialState = [
502.0, 0.03887505597600522, 0.0, 0.0, 0.03887505597600522, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1000.0, 9.05666543872074
]
# if not None will do animation. Try a filename ending in .gif or .mp4 (slow). Using '' will plot to the screen.
animFilename = 'gcas_stable.gif'
# Select Desired F-16 Plant
f16_plant = 'morelli' # 'stevens' or 'morelli'
tMax = 15 # simulation time
xcg_mult = 1.0 # center of gravity multiplier
val = 1.0 # other aerodynmic coefficient multipliers
cxt_mult = val
cyt_mult = val
czt_mult = val
clt_mult = val
cmt_mult = val
cnt_mult = val
multipliers = (xcg_mult, cxt_mult, cyt_mult, czt_mult, clt_mult, cmt_mult,
cnt_mult)
der_func = lambda t, y: controlledF16(
t, y, f16_plant, ap, llc, multipliers=multipliers)[0]
passed, times, states, modes, ps_list, Nz_list, u_list = RunF16Sim(\
initialState, tMax, der_func, f16_plant, ap, llc, pass_fail, multipliers=multipliers)
print("Simulation Conditions Passed: {}".format(passed))
if animFilename is not None:
plot3d_anim(times,
states,
modes,
ps_list,
Nz_list,
filename=animFilename)
def main():
'main function'
flightLimits = FlightLimits()
ctrlLimits = CtrlLimits()
llc = LowLevelController(ctrlLimits)
ap = GcasAutopilot(llc.xequil, llc.uequil, flightLimits, ctrlLimits)
pass_fail = FlightLimitsPFA(flightLimits)
pass_fail.break_on_error = False
### Initial Conditions ###
power = 9 # Power
# Default alpha & beta
alpha = deg2rad(2.1215) # Trim Angle of Attack (rad)
beta = 0 # Side slip angle (rad)
# Initial Attitude
alt = 3600
Vt = 540 # Pass at Vtg = 540; Fail at Vtg = 550;
phi = (pi / 2) * 0.5 # Roll angle from wings level (rad)
theta = (-pi / 2) * 0.8 # Pitch angle from nose level (rad)
psi = -pi / 4 # Yaw angle from North (rad)
# Build Initial Condition Vectors
# state = [VT, alpha, beta, phi, theta, psi, P, Q, R, pn, pe, h, pow]
initialState = [
Vt, alpha, beta, phi, theta, psi, 0, 0, 0, 0, 0, alt, power
]
# if not None will do animation. Try a filename ending in .gif or .mp4 (slow). Using '' will plot to the screen.
animFilename = 'gcas.gif'
# Select Desired F-16 Plant
f16_plant = 'morelli' # 'stevens' or 'morelli'
tMax = 15 # simulation time
xcg_mult = 1.0 # center of gravity multiplier
val = 1.0 # other aerodynmic coefficient multipliers
cxt_mult = val
cyt_mult = val
czt_mult = val
clt_mult = val
cmt_mult = val
cnt_mult = val
multipliers = (xcg_mult, cxt_mult, cyt_mult, czt_mult, clt_mult, cmt_mult,
cnt_mult)
der_func = lambda t, y: controlledF16(
t, y, f16_plant, ap, llc, multipliers=multipliers)[0]
passed, times, states, modes, ps_list, Nz_list, u_list = RunF16Sim(\
initialState, tMax, der_func, f16_plant, ap, llc, pass_fail, multipliers=multipliers)
print("Simulation Conditions Passed: {}".format(passed))
if animFilename is not None:
plot3d_anim(times,
states,
modes,
ps_list,
Nz_list,
filename=animFilename)
def main():
'main function'
ctrlLimits = CtrlLimits()
flightLimits = FlightLimits()
llc = LowLevelController(ctrlLimits)
setpoint = 550 # altitude setpoint
ap = FixedAltitudeAutopilot(setpoint, llc.xequil, llc.uequil, flightLimits, ctrlLimits)
pass_fail = FlightLimitsPFA(flightLimits)
pass_fail.break_on_error = False
### Initial Conditions ###
power = 0 # Power
# Default alpha & beta
alpha = 0 # angle of attack (rad)
beta = 0 # Side slip angle (rad)
alt = 500 # Initial Attitude
Vt = 540 # Initial Speed
phi = 0
theta = alpha
psi = 0
# Build Initial Condition Vectors
# state = [VT, alpha, beta, phi, theta, psi, P, Q, R, pn, pe, h, pow]
initialState = [Vt, alpha, beta, phi, theta, psi, 0, 0, 0, 0, 0, alt, power]
# Select Desired F-16 Plant
f16_plant = 'morelli' # 'stevens' or 'morelli'
tMax = 15.0 # simulation time
def der_func(t, y):
'derivative function for RK45'
der = controlledF16(t, y, f16_plant, ap, llc)[0]
rv = np.zeros((y.shape[0],))
rv[0] = der[0] # air speed
rv[1] = der[1] # alpha
rv[4] = der[4] # pitch angle
rv[7] = der[7] # pitch rate
rv[11] = der[11] # altitude
rv[12] = der[12] # power lag term
rv[13] = der[13] # Nz integrator
return rv
passed, times, states, modes, ps_list, Nz_list, u_list = \
RunF16Sim(initialState, tMax, der_func, f16_plant, ap, llc, pass_fail, sim_step=0.01)
print("Simulation Conditions Passed: {}".format(passed))
# plot
filename = None # longitudinal.png
plot2d(filename, times, [
(states, [(0, 'Vt'), (11, 'Altitude')]), \
(u_list, [(0, 'Throttle'), (1, 'elevator')]), \
(Nz_list, [(0, 'Nz')]) \
])
def getSimulationsMedium(self, states):
# Initial Conditions ###
power = 0 # Power
# Default alpha & beta
# alpha = 0 # angle of attack (rad)
beta = 0 # Side slip angle (rad)
# alt = 500 # Initial Attitude
# Vt = 540 # Initial Speed
phi = 0
# theta = alpha
psi = 0
trajectories = []
for state in states:
Vt = state[0]
alt = state[1]
alpha = state[2]
theta = alpha
pitchRate = state[3]
if alt > 550:
setpoint = 500
else:
setpoint = 550
ctrlLimits = CtrlLimits()
flightLimits = FlightLimits()
llc = LowLevelController(ctrlLimits)
ap = FixedAltitudeAutopilot(setpoint, llc.xequil, llc.uequil,
flightLimits, ctrlLimits)
def der_func(t, y):
'derivative function for RK45'
der = controlledF16(t, y, self.f16_plant, ap, llc)[0]
rv = np.zeros((y.shape[0], ))
rv[0] = der[0] # air speed (Vt)
rv[1] = der[1] # alpha
rv[4] = der[4] # pitch angle
rv[7] = der[7] # pitch rate
rv[11] = der[11] # altitude (alt)
rv[12] = der[12] # power lag term
rv[13] = der[13] # Nz integrator
return rv
pass_fail = FlightLimitsPFA(flightLimits)
pass_fail.break_on_error = False
# Build Initial Condition Vectors
# state = [VT, alpha, beta, phi, theta, psi, P, Q, R, pn, pe, h, pow]
initialState = [
Vt, alpha, beta, phi, theta, psi, 0, pitchRate, 0, 0, 0, alt,
power
]
print(initialState)
passed, times, states, modes, ps_list, Nz_list, u_list = \
RunF16Sim(initialState, self.tMax, der_func, self.f16_plant, ap, llc, pass_fail, sim_step=self.sim_step)
print("Simulation Conditions Passed: {}".format(passed))
statesArr = np.array(states)
traj = np.take(statesArr, [0, 1, 4, 7, 11, 13], axis=1)
# print(traj[0])
# print(traj.T[5].shape)
traj.T[5] = np.array(Nz_list)
trajectories += [traj]
# filename = None # longitudinal.png
# plot2d(filename, times, [
# (states, [(0, 'Vt'), (11, 'Altitude')]), (u_list, [(0, 'Throttle'), (1, 'elevator')]),
# (Nz_list, [(0, 'Nz')])])
return trajectories