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