def run(self):
"""Run in a thread, hands raw telemetry readings to telemetry
instance.
"""
failed_to_switch_mode = False
while self._run:
try:
self._run_inner()
except EnvironmentError as env:
self._logger.debug('Failed to switch mode: {}'.format(env))
if not failed_to_switch_mode:
failed_to_switch_mode = True
import traceback
self._logger.debug(traceback.format_exc())
# Maybe resetting the module mode will help
try:
if self._nmea_mode:
switch_to_nmea_mode(self._serial)
else:
switch_to_binary_mode(self._serial)
except:
pass
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Telemetry data caught exception: {}'.format(
exc
)
)
def run(self):
"""Run in a thread, hands raw telemetry readings to telemetry
instance.
"""
failed_to_switch_mode = False
while self._run:
try:
self._run_inner()
except EnvironmentError as env:
self._logger.debug('Failed to switch mode: {}'.format(env))
if not failed_to_switch_mode:
failed_to_switch_mode = True
import traceback
self._logger.debug(traceback.format_exc())
# Maybe resetting the module mode will help
try:
if self._nmea_mode:
switch_to_nmea_mode(self._serial)
else:
switch_to_binary_mode(self._serial)
except:
pass
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Telemetry data caught exception: {}'.format(exc))
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 _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 _run_inner(self):
"""Inner part of run."""
binary_count = 0
while self._run:
if self._calibrate_compass_end_time is not None:
self._calibrate_compass()
# NMEA mode
if self._nmea_mode:
if self._get_gprc():
switch_to_binary_mode(self._serial)
self._nmea_mode = False
else:
if self._get_binary():
binary_count += 1
if binary_count >= 3:
switch_to_nmea_mode(self._serial)
self._nmea_mode = True
binary_count = 0
def _run_inner(self):
"""Inner part of run."""
binary_count = 0
while self._run:
if self._calibrate_compass_end_time is not None:
self._calibrate_compass()
# NMEA mode
if self._nmea_mode:
if self._get_gprc():
switch_to_binary_mode(self._serial)
self._nmea_mode = False
else:
if self._get_binary():
binary_count += 1
if binary_count >= 3:
switch_to_nmea_mode(self._serial)
self._nmea_mode = True
binary_count = 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
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
