def predict(self, accel_ned_magnetic, t):
# log new prediction and apply it estimating new state
# subtract gravity
residual_accel_magnetic = vector.sub(accel_ned_magnetic, [0, 0, 1])
# rotate by declination
decl_q = quaternion.angvec2quat(self.declination.value, [0, 0, 1])
accel_true = quaternion.rotvecquat(residual_accel_magnetic, decl_q)
# apply predicted compass error
error_q = quaternion.angvec2quat(self.compass_offset.value, [0, 0, 1])
accel_ned = quaternion.rotvecquat(accel_true, error_q)
U = 9.81 * np.array(accel_ned)
if not self.use3d:
U = U[:2]
t = time.monotonic()
dt = t - self.predict_t
self.predict_t = t
if dt < 0 or dt > .5:
self.reset()
if not self.X: # do not have a trusted measurement yet, so cannot perform predictions
return
self.apply_prediction(dt, U)
self.history.append({
't': t,
'dt': dt,
'U': U,
'X': self.X,
'P': self.P
})
# filtered position
ll = xy_to_ll(self.X[0], self.X[1], *self.lastll)
self.lat.set(ll[0])
self.lon.set(ll[1])
# filtered speed and track
c = 3 if self.use3d else 2
vx = self.X[c]
vy = self.X[c + 1]
self.speed.set(math.hypot(vx, vy))
self.track.set(resolv(math.degrees(math.atan2(vx, vy)), 180))
# filtered altitude and climb
if self.use3d:
self.alt.set(self.X[2])
self.climb.set(self.X[5])
def update_alignment(self, q): a2 = 2*math.atan2(q[3], q[0]) heading_offset = a2*180/math.pi off = self.heading_off.value - heading_offset o = quaternion.angvec2quat(off*math.pi/180, [0, 0, 1]) self.alignmentQ.update(quaternion.normalize(quaternion.multiply(q, o))) self.auto_cal.SetNorm(quaternion.rotvecquat([0, 0, 1], self.alignmentQ.value))
def predict(self, ap):
if not self.source.value == 'none':
return
accel = ap.boatimu.SensorValues['accel'].value
fusionQPose = ap.boatimu.SensorValues['fusionQPose'].value
if accel and fusionQPose:
self.filtered.predict(quaternion.rotvecquat(accel, fusionQPose),
time.monotonic())
def CalibrationProcess(cal_pipe, client):
accel_points = SigmaPoints(.05**2, 12, 10)
compass_points = SigmaPoints(1.1**2, 28, 3)
norm = [0, 0, 1]
accel_calibration = RegisterCalibration(client, 'imu.accel', [[0, 0, 0, 1], 1])
compass_calibration = RegisterCalibration(client, 'imu.compass', [[0, 0, 0, 30, 0], [1, 1], 0])
client.watch('imu.alignmentQ')
if not cal_pipe: # get these through client rather than direct pipe
client.watch('imu.accel')
client.watch('imu.compass')
client.watch('imu.fusionQPose')
def debug(name):
def debug_by_name(*args):
s = ''
for a in args:
s += str(a) + ' '
client.set('imu.'+name+'.calibration.log', s)
return debug_by_name
last_compass_coverage = 0
while True:
t = time.monotonic()
addedpoint = False
down = False
while time.monotonic() - t < calibration_fit_period:
# receive pypilot messages
msg = client.receive(1)
for name in msg:
value = msg[name]
if name == 'imu.alignmentQ' and value:
norm = quaternion.rotvecquat([0, 0, 1], value)
compass_points.Reset()
elif name == 'imu.accel':
if value:
accel_points.AddPoint(value)
addedpoint = True
elif name == 'imu.compass' and down:
if value and down:
compass_points.AddPoint(value, down)
addedpoint = True
elif name == 'imu.fusionQPose':
if value:
down = quaternion.rotvecquat([0, 0, 1], quaternion.conjugate(value))
# receive calibration data
if cal_pipe:
p = cal_pipe.recv()
while p:
if 'accel' in p:
accel_points.AddPoint(p['accel'])
addedpoint = True
if 'compass' in p:
down = quaternion.rotvecquat([0, 0, 1], quaternion.conjugate(p['fusionQPose']))
compass_points.AddPoint(p['compass'], down)
addedpoint = True
p = cal_pipe.recv()
cals = [(accel_calibration, accel_points), (compass_calibration, compass_points)]
for calibration, points in cals:
calibration.age.update()
if points.Updated():
calibration.sigmapoints.set(points.Points())
if not addedpoint: # don't bother to run fit if no new data
continue
accel_points.RemoveOlder(10*60) # 10 minutes
fit = FitAccel(debug('accel'), accel_points)
if fit: # reset compass sigmapoints on accel cal
dist = vector.dist(fit[0][:3], accel_calibration.value[0][:3])
if dist > .01: # only update when bias changes more than this
if dist > .08: # reset compass cal from large change in accel bias
compass_points.Reset()
debug('accel')('reset compass from large accel bias')
accel_calibration.set(fit)
accel_calibration.points.set(accel_points.Points())
else:
debug('accel')('calibration distance too small ', dist)
compass_points.RemoveOlder(20*60) # 20 minutes
fit = FitCompass(debug('compass'), compass_points, compass_calibration.value[0], norm)
if fit:
# ignore decreasing compass coverage
new_coverage = fit[1][2]
if new_coverage < last_compass_coverage:
debug('compass')('ignoring decreasing coverage')
else:
compass_calibration.set(fit)
compass_calibration.points.set(compass_points.Points())
else:
last_compass_coverage = 0 # reset
def ang(p):
c = quaternion.rotvecquat(vector.sub(p[:3], bias), q)
d = quaternion.rotvecquat(p[3:6], q)
v = quaternion.rotvecquat(c, quaternion.vec2vec2quat(d, [0, 0, 1]))
v = vector.normalize(v)
return math.degrees(math.atan2(v[1], v[0]))
def CalibrationProcess(cal_pipe):
import os
if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()):
print(
'warning, failed to make calibration process idle, trying renice')
if os.system("renice 20 %d" % os.getpid()):
print('warning, failed to renice calibration process')
accel_cal = SigmaPoints(.05**2, 12, 10)
compass_cal = SigmaPoints(1.1**2, 24, 3)
accel_calibration = [0, 0, 0, 1]
norm = [0, 0, 1]
def on_con(client):
client.watch('imu.alignmentQ')
client.watch('imu.compass.calibration')
while True:
time.sleep(2)
try:
client = pypilotClient(on_con, 'localhost', autoreconnect=True)
break
except Exception as e:
print('nmea process failed to connect pypilot', e)
def debug(name):
def debug_by_name(*args):
s = ''
for a in args:
s += str(a) + ' '
if client:
client.set('imu.' + name + '.calibration.log', s)
return debug_by_name
while True:
t = time.time()
addedpoint = False
while time.time() - t < calibration_fit_period:
# receive pypilot messages
msg = client.receive()
for name in msg:
value = msg[name]['value']
if name == 'imu.alignmentQ' and value:
norm = quaternion.rotvecquat([0, 0, 1], value)
compass_cal.Reset()
elif name == 'imu.compass.calibration':
compass_calibration = value[0]
# receive calibration data
p = cal_pipe.recv(1)
if p:
if 'accel' in p:
accel_cal.AddPoint(p['accel'])
addedpoint = True
if 'compass' in p:
compass_cal.AddPoint(p['compass'], p['down'])
addedpoint = True
# send updated sigmapoints as well
cals = {'accel': accel_cal, 'compass': compass_cal}
for name in cals:
cal = cals[name]
if cal.Updated():
points = cal.Points()
client.set('imu.' + name + '.calibration.sigmapoints',
points)
if not addedpoint: # don't bother to run fit if no new data
continue
accel_cal.RemoveOlder(10 * 60) # 10 minutes
fit = FitAccel(debug('accel'), accel_cal)
if fit: # reset compass sigmapoints on accel cal
dist = vector.dist(fit[0][:3], accel_calibration[:3])
if dist > .01: # only update when bias changes more than this
if dist > .08: # reset compass cal from large change in accel bias
compass_cal.Reset()
accel_calibration = fit[0]
client.set('imu.accel.calibration', fit)
compass_cal.RemoveOlder(20 * 60) # 20 minutes
fit = FitCompass(debug('compass'), compass_cal, compass_calibration,
norm)
if fit:
client.set('imu.compass.calibration', fit)
compass_calibration = fit[0]
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print 'IMURead failed!'
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print 'vector n', data['accel']
return False
self.last_imuread = time.time()
self.loopfreq.strobe()
if not self.FirstTimeStamp:
self.FirstTimeStamp = data['timestamp']
data['timestamp'] -= self.FirstTimeStamp
#data['accel_comp'] = quaternion.rotvecquat(vector.sub(data['accel'], down), self.alignmentQ.value)
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
self.server.TimeStamp('imu', data['timestamp'])
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
compass, accel, down = False, False, False
if not self.accel_calibration.locked.value:
accel = list(data['accel'])
if not self.compass_calibration.locked.value:
down = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(origfusionQPose))
compass = list(data['compass']) + down
if accel or compass:
self.auto_cal.AddPoint((accel, compass, down))
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
if self.heading_off.value != self.last_heading_off:
self.update_alignment(self.alignmentQ.value)
self.last_heading_off = self.heading_off.value
result = self.auto_cal.UpdatedCalibration()
if result:
if result[0] == 'accel' and not self.accel_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.accel_calibration.sigmapoints.set(result[2])
if result[1]:
self.accel_calibration.age.reset()
self.accel_calibration.set(result[1])
elif result[
0] == 'compass' and not self.compass_calibration.locked.value:
#print '[boatimu] cal result', result[0]
self.compass_calibration.sigmapoints.set(result[2])
if result[1]:
self.compass_calibration.age.reset()
self.compass_calibration.set(result[1])
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
def read(self):
data = self.IMUread()
if not data:
if time.monotonic(
) - self.last_imuread > 1 and self.frequency.value:
print('IMURead failed!')
self.frequency.set(False)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
self.last_imuread = time.monotonic()
self.frequency.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
aligned = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(aligned))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .01 and dt < .2:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose, aligned))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or \
self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
if self.auto_cal.cal_pipe:
#print('warning, cal pipe always sending despite locks')
cal_data = {}
#how to check this here?? if not 'imu.accel.calibration.locked'
cal_data['accel'] = list(data['accel'])
#how to check this here?? if not 'imu.compass.calibration.locked'
cal_data['compass'] = list(data['compass'])
cal_data['fusionQPose'] = list(data['fusionQPose'])
if cal_data:
#print('send', cal_data)
self.auto_cal.cal_pipe.send(cal_data)
return data
def IMURead(self):
data = False
while self.poller.poll(0): # read all the data from the pipe
data = self.imu_pipe.recv()
if not data:
if time.time() - self.last_imuread > 1 and self.loopfreq.value:
print('IMURead failed!')
self.loopfreq.set(0)
for name in self.SensorValues:
self.SensorValues[name].set(False)
self.uptime.reset()
return False
if vector.norm(data['accel']) == 0:
print('accel values invalid', data['accel'])
return False
t = time.time()
self.timestamp.set(t - self.starttime)
self.last_imuread = t
self.loopfreq.strobe()
# apply alignment calibration
gyro_q = quaternion.rotvecquat(data['gyro'], data['fusionQPose'])
data['pitchrate'], data['rollrate'], data['headingrate'] = map(
math.degrees, gyro_q)
origfusionQPose = data['fusionQPose']
aligned = quaternion.multiply(data['fusionQPose'],
self.alignmentQ.value)
data['fusionQPose'] = quaternion.normalize(
aligned) # floating point precision errors
data['roll'], data['pitch'], data['heading'] = map(
math.degrees, quaternion.toeuler(data['fusionQPose']))
if data['heading'] < 0:
data['heading'] += 360
dt = data['timestamp'] - self.lasttimestamp
self.lasttimestamp = data['timestamp']
if dt > .02 and dt < .5:
data['headingraterate'] = (data['headingrate'] -
self.headingrate) / dt
else:
data['headingraterate'] = 0
self.headingrate = data['headingrate']
data['heel'] = self.heel = data['roll'] * .03 + self.heel * .97
#data['roll'] -= data['heel']
data['gyro'] = list(map(math.degrees, data['gyro']))
data['gyrobias'] = list(map(math.degrees, data['gyrobias']))
# lowpass heading and rate
llp = self.heading_lowpass_constant.value
data['heading_lowpass'] = heading_filter(
llp, data['heading'], self.SensorValues['heading_lowpass'].value)
llp = self.headingrate_lowpass_constant.value
data['headingrate_lowpass'] = llp * data['headingrate'] + (
1 - llp) * self.SensorValues['headingrate_lowpass'].value
llp = self.headingraterate_lowpass_constant.value
data['headingraterate_lowpass'] = llp * data['headingraterate'] + (
1 - llp) * self.SensorValues['headingraterate_lowpass'].value
# set sensors
for name in self.SensorValues:
self.SensorValues[name].set(data[name])
self.uptime.update()
# count down to alignment
if self.alignmentCounter.value != self.last_alignmentCounter:
self.alignmentPose = [0, 0, 0, 0]
if self.alignmentCounter.value > 0:
self.alignmentPose = list(
map(lambda x, y: x + y, self.alignmentPose,
data['fusionQPose']))
self.alignmentCounter.set(self.alignmentCounter.value - 1)
if self.alignmentCounter.value == 0:
self.alignmentPose = quaternion.normalize(self.alignmentPose)
adown = quaternion.rotvecquat([0, 0, 1],
quaternion.conjugate(
self.alignmentPose))
alignment = []
alignment = quaternion.vec2vec2quat([0, 0, 1], adown)
alignment = quaternion.multiply(self.alignmentQ.value,
alignment)
if len(alignment):
self.update_alignment(alignment)
self.last_alignmentCounter = self.alignmentCounter.value
# if alignment or heading offset changed:
if self.heading_off.value != self.heading_off.last or self.alignmentQ.value != self.alignmentQ.last:
self.update_alignment(self.alignmentQ.value)
self.heading_off.last = self.heading_off.value
self.alignmentQ.last = self.alignmentQ.value
cal_data = {}
if not self.accel_calibration.locked.value:
cal_data['accel'] = list(data['accel'])
if not self.compass_calibration.locked.value:
cal_data['compass'] = list(data['compass'])
cal_data['down'] = quaternion.rotvecquat(
[0, 0, 1], quaternion.conjugate(origfusionQPose))
if cal_data:
self.auto_cal.cal_pipe.send(cal_data)
self.accel_calibration.age.update()
self.compass_calibration.age.update()
return data
