def __init__(self, server):
self.server = server
self.values = {'gps': {}, 'wind': {}, 'rudder': {}}
def make_source(name, dirname, speed=True):
timestamp = server.TimeStamp(name)
self.values[name][dirname] = server.Register(
SensorValue(name + '.' + dirname, timestamp, directional=True))
if speed:
self.values[name]['speed'] = server.Register(
SensorValue(name + '.speed', timestamp))
self.values[name]['source'] = server.Register(
StringValue(name + '.source', 'none'))
self.values[name]['lastupdate'] = 0
make_source('gps', 'track')
make_source('wind', 'direction')
make_source('rudder', 'angle')
self.devices = []
self.devices_lastmsg = {}
self.probedevice = None
self.primarydevices = {'gps': None, 'wind': None, 'rudder': None}
self.gpsdpoller = GpsdPoller(self)
self.process = NmeaBridgeProcess()
self.process.start()
self.poller = select.poll()
self.process_fd = self.process.pipe.fileno()
self.poller.register(self.process_fd, select.POLLIN)
self.gps_fd = self.gpsdpoller.process.pipe.fileno()
self.poller.register(self.gps_fd, select.POLLIN)
self.device_fd = {}
self.nmea_times = {}
self.last_imu_time = time.time()
self.last_rudder_time = time.time()
self.starttime = time.time()
class Nmea(object):
def __init__(self, server):
self.server = server
self.values = {'gps': {}, 'wind': {}}
def make_source(name, dirname):
timestamp = server.TimeStamp(name)
self.values[name][dirname] = server.Register(
SensorValue(name + '.' + dirname, timestamp, directional=True))
self.values[name]['speed'] = server.Register(
SensorValue(name + '.speed', timestamp))
self.values[name]['source'] = server.Register(
StringValue(name + '.source', 'none'))
self.values[name]['lastupdate'] = 0
make_source('gps', 'track')
make_source('wind', 'direction')
self.devices = []
self.devices_lastmsg = {}
self.probedevice = None
self.primarydevices = {'gps': None, 'wind': None}
self.gpsdpoller = GpsdPoller(self)
self.process = NmeaBridgeProcess()
self.process.start()
self.poller = select.poll()
self.process_fd = self.process.pipe.fileno()
self.poller.register(self.process_fd, select.POLLIN)
self.gps_fd = self.gpsdpoller.process.pipe.fileno()
self.poller.register(self.gps_fd, select.POLLIN)
self.device_fd = {}
self.nmea_times = {}
self.last_imu_time = time.time()
self.last_rudder_time = time.time()
self.starttime = time.time()
def __del__(self):
print 'terminate nmea process'
self.process.terminate()
def read_process_pipe(self):
msgs = self.process.pipe.recv()
if msgs == 'sockets':
self.process.sockets = True
elif msgs == 'nosockets':
self.process.sockets = False
else:
self.handle_messages(msgs, 'tcp')
def read_serial_device(self, device, serial_msgs):
t = time.time()
line = device.readline()
if not line:
return
if self.process.sockets:
nmea_name = line[:6]
# do not output nmea data over tcp faster than 5hz
# for each message time
if not nmea_name in self.nmea_times or t - self.nmea_times[
nmea_name] > .2:
self.process.pipe.send(line, False)
self.nmea_times[nmea_name] = t
self.devices_lastmsg[device] = t
parsers = []
if not self.primarydevices['wind'] or self.primarydevices[
'wind'] == device:
parsers.append(parse_nmea_wind)
if self.values['gps']['source'] != 'gpsd' and \
(not self.primarydevices['gps'] or self.primarydevices['gps'] == device):
parsers.append(parse_nmea_gps)
for parser in parsers:
result = parser(line)
if result:
name, msg = result
if not self.primarydevices[name]:
print 'found primary serial device for', name
self.primarydevices[name] = device
serial_msgs[name] = msg
break
def remove_serial_device(self, device):
index = self.devices.index(device)
print 'lost serial nmea%d' % index
self.devices[index] = False
self.poller.unregister(device.device.fileno())
del self.devices_lastmsg[device]
for name in self.primarydevices:
if device == self.primarydevices[name]:
self.primarydevices[name] = None
device.close()
def poll(self):
t0 = time.time()
self.probe_serial()
t1 = time.time()
# handle tcp nmea messages
serial_msgs = {}
while True:
events = self.poller.poll(0)
if not events:
break
while events:
event = events.pop()
fd, flag = event
if fd == self.process_fd:
if flag != select.POLLIN:
print 'nmea got flag for process pipe:', flag
else:
self.read_process_pipe()
elif fd == self.gps_fd:
if flag != select.POLLIN:
print 'nmea got flag for gpsdpoller pipe:', flag
else:
self.gpsdpoller.read()
elif flag == select.POLLIN:
#if flag & (select.POLLHUP | select.POLLERR | select.POLLNVAL):
self.read_serial_device(self.device_fd[fd], serial_msgs)
else:
self.remove_serial_device(self.device_fd[fd])
t2 = time.time()
self.handle_messages(serial_msgs, 'serial')
t3 = time.time()
for device in self.devices:
# timeout serial devices
if not device:
continue
dt = time.time() - self.devices_lastmsg[device]
if dt > 1:
print 'serial device dt', dt, device
if dt > 15:
print 'serial device timed out', dt, device
self.remove_serial_device(device)
t4 = time.time()
# timeout sources
for name in self.values:
value = self.values[name]
if value['source'].value == 'none':
continue
if t4 - value['lastupdate'] > 8:
print 'nmea timeout for', name, 'source', value[
'source'].value, time.time(), t4 - value['lastupdate']
value['source'].set('none')
# send nmea messages to sockets
dt = time.time() - self.last_imu_time
if self.process.sockets and (dt > .5 or dt < 0) and \
'imu.pitch' in self.server.values:
self.send_nmea('APXDR,A,%.3f,D,PTCH' %
self.server.values['imu.pitch'].value)
self.send_nmea('APXDR,A,%.3f,D,ROLL' %
self.server.values['imu.roll'].value)
self.send_nmea('APHDM,%.3f,M' %
self.server.values['imu.heading_lowpass'].value)
self.last_imu_time = time.time()
dt = time.time() - self.last_rudder_time
if self.process.sockets and (dt > .2 or dt < 0) and \
'servo.rudder' in self.server.values:
value = self.server.values['servo.rudder'].value
if value:
self.send_nmea('APRSA,%.3f,A,,' % value)
self.last_rudder_time = time.time()
t5 = time.time()
if t5 - t0 > .1:
print 'nmea poll times', t1 - t0, t2 - t1, t3 - t2, t4 - t3, t5 - t4
def probe_serial(self):
# probe new nmea data devices
if not self.probedevice:
try:
self.probeindex = self.devices.index(False)
except:
self.probeindex = len(self.devices)
self.probedevicepath = serialprobe.probe(
'nmea%d' % self.probeindex, [38400, 4800])
if self.probedevicepath:
try:
self.probedevice = NMEASerialDevice(self.probedevicepath)
self.probetime = time.time()
except serial.serialutil.SerialException:
print 'failed to open', self.probedevicepath, 'for nmea data'
pass
elif time.time() - self.probetime > 5:
print 'nmea serial probe timeout', self.probedevicepath
self.probedevice = None # timeout
else:
# see if the probe device gets a valid nmea message
if self.probedevice:
if self.probedevice.readline():
print 'new nmea device', self.probedevicepath
serialprobe.success('nmea%d' % self.probeindex,
self.probedevicepath)
if self.probeindex < len(self.devices):
self.devices[self.probeindex] = self.probedevice
else:
self.devices.append(self.probedevice)
fd = self.probedevice.device.fileno()
self.device_fd[fd] = self.probedevice
self.poller.register(fd, select.POLLIN)
self.devices_lastmsg[self.probedevice] = time.time()
self.probedevice = None
def send_nmea(self, msg):
line = '$' + msg + ('*%02X' % nmea_cksum(msg))
self.process.pipe.send(line, False)
def handle_messages(self, msgs, source):
for name in msgs:
if not name in self.values:
print 'unknown data parsed!', name
break
value = self.values[name]
if source_priority[
value['source'].value] < source_priority[source]:
continue
msg = msgs[name]
timestamp = msg['timestamp'] if 'timestamp' in msg else time.time(
) - self.starttime
self.server.TimeStamp(name, timestamp)
for vname in msg:
if vname != 'timestamp':
value[vname].set(msg[vname])
if value['source'].value != source:
print 'nmea source for', name, 'source', source, time.time()
value['source'].update(source)
value['lastupdate'] = time.time()