import sys
sys.path.append('..')
from mav_viewer import MAV_Viewer
from mav_dynamics import mav_dynamics as Dynamics
import parameters.sim_params as SIM
from messages.state_msg import StateMsg
from data_viewer import data_viewer
from wind_simulation import wind_simulation
from trim import compute_trim
import numpy as np
from tools.tools import Quaternion2Euler
# initialize dynamics object
dyn = Dynamics(SIM.ts_sim)
wind = wind_simulation(SIM.ts_sim)
mav_view = MAV_Viewer()
data_view = data_viewer()
# initialize the simulation time
sim_time = SIM.t0
#get trim input and states
Va_star = 25.
gamma_star = 0.
trim_state, trim_input = compute_trim(dyn, Va_star, gamma_star)
delta = np.copy(trim_input)
# main simulation loop
[[0., 0., -Va * np.sin(gamma), 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0.]]).T
mav._state = state
mav._state = mav._state.reshape((13, 1))
mav.updateVelocityData()
forces_moments = mav.calcForcesAndMoments(delta)
f = mav._derivatives(mav._state, forces_moments)
temp = x_dot[2:] - f[2:]
J = np.linalg.norm(temp)**2
return J
if __name__ == "__main__":
mav = Dynamics(.02)
Va = 25.0
gamma = 0.0
mav._Va = Va
# x = np.array([[0., 0., -100., Va, 0., 0., # last element is 0.1 for f and 0 for f_m, Va is 25 and gamma is 0
# 1., 0., 0., 0., 0., 0., 0.]]).T
# delta = np.array([[0., 0.5, 0., 0.]]).T
# mav._state = x
# mav.updateVelocityData()
# f_m = mav.calcForcesAndMoments(delta)
# f = mav._derivatives(mav._state, f_m)
# print(delta)
# # print(f)
trim_state, trim_input = compute_trim(mav, Va,