def __init__(self, sp, ref_impl=ctl.att_ref_default, omega=6., xi=0.8, max_vel=pu.rad_of_deg(100),
max_accel=pu.rad_of_deg(500)):
gui.Worker.__init__(self)
self.impl = ref_impl()
self.sp = sp
self.reset_outputs(sp)
self.update(sp, ref_impl, omega, xi, max_vel, max_accel)
self.do_work = True
def on_sp_changed(self, widget):
b = self.gui.b
_type = b.get_object("combo_sp_type").get_active()
names = ["spin_sp_duration", "spin_sp_step_duration", "spin_sp_step_amplitude"]
_duration, _step_duration, _step_amplitude = [b.get_object(name).get_value() for name in names]
#print('_on_sp_changed', _type, _duration, _step_duration, _step_amplitude)
_step_amplitude = pu.rad_of_deg(_step_amplitude)
self.sp.update(_type, _duration, _step_duration, _step_amplitude)
# somehow running two threads to update both references at the same time produces bogus data..
# as a workaround we simply run them one after the other
for r in self.refs:
r.update_sp(self.sp)
#r.recompute()
self.recompute_sequentially()
("$\dot{p}$", "deg/s2"), ("$\dot{q}$", "deg/s2"),
("$\dot{r}$", "deg/s2")]
for i, (title, ylab) in enumerate(plots):
ax = plt.subplot(3, 3, i + 1)
if xref is not None: plt.plot(time, pu.deg_of_rad(xref[:, i]))
pu.decorate(ax, title=title, ylab=ylab)
if sp is not None and i < 3:
plt.plot(time, pu.deg_of_rad(sp[:, i]))
return figure
dt = 1. / 512.
time = np.arange(0., 4, dt)
sp = np.zeros((len(time), 3))
sp[:,
0] = pu.rad_of_deg(45.) * scipy.signal.square(math.pi / 2 * time + math.pi)
# sp[:, 1] = pu.rad_of_deg(5.)*scipy.signal.square(math.pi/2*time)
# sp[:, 2] = pu.rad_of_deg(45.)
# sp = random_setpoint(time)
# rs = [ctl.att_ref_analytic_disc(axis=0), ctl.att_ref_analytic_cont(axis=0), ctl.att_ref_default()]
args = {
'omega': 10.,
'xi': 0.7,
'sat_vel': pu.rad_of_deg(150.),
'sat_accel': pu.rad_of_deg(1800),
'sat_jerk': pu.rad_of_deg(27000)
}
rs = [
ctl.att_ref_sat_naive(**args),
def __init__(self, type=t_static, duration=10., step_duration=5., step_ampl=pu.rad_of_deg(10.)):
self.dt = 1. / 512
self.update(type, duration, step_duration, step_ampl)
plots = [("$\phi$", "deg"), ("$\\theta$", "deg"), ("$\\psi$", "deg"),
("$p$", "deg/s"), ("$q$", "deg/s"), ("$r$", "deg/s"),
("$\dot{p}$", "deg/s2"), ("$\dot{q}$", "deg/s2"), ("$\dot{r}$", "deg/s2")]
for i, (title, ylab) in enumerate(plots):
ax = plt.subplot(3, 3, i + 1)
if xref is not None: plt.plot(time, pu.deg_of_rad(xref[:, i]))
pu.decorate(ax, title=title, ylab=ylab)
if sp is not None and i < 3:
plt.plot(time, pu.deg_of_rad(sp[:, i]))
return figure
dt = 1. / 512.
time = np.arange(0., 4, dt)
sp = np.zeros((len(time), 3))
sp[:, 0] = pu.rad_of_deg(45.) * scipy.signal.square(math.pi / 2 * time + math.pi)
# sp[:, 1] = pu.rad_of_deg(5.)*scipy.signal.square(math.pi/2*time)
# sp[:, 2] = pu.rad_of_deg(45.)
# sp = random_setpoint(time)
# rs = [ctl.att_ref_analytic_disc(axis=0), ctl.att_ref_analytic_cont(axis=0), ctl.att_ref_default()]
args = {'omega': 10., 'xi': 0.7, 'sat_vel': pu.rad_of_deg(150.), 'sat_accel': pu.rad_of_deg(1800),
'sat_jerk': pu.rad_of_deg(27000)}
rs = [ctl.att_ref_sat_naive(**args), ctl.att_ref_sat_nested(**args), ctl.att_ref_sat_nested2(**args)]
# beware that the saturation parameters of the native implementations are currently defined at compile time!!
# rs.append(ctl.AttRefIntNative(**args))
rs.append(ctl.AttRefFloatNative(**args))
xrs = [test_ref(r, time, sp) for r in rs]
figure = None
def __init__(self, **kwargs):
super(att_ref_sat_nested2, self).__init__(**kwargs)
self.sat_jerk = kwargs.get('sat_jerk', pu.rad_of_deg(3000))
self.ensure_vect('sat_jerk')
self.jerk = np.zeros(3)
self._compute_auxiliary()
def __init__(self, **kwargs):
super(att_ref_sat_naive, self).__init__(**kwargs)
self.sat_vel = kwargs.get('sat_vel', pu.rad_of_deg(100))
self.sat_accel = kwargs.get('sat_accel', pu.rad_of_deg(500))
for p in ['sat_vel', 'sat_accel']:
self.ensure_vect(p)
