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 set_time(): """Sets the system time from GPS.""" serial_ = serial.Serial('/dev/ttyAMA0', 115200) serial_.setTimeout(1.0) for _ in range(10): serial_.readline() try: switch_to_nmea_mode(serial_) except: # pylint: disable=W0702 print('Unable to set mode') sys.exit(1) for _ in range(10): serial_.readline() for _ in range(100): line = serial_.readline() try: line = line.decode('utf-8') except: raise EnvironmentError('Not a UTF-8 message') if line.startswith('$GPRMC'): print(line) time_str = line.split(',')[1] date_str = line.split(',')[9] day = int(date_str[0:2]) month = int(date_str[2:4]) year = 2000 + int(date_str[4:6]) hour = int(time_str[0:2]) minute = int(time_str[2:4]) second = int(time_str[4:6]) print( '{}-{}-{} {}:{}:{}'.format( year, month, day, hour, minute, second ) ) subprocess.call([ 'date', '--set', '{}-{}-{}'.format(year, month, day) ]) subprocess.call([ 'date', '--set', '{}:{}:{}'.format(hour, minute, second) ]) return print('No GPRMC message seen')
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(): logger = LoggerDummy() serial_ = serial.Serial('/dev/ttyAMA0', 115200) serial_.setTimeout(1.0) for _ in range(10): serial_.readline() try: switch_to_nmea_mode(serial_) except: logger.error('Unable to set mode') for _ in range(10): serial_.readline() logger.info('Done') serial_ = serial.Serial('/dev/ttyAMA0', 115200) telemetry = TelemetryDummy() sup800f_telemetry = Sup800fTelemetry(telemetry, serial_, logger) sup800f_telemetry.start() sup800f_telemetry.join(1000000)
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 start_threads( waypoint_generator, logger, web_socket_handler, max_throttle, kml_file_name, ): """Runs everything.""" logger.info('Creating Telemetry') telemetry = Telemetry(kml_file_name) telemetry_dumper = TelemetryDumper( telemetry, waypoint_generator, web_socket_handler ) logger.info('Done creating Telemetry') global DRIVER DRIVER = Driver(telemetry) DRIVER.set_max_throttle(max_throttle) logger.info('Setting SUP800F to NMEA mode') serial_ = serial.Serial('/dev/ttyAMA0', 115200) serial_.setTimeout(1.0) for _ in range(10): serial_.readline() try: switch_to_nmea_mode(serial_) except: # pylint: disable=W0702 logger.error('Unable to set mode') for _ in range(10): serial_.readline() logger.info('Done') # The following objects must be created in order, because of message # exchange dependencies: # sup800f_telemetry: reads from command forwarded # command: reads from command, writes to command forwarded # button: writes to command # cherry_py_server: writes to command # TODO(2016-08-21) Have something better than sleeps to work around race # conditions logger.info('Creating threads') sup800f_telemetry = Sup800fTelemetry(serial_) time.sleep(0.5) command = Command(telemetry, DRIVER, waypoint_generator) time.sleep(0.5) button = Button() port = int(get_configuration('PORT', 8080)) address = get_configuration('ADDRESS', '0.0.0.0') cherry_py_server = CherryPyServer( port, address, telemetry, waypoint_generator ) time.sleep(0.5) global THREADS THREADS += ( button, cherry_py_server, command, sup800f_telemetry, telemetry_dumper, ) for thread in THREADS: thread.start() logger.info('Started all threads') # Use a fake timeout so that the main thread can still receive signals sup800f_telemetry.join(100000000000) # Once we get here, sup800f_telemetry has died and there's no point in # continuing because we're not receiving telemetry messages any more, so # stop the command module command.stop() command.join(100000000000) cherry_py_server.kill() cherry_py_server.join(100000000000) button.kill() button.join(100000000000)