def _handleBuzzer(self):
length = self._getLength()
assert length == 1
data = self.stdout.read(length)
songType = int(data[0])
LOGGER.info(f"SIM: Bell rang with song type {songType} (device_address={self.device_address})")
def _handlePinMode(self):
length = self._getLength()
assert length == 2
pin, mode = self.stdout.read(2)
self._hw_sim.set_pin_mode(pin, mode)
LOGGER.info(f"SIM: Mode for pin {pin} set to {mode} (device_address={self.device_address})")
def config(self, byte_stream: BytesIO, header: Header):
"""
:param byte_stream:
:type byte_stream:
:param header:
:type header:
:return:
:rtype:
"""
data = {}
data[DataEntryIds.IS_SIM] = bool(byte_stream.read(1)[0])
data[DataEntryIds.DEVICE_TYPE] = ID_TO_DEVICE_TYPE[byte_stream.read(1)
[0]]
version_id = byte_stream.read(VERSION_ID_LEN)
data[DataEntryIds.VERSION_ID] = version_id.decode('ascii')
LOGGER.info("Config: SIM? %s, Rocket type = %s, Version ID = %s",
str(data[DataEntryIds.IS_SIM]),
str(data[DataEntryIds.DEVICE_TYPE]),
str(data[DataEntryIds.VERSION_ID]))
CONFIG_EVENT.increment()
return data
def _handleDigitalPinWrite(self):
length = self._getLength()
assert length == 2
pin, value = self.stdout.read(2)
self._hw_sim.set_pin_mode(pin, PinModes.INPUT)
self._hw_sim.digital_write(pin, value)
LOGGER.info(f"SIM: Pin {pin} set to {value} (device_address={self.device_address})")
def launch(self) -> None:
assert self.get_flight_state() == FlightState.STANDBY
self._state = FlightState.FLIGHT
self._launch_time = self.get_time()
LOGGER.info(
f"Rocket launched at time {self.get_time()} s, and altitude {self.get_data(FlightDataType.TYPE_ALTITUDE)} m")
def run(self):
"""
"""
LOGGER.debug("Send thread started")
# TODO : Once we have multiple connections, we will loop over and send a config request to each
# Starting up, request hello/ha ndshake/identification
for connection in self.connections.values():
try:
connection.broadcast(self.command_parser.broadcast_data(CommandType.CONFIG))
except Exception as ex:
LOGGER.exception("Exception in send thread while sending config requests")
while True:
try:
message = self.commandQueue.get(block=True, timeout=None) # Block until something new
self.commandQueue.task_done()
if message is None: # Either received None or woken up for shutdown
with self._shutdown_lock:
if self._is_shutting_down:
break
else:
continue
try:
(device, command, data) = self.command_parser.pase_command(message)
except CommandParsingError as ex:
LOGGER.error(f"Error parsing command: {str(ex)}")
continue
full_address = self.device_manager.get_full_address(device)
if full_address is None:
LOGGER.error(f"Device not yet connected: {device.name}")
continue
connection = self.connections[full_address.connection_name]
LOGGER.info(f"Sending command {command.name} to device {device.name} ({full_address})")
connection.send(full_address.device_address, data)
LOGGER.info("Sent command!")
COMMAND_SENT_EVENT.increment()
except TimeoutException: # TODO: Connection should have converted this to a generic exception for decoupling
LOGGER.error("Message timed-out!")
except queue.Empty:
pass
except Exception as ex:
LOGGER.exception("Unexpected error while sending!") # Automatically grabs and prints exception info
LOGGER.warning("Send thread shut down")
def __init__(self, connections: Dict[str, Connection], rocket_profile: RocketProfile) -> None:
"""
:param connections:
:param rocket_profile:
"""
super().__init__(connections, rocket_profile)
self.map_data = map_data.MapData()
self.im = None # Plot im
self.command_history = []
self.command_history_index = None
# Attach functions for static buttons
self.sendButton.clicked.connect(self.send_button_pressed)
self.actionSave.triggered.connect(self.save_file)
self.actionSave.setShortcut("Ctrl+S")
self.actionReset.triggered.connect(self.reset_view)
# Hook into some of commandEdit's events
qtHook(self.commandEdit, 'focusNextPrevChild', lambda _: False, override_return=True) # Prevents tab from changing focus
qtHook(self.commandEdit, 'keyPressEvent', self.command_edit_key_pressed)
qtHook(self.commandEdit, 'showPopup', self.command_edit_show_popup)
# Hook-up logger to UI text output
# Note: Currently doesn't print logs from other processes (e.g. mapping process)
log_format = logging.Formatter("[%(asctime)s] (%(levelname)s) %(message)s")
log_handler = qtSignalLogHandler(exception_traces=False)
log_handler.setLevel(logging.INFO)
log_handler.setFormatter(log_format)
log_handler.qt_signal.connect(self.print_to_ui)
LOGGER.addHandler(log_handler)
# Setup dynamic UI elements
self.setup_buttons()
self.setup_labels()
self.setup_subwindow().showMaximized()
self.setup_view_menu()
self.setWindowIcon(QtGui.QIcon(mapbox_utils.MARKER_PATH))
# Setup user window preferences
self.original_geometry = self.saveGeometry()
self.original_state = self.saveState()
self.settings = QtCore.QSettings('UBCRocket', 'UBCRGS')
self.restore_view()
# Init and connection of MappingThread
self.MappingThread = MappingThread(self.connections, self.map_data, self.rocket_data, self.rocket_profile)
self.MappingThread.sig_received.connect(self.receive_map)
self.MappingThread.start()
LOGGER.info(f"Successfully started app (version = {GIT_HASH})")
def broadcast(self, data) -> None: # must be thead safe
LOGGER.info(
f"{data} sent to address={self.device_address} on DebugConnection")
with self.lock:
if data == bytes([0x41]):
ARMED_EVENT.increment()
elif data == bytes([0x44]):
DISARMED_EVENT.increment()
elif data == bytes([0x50]):
self.receive(self.status_ping_mock_set_values())
elif data == bytes([0x43]):
self.receive(self.config_mock_set_values())
def register_device(self, device_type: DeviceType, device_version: str,
full_address: FullAddress) -> None:
with self._lock:
if device_type not in self._device_type_to_device and full_address not in self._full_address_to_device:
pass
elif device_type in self._device_type_to_device and full_address != self._device_type_to_device[
device_type].full_address:
raise InvalidRegistration(
f"Cannot reassign device_type={device_type.name} (full_address={self._device_type_to_device[device_type].full_address}) to full_address={full_address}"
)
elif full_address in self._full_address_to_device and device_type != self._full_address_to_device[
full_address].device_type:
raise InvalidRegistration(
f"Cannot reassign full_address={full_address} (device={self._full_address_to_device[full_address].device_type}) to device={device_type}"
)
else:
LOGGER.info(
f"Already registered. Device={device_type.name}, full_address={full_address}"
)
return
if device_type in self.required_versions and device_version != self.required_versions[
device_type]:
error_str = f"Version {device_version} does not match required version {self.required_versions[device_type]} for device {device_type.name}"
if self.strict_versions:
raise InvalidDeviceVersion(error_str)
else:
LOGGER.warning(error_str)
self._device_type_to_device[device_type] = RegisteredDevice(
device_type=device_type, full_address=full_address)
self._full_address_to_device = {
d.full_address: d
for d in self._device_type_to_device.values()
}
# If two devices somehow have the same full address, mapping wont be one-to-one.
assert len(self._device_type_to_device) == len(
self._full_address_to_device)
if device_type in self.expected_devices:
LOGGER.info(
f"Registered expected device={device_type.name}, full_address={full_address}, {self.num_expected_registered()}/{len(self.expected_devices)} expected devices"
)
else:
LOGGER.warning(
f"Registered unexpected device={device_type.name}, full_address={full_address}, {self.num_expected_registered()}/{len(self.expected_devices)} expected devices"
)
DEVICE_REGISTERED_EVENT.increment()
def _getEndianness(self):
id = self.stdout.read(1)[0]
assert id == SimRxId.CONFIG.value
length = self._getLength()
assert length == 8
data = self.stdout.read(length)
self.bigEndianInts = data[0] == 0x04
self.bigEndianFloats = data[4] == 0xC0
LOGGER.info(
f"SIM: Big Endian Ints - {self.bigEndianInts}, Big Endian Floats - {self.bigEndianFloats} (device_address={self.device_address})"
)
def __init__(self, connections: Dict[str, Connection], rocket_profile: RocketProfile) -> None:
"""
:param connection:
:type connection: Connection
:param rocket_profile:
:type rocket_profile: RocketProfile
"""
super().__init__(connections, rocket_profile)
self.map = map_data.MapData()
self.im = None # Plot im
# Attach functions for static buttons
self.sendButton.clicked.connect(self.send_button_pressed)
self.commandEdit.returnPressed.connect(self.send_button_pressed)
self.actionSave.triggered.connect(self.save_file)
self.actionSave.setShortcut("Ctrl+S")
self.actionReset.triggered.connect(self.reset_view)
# Hook-up logger to UI text output
# Note: Currently doesnt print logs from other processes (e.g. mapping process)
log_format = logging.Formatter("[%(asctime)s] (%(levelname)s) %(message)s")
log_handler = qtSignalLogHandler(exception_traces=False)
log_handler.setLevel(logging.INFO)
log_handler.setFormatter(log_format)
log_handler.qt_signal.connect(self.print_to_ui)
LOGGER.addHandler(log_handler)
self.setup_buttons()
self.setup_labels()
self.setup_subwindow().showMaximized()
self.setWindowIcon(QtGui.QIcon(mapbox_utils.MARKER_PATH))
self.original_geometry = self.saveGeometry()
self.original_state = self.saveState()
self.settings = QtCore.QSettings('UBCRocket', 'UBCRGS')
self.restore_view()
# Init and connection of MappingThread
self.MappingThread = MappingThread(self.connections, self.map, self.rocket_data, self.rocket_profile)
self.MappingThread.sig_received.connect(self.receive_map)
self.MappingThread.start()
LOGGER.info(f"Successfully started app (version = {GIT_HASH})")
def state(self, byte_stream: BytesIO, header: Header, print_state=True):
"""
:param byte_stream:
:param header:
:return:
"""
data = {}
state_id = self.bytestoint(byte_stream.read(2))
data_entry_value = STATE_IDS[state_id]
data[DataEntryIds.STATE] = data_entry_value
if print_state:
LOGGER.info("State: %s", str(data_entry_value.name))
STATE_EVENT.increment()
return data
def event(self, byte_stream: BytesIO, header: Header):
"""
:param byte_stream:
:type byte_stream:
:param header:
:type header:
:return:
"""
data: Dict = {}
event_bytes = byte_stream.read(2)
event_int = self.bytestoint(event_bytes)
data_entry_value = EVENT_IDS[event_int]
data[DataEntryIds.EVENT] = data_entry_value
LOGGER.info("Event: %s", str(data_entry_value.name))
EVENT_EVENT.increment()
return data
def _deploy_recovery(self):
if self.get_flight_state() == FlightState.DROGUE_DESCENT: # Drogue deploying
self._drogue_deployment_time = self.get_time_since_launch()
elif self.get_flight_state() == FlightState.MAIN_DESCENT: # Main deploying
self._main_deployment_time = self.get_time_since_launch()
else:
assert False
new_data, new_events = self._run_simulation()
assert abs(new_events[FlightEvent.RECOVERY_DEVICE_DEPLOYMENT][-1] - self.get_time_since_launch()) < 0.1
merged_events = dict()
# Retain original events if already passed
for event, times in self._events.items():
for i in range(len(times)):
time = times[i]
if time <= self.get_time_since_launch():
if i in range(len(new_events[event])):
assert abs(new_events[event][i] - time) < 0.5 # TODO: Figure out where slight discrepancies come from
if event in merged_events:
merged_events[event].append(time)
else:
merged_events[event] = [time]
# Update with new events
for event, times in new_events.items():
for time in times:
if time >= new_events[FlightEvent.RECOVERY_DEVICE_DEPLOYMENT][-1]:
if event in merged_events:
merged_events[event].append(time)
else:
merged_events[event] = [time]
self._data = new_data
self._events = merged_events
LOGGER.info(
f"Recovery deployed at time {self.get_time()} s, and altitude {self.get_data(FlightDataType.TYPE_ALTITUDE)} m")
def shutdown(self):
"""
This is called when the app is being requested to shut down
:return:
:rtype:
"""
LOGGER.debug(f"MainApp shutting down")
self.ReadThread.shutdown()
self.SendThread.shutdown()
self.rocket_data.shutdown()
for connection in self.connections.values():
connection.shutdown()
LOGGER.debug(
f"All threads shut down, remaining threads: {threading.enumerate()}"
)
LOGGER.info("Saving...")
self.rocket_data.save(
os.path.join(LOGS_DIR, "finalsave_" + SESSION_ID + ".csv"))
LOGGER.info("Saved!")
def _run_self_test(self):
try:
LOGGER.info("SELF TEST STARTED")
snapshot = get_event_stats_snapshot()
sleep(20)
# Dont wait, check difference now all at once
# Add any other common events here
assert LABLES_UPDATED_EVENT.wait(snapshot, timeout=0) >= 2
assert MAP_UPDATED_EVENT.wait(snapshot, timeout=0) >= 2
LOGGER.info("SELF TEST PASSED")
ret_code = 0
except AssertionError:
LOGGER.exception("SELF TEST FAILED")
ret_code = 1
self.main_app.shutdown()
os._exit(ret_code)
def message(self, byte_stream: BytesIO, header: Header):
"""
Save and log an extracted message.
:param byte_stream:
:type byte_stream:
:param header:
:type header:
:return:
:rtype:
"""
data: Dict = {}
length: int = byte_stream.read(1)[0]
# Two step: immutable int[] -> string
byte_data = byte_stream.read(length)
data[DataEntryIds.MESSAGE] = byte_data.decode('ascii')
# Do something with data
LOGGER.info("Incoming message: " + str(data[DataEntryIds.MESSAGE]))
return data
def status_ping(self, byte_stream: BytesIO, header: Header):
"""
Convert bit field into a series of statuses
:param byte_stream:
:type byte_stream:
:param header:
:type header:
:return:
:rtype:
"""
sensor_bit_field_length = 16
other_bit_field_length = 16
data: Dict = {}
curr_byte: int = byte_stream.read(1)[0]
# Overall status from 6th and 7th bits
overall_status = curr_byte & 0b11
data[DataEntryIds.OVERALL_STATUS] = BITARRAY_TO_STATUS[overall_status]
# save since we do multiple passes over each byte
byte_list: List[int] = [b for b in byte_stream.read(2)]
# Sensor status
num_assigned_bits = min(
sensor_bit_field_length,
len(SENSOR_TYPES)) # only go as far as is assigned
for i in range(0, num_assigned_bits):
byte_index = math.floor(
i / 8) # 0 based index, of byte out of current group
relative_bit_index = 7 - (i % 8) # get the bits left to right
data[SENSOR_TYPES[i]] = self.bitfrombyte(byte_list[byte_index],
relative_bit_index)
byte_list: List[int] = [b for b in byte_stream.read(2)]
# Other misc statuses
num_assigned_bits = min(
other_bit_field_length,
len(OTHER_STATUS_TYPES)) # only go as far as is assigned
for i in range(0, num_assigned_bits):
byte_index = math.floor(i / 8)
relative_bit_index = 7 - (i % 8)
data[OTHER_STATUS_TYPES[i]] = self.bitfrombyte(
byte_list[byte_index], relative_bit_index)
LOGGER.info("Overall rocket status: %s",
str(data[DataEntryIds.OVERALL_STATUS]))
LOGGER.info(
" - status of sensors" + ", %s" * len(SENSOR_TYPES), *[
sensor.name + ": " + str(data[sensor])
for sensor in SENSOR_TYPES
])
LOGGER.info(
" - status of others" + ", %s" * len(OTHER_STATUS_TYPES), *[
other.name + ": " + str(data[other])
for other in OTHER_STATUS_TYPES
])
return data
def run(self):
"""
"""
LOGGER.debug("Send thread started")
# TODO : Once we have multiple connections, we will loop over and send a config request to each
# Starting up, request hello/ha ndshake/identification
for connection in self.connections.values():
try:
connection.broadcast(bytes([CommandType.CONFIG.value]))
except Exception as ex:
LOGGER.exception(
"Exception in send thread while sending config requests")
while True:
try:
message = self.commandQueue.get(
block=True, timeout=None) # Block until something new
self.commandQueue.task_done()
if message is None: # Either received None or woken up for shutdown
with self._shutdown_lock:
if self._is_shutting_down:
break
else:
continue
message_parts = message.split('.')
if len(message_parts) != 2:
LOGGER.error("Bad command format")
continue
(device_str, command_str) = message_parts
try:
device = DeviceType[device_str.upper()]
except KeyError:
LOGGER.error(f"Unknown device: {device_str}")
continue
full_address = self.device_manager.get_full_address(device)
if full_address is None:
LOGGER.error(f"Device not yet connected: {device.name}")
continue
connection = self.connections[full_address.connection_name]
try:
command = CommandType[command_str.upper()]
except KeyError:
LOGGER.error(f"Unknown command {command_str}")
continue
LOGGER.info(
f"Sending command {command} to device {device.name} ({full_address})"
)
data = bytes([command.value])
connection.send(full_address.device_address, data)
LOGGER.info("Sent command!")
COMMAND_SENT_EVENT.increment()
except TimeoutException: # TODO: Connection should have converted this to a generic exception for decoupling
LOGGER.error("Message timed-out!")
except queue.Empty:
pass
except Exception as ex:
LOGGER.exception(
"Unexpected error while sending!"
) # Automatically grabs and prints exception info
LOGGER.warning("Send thread shut down")
