class IMUAutomaticCalibration(object):
def __init__(self, cal_pipe, accel_calibration, compass_calibration):
self.cal_pipe = cal_pipe
points, self.points = NonBlockingPipe('points pipe', True)
norm_pipe, self.norm_pipe = NonBlockingPipe('norm pipe', True)
self.fit_output, fit_output = NonBlockingPipe('fit output', True)
self.process = multiprocessing.Process(target=CalibrationProcess, args=(points, norm_pipe, fit_output, accel_calibration, compass_calibration))
#print('start cal process')
self.process.start()
def __del__(self):
print('terminate calibration process')
self.process.terminate()
def AddPoint(self, point):
self.points.send(point, False)
def SetNorm(self, norm):
self.norm_pipe.send(norm)
def UpdatedCalibration(self):
result = self.fit_output.recv()
# use new bias fit
if result:
self.cal_pipe.send(result)
return result
class MagnetometerAutomaticCalibration(object):
def __init__(self, cal_pipe, current):
self.cal_pipe = cal_pipe
self.sphere_fit = current
points, self.points = NonBlockingPipe('points pipe', True)
norm_pipe, self.norm_pipe = NonBlockingPipe('norm pipe', True)
self.fit_output, fit_output = NonBlockingPipe('fit output', True)
self.process = multiprocessing.Process(target=CalibrationProcess,
args=(points, norm_pipe,
fit_output,
self.sphere_fit))
#print 'start cal process'
self.process.start()
def __del__(self):
print 'terminate calibration process'
self.process.terminate()
def AddPoint(self, point):
self.points.send(point, False)
def SetNorm(self, norm):
self.norm_pipe.send(norm)
def UpdatedCalibration(self):
result = self.fit_output.recv()
if not result:
return
# use new bias fit
self.cal_pipe.send(tuple(result[0][0][:3]))
return result
class NmeaBridgeProcess(multiprocessing.Process):
def __init__(self):
self.pipe, pipe = NonBlockingPipe('nmea pipe', True)
self.sockets = False
super(NmeaBridgeProcess, self).__init__(target=self.process, args=(pipe,))
def setup_watches(self, watch=True):
watchlist = ['gps.source', 'wind.source', 'rudder.source', 'apb.source']
for name in watchlist:
self.client.watch(name, watch)
def receive_nmea(self, line, device, msgs):
parsers = []
# optimization to only to parse sentences here that would be discarded
# in the main process anyway because they are already handled by a source
# with a higher priority than tcp
tcp_priority = source_priority['tcp']
for name in nmea_parsers:
if source_priority[self.last_values[name + '.source']] >= tcp_priority:
parsers.append(nmea_parsers[name])
for parser in parsers:
result = parser(line)
if result:
name, msg = result
msg['device'] = line[1:3]+device
msgs[name] = msg
return
def new_socket_connection(self, server):
connection, address = server.accept()
max_connections = 10
if len(self.sockets) == max_connections:
connection.close()
print('nmea server has too many connections')
return
if not self.sockets:
self.setup_watches()
self.pipe.send('sockets')
sock = NMEASocket(connection)
self.sockets.append(sock)
#print('new nmea connection: ', address)
self.addresses[sock] = address
fd = sock.socket.fileno()
self.fd_to_socket[fd] = sock
self.poller.register(sock.socket, select.POLLIN)
print('new nmea connection: ', address)
def socket_lost(self, sock, fd):
print('lost nmea connection: ', self.addresses[sock])
try:
self.sockets.remove(sock)
except:
print('nmea sock not in sockets!')
return
self.pipe.send('lostsocket' + str(sock.socket.fileno()))
if not self.sockets:
self.setup_watches(False)
self.pipe.send('nosockets')
try:
self.poller.unregister(fd)
except Exception as e:
print('nmea failed to unregister socket', e)
try:
del self.fd_to_socket[fd]
except Exception as e:
print('nmea failed to remove fd', e)
sock.close()
def client_message(self, name, value):
self.last_values[name] = value
def process(self, pipe):
import os
self.pipe = pipe
self.sockets = []
def on_con(client):
print('nmea ready for connections')
if self.sockets:
self.setup_watches()
while True:
time.sleep(2)
try:
self.client = SignalKClient(on_con, 'localhost', autoreconnect=True)
break
except:
pass
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.setblocking(0)
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
port = DEFAULT_PORT
try:
server.bind(('0.0.0.0', port))
except:
print('nmea_bridge: bind failed.')
exit(1)
print('listening on port', port, 'for nmea connections')
server.listen(5)
self.last_values = {'gps.source' : 'none', 'wind.source' : 'none', 'rudder.source': 'none', 'apb.source': 'none'}
self.addresses = {}
cnt = 0
self.poller = select.poll()
self.poller.register(server, select.POLLIN)
self.poller.register(pipe, select.POLLIN)
self.fd_to_socket = {server.fileno() : server, pipe.fileno() : pipe}
msgs = {}
while True:
timeout = 100 if self.sockets else 10000
t0 = time.time()
events = self.poller.poll(timeout)
t1 = time.time()
while events:
fd, flag = events.pop()
sock = self.fd_to_socket[fd]
if flag & (select.POLLHUP | select.POLLERR | select.POLLNVAL):
if sock == server:
print('nmea bridge lost server connection')
exit(2)
if sock == pipe:
print('nmea bridge pipe to autopilot')
exit(2)
print('lost')
self.socket_lost(sock, fd)
elif sock == server:
self.new_socket_connection(server)
elif sock == pipe:
while True: # receive all messages in pipe
msg = self.pipe.recv()
if not msg:
break
msg += '\r\n'
for sock in self.sockets:
sock.send(msg)
pass
elif flag & select.POLLIN:
if not sock.recv():
print('sock recv lost')
self.socket_lost(sock, fd)
else:
while True:
line = sock.readline()
if not line:
break
self.receive_nmea(line, 'socket' + str(sock.socket.fileno()), msgs)
else:
print('nmea bridge unhandled poll flag', flag)
t2 = time.time()
if msgs:
if self.pipe.send(msgs): ## try , False
msgs = {}
t3 = time.time()
try:
signalk_msgs = self.client.receive()
for name in signalk_msgs:
self.client_message(name, signalk_msgs[name]['value'])
except Exception as e:
print('nmea exception receiving:', e)
t4 = time.time()
for sock in self.sockets:
sock.flush()
t5 = time.time()
if t5-t1 > .1:
print('nmea process loop too slow:', t1-t0, t2-t1, t3-t2, t4-t3, t5-t4)
else:
dt = .1 - (t5 - t0)
if dt > 0 and dt < .1:
time.sleep(dt)
class NmeaBridgeProcess(multiprocessing.Process):
def __init__(self):
self.pipe, pipe = NonBlockingPipe('nmea pipe', True)
self.sockets = False
super(NmeaBridgeProcess, self).__init__(target=self.process,
args=(pipe, ))
def setup_watches(self, watch=True):
watchlist = [
'ap.enabled', 'ap.mode', 'ap.heading_command', 'gps.source',
'wind.source'
]
for name in watchlist:
self.client.watch(name, watch)
def receive_nmea(self, line, msgs):
parsers = []
if source_priority[
self.last_values['gps.source']] >= source_priority['tcp']:
parsers.append(parse_nmea_gps)
if source_priority[
self.last_values['wind.source']] >= source_priority['tcp']:
parsers.append(parse_nmea_wind)
for parser in parsers:
result = parser(line)
if result:
name, msg = result
msgs[name] = msg
return
def receive_apb(self, line, msgs):
# also allow ap commands (should we allow via serial too??)
'''
** APB - Autopilot Sentence "B"
** 13 15
** 1 2 3 4 5 6 7 8 9 10 11 12| 14|
** | | | | | | | | | | | | | | |
** $--APB,A,A,x.x,a,N,A,A,x.x,a,c--c,x.x,a,x.x,a*hh<CR><LF>
**
** 1) Status
** V = LORAN-C Blink or SNR warning
** V = general warning flag or other navigation systems when a reliable
** fix is not available
** 2) Status
** V = Loran-C Cycle Lock warning flag
** A = OK or not used
** 3) Cross Track Error Magnitude
** 4) Direction to steer, L or R
** 5) Cross Track Units, N = Nautical Miles
** 6) Status
** A = Arrival Circle Entered
** 7) Status
** A = Perpendicular passed at waypoint
** 8) Bearing origin to destination
** 9) M = Magnetic, T = True
** 10) Destination Waypoint ID
** 11) Bearing, present position to Destination
** 12) M = Magnetic, T = True
** 13) Heading to steer to destination waypoint
** 14) M = Magnetic, T = True
** 15) Checksum
'''
#
if line[3:6] == 'APB' and time.time() - self.last_apb_time > 1:
self.last_apb_time = time.time()
data = line[7:len(line) - 3].split(',')
if self.last_values['ap.enabled']:
mode = 'compass' if data[13] == 'M' else 'gps'
if self.last_values['ap.mode'] != mode:
self.client.set('ap.mode', mode)
command = float(data[12])
xte = float(data[2])
xte = min(xte, 0.15) # maximum 0.15 miles
if data[3] == 'L':
xte = -xte
command += 300 * xte
# 30 degrees for 1/10th mile
if abs(self.last_values['ap.heading_command'] - command) > .1:
self.client.set('ap.heading_command', command)
return True
return False
def new_socket_connection(self, server):
connection, address = server.accept()
max_connections = 10
if len(self.sockets) == max_connections:
connection.close()
print 'nmea server has too many connections'
return
if not self.sockets:
self.setup_watches()
self.pipe.send('sockets')
sock = NMEASocket(connection)
self.sockets.append(sock)
#print 'new nmea connection: ', address
self.addresses[sock] = address
fd = sock.socket.fileno()
self.fd_to_socket[fd] = sock
self.poller.register(sock.socket, select.POLLIN)
def socket_lost(self, sock):
#print 'lost connection: ', self.addresses[sock]
try:
self.sockets.remove(sock)
except:
print 'sock not in sockets!'
pass
if not self.sockets:
self.setup_watches(False)
self.pipe.send('nosockets')
try:
self.poller.unregister(sock.socket)
except Exception as e:
print 'failed to unregister socket', e
try:
fd = sock.socket.fileno()
del self.fd_to_socket[fd]
except Exception as e:
print 'failed to remove fd', e
sock.close()
def client_message(self, name, value):
self.last_values[name] = value
def process(self, pipe):
import os
#print 'nmea bridge on', os.getpid()
self.pipe = pipe
self.sockets = []
self.last_apb_time = time.time()
def on_con(client):
print 'nmea client connected'
if self.sockets:
self.setup_watches()
while True:
time.sleep(2)
try:
self.client = SignalKClient(on_con,
'localhost',
autoreconnect=True)
break
except:
pass
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.setblocking(0)
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
port = DEFAULT_PORT
try:
server.bind(('0.0.0.0', port))
except:
print 'nmea_bridge: bind failed.'
exit(1)
print 'listening on port', port, 'for nmea connections'
server.listen(5)
self.last_values = {
'ap.enabled': False,
'ap.mode': 'N/A',
'ap.heading_command': 1000,
'gps.source': 'none',
'wind.source': 'none'
}
self.addresses = {}
cnt = 0
self.poller = select.poll()
self.poller.register(server, select.POLLIN)
self.poller.register(pipe, select.POLLIN)
self.fd_to_socket = {server.fileno(): server, pipe.fileno(): pipe}
msgs = {}
while True:
timeout = 100 if self.sockets else 10000
t0 = time.time()
events = self.poller.poll(timeout)
t1 = time.time()
while events:
fd, flag = events.pop()
sock = self.fd_to_socket[fd]
if flag & (select.POLLHUP | select.POLLERR | select.POLLNVAL):
if sock == server:
print 'nmea bridge lost server connection'
exit(2)
if sock == pipe:
print 'nmea bridge pipe to autopilot'
exit(2)
self.socket_lost(sock)
elif sock == server:
self.new_socket_connection(server)
elif sock == pipe:
while True: # receive all messages in pipe
msg = self.pipe.recv()
if not msg:
break
if not self.receive_apb(msg, msgs):
msg += '\r\n'
for sock in self.sockets:
sock.send(msg)
elif flag & select.POLLIN:
if not sock.recv():
self.socket_lost(sock)
else:
while True:
line = sock.readline()
if not line:
break
if not self.receive_apb(line, msgs):
self.receive_nmea(line, msgs)
else:
print 'nmea bridge unhandled poll flag', flag
t2 = time.time()
if msgs:
if self.pipe.send(msgs): ## try , False
msgs = {}
t3 = time.time()
try:
signalk_msgs = self.client.receive()
for name in signalk_msgs:
self.client_message(name, signalk_msgs[name]['value'])
except Exception, e:
print 'nmea exception receiving:', e
t4 = time.time()
for sock in self.sockets:
sock.flush()
t5 = time.time()
if t5 - t1 > .1:
print 'nmea process loop too slow:', t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4
else:
dt = .1 - (t5 - t0)
if dt > 0 and dt < .1:
time.sleep(dt)
class BoatIMU(object):
def __init__(self, server, *args, **keywords):
self.server = server
self.rate = self.Register(EnumProperty,
'rate',
10, [10, 25],
persistent=True)
self.period = 1.0 / self.rate.value
self.loopfreq = self.Register(LoopFreqValue, 'loopfreq', 0)
self.alignmentQ = self.Register(QuaternionValue,
'alignmentQ', [1, 0, 0, 0],
persistent=True)
self.heading_off = self.Register(RangeProperty,
'heading_offset',
0,
-180,
180,
persistent=True)
self.alignmentCounter = self.Register(Property, 'alignmentCounter', 0)
self.last_alignmentCounter = False
self.uptime = self.Register(TimeValue, 'uptime')
def calibration(name, default):
calibration = self.Register(RoundedValue,
name + '.calibration',
default,
persistent=True)
calibration.age = self.Register(AgeValue,
name + '.calibration.age',
persistent=True)
calibration.locked = self.Register(BooleanProperty,
name + '.calibration.locked',
False,
persistent=True)
calibration.sigmapoints = self.Register(
RoundedValue, name + '.calibration.sigmapoints', False)
return calibration
self.accel_calibration = calibration('accel', [[0, 0, 0, 1], 1])
self.compass_calibration = calibration('compass',
[[0, 0, 0, 30, 0], [1, 1], 0])
self.imu_pipe, imu_pipe = NonBlockingPipe('imu_pipe')
imu_cal_pipe = NonBlockingPipe('imu_cal_pipe')
self.poller = select.poll()
self.poller.register(self.imu_pipe, select.POLLIN)
self.auto_cal = IMUAutomaticCalibration(
imu_cal_pipe[1], self.accel_calibration.value[0],
self.compass_calibration.value[0])
self.lastqpose = False
self.FirstTimeStamp = False
self.headingrate = self.heel = 0
self.heading_lowpass_constant = self.Register(
RangeProperty, 'heading_lowpass_constant', .1, .01, 1)
self.headingrate_lowpass_constant = self.Register(
RangeProperty, 'headingrate_lowpass_constant', .1, .01, 1)
self.headingraterate_lowpass_constant = self.Register(
RangeProperty, 'headingraterate_lowpass_constant', .1, .01, 1)
sensornames = [
'accel', 'gyro', 'compass', 'accel.residuals', 'pitch', 'roll'
]
sensornames += [
'pitchrate', 'rollrate', 'headingrate', 'headingraterate', 'heel'
]
sensornames += ['headingrate_lowpass', 'headingraterate_lowpass']
directional_sensornames = ['heading', 'heading_lowpass']
sensornames += directional_sensornames
self.SensorValues = {}
timestamp = server.TimeStamp('imu')
for name in sensornames:
self.SensorValues[name] = self.Register(SensorValue,
name,
timestamp,
directional=name
in directional_sensornames)
# quaternion needs to report many more decimal places than other sensors
sensornames += ['fusionQPose']
self.SensorValues['fusionQPose'] = self.Register(SensorValue,
'fusionQPose',
timestamp,
fmt='%.7f')
sensornames += ['gyrobias']
self.SensorValues['gyrobias'] = self.Register(SensorValue,
'gyrobias',
timestamp,
persistent=True)
self.imu_process = multiprocessing.Process(
target=imu_process,
args=(imu_pipe, imu_cal_pipe[0], self.accel_calibration.value[0],
self.compass_calibration.value[0],
self.SensorValues['gyrobias'].value, self.period))
self.imu_process.start()
self.last_imuread = time.time()
self.lasttimestamp = 0
self.last_heading_off = 3000 # invalid
def __del__(self):
print 'terminate imu process'
self.imu_process.terminate()
def Register(self, _type, name, *args, **kwargs):
value = _type(*(['imu.' + name] + list(args)), **kwargs)
return self.server.Register(value)
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 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
