예제 #1
0
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
예제 #2
0
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
예제 #3
0
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)
예제 #4
0
파일: nmea.py 프로젝트: motobig/pypilot
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)
예제 #5
0
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