Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
self.s.CompassCalEllipsoidOffset = tuple(value[0][:3])
#rtimu.resetFusion()
elif name == 'imu.rate':
self.rate = value
if not self.lastdata:
return
gyro, compass = self.lastdata
# see if gyro is out of range, sometimes the sensors read
# very high gyro readings and the sensors need to be reset by software
# this is probably a bug in the underlying driver with fifo misalignment
d = .05/self.rate # filter constant
for i in range(3): # filter gyro vector
self.avggyro[i] = (1-d)*self.avggyro[i] + d*gyro[i]
if vector.norm(self.avggyro) > .8: # 55 degrees/s
print('too high standing gyro bias, resetting sensors', gyro, self.avggyro)
self.init()
# detects the problem even faster:
if any(map(lambda x : abs(x) > 1000, compass)):
print('compass out of range, resetting', compass)
self.init()
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[3]*(beta[4]-dip(y, z)), m, g)
def ComputeDeviation(points, fit):
m, d = 0, 0
for p in points:
v = vector.sub(p[:3], fit[:3])
m += (1 - vector.dot(v, v) / fit[3]**2)**2
if len(fit) > 4:
n = vector.dot(v, p[3:]) / vector.norm(v)
if abs(n) <= 1:
ang = math.degrees(math.asin(n))
d += (fit[4] - ang)**2
else:
d += 1e111
m /= len(points)
d /= len(points)
return [m**.5, d**.5]
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[2]*(beta[3]-dip(y, z)), m, g)
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[1]*(beta[2]-dip(y, z)), m, g)