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