def test_trim(*args, **kwargs):
tInit = 0.0
tFinal = 5.0
dt = 0.01
t = np.arange(tInit, tFinal, dt)
fsSim = int(1 / dt)
N = t.size
wind_n = np.zeros(3)
n_test = 10
Va = np.random.uniform(20, 30, n_test)
# R = np.inf(n_test) #
R = np.random.uniform(100, 200, n_test)
gamma = np.random.uniform(0 * np.pi / 180, 20 * np.pi / 180, n_test)
# gamma = np.zeros(n_test) #np.random.uniform(-20*np.pi/180 , 20*np.pi/180 , n_test)
pos = np.zeros(3)
for i in range(n_test):
model.Param = model.P
res = trim.trim(Va[i], R[i], gamma[i], model, print_level=5)
# res = trim.trim(35, np.inf, 0.0, model, print_level=5)
# res = trim.trim(Va[i], np.inf, gamma[i], model, print_level=5)
# res = trim.trim(Va[i], np.inf, 0.0, model, print_level=5)
#Simulation setup
# Euler = res[0][6:9]
# vel = res[0][3:6]
# omega = res[0][9:]
# u = res[1]
# quat = ng.quaternion_rpy(Euler[0], Euler[1], Euler[2])
# x = np.concatenate([pos, quat, vel, omega])
x = res[0]
u = res[1]
uav = UAV.Vehicle(x, u, wind_n, model, {})
X = np.zeros((13, N))
for k in range(N):
uav.update(t[k])
X[:, k] = uav.getState()
# print('Va: ' + str(Va[i]))
# print('R: ' + str(R[i]))
# print('gamma: ' + str(gamma[i]))
print('Va - Va_d: ' + str(uav.getAirspeed() - Va[i]))
print('gamma - gamma_d: ' + str(uav.getFlightPathAngle() - gamma[i]))
plot.plotState(t, X)
plt.show()
#Simulation setup
tInit = 0.0
tFinal = 50.0
dt = 0.01
fsSim = int(1 / dt)
t = np.arange(tInit, tFinal, dt)
nSample = t.size
wind_n = np.array([-5.0, 0.0, 0.0])
model.Param = model.P #parameters.loadParameters('uav/models/gryteX8_param.mat')
model.Param['rudder_min'] = model.Param['rudder_max'] = 0
# Trim
Va = 18
R = np.inf
gamma = 0
res = trim.trim(Va, R, gamma, model)
x_trim = res[0]
u_trim = res[1]
x_init = x_trim
u_init = u_trim
# Reference
ref_roll = np.zeros(nSample)
ref_pitch = np.zeros(nSample)
ref_pitch[round(nSample /
2):] = (20 * np.pi / 180) * np.ones(round(nSample / 2))
ref_yaw = np.zeros(nSample)
ref_V = Va * np.ones(nSample)
# Controller
tInit = 0.0
tFinal = 10.0
dt = 0.01
fsSim = int(1 / dt)
t = np.arange(tInit, tFinal, dt)
nSample = t.size
N = nSample
wind_n = np.array([0.0, 0.0, 0.0])
model.Param = model.P #parameters.loadParameters('uav/models/gryteX8_param.mat')
model.Param['rudder_min'] = model.Param['rudder_max'] = 0
# Trim
Va = 18
R = np.inf
gamma = 0
res = trim.trim(Va, R, gamma, model, print_level=5)
x_trim = res[0]
u_trim = res[1]
x_init = x_trim
u_init = u_trim
# Controller
P = model.P
pidRoll = PID(0.78, 0.01, -0.11, -0.09, 0.09, 0.1, P['aileron_min'],
P['aileron_max'])
pidPitch = PID(-0.78, -0.30, -0.16, -0.1, 0.1, 0.1, P['elevator_min'],
P['elevator_max'])
pidYaw = PID(1.08, 0.36, 0.0, -0.09, 0.09, 0.1)
pidV = PID(0.69, 1.0, 0.0, -0.9, 0.9, 1.0, P['throttle_min'],
P['throttle_max'])
pidRoll = PID(0.78, 0.01, -0.11, -0.09, 0.09, 0.1, P['aileron_min'],
P['aileron_max'])
pidPitch = PID(-0.78, -0.30, -0.16, -0.1, 0.1, 0.1, P['elevator_min'],
P['elevator_max'])
pidYaw = PID(1.08, 0.036, 0.0, -0.09, 0.09, 0.1)
pidV = PID(0.69, 10.0, 0.0, -0.09, 0.09, 1.0, P['throttle_min'],
P['throttle_max'])
def is_valid_delta(delta, a, b):
return delta < min([2 - a - b, a - 1, a + 2 * b - 1])
model.P = model.Param
res = trim.trim(35, np.inf, 0.0, model)
x_trim = res[0]
Euler_trim = ng.rpy_quaternion(x_trim[3:7])
for tracking in [False]:
f = 0.2
if tracking:
ref = design_reference(0,
np.pi / 12,
0.1,
0,
np.pi / 12,
0.1,
t,
run_test=False)
Gamma_d = ref[0]