class Sup800fTelemetry(threading.Thread):
"""Reader of GPS module that implements the TelemetryData interface."""
def __init__(self, serial):
"""Create the TelemetryData thread."""
super(Sup800fTelemetry, self).__init__()
self.name = self.__class__.__name__
self._telemetry = TelemetryProducer()
self._serial = serial
self._logger = AsyncLogger()
self._run = True
self._iterations = 0
# These initial measurements are from a calibration observation
self._compass_offsets = (-11.87, -5.97)
self._magnitude_mean = 353.310
self._magnitude_std_dev = 117.918
self._calibrate_compass_end_time = None
self._nmea_mode = True
self._last_compass_heading_d = 0.0
self._dropped_compass_messages = 0
self._dropped_threshold = 10
self._hdop = 5.0
def handle_message(message):
"""Handles command messages. Only cares about calibrate compass;
other messages are ignored.
"""
if message == 'calibrate-compass':
self.calibrate_compass(10)
consume = lambda: consume_messages(config.COMMAND_FORWARDED_EXCHANGE,
handle_message)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__, config.COMMAND_FORWARDED_EXCHANGE)
thread.start()
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_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 _get_gprc(self):
"""Gets and processes a single GPRC message."""
# This blocks until a new message is received
line = self._serial.readline()
try:
line = line.decode('utf-8')
except:
raise EnvironmentError('Not a UTF-8 message')
if line.startswith('$GPRMC'):
self._handle_gprmc(line)
return True
elif line.startswith('$GPGSA'):
self._handle_gpgsa(line)
return True
return False
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
@staticmethod
def _timestamp(dt):
"""Computes the Unix timestamp from a datetime object. This is needed
because Python < 3.2 doesn't have .timestamp built in.
"""
return (dt - datetime.datetime(1970, 1, 1, tzinfo=pytz.utc)) \
/ datetime.timedelta(seconds=1)
def _handle_gprmc(self, gprmc_message):
"""Handles GPRMC (recommended minimum specific GNSS data) messages."""
parts = gprmc_message.split(',')
latitude_str = parts[3]
longitude_str = parts[5]
decimal_index = latitude_str.find('.')
latitude_degrees = float(latitude_str[:decimal_index - 2])
latitude_minutes = float(latitude_str[decimal_index - 2:])
decimal_index = longitude_str.find('.')
longitude_degrees = float(longitude_str[:decimal_index - 2])
longitude_minutes = float(longitude_str[decimal_index - 2:])
latitude = latitude_degrees + latitude_minutes / 60.0
longitude = longitude_degrees + longitude_minutes / 60.0
if parts[4] == 'S':
latitude = -latitude
if parts[6] == 'W':
longitude = -longitude
speed_knots = float(parts[7])
speed_m_s = speed_knots * 0.514444444
course = float(parts[8])
# Below a certain speed, the module uses the compass to determine
# course, which is not calibrated, so we need to use our own value.
if speed_m_s < COMPASS_SPEED_CUTOFF_M_S:
course = self._last_compass_heading_d
time_ = parts[1]
hours = int(time_[0:2])
minutes = int(time_[2:4])
seconds = float(time_[4:])
date = parts[9]
day = int(date[0:2])
month = int(date[2:4])
year = int(date[4:]) + 2000
# datetime doesn't do float seconds, so we need to fudge it later
datetime_ = datetime.datetime(year,
month,
day,
hours,
minutes,
0,
tzinfo=pytz.utc)
timestamp_s = self._timestamp(datetime_) + seconds
self._telemetry.gps_reading(
latitude,
longitude,
self._hdop * 5.0, # This is a guess. Smaller HDOP is more precise.
course,
speed_m_s,
timestamp_s,
'sup800f')
def _handle_gpgsa(self, gpgsa_message):
"""Handles GSA (GNSS DOP and active satellites) messages."""
# $GPGSA,A,3,23,03,26,09,27,16,22,31,,,,,1.9,1.1,1.5*31\r\n
# type, mode M=manual or A=automatic, fix type 1=N/A 2=2D 3=3D,
# satellites used 1-12, PDOP, HDOP, VDOP + checksum
parts = gpgsa_message.split(',')
#pdop = float(parts[-3])
hdop = float(parts[-2])
#vdop = float(parts[-1].split('*')[0])
self._hdop = hdop
def _handle_binary(self, message):
"""Handles properietary SUP800F binary messages."""
if message is None:
return
flux_x = float(message.magnetic_flux_ut_x - self._compass_offsets[0])
flux_y = float(message.magnetic_flux_ut_y - self._compass_offsets[1])
if flux_x == 0.0:
# TODO: Figure out what to do here
return
self._last_compass_heading_d = Telemetry.wrap_degrees(
270.0 - math.degrees(math.atan2(flux_y, flux_x)) +
8.666 # Boulder declination
)
magnitude = flux_x**2 + flux_y**2
std_devs_away = abs(self._magnitude_mean -
magnitude) / self._magnitude_std_dev
# In a normal distribution, 95% of readings should be within 2 std devs
if std_devs_away > 2.0:
self._dropped_compass_messages += 1
if self._dropped_compass_messages > self._dropped_threshold:
self._logger.warn(
'Dropped {} compass messages in a row, std dev = {}'.
format(self._dropped_compass_messages,
round(std_devs_away, 3)))
self._dropped_compass_messages = 0
self._dropped_threshold += 10
return
self._dropped_compass_messages = 0
self._dropped_threshold = 10
if std_devs_away > 1.0:
confidence = 2.0 - std_devs_away
else:
confidence = 1.0
self._telemetry.compass_reading(self._last_compass_heading_d,
confidence, 'sup800f')
self._telemetry.accelerometer_reading(message.acceleration_g_x,
message.acceleration_g_y,
message.acceleration_g_z,
'sup800f')
def kill(self):
"""Stops any data collection."""
self._run = False
def calibrate_compass(self, seconds):
"""Requests that the car calibrate the compasss."""
if self._calibrate_compass_end_time is None:
self._calibrate_compass_end_time = time.time() + seconds
else:
self._logger.warn('Compass is already being calibrated')
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
class Sup800fTelemetry(threading.Thread):
"""Reader of GPS module that implements the TelemetryData interface."""
def __init__(self, serial):
"""Create the TelemetryData thread."""
super(Sup800fTelemetry, self).__init__()
self.name = self.__class__.__name__
self._telemetry = TelemetryProducer()
self._serial = serial
self._logger = AsyncLogger()
self._run = True
self._iterations = 0
# These initial measurements are from a calibration observation
self._compass_offsets = (-11.87, -5.97)
self._magnitude_mean = 353.310
self._magnitude_std_dev = 117.918
self._calibrate_compass_end_time = None
self._nmea_mode = True
self._last_compass_heading_d = 0.0
self._dropped_compass_messages = 0
self._dropped_threshold = 10
self._hdop = 5.0
def handle_message(message):
"""Handles command messages. Only cares about calibrate compass;
other messages are ignored.
"""
if message == 'calibrate-compass':
self.calibrate_compass(10)
consume = lambda: consume_messages(
config.COMMAND_FORWARDED_EXCHANGE,
handle_message
)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__,
config.COMMAND_FORWARDED_EXCHANGE
)
thread.start()
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_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 _get_gprc(self):
"""Gets and processes a single GPRC message."""
# This blocks until a new message is received
line = self._serial.readline()
try:
line = line.decode('utf-8')
except:
raise EnvironmentError('Not a UTF-8 message')
if line.startswith('$GPRMC'):
self._handle_gprmc(line)
return True
elif line.startswith('$GPGSA'):
self._handle_gpgsa(line)
return True
return False
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
@staticmethod
def _timestamp(dt):
"""Computes the Unix timestamp from a datetime object. This is needed
because Python < 3.2 doesn't have .timestamp built in.
"""
return (dt - datetime.datetime(1970, 1, 1, tzinfo=pytz.utc)) \
/ datetime.timedelta(seconds=1)
def _handle_gprmc(self, gprmc_message):
"""Handles GPRMC (recommended minimum specific GNSS data) messages."""
parts = gprmc_message.split(',')
latitude_str = parts[3]
longitude_str = parts[5]
decimal_index = latitude_str.find('.')
latitude_degrees = float(latitude_str[:decimal_index - 2])
latitude_minutes = float(latitude_str[decimal_index - 2:])
decimal_index = longitude_str.find('.')
longitude_degrees = float(longitude_str[:decimal_index - 2])
longitude_minutes = float(longitude_str[decimal_index - 2:])
latitude = latitude_degrees + latitude_minutes / 60.0
longitude = longitude_degrees + longitude_minutes / 60.0
if parts[4] == 'S':
latitude = -latitude
if parts[6] == 'W':
longitude = -longitude
speed_knots = float(parts[7])
speed_m_s = speed_knots * 0.514444444
course = float(parts[8])
# Below a certain speed, the module uses the compass to determine
# course, which is not calibrated, so we need to use our own value.
if speed_m_s < COMPASS_SPEED_CUTOFF_M_S:
course = self._last_compass_heading_d
time_ = parts[1]
hours = int(time_[0:2])
minutes = int(time_[2:4])
seconds = float(time_[4:])
date = parts[9]
day = int(date[0:2])
month = int(date[2:4])
year = int(date[4:]) + 2000
# datetime doesn't do float seconds, so we need to fudge it later
datetime_ = datetime.datetime(
year,
month,
day,
hours,
minutes,
0,
tzinfo=pytz.utc
)
timestamp_s = self._timestamp(datetime_) + seconds
self._telemetry.gps_reading(
latitude,
longitude,
self._hdop * 5.0, # This is a guess. Smaller HDOP is more precise.
course,
speed_m_s,
timestamp_s,
'sup800f'
)
def _handle_gpgsa(self, gpgsa_message):
"""Handles GSA (GNSS DOP and active satellites) messages."""
# $GPGSA,A,3,23,03,26,09,27,16,22,31,,,,,1.9,1.1,1.5*31\r\n
# type, mode M=manual or A=automatic, fix type 1=N/A 2=2D 3=3D,
# satellites used 1-12, PDOP, HDOP, VDOP + checksum
parts = gpgsa_message.split(',')
#pdop = float(parts[-3])
hdop = float(parts[-2])
#vdop = float(parts[-1].split('*')[0])
self._hdop = hdop
def _handle_binary(self, message):
"""Handles properietary SUP800F binary messages."""
if message is None:
return
flux_x = float(message.magnetic_flux_ut_x - self._compass_offsets[0])
flux_y = float(message.magnetic_flux_ut_y - self._compass_offsets[1])
if flux_x == 0.0:
# TODO: Figure out what to do here
return
self._last_compass_heading_d = Telemetry.wrap_degrees(
270.0 - math.degrees(
math.atan2(flux_y, flux_x)
) + 8.666 # Boulder declination
)
magnitude = flux_x ** 2 + flux_y ** 2
std_devs_away = abs(
self._magnitude_mean - magnitude
) / self._magnitude_std_dev
# In a normal distribution, 95% of readings should be within 2 std devs
if std_devs_away > 2.0:
self._dropped_compass_messages += 1
if self._dropped_compass_messages > self._dropped_threshold:
self._logger.warn(
'Dropped {} compass messages in a row, std dev = {}'.format(
self._dropped_compass_messages,
round(std_devs_away, 3)
)
)
self._dropped_compass_messages = 0
self._dropped_threshold += 10
return
self._dropped_compass_messages = 0
self._dropped_threshold = 10
if std_devs_away > 1.0:
confidence = 2.0 - std_devs_away
else:
confidence = 1.0
self._telemetry.compass_reading(
self._last_compass_heading_d,
confidence,
'sup800f'
)
self._telemetry.accelerometer_reading(
message.acceleration_g_x,
message.acceleration_g_y,
message.acceleration_g_z,
'sup800f'
)
def kill(self):
"""Stops any data collection."""
self._run = False
def calibrate_compass(self, seconds):
"""Requests that the car calibrate the compasss."""
if self._calibrate_compass_end_time is None:
self._calibrate_compass_end_time = time.time() + seconds
else:
self._logger.warn('Compass is already being calibrated')
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