def main(param_filename,
force_recompute=False,
save_filename='/tmp/pat_glider_wind.npz'):
dm = p1_fw_dyn.DynamicModel(param_filename)
trim_args = {'h': 0, 'va': 11, 'gamma': 0}
ref_traj = p3_traj3d.LineRefTraj(p1=[0, 0, 0], p2=[250, 0, 0])
tf = 5.5
#atm = None
#atm = p3_atm.AtmosphereCalm()
#atm = p3_atm.AtmosphereCstWind([-1, 0, 0])
#atm = p3_atm.AtmosphereVgradient([0, 0, 3])
#atm = p3_atm.AtmosphereSinetWind([1, 0, 0])
atm = p3_atm.AtmosphereSinetWind([-5, 0, -1])
#atm = p3_atm.AtmosphereSinedWind([-5, 0, -1])
#atm = p3_atm.AtmosphereThermal1()
dt = 0.01
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
ctl.set_vmode(ctl.v_mode_throttle, ctl.Ue[0])
#ctl.att_ctl.reset(0, ctl.Xe) # Xe must be SixDOFAeroEuler
ctl_logger = p3_guid.GuidancePurePursuitLogger()
#p3_pu.plot_3D_wind(atm)
#p3_pu.plot_slice_wind(atm)
#plt.show()
#time, X, U = test_03_guidance.run_simulation(dm, ctl, None, tf=tf, dt=dt, trim_args=trim_args, atm=atm,
# cbk=lambda:ctl_logger.record(ctl))
time, X, U = test_03_guidance._run_or_load_sim(dm, ctl, None, tf, dt,
trim_args, atm, ctl_logger,
force_recompute,
save_filename)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
#ctl_logger.plot_debug_chronograms(time, X, U, ctl, atm)
plt.show()
def test_vctl(dm, trim_args, force_recompute=False, dt=0.01, tf=80.5, atm=None,
save_filename = '/tmp/pat_glider_vctl_w.npz'):
atm = p3_atm.AtmosphereCalm()
#atm = p3_atm.AtmosphereCstWind([1, 0, 0])
#trim_args={'h':26, 'va':17, 'gamma':0}
#ref_traj = p3_traj3d.CircleRefTraj(c=[70, 0, -15], r=80)
#ref_traj = p3_traj3d.ZStepCircleRefTraj(c=[70, 0, -10], r=80)
ref_traj = p3_traj3d.ZdStepCircleRefTraj(c=[70, 0, -10], r=120)
#ref_traj = p3_traj3d.BankedCircleRefTraj(c=[70, 0, -30], r=150)
#ref_traj = p3_traj3d.SquareRefTraj()
#ref_traj = p3_traj3d.OctogonRefTraj() # kindof broken
#ref_traj = p3_traj3d.OvalRefTraj() #
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
#ctl.set_vmode(ctl.v_mode_throttle, 0.)
#ctl.set_vmode(ctl.v_mode_vz, -1.)
#ctl.set_vmode(ctl.v_mode_alt, -12)
ctl.set_vmode(ctl.v_mode_traj, None)
ctl.set_initial_conditions((70, -80, -4), None)
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = _run_or_load_sim(dm, ctl, None, tf, dt, trim_args, atm, ctl_logger, force_recompute, save_filename)
ctl_logger.plot3D(time, X, ctl, atm, ref_traj)
ctl_logger.plot_slice_nu(time, X, U, ctl, atm)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
def test_circle(dm, trim_args, force_recompute=False, dt=0.01, tf=30.5, atm=None,
save_filename = '/tmp/pat_glider_circle.npz'):
ref_traj = p3_traj3d.CircleRefTraj(c=[0, 0, 0], r=20)
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
ctl.v_mode = ctl.v_mode_throttle; ctl.throttle_sp = ctl.Ue[0]
ctl.set_initial_conditions((0, 20, 0), None)
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = _run_or_load_sim(dm, ctl, None, tf, dt, trim_args, atm, ctl_logger, force_recompute, save_filename)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)
ctl_logger.plot3D(time, X, ctl, atm, ref_traj)
def test_slope_soaring(dm, trim_args, force_recompute=False, dt=0.01, tf=120.5,
save_filename = '/tmp/pat_glider_slope_soaring.npz'):
atm = p3_atm.AtmosphereRidge()
trim_args={'h':30, 'va':12, 'gamma':0}
#ref_traj = p3_traj3d.CircleRefTraj(c=[-10, 0, -50], r=25)
ref_traj = p3_traj3d.OvalRefTraj( _r=25., _l=200, _c=np.array([-10, 0, -50])) #
ctl = p3_guid.GuidancePurePursuit(dm, ref_traj, trim_args, dt)
ctl.v_mode = ctl.v_mode_throttle; ctl.throttle_sp = 0. # we glide
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = _run_or_load_sim(dm, ctl, None, tf, dt, trim_args, atm, ctl_logger, force_recompute, save_filename)
ctl_logger.plot3D(time, X, ctl, atm, ref_traj)
ctl_logger.plot_slice_nu(time, X, U, ctl, atm, n0=-50, n1=40)
ctl_logger.plot_chronograms(time, X, U, ctl, atm)