def simulate(time, altitude, velocity, timestep, PID, callback):
roll_accel = []
roll_rate = [0]
roll_angle = [0]
fin_angle = []
pids = []
for i, t in enumerate(time):
# get correction from PID loop
correction = PID.step(roll_rate[-1])
# estimate fin position
a = lv2.estimate_alpha(correction, altitude[i], velocity[i], t)
# run through physical servo model
a = lv2.servo(a, t)
# integrate motion and record
aa = lv2.angular_accel(a, altitude[i], velocity[i], t)
# adjust actual angular aceleration by caller
aa = callback(i, t, aa)
roll_accel.append(aa)
roll_angle.append(simps(roll_rate, time[:i + 1]))
roll_rate.append(simps(roll_accel, time[:i + 1]))
fin_angle.append(a)
pids.append(correction)
return roll_accel, roll_rate, roll_angle, fin_angle, pids
def test_reverselookup_net(self):
aa = lv2.angular_accel(3, 1000, 100, 10)
alpha = lv2.estimate_alpha(-aa, 1000, 100, 10)
self.assertLess(alpha, 0)
# not worse than 5% error:
percent_diff = (fabs(fabs(alpha) - 3) / 3) * 100.0
self.assertLess(percent_diff, 5)
def sim():
with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as sock:
sock.bind(('', 35020))
sock.connect(('127.0.0.1', 36000))
sock.settimeout(0.01)
net = io.Network(sock)
finangle = 0
roll = [0]
for i, t in enumerate(ortime):
if not q.empty():
# finangle = lv2.servo(q.get(), t)
finangle = q.get()
print t, finangle, r
x = altitude[i]
v = velocity[i]
r = roll[-1]
aa = lv2.angular_accel(finangle, x, v, t)
if t > 2.5 and t < 2.7:
aa = 100
r += aa/or_sample_rate
roll.append(r)
if r > 399:
r = 399
if r < -399:
r = -399
# Data to pack
data = {
'VCC': 5.0,
'Gyro_X': r/360.0,
'Gyro_Y': 0,
'Gyro_Z': 1,
'Acc_X': accel[i],
'Acc_Y': 0,
'Acc_Z': 0,
'Magn_X': 53e-6,
'Magn_Y': 0,
'Magn_Z': 0,
'Temp': 20,
'Aux_ADC': 0,
}
net.send_data(ADIS, i, data)
if i == 5:
arm()
time.sleep(0.001)
def simulate(time, altitude, velocity, timestep, PID, PID2, callback):
"""Function simulates lv2 flight
:param time: time in seconds
:param altitude: altitude in meters
:param velocity: velocity in meters/second
:param timestep: change in time (seconds)
:param PID: PID loop for controlling roll angle
:param PID2: PID loop for controlling roll rate
:param callback:
"""
roll_accel = []
roll_rate = [0]
roll_angle = [0]
fin_angle = []
pids = []
pids2 = []
for i, t in enumerate(time):
# roll angle PID loop
correction0 = PID.step(roll_angle[-1])
pids.append(correction0)
# set correction to roll rate
# PID loop target
PID2.setTarget(correction0)
# roll rate PID loop
correction1 = PID2.step(roll_rate[-1])
pids2.append(correction1)
# estimate fin position
a = lv2.estimate_alpha(correction1, altitude[i], velocity[i], t)
# run through physical servo model
a = lv2.servo(a, t)
# integrate motion and record
aa = lv2.angular_accel(a, altitude[i], velocity[i], t)
# adjust actual angular aceleration by caller
aa = callback(i, t, aa)
roll_accel.append(aa)
roll_angle.append(simps(roll_rate, time[:i + 1]))
roll_rate.append(simps(roll_accel, time[:i + 1]))
fin_angle.append(a)
pids.append(correction0)
pids2.append(correction1)
return roll_accel, roll_rate, roll_angle, fin_angle, pids, pids2
def test_reverselookup(self):
for test_alpha in range(1, 15):
for alt in range(100, 10000, 500):
for vel in range(50, 400, 50):
for t in range(30):
aa = lv2.angular_accel(test_alpha, alt, vel, t)
alpha = lv2.estimate_alpha(aa, alt, vel, t)
# not worse than 5% error:
percent_diff = (fabs(alpha - test_alpha) /
test_alpha) * 100.0
self.assertLess(percent_diff, 5)
alpha = []
pids = []
rand_stuff = []
for i, t in enumerate(time):
# current time step:
x = altitude[i]
v = velocity[i]
r = roll[-1]
#correction = pid.step(angle[-1])
correction = pid.step(r)
a = lv2.estimate_alpha(correction, x, v, t)
a = lv2.servo(a, t)
aa = lv2.angular_accel(a, x, v, t)
# step response:
#if t > 7 and t < 7.1:
# aa += 100
# random acceleration:
#aa_offset = 80 # ang acc offset in degs/s^2
aa_offset = v / 2.0
aa_rand = random.gauss(aa_offset, 20) # random ang acc on the rocket
rand_stuff.append(aa_rand)
aa += aa_rand # add random torque to rocket
# next time step:
r += aa / or_sample_rate
roll.append(r)
def test_aa_neg(self):
aa = lv2.angular_accel(-13, 4000, 100, 20)
self.assertLess(aa, 10)
def test_aa_pos(self):
aa = lv2.angular_accel(13, 4000, 100, 20)
self.assertGreater(aa, 10)
def test_aa_case1(self):
aa = lv2.angular_accel(13, 4000, 100, 20)
self.assertAlmostEqual(aa, 742.1556022, places=4)
def test_aa_case0(self):
aa = lv2.angular_accel(0.5, 3000, 300, 5)
self.assertAlmostEqual(aa, 272.8236256, places=4)
def test_aa_zero_a(self):
for velocity in range(0, 500, 100):
for altitude in range(0, 10000, 1000):
aa = lv2.angular_accel(0, altitude, velocity, 1)
self.assertEqual(aa, 0.0)
