Esempio n. 1
0
    def _calibrate_compass(self):
        """Calibrates the compass."""
        self._logger.info('Calibrating compass; setting to binary mode')
        switch_to_binary_mode(self._serial)
        self._nmea_mode = False
        for _ in range(10):
            self._serial.readline()

        maxes = [-float('inf')] * 2
        mins = [float('inf')] * 2
        flux_readings = []
        # We should be driving for this long
        while time.time() < self._calibrate_compass_end_time:
            data = get_message(self._serial)
            try:
                binary = parse_binary(data)
            except ValueError as ve:
                self._logger.info(
                    'Unable to parse binary message {}: {}'.format(
                        data,
                        ve
                    )
                )
                continue
            # TODO: This should never be None, see comment in sup800f.py
            if binary is None:
                continue
            flux_values = (
                binary.magnetic_flux_ut_x,
                binary.magnetic_flux_ut_y,
            )
            maxes = [max(a, b) for a, b in zip(maxes, flux_values)]
            mins = [min(a, b) for a, b in zip(mins, flux_values)]
            flux_readings.append(flux_values)

        self._compass_offsets = [
            (max_ + min_) * 0.5 for max_, min_ in zip(maxes, mins)
        ]
        self._logger.info(
            'Compass calibrated, offsets are {}'.format(
                [round(i, 2) for i in self._compass_offsets]
            )
        )
        total_magnitudes = numpy.array([
            (x - self._compass_offsets[0]) ** 2 +
            (y - self._compass_offsets[1]) ** 2
            for x, y in flux_readings
        ])
        self._magnitude_mean = total_magnitudes.mean()
        self._magnitude_std_dev = total_magnitudes.std()
        self._logger.info(
            'Magnitudes mean: {}, standard deviation: {}'.format(
                round(self._magnitude_mean, 3),
                round(self._magnitude_std_dev, 3)
            )
        )

        self._calibrate_compass_end_time = None
        switch_to_nmea_mode(self._serial)
        self._nmea_mode = True
Esempio n. 2
0
    def _get_binary(self):
        """Gets and processes a single binary message."""
        try:
            message = get_message(self._serial, 1000)
        except ValueError:
            self._logger.error('No binary message received')
            return False

        parsed = parse_binary(message)
        if parsed is None:
            return False
        self._handle_binary(parsed)
        return True
Esempio n. 3
0
    def _get_binary(self):
        """Gets and processes a single binary message."""
        try:
            message = get_message(self._serial, 1000)
        except ValueError:
            self._logger.error('No binary message received')
            return False

        parsed = parse_binary(message)
        if parsed is None:
            return False
        self._handle_binary(parsed)
        return True
Esempio n. 4
0
    def _calibrate_compass(self):
        """Calibrates the compass."""
        self._logger.info('Calibrating compass; setting to binary mode')
        switch_to_binary_mode(self._serial)
        self._nmea_mode = False
        for _ in range(10):
            self._serial.readline()

        maxes = [-float('inf')] * 2
        mins = [float('inf')] * 2
        flux_readings = []
        # We should be driving for this long
        while time.time() < self._calibrate_compass_end_time:
            data = get_message(self._serial)
            try:
                binary = parse_binary(data)
            except ValueError as ve:
                self._logger.info(
                    'Unable to parse binary message {}: {}'.format(data, ve))
                continue
            # TODO: This should never be None, see comment in sup800f.py
            if binary is None:
                continue
            flux_values = (
                binary.magnetic_flux_ut_x,
                binary.magnetic_flux_ut_y,
            )
            maxes = [max(a, b) for a, b in zip(maxes, flux_values)]
            mins = [min(a, b) for a, b in zip(mins, flux_values)]
            flux_readings.append(flux_values)

        self._compass_offsets = [(max_ + min_) * 0.5
                                 for max_, min_ in zip(maxes, mins)]
        self._logger.info('Compass calibrated, offsets are {}'.format(
            [round(i, 2) for i in self._compass_offsets]))
        total_magnitudes = numpy.array([(x - self._compass_offsets[0])**2 +
                                        (y - self._compass_offsets[1])**2
                                        for x, y in flux_readings])
        self._magnitude_mean = total_magnitudes.mean()
        self._magnitude_std_dev = total_magnitudes.std()
        self._logger.info('Magnitudes mean: {}, standard deviation: {}'.format(
            round(self._magnitude_mean, 3), round(self._magnitude_std_dev, 3)))

        self._calibrate_compass_end_time = None
        switch_to_nmea_mode(self._serial)
        self._nmea_mode = True
Esempio n. 5
0
def main():
    """Main function."""
    parser = make_parser()
    args = parser.parse_args()

    ser = serial.Serial(args.tty, args.baud_rate)
    if args.gps or (args.gps is None and args.binary is None):
        switch_to_nmea_mode(ser)
        while True:
            print(ser.readline())
    else:
        switch_to_binary_mode(ser)

        format_string = '\n'.join((
            'acceleration X: {}',
            'acceleration Y: {}',
            'acceleration Z: {}',
            'magnetic X: {}',
            'magnetic Y: {}',
            'magnetic Z: {}',
            'pressure: {}',
            'temperature: {}',
        ))

        # The first message back should be an ack, ignore it
        get_message(ser)
        # The next message back is navigation data message, ignore it
        get_message(ser)

        while True:
            data = get_message(ser)
            if len(data) != 42:
                continue
            print(data)
            binary = parse_binary(data)  # pylint: disable=protected-access
            print(format_string.format(*binary))  # pylint: disable=star-args
Esempio n. 6
0
def main():
    """Main function."""
    parser = make_parser()
    args = parser.parse_args()

    ser = serial.Serial(args.tty, args.baud_rate)
    if args.gps or (args.gps is None and args.binary is None):
        switch_to_nmea_mode(ser)
        while True:
            print(ser.readline())
    else:
        switch_to_binary_mode(ser)

        format_string = '\n'.join((
            'acceleration X: {}',
            'acceleration Y: {}',
            'acceleration Z: {}',
            'magnetic X: {}',
            'magnetic Y: {}',
            'magnetic Z: {}',
            'pressure: {}',
            'temperature: {}',
        ))

        # The first message back should be an ack, ignore it
        get_message(ser)
        # The next message back is navigation data message, ignore it
        get_message(ser)

        while True:
            data = get_message(ser)
            if len(data) != 42:
                continue
            print(data)
            binary = parse_binary(data)  # pylint: disable=protected-access
            print(format_string.format(*binary))  # pylint: disable=star-args