def reset(self, debug=False):
"""Reset the module and read the boot message.
ToDo: Interpret the boot message and do something reasonable with
it, if possible."""
boot_log = []
if debug:
start = micros()
self._execute_command(CMDS_GENERIC['RESET'], debug=debug)
# wait for module to boot and messages appearing on self.uart
timeout = 300
while not self.uart.any() and timeout > 0:
delay(10)
timeout -= 1
if debug and timeout == 0:
print("%8i - RX timeout occured!" % (elapsed_micros(start)))
# wait for messages to finish
timeout = 300
while timeout > 0:
if self.uart.any():
boot_log.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(boot_log[-1])))
delay(20)
timeout -= 1
if debug and timeout == 0:
print("%8i - RTimeout occured while waiting for module to boot!" %
(elapsed_micros(start)))
return boot_log[-1].rstrip() == b'ready'
def balance():
gangle = 0.0
start = pyb.micros()
controlspeed = 0
fspeed = 0
while abs(gangle) < 45: # give up if inclination angle >=45 degrees
angle = imu.pitch()
rate = imu.get_gy()
gangle = compf(gangle, angle, rate, pyb.elapsed_micros(start), 0.99)
start = pyb.micros()
# speed control
actualspeed = (motor1.get_speed() + motor2.get_speed()) / 2
fspeed = 0.95 * fspeed + 0.05 * actualspeed
cmd = radio.poll() # cmd[0] is turn speed, cmd[1] is fwd/rev speed
tangle = speedcontrol(800 * cmd[1], fspeed)
# stability control
controlspeed += stability(tangle, gangle, rate)
controlspeed = constrain(controlspeed, -MAX_VEL, MAX_VEL)
# set motor speed
motor1.set_speed(-controlspeed - int(300 * cmd[0]))
motor2.set_speed(-controlspeed + int(300 * cmd[0]))
pyb.udelay(5000 - pyb.elapsed_micros(start))
# stop and turn off motors
motor1.set_speed(0)
motor2.set_speed(0)
motor1.set_off()
motor2.set_off()
def getcal(self, minutes=5):
rtc.calibration(0) # Clear existing cal
self.save_time() # Set DS3231 from RTC
self.await_transition(
) # Wait for DS3231 to change: on a 1 second boundary
tus = pyb.micros()
st = rtc.datetime()[7]
while rtc.datetime()[7] == st: # Wait for RTC to change
pass
t1 = pyb.elapsed_micros(
tus) # t1 is duration (uS) between DS and RTC change (start)
rtcstart = nownr() # RTC start time in mS
dsstart = utime.mktime(self.get_time()) # DS start time in secs
pyb.delay(minutes * 60000)
self.await_transition() # DS second boundary
tus = pyb.micros()
st = rtc.datetime()[7]
while rtc.datetime()[7] == st:
pass
t2 = pyb.elapsed_micros(
tus) # t2 is duration (uS) between DS and RTC change (end)
rtcend = nownr()
dsend = utime.mktime(self.get_time())
dsdelta = (
dsend - dsstart
) * 1000000 # Duration (uS) between DS edges as measured by DS3231
rtcdelta = (
rtcend - rtcstart
) * 1000 + t1 - t2 # Duration (uS) between DS edges as measured by RTC and corrected
ppm = (1000000 * (rtcdelta - dsdelta)) / dsdelta
return int(-ppm / 0.954)
def wait(self, delay):
t0 = pyb.millis()
if delay == 0:
return
if delay == -1:
# if delay == -1 the queue got emptied without stopping the loop
blue_led.on()
return
blue_led.off()
self._led.on()
ust = pyb.micros()
# Do possibly useful things in our idle time
if delay > 6:
gct0 = pyb.micros()
gc.collect() # we have the time, so might as well clean up
self.max_gc_us = max(pyb.elapsed_micros(gct0), self.max_gc_us)
while pyb.elapsed_millis(t0) < delay:
# Measure the idle time
# Anything interesting at this point will require an interrupt
# If not some pin or peripheral or user timer, then it will be
# the 1ms system-tick interrupt, which matches our wait resolution.
# So we can save power by waiting for interrupt.
pyb.wfi()
self.idle_us += pyb.elapsed_micros(ust)
self._led.off()
def balance():
gangle = 0.0
start = pyb.micros()
controlspeed = 0
fspeed = 0
while abs(gangle) < 45: # give up if inclination angle >=45 degrees
angle = imu.pitch()
rate = imu.get_gy()
gangle = compf(gangle, angle, rate, pyb.elapsed_micros(start),0.99)
start = pyb.micros()
# speed control
actualspeed = (motor1.get_speed()+motor2.get_speed())/2
fspeed = 0.95 * fspeed + 0.05 * actualspeed
cmd = radio.poll() # cmd[0] is turn speed, cmd[1] is fwd/rev speed
tangle = speedcontrol(800*cmd[1],fspeed)
# stability control
controlspeed += stability(tangle, gangle, rate)
controlspeed = constrain(controlspeed,-MAX_VEL,MAX_VEL)
# set motor speed
motor1.set_speed(-controlspeed-int(300*cmd[0]))
motor2.set_speed(-controlspeed+int(300*cmd[0]))
pyb.udelay(5000-pyb.elapsed_micros(start))
# stop and turn off motors
motor1.set_speed(0)
motor2.set_speed(0)
motor1.set_off()
motor2.set_off()
def process_acquisition(self):
"""Continue processing the acquisition. To be called periodically within the mainloop.
This is where the state machine keeps track on progress. """
if self._ms5837 and self._tsys01:
acquisition_duration = 0
if self._ms5837_awaiting_valid_measurements:
start_micro = pyb.micros()
read_success = self._ms5837.read()
if read_success:
self._ms5837_pressure = self._ms5837.pressure()
self._ms5837_temperature = self._ms5837.temperature()
self._ms5837_awaiting_valid_measurements = False
acquisition_duration += pyb.elapsed_micros(start_micro)
pyb.delay(10) # in ms
if self._tsys01_awaiting_valid_measurements:
start_micro = pyb.micros()
read_success = self._tsys01.read()
if read_success:
self._tsys01_temperature = self._tsys01.temperature()
self._tsys01_awaiting_valid_measurements = False
acquisition_duration += pyb.elapsed_micros(start_micro)
self._acquisition_duration = acquisition_duration
else:
raise Exception("Sensors are not initialized!")
def timing(): # Test removes overhead of pyb function calls
t = pyb.micros()
fir(data, coeffs, 100)
t1 = pyb.elapsed_micros(t)
t = pyb.micros()
fir(data, coeffs, 100)
fir(data, coeffs, 100)
t2 = pyb.elapsed_micros(t)
print(t2-t1,"uS")
def timing():
t = pyb.micros()
avg(data, 10)
t1 = pyb.elapsed_micros(t) # Time for one call with timing overheads
t = pyb.micros()
avg(data, 10)
avg(data, 10)
t2 = pyb.elapsed_micros(t) # Time for two calls with timing overheads
print(t2 - t1, "uS") # Time to execute the avg() call
def blink_micros():
start = pyb.micros()
led.on()
while pyb.elapsed_micros(start) < 100000:
pass
led.off()
while pyb.elapsed_micros(start) < 200000:
pass
led.on()
while pyb.elapsed_micros(start) < 300000:
pass
led.off()
while pyb.elapsed_micros(start) < 1000000:
pass
def testfunc(a):
start = pyb.micros()
while not a.mag_ready:
pass
dt = pyb.elapsed_micros(start)
print("Wait time = {:5.2f}mS".format(dt / 1000))
start = pyb.micros()
xyz = a.mag.xyz
dt = pyb.elapsed_micros(start)
print("Time to get = {:5.2f}mS".format(dt / 1000))
print("x = {:5.3f} y = {:5.3f} z = {:5.3f}".format(xyz[0], xyz[1], xyz[2]))
print("Mag status should be not ready (False): ", a.mag_ready)
print("Correction factors: x = {:5.3f} y = {:5.3f} z = {:5.3f}".format(
a.mag_correction[0], a.mag_correction[1], a.mag_correction[2]))
def blink_micros():
start = pyb.micros()
led.on()
while pyb.elapsed_micros(start) < 100000:
pass
led.off()
while pyb.elapsed_micros(start) < 200000:
pass
led.on()
while pyb.elapsed_micros(start) < 300000:
pass
led.off()
while pyb.elapsed_micros(start) < 1000000:
pass
def IRQ(self, pin):
if self.return_pin.value():
self.start_micros = micros()
elif self.start_micros is not None:
self.elapsed = elapsed_micros(self.start_micros)
else:
self.bogus = True
def thr_instrument(objSch, lstResult):
yield # Don't measure initialisation phase (README.md)
while True:
start = pyb.micros() # More typically we'd measure our own code
yield # but here we're measuring yield delays
lstResult[0] = max(lstResult[0], pyb.elapsed_micros(start))
lstResult[1] += 1
def thr_instrument(objSch, lstResult):
yield # Don't measure initialisation phase (README.md)
while True:
start = pyb.micros() # More typically we'd measure our own code
yield # but here we're measuring yield delays
lstResult[0] = max(lstResult[0], pyb.elapsed_micros(start))
lstResult[1] += 1
def ni():
print ('Native: how many instructions can we do in 100 us')
count = 0
u = pyb.micros()
while pyb.elapsed_micros(u) < 100:
count+=1
print(count)
def print_elapsed_time(baud, start_time, bursts, nr_bytes):
time = elapsed_micros(start_time)
t_p_burst = time / bursts
nb = ''
if bursts > 1 and t_p_burst > 500: # Set to discrimanate between nnormal and "slow" transfers
nb = 'SLOW !'
print("%6s %7.3f Mbaud Tot_time:%7.3f s %6.1f us between calls" % (nb, baud / 1e6, time / 1e6, t_p_burst))
def update_nomag(self, accel, gyro): # 3-tuples (x, y, z) for accel, gyro
ax, ay, az = accel # Units G (but later normalised)
gx, gy, gz = (radians(x) for x in gyro) # Units deg/s
if self.start_time is None:
self.start_time = pyb.micros() # First run
q1, q2, q3, q4 = (self.q[x] for x in range(4)
) # short name local variable for readability
# Auxiliary variables to avoid repeated arithmetic
_2q1 = 2 * q1
_2q2 = 2 * q2
_2q3 = 2 * q3
_2q4 = 2 * q4
_4q1 = 4 * q1
_4q2 = 4 * q2
_4q3 = 4 * q3
_8q2 = 8 * q2
_8q3 = 8 * q3
q1q1 = q1 * q1
q2q2 = q2 * q2
q3q3 = q3 * q3
q4q4 = q4 * q4
# Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az)
if (norm == 0):
return # handle NaN
norm = 1 / norm # use reciprocal for division
ax *= norm
ay *= norm
az *= norm
# Gradient decent algorithm corrective step
s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
norm = 1 / sqrt(
s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude
s1 *= norm
s2 *= norm
s3 *= norm
s4 *= norm
# Compute rate of change of quaternion
qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
# Integrate to yield quaternion
deltat = pyb.elapsed_micros(self.start_time) / 1000000
self.start_time = pyb.micros()
q1 += qDot1 * deltat
q2 += qDot2 * deltat
q3 += qDot3 * deltat
q4 += qDot4 * deltat
norm = 1 / sqrt(
q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion
self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
def timer():
start = pyb.micros()
#reform_list(([[1,2,3,4],[1,2,3,4],[1,2,3,4],[1,2,3,4]],15,[[1,2,3,4],[1,2,3,4],[1,2,3,4],[1,2,3,4]]))
#Encryptie.Vercijfering(12,[255,255,1,0,255,110,211,0,0,255,144,255,0,5,10,240])
#read(NB_READINGS)
encrypt(
[255, 255, 1, 0, 255, 110, 211, 0, 0, 255, 144, 255, 0, 5, 10, 240])
return pyb.elapsed_micros(start)
def timing(): # Check magnetometer call timings
imu.mag_triggered = False # May have been left True by above code
start = pyb.micros()
imu.get_mag_irq()
t1 = pyb.elapsed_micros(start)
start = pyb.micros()
imu.get_mag_irq()
t2 = pyb.elapsed_micros(start)
pyb.delay(200)
start = pyb.micros()
imu.get_mag_irq()
t3 = pyb.elapsed_micros(start)
# 1st call initialises hardware
# 2nd call tests it (not ready)
# 3rd call tests ready (it will be after 200mS) and reads data
print(t1, t2, t3) # 1st call takes 265uS second takes 175uS. 3rd takes 509uS
def testfunc(a):
start = pyb.micros()
while not a.mag_ready:
pass
dt = pyb.elapsed_micros(start)
print("Wait time = {:5.2f}mS".format(dt / 1000))
start = pyb.micros()
xyz = a.mag.xyz
dt = pyb.elapsed_micros(start)
print("Time to get = {:5.2f}mS".format(dt / 1000))
print("x = {:5.3f} y = {:5.3f} z = {:5.3f}".format(xyz[0], xyz[1], xyz[2]))
print("Mag status should be not ready (False): ", a.mag_ready)
print(
"Correction factors: x = {:5.3f} y = {:5.3f} z = {:5.3f}".format(
a.mag_correction[0], a.mag_correction[1], a.mag_correction[2]
)
)
def timing(): # Check magnetometer call timings
imu.mag_triggered = False # May have been left True by above code
start = pyb.micros()
imu.get_mag_irq()
t1 = pyb.elapsed_micros(start)
start = pyb.micros()
imu.get_mag_irq()
t2 = pyb.elapsed_micros(start)
pyb.delay(200)
start = pyb.micros()
imu.get_mag_irq()
t3 = pyb.elapsed_micros(start)
# 1st call initialises hardware
# 2nd call tests it (not ready)
# 3rd call tests ready (it will be after 200mS) and reads data
print(t1, t2, t3) # 1st call takes 265uS second takes 175uS. 3rd takes 509uS
def writer():
global handlerIndex,outData,interruptCounter,period,start
if outData.nbElts:
pdov(outData.get()%2)
else:
handlerIndex ^=1
period[interruptCounter] = pyb.elapsed_micros(start)
interruptCounter+=1
start=pyb.micros()
def writerCirc():
global handlerIndex,outData,tbInData,interruptCounter,period,start
if outData.nbElts:
tbInData.put(outData.get())
else:
handlerIndex ^=1
period[interruptCounter] = pyb.elapsed_micros(start)
interruptCounter+=1
start=pyb.micros()
def update_nomag(self, accel, gyro): # 3-tuples (x, y, z) for accel, gyro
ax, ay, az = accel # Units G (but later normalised)
gx, gy, gz = (radians(x) for x in gyro) # Units deg/s
if self.start_time is None:
self.start_time = pyb.micros() # First run
q1, q2, q3, q4 = (self.q[x] for x in range(4)) # short name local variable for readability
# Auxiliary variables to avoid repeated arithmetic
_2q1 = 2 * q1
_2q2 = 2 * q2
_2q3 = 2 * q3
_2q4 = 2 * q4
_4q1 = 4 * q1
_4q2 = 4 * q2
_4q3 = 4 * q3
_8q2 = 8 * q2
_8q3 = 8 * q3
q1q1 = q1 * q1
q2q2 = q2 * q2
q3q3 = q3 * q3
q4q4 = q4 * q4
# Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az)
if (norm == 0):
return # handle NaN
norm = 1 / norm # use reciprocal for division
ax *= norm
ay *= norm
az *= norm
# Gradient decent algorithm corrective step
s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude
s1 *= norm
s2 *= norm
s3 *= norm
s4 *= norm
# Compute rate of change of quaternion
qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
# Integrate to yield quaternion
deltat = pyb.elapsed_micros(self.start_time) / 1000000
self.start_time = pyb.micros()
q1 += qDot1 * deltat
q2 += qDot2 * deltat
q3 += qDot3 * deltat
q4 += qDot4 * deltat
norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion
self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
def tone1(freq):
t0 = micros()
dac = DAC(1)
while True:
theta = 2*math.pi*float(elapsed_micros(t0))*freq/1e6
fv = math.sin(theta)
v = int(126.0 * fv) + 127
#print("Theta %f, sin %f, scaled %d" % (theta, fv, v))
#delay(100)
dac.write(v)
def readerCirc():
global bitsExpected,inData,tbOutData,interruptCounter,period,start
if bitsExpected == inData.nbElts:
# we're done
stop()
else:
inData.put(tbOutData.get())
period[interruptCounter] = pyb.elapsed_micros(start)
interruptCounter+=1
start=pyb.micros()
def get_gear(self): if wheel_time==0: return 0 d_wheel_time=elapsed_micros(wheel_start)*(wheel_time-l_wheel_time)/wheel_time rpm_ratio_diff=lambda ratio: abs(self.rpm_times/self.rpm_count*ratio-wheel_time-d_wheel_time) diffs=tuple(map(rpm_ratio_diff,self.gears)) d=min(diffs) if d>(wheel_time+d_wheel_time)/5: return 0 return diffs.index(d)+1
def test_render_time(self):
# Rendering takes the expected amount of time
n = 10
t0 = pyb.micros()
for i in range(n):
self.p.render()
dt = pyb.elapsed_micros(t0)
average_ms = dt / (n * 1000)
print("%d renders average %f ms" % (n, average_ms), end='')
self.assertTrue(average_ms < 15, "average render time %f ms" % (average_ms))
def update(self, round_start):
if not self.ignited:
while elapsed_micros(round_start) < self.ign_point:
self.pin.low()
self.pin.high()
udelay(self.ignition_length)
self.pin.low()
self.ignited = True
else:
self.pin.low()
def pulseIn(pin, value, timeout=100000, width=1000000):
try:
if value == LOW:
v0 = 1
v1 = 0
elif value == HIGH:
v0 = 0
v1 = 1
start = pyb.micros()
while pin.value() == v0:
if pyb.elapsed_micros(start) > timeout:
return
start = pyb.micros()
while pin.value() == v1:
if pyb.elapsed_micros(start) >= width:
break
return pyb.elapsed_micros(start)
except:
pass
def test_render_time(self):
# Rendering takes the expected amount of time
n = 10
t0 = pyb.micros()
for i in range(n):
self.p.render()
dt = pyb.elapsed_micros(t0)
average_ms = dt / (n * 1000)
print("%d renders average %f ms" % (n, average_ms), end='')
self.assertTrue(average_ms < 15,
"average render time %f ms" % (average_ms))
def tn(c='X8'):
print ('NATIVE: how many pin values can we do in 100 us')
p = pyb.Pin(c,pyb.Pin.OUT_PP,pull=pyb.Pin.PULL_NONE)
bit = 1
p.value(bit)
count = 0
u = pyb.micros()
while pyb.elapsed_micros(u) < 100:
bit ^= 1
p.value(bit)
count+=1
print(count)
def run(out,bitsIn,doTimeOut): #,bitsOut=10):
"""
This is the workhorse of the module
"""
global outData,inData,bitsExpected,bits2Send,outBitCount,inBitCount,handlerIndex
## experimental stuff
global resetOnExit
### end experimental stuff
inBitCount=outBitCount=0
bitsExpected = bitsIn
#bits2Send = bitsOut
bits2Send = 10 # len(out)
handlerIndex = 1
# a place for the host to receive data from the trackball
inData=[None for i in range(bitsExpected)]
# a place for the host to keep data to send to the trackball and
outData = [b for b in out]
# set up for loop exit
doneWriting=False
doneReading=False
# experimental stuff
# implement a time out to return from function if timed out
# time out value is (nb total bits/11) 1200uSecs, ie. the frame time is around 1100 us so give it
# a tiny bit more
resetOnExit=False
timeOut = (11 + bitsIn) * 1200 /11
start = pyb.micros()
tFunc = lambda : pyb.elapsed_micros(start)>timeOut
if not doTimeOut:
tFunc = lambda : False
#### end of experimental secion
dv(0) # this is the start bit!
cv(1)
while True:
doneWriting = (outBitCount == bits2Send)
doneReading = (inBitCount == bitsExpected)
if doneWriting and doneReading:
#print('Done.')
break
elif tFunc():
#print('timed out...')
pyb.delay(1)
resetOnExit = True
break
def integrate_continuously(self, nap=10):
#print("integrating continuously, napping %d" % nap)
tscale = 1 / 1000000
then = pyb.micros()
#should_be_less_than = (nap + 30) * 1000
while True:
dt = pyb.elapsed_micros(then)
#if dt >= should_be_less_than:
# print("integration dt was", dt)
then = pyb.micros()
self.integrate(dt * tscale)
self.show_balls()
yield from sleep(nap)
def poll(self):
for i, pin in enumerate(self.pinval):
if pyb.elapsed_micros(self.timer[i]) > self.rate[i]:
if pin > self.target[i]:
self.pinval[i] -= 1
elif pin < self.target[i]:
self.pinval[i] += 1
LED[i]['B'].pwm(self.pinval[i])
self.timer[i] = pyb.micros()
def getcal(self, minutes=5):
rtc.calibration(0) # Clear existing cal
self.save_time() # Set DS3231 from RTC
self.await_transition() # Wait for DS3231 to change: on a 1 second boundary
tus = pyb.micros()
st = rtc.datetime()[7]
while rtc.datetime()[7] == st: # Wait for RTC to change
pass
t1 = pyb.elapsed_micros(tus) # t1 is duration (uS) between DS and RTC change (start)
rtcstart = nownr() # RTC start time in mS
dsstart = utime.mktime(self.get_time()) # DS start time in secs
pyb.delay(minutes * 60000)
self.await_transition() # DS second boundary
tus = pyb.micros()
st = rtc.datetime()[7]
while rtc.datetime()[7] == st:
pass
t2 = pyb.elapsed_micros(tus) # t2 is duration (uS) between DS and RTC change (end)
rtcend = nownr()
dsend = utime.mktime(self.get_time())
dsdelta = (dsend - dsstart) * 1000000 # Duration (uS) between DS edges as measured by DS3231
rtcdelta = (rtcend - rtcstart) * 1000 + t1 -t2 # Duration (uS) between DS edges as measured by RTC and corrected
ppm = (1000000* (rtcdelta - dsdelta))/dsdelta
return int(-ppm/0.954)
def run(out,bitsIn=33,bitsOut=10):
"""
try some advanced tests of reading and writing
"""
global outData,inData,bitsExpected,bits2Send,outBitCount,inBitCount,handlerIndex #period,interruptCounter,
inBitCount=outBitCount=0
#interruptCounter=0
bitsExpected = bitsIn
bits2Send = bitsOut
handlerIndex = 1
# a place for the host to receive data from the trackball
inData=[None for i in range(bitsExpected)]
# a place for the host to keep data to send to the trackball and
# bits for the host to send to the trackball
#outData= [i for i in range(bitsOut)]
#outData = [0]
#outData = [1 for i in range(bitsOut)]
outData = [b for b in out]
# a place to keep the timing measurements
period=[None for i in range(bitsOut+bitsIn+2)]
# set up for loop exit
doneWriting=False
doneReading=False
# give it a kick and release the wild PS/2 beast!
init(False)
#pyb.delay(10)
#cv(0)
#pyb.udelay(300)
dv(0) # this is the start bit!
cv(1)
while True:
doneWriting = (outBitCount == bits2Send)
doneReading = (inBitCount == bitsExpected)
if doneWriting and doneReading:
print('Done.')
break
init(False)
fulltime = pyb.elapsed_micros(start)
print('full time:\t' +repr(fulltime))
print('time/op:\t' +repr(fulltime/(bits2Send+bitsExpected)))
def _update(self):
diff_time = elapsed_micros(self._time)
self._time += diff_time
self._buf = diff_time
del diff_time
if self._buf % 200 > 101:
self._buf = (self._buf // 200) + 1
else:
self._buf //= 200
if self._buf == 0:
self._buf = 1
if self._state == 0:
if not self._impulse:
self._state += 1
self._res = 0 << self._buf
else:
if self._impulse:
self._res <<= self._buf
for i in range(self._buf):
self._res |= 1 << i
else:
self._res <<= self._buf
if (self._res & 0x7F) == 0x38:
self._buf = 1
for i in range(1, self._SHIFT):
self._buf = (self._buf << 1) + 1
channel_buf = ((self._res >> 6) & self._buf) & 0x7F # channel_buf is byte
if channel_buf == self._channel:
self._button_id = self._res >> (6 + self._SHIFT)
while not (self._button_id & 1):
self._button_id >>= 1
self._timeout_mark = millis()
self._state = 0
self._res = 0
self._impulse = not self._impulse
def get_power(self,v=0): v/=3.6 m_mz=105 m_drv=90 cw=0.7 A=1.0 rho=1.189 P_cw=v*(cw*A*rho/2*v**2) t=elapsed_micros(wheel_start)/1000000 P=P_cw+self.P_a+self.P_h P_kW=P/1000 if P_kW<0: return 0 elif P_kW>20: return 20 else: return P_kW
def goVelocityAngular(self, omega_alt, omega_az, omega_rot, tstep=1):
Walt = omega_alt
Waz = omega_az
Wrot = omega_rot
print('Walt: ' + str(Walt) + ' Waz: ' + str(Waz) + ' Wrot: ' +
str(Wrot))
V = [0, 0, 0]
# Use current position to find new target position
vcp = pyb.USB_VCP()
while not vcp.any(): # causes any keypress to interrupt
alt_new = self.alt_cur + Walt * tstep
az_new = self.az_cur + Waz * tstep
rot_new = self.rot_cur + Wrot * tstep
L_new = self.goToAngles(alt_new, az_new, rot_new)
print('New Angles are: ' + str(alt_new) + ' ' + str(az_new) + ' ' +
str(rot_new))
print('L_new is: ' + str(L_new))
L1 = self.actuatorOne.getCurrentLength() + self.L_p1min
L2 = self.actuatorTwo.getCurrentLength() + self.L_p2min
L3 = self.actuatorThree.getCurrentLength() + self.L_p3min
V[0] = (L_new[0] - (L1)) / tstep / NUMBER_OF_MICROSTEPS
V[1] = (L_new[1] - (L2)) / tstep / NUMBER_OF_MICROSTEPS
V[2] = (L_new[2] - (L3)) / tstep / NUMBER_OF_MICROSTEPS
print('Current Lengths are: ' + str(L1) + ' ' + str(L2) + ' ' +
str(L3))
print('Velocities are: ' + str(V))
self.actuatorOne.commandVelocity(V[0])
self.actuatorTwo.commandVelocity(V[1])
self.actuatorThree.commandVelocity(V[2])
start = pyb.micros()
self.alt_cur = alt_new
self.az_cur = az_new
self.rot_cur = rot_new
while pyb.elapsed_micros(start) < int(tstep * (10**6)):
pass
self.actuatorOne.stop()
self.actuatorTwo.stop()
self.actuatorThree.stop()
self.alt_cur = alt_new
self.az_cur = az_new
self.rot_cur = rot_new
return ([self.alt_cur, self.az_cur, self.rot_cur])
def align():
lcd.clear()
lcd.text("Acc",0,56,1)
lcd.text("CompF",64,56,1)
graphics.drawCircle(lcd,32,26,26,1)
graphics.drawCircle(lcd,96,26,26,1)
start = pyb.micros()
cangle = 90.0
while abs(cangle)>2.0:
angle = imu.pitch()
cangle = compf(cangle, angle, imu.get_gy(), pyb.elapsed_micros(start),0.91)
start = pyb.micros()
graphics.line(lcd,32,26,angle,24,1)
graphics.line(lcd,96,26,cangle,24,1)
lcd.display()
graphics.line(lcd,32,26,angle,24,0)
graphics.line(lcd,96,26,cangle,24,0)
lcd.clear()
lcd.text("Start balancing!.",0,24,1)
lcd.text('zero:{:5.2f}'.format(cangle),0,32,1)
lcd.display()
def align():
lcd.clear()
lcd.text("Acc", 0, 56, 1)
lcd.text("CompF", 64, 56, 1)
graphics.drawCircle(lcd, 32, 26, 26, 1)
graphics.drawCircle(lcd, 96, 26, 26, 1)
start = pyb.micros()
cangle = 90.0
while abs(cangle) > 2.0:
angle = imu.pitch()
cangle = compf(cangle, angle, imu.get_gy(), pyb.elapsed_micros(start),
0.91)
start = pyb.micros()
graphics.line(lcd, 32, 26, angle, 24, 1)
graphics.line(lcd, 96, 26, cangle, 24, 1)
lcd.display()
graphics.line(lcd, 32, 26, angle, 24, 0)
graphics.line(lcd, 96, 26, cangle, 24, 0)
lcd.clear()
lcd.text("Start balancing!.", 0, 24, 1)
lcd.text('zero:{:5.2f}'.format(cangle), 0, 32, 1)
lcd.display()
def get_speed(self,last_velocity): if wheel_time==0: self.P_a=0 return 0 d_wheel_time=elapsed_micros(wheel_start)*(wheel_time-l_wheel_time)/wheel_time velocity= wheel_scope/1000/((wheel_time+d_wheel_time)/1000000)*3600 dv=(velocity-last_velocity)/3.6 m=195 self.P_a=m*dv**2/(wheel_time/1000000) if dv<0: self.P_a*=-1 yaw,pitch,roll=self.read_arduino() m=105+90 g=9.81 self.P_h=m*g*sin(roll*pi/180)*velocity/3.6*(wheel_time+d_wheel_time)/1000000 #print(P_h) if 0<velocity<150: return velocity return -1
def timernot():
start = pyb.micros()
x = 0
for i in range(10000):
x += 1
return pyb.elapsed_micros(start)
compTimePack = struct.unpack(compTimeFmt, compTimeBuff)
compTimeStr = compTimePack[0].decode()
compTime = int(compTimeStr)
mjpegname = ''.join([compTimeStr, '.mjpeg'])
# Turn on red LED to indicate frame capture.
pyb.LED(RED_LED_PIN).on()
os.mkdir(compTimeStr)
f = open(''.join([compTimeStr, '/', compTimeStr, '.txt']), 'w')
endRead = False
endReadBuff = bytearray(4)
recordTime = pyb.micros()
# Find time elapsed since start signal was read.
recordLatency = pyb.elapsed_micros(readTime)
i = 0
while not endRead:
#capture 5 frames before checking for end signal
for x in range(5):
frameStart = pyb.elapsed_micros(recordTime)
img = sensor.snapshot()
img.save(compTimeStr + "/" + "%06d.jpg" % i)
f.write(str(frameStart + recordLatency))
f.write('\r\n')
i += 1
endBytesRead = usb_vcp.recv(endReadBuff, timeout=0)
if endReadBuff == b'stop':
endRead = True
pyb.LED(RED_LED_PIN).off()
vidStart = int(recordLatency * 0.001) + compTime
def _send_command(self, cmd, timeout=0, debug=False):
"""Send a command to the ESP8266 module over UART and return the
output.
After sending the command there is a 1 second timeout while
waiting for an anser on UART. For long running commands (like AP
scans) there is an additional 3 seconds grace period to return
results over UART.
Raises an CommandError if an error occurs and an CommandFailure
if a command fails to execute."""
if debug:
start = micros()
cmd_output = []
okay = False
if cmd == '' or cmd == b'':
raise CommandError("Unknown command '" + cmd + "'!")
# AT commands must be finalized with an '\r\n'
cmd += '\r\n'
if debug:
print("%8i - TX: %s" % (elapsed_micros(start), str(cmd)))
self.uart.write(cmd)
# wait at maximum one second for a command reaction
cmd_timeout = 100
while cmd_timeout > 0:
if self.uart.any():
cmd_output.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(cmd_output[-1])))
if cmd_output[-1].rstrip() == b'OK':
if debug:
print("%8i - 'OK' received!" % (elapsed_micros(start)))
okay = True
delay(10)
cmd_timeout -= 1
if cmd_timeout == 0 and len(cmd_output) == 0:
if debug == True:
print("%8i - RX timeout of answer after sending AT command!" %
(elapsed_micros(start)))
else:
print("RX timeout of answer after sending AT command!")
# read output if present
while self.uart.any():
cmd_output.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(cmd_output[-1])))
if cmd_output[-1].rstrip() == b'OK':
if debug:
print("%8i - 'OK' received!" % (elapsed_micros(start)))
okay = True
# handle output of AT command
if len(cmd_output) > 0:
if cmd_output[-1].rstrip() == b'ERROR':
raise CommandError('Command error!')
elif cmd_output[-1].rstrip() == b'OK':
okay = True
elif not okay:
# some long running commands do not return OK in case of success
# and/or take some time to yield all output.
if timeout == 0:
cmd_timeout = 300
else:
if debug:
print("%8i - Using RX timeout of %i ms" %
(elapsed_micros(start), timeout))
cmd_timeout = timeout / 10
while cmd_timeout > 0:
delay(10)
if self.uart.any():
cmd_output.append(self.uart.readline())
if debug:
print("%8i - RX: %s" %
(elapsed_micros(start), str(cmd_output[-1])))
if cmd_output[-1].rstrip() == b'OK':
okay = True
break
elif cmd_output[-1].rstrip() == b'FAIL':
raise CommandFailure()
cmd_timeout -= 1
if not okay and cmd_timeout == 0 and debug:
print("%8i - RX-Timeout occured and no 'OK' received!" %
(elapsed_micros(start)))
return cmd_output
def timerhash():
start = pyb.micros()
c = uhashlib.sha256(b).digest()
return c, pyb.elapsed_micros(start)
def new_func(*args, **kwargs):
t = pyb.micros()
result = f(*args, **kwargs)
delta = pyb.elapsed_micros(t)
print('{} Time = {:6.3f}mS'.format(myname, delta / 1000))
return result
import stm
# Since we're toggling, the real freq will be half of the freq parameter.
T5 = pyb.Timer(5, freq=20000)
T5_CH4 = T5.channel(4, pyb.Timer.OC_TOGGLE, pin=pyb.Pin.board.X4)
# MicroPython doesn't directly support this particular timer mode, so we
# set the timer up as a regular timer and then adjust the clock source
# for the timer. The clock source is configured using the SMCR register.
pyb.Pin('X1', pyb.Pin.AF_PP, af=pyb.Pin.AF1_TIM2)
T2 = pyb.Timer(2, prescaler=0, period=0x3fffffff)
# SMCR
# - ETP (bit 15) = 0 - selects rising edge on ETR input
# - ECE (bit 14) = 1 - sets external clock mode 2
smcr = stm.mem32[stm.TIM2 + stm.TIM_SMCR]
smcr &= ~(1 << 15) # clear ETP bit
smcr |= (1 << 14) # set ECE bit
stm.mem32[stm.TIM2 + stm.TIM_SMCR] = smcr
while True:
start_count = T2.counter()
start_micros = pyb.micros()
pyb.delay(100)
end_count = T2.counter()
delta_micros = pyb.elapsed_micros(start_micros)
if end_count < start_count:
end_count += 0x40000000
delta_count = end_count - start_count
print('Freq = {}'.format(int(delta_count / delta_micros * 1e6 + 0.5)))
pyb.delay(900)
def update(self, accel, gyro, mag): # 3-tuples (x, y, z) for accel, gyro and mag data
mx, my, mz = (mag[x] - self.magbias[x] for x in range(3)) # Units irrelevant (normalised)
ax, ay, az = accel # Units irrelevant (normalised)
gx, gy, gz = (radians(x) for x in gyro) # Units deg/s
if self.start_time is None:
self.start_time = pyb.micros() # First run
q1, q2, q3, q4 = (self.q[x] for x in range(4)) # short name local variable for readability
# Auxiliary variables to avoid repeated arithmetic
_2q1 = 2 * q1
_2q2 = 2 * q2
_2q3 = 2 * q3
_2q4 = 2 * q4
_2q1q3 = 2 * q1 * q3
_2q3q4 = 2 * q3 * q4
q1q1 = q1 * q1
q1q2 = q1 * q2
q1q3 = q1 * q3
q1q4 = q1 * q4
q2q2 = q2 * q2
q2q3 = q2 * q3
q2q4 = q2 * q4
q3q3 = q3 * q3
q3q4 = q3 * q4
q4q4 = q4 * q4
# Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az)
if (norm == 0):
return # handle NaN
norm = 1 / norm # use reciprocal for division
ax *= norm
ay *= norm
az *= norm
# Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz)
if (norm == 0):
return # handle NaN
norm = 1 / norm # use reciprocal for division
mx *= norm
my *= norm
mz *= norm
# Reference direction of Earth's magnetic field
_2q1mx = 2 * q1 * mx
_2q1my = 2 * q1 * my
_2q1mz = 2 * q1 * mz
_2q2mx = 2 * q2 * mx
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4
_2bx = sqrt(hx * hx + hy * hy)
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4
_4bx = 2 * _2bx
_4bz = 2 * _2bz
# Gradient descent algorithm corrective step
s1 = (-_2q3 * (2 * q2q4 - _2q1q3 - ax) + _2q2 * (2 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5 - q3q3 - q4q4)
+ _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+ _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
s2 = (_2q4 * (2 * q2q4 - _2q1q3 - ax) + _2q1 * (2 * q1q2 + _2q3q4 - ay) - 4 * q2 * (1 - 2 * q2q2 - 2 * q3q3 - az)
+ _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4)
+ _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
s3 = (-_2q1 * (2 * q2q4 - _2q1q3 - ax) + _2q4 * (2 * q1q2 + _2q3q4 - ay) - 4 * q3 * (1 - 2 * q2q2 - 2 * q3q3 - az)
+ (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
+ (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+ (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
s4 = (_2q2 * (2 * q2q4 - _2q1q3 - ax) + _2q3 * (2 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5 - q3q3 - q4q4)
+ _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+ _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude
s1 *= norm
s2 *= norm
s3 *= norm
s4 *= norm
# Compute rate of change of quaternion
qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
# Integrate to yield quaternion
deltat = pyb.elapsed_micros(self.start_time) / 1000000
self.start_time = pyb.micros()
q1 += qDot1 * deltat
q2 += qDot2 * deltat
q3 += qDot3 * deltat
q4 += qDot4 * deltat
norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion
self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
def wheel_callback(line): global wheel_start,wheel_time,l_wheel_time l_wheel_time,wheel_time=wheel_time,elapsed_micros(wheel_start) wheel_start=micros()
import pyb # This code can be run on your pyboard without modifications pyb.delay(1000) pyb.udelay(1000000) pyb.millis() pyb.micros() pyb.elapsed_millis(pyb.millis()) pyb.elapsed_micros(pyb.micros()) pyb.hard_reset() pyb.delay(1000) pyb.udelay(1000000) pyb.millis() pyb.micros()
# Choose test to run
Calibrate = True
Timing = False
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if Calibrate:
print("Calibrating. Press switch when done.")
sw = pyb.Switch()
fuse.calibrate(getmag, sw, lambda : pyb.delay(100))
print(fuse.magbias)
if Timing:
mag = imu.mag.xyz # Don't include blocking read in time
accel = imu.accel.xyz # or i2c
gyro = imu.gyro.xyz
start = pyb.micros()
fuse.update(accel, gyro, mag) # 1.65mS on Pyboard
t = pyb.elapsed_micros(start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update(imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz) # Note blocking mag read
if count % 50 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
pyb.delay(20)
count += 1
lijst.append(number)
teller += 1
number = 0
else:
lijst.append(number%256)
number = number//256
teller += 1
while teller < 16:
lijst.append(0)
teller += 1
for position in range(4):
matrix.append(lijst[position*4:position*4+4])
return matrix, pyb.elapsed_micros(start)
def MakeCTRimproved(number):
"""
Een nummer (de counter) wordt omgezet in een vier op vier matrix.
:param number: Een getal tussen de 0 en de 2**128-1
:return matrix: 4x4-matrix
"""
start = pyb.micros()
lijst = []
matrix = []
teller = 0
while number is not 0:
if number <= 255:
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if Calibrate:
print("Calibrating. Press switch when done.")
sw = pyb.Switch()
fuse.calibrate(getmag, sw, lambda: pyb.delay(100))
print(fuse.magbias)
if Timing:
mag = imu.mag.xyz # Don't include blocking read in time
accel = imu.accel.xyz # or i2c
gyro = imu.gyro.xyz
start = pyb.micros()
fuse.update(accel, gyro, mag) # 1.65mS on Pyboard
t = pyb.elapsed_micros(start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update(imu.accel.xyz, imu.gyro.xyz,
imu.mag.xyz) # Note blocking mag read
if count % 50 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
pyb.delay(20)
count += 1
def profiled_func(*args, **kwargs):
t0 = pyb.micros()
func_returns = func(*args, **kwargs)
cls.results[str(func).split(' ')[1]]['dt'].append(pyb.elapsed_micros(t0))
return func_returns
def update(self, accel, gyro, mag): # 3-tuples (x, y, z) for accel, gyro and mag data
mx, my, mz = (mag[x] - self.magbias[x] for x in range(3)) # Units irrelevant (normalised)
ax, ay, az = accel # Units irrelevant (normalised)
gx, gy, gz = (radians(x) for x in gyro) # Units deg/s
if self.start_time is None:
self.start_time = pyb.micros() # First run
q1, q2, q3, q4 = (self.q[x] for x in range(4)) # short name local variable for readability
# Auxiliary variables to avoid repeated arithmetic
_2q1 = 2 * q1
_2q2 = 2 * q2
_2q3 = 2 * q3
_2q4 = 2 * q4
_2q1q3 = 2 * q1 * q3
_2q3q4 = 2 * q3 * q4
q1q1 = q1 * q1
q1q2 = q1 * q2
q1q3 = q1 * q3
q1q4 = q1 * q4
q2q2 = q2 * q2
q2q3 = q2 * q3
q2q4 = q2 * q4
q3q3 = q3 * q3
q3q4 = q3 * q4
q4q4 = q4 * q4
# Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az)
if (norm == 0):
return # handle NaN
norm = 1 / norm # use reciprocal for division
ax *= norm
ay *= norm
az *= norm
# Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz)
if (norm == 0):
return # handle NaN
norm = 1 / norm # use reciprocal for division
mx *= norm
my *= norm
mz *= norm
# Reference direction of Earth's magnetic field
_2q1mx = 2 * q1 * mx
_2q1my = 2 * q1 * my
_2q1mz = 2 * q1 * mz
_2q2mx = 2 * q2 * mx
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4
_2bx = sqrt(hx * hx + hy * hy)
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4
_4bx = 2 * _2bx
_4bz = 2 * _2bz
# Gradient descent algorithm corrective step
s1 = (-_2q3 * (2 * q2q4 - _2q1q3 - ax) + _2q2 * (2 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5 - q3q3 - q4q4)
+ _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+ _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
s2 = (_2q4 * (2 * q2q4 - _2q1q3 - ax) + _2q1 * (2 * q1q2 + _2q3q4 - ay) - 4 * q2 * (1 - 2 * q2q2 - 2 * q3q3 - az)
+ _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4)
+ _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
s3 = (-_2q1 * (2 * q2q4 - _2q1q3 - ax) + _2q4 * (2 * q1q2 + _2q3q4 - ay) - 4 * q3 * (1 - 2 * q2q2 - 2 * q3q3 - az)
+ (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
+ (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+ (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
s4 = (_2q2 * (2 * q2q4 - _2q1q3 - ax) + _2q3 * (2 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5 - q3q3 - q4q4)
+ _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+ _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalise step magnitude
s1 *= norm
s2 *= norm
s3 *= norm
s4 *= norm
# Compute rate of change of quaternion
qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
# Integrate to yield quaternion
deltat = pyb.elapsed_micros(self.start_time) / 1000000
self.start_time = pyb.micros()
q1 += qDot1 * deltat
q2 += qDot2 * deltat
q3 += qDot3 * deltat
q4 += qDot4 * deltat
norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalise quaternion
self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
