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
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
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
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
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
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
