def __init__(self, tabWidget, helper, *args):
super(LighthouseTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Lighthouse Positioning"
self.menuName = "Lighthouse Positioning Tab"
self.tabWidget = tabWidget
self._helper = helper
# Always wrap callbacks from Crazyflie API though QT Signal/Slots
# to avoid manipulating the UI when rendering it
self._connected_signal.connect(self._connected)
self._disconnected_signal.connect(self._disconnected)
self._log_error_signal.connect(self._logging_error)
self._cb_param_to_detect_lighthouse_deck_signal.connect(
self._cb_param_to_detect_lighthouse_deck)
self._status_report_signal.connect(self._status_report_received)
self._new_data_geo_written_to_cf_ram_signal.connect(
self._new_data_geo_written_to_cf_ram_cbl)
self._received_location_packet_signal.connect(
self._received_location_packet_cb)
self._geometry_read_signal.connect(self._geometry_read_cb)
# Connect the Crazyflie API callbacks to the signals
self._helper.cf.connected.add_callback(self._connected_signal.emit)
self._helper.cf.disconnected.add_callback(
self._disconnected_signal.emit)
self._set_up_plots()
self.is_lighthouse_deck_active = False
self._lh_memory_helper = None
self._lh_geos = {}
self._bs_receives_light = set()
self._bs_calibration_data_exists = set()
self._bs_calibration_data_confirmed = set()
self._bs_calibration_data_updated = set()
self._bs_geometry_data_exists = set()
self._bs_data_to_estimator = set()
self._clear_state_indicator()
self._bs_stats = [
self._bs_receives_light, self._bs_calibration_data_exists,
self._bs_calibration_data_confirmed,
self._bs_calibration_data_updated, self._bs_geometry_data_exists,
self._bs_data_to_estimator
]
self._lh_status = self.STATUS_NOT_RECEIVING
self._graph_timer = QTimer()
self._graph_timer.setInterval(1000 / self.FPS)
self._graph_timer.timeout.connect(self._update_graphics)
self._graph_timer.start()
self._basestation_geometry_dialog = LighthouseBsGeometryDialog(self)
self._manage_estimate_geometry_button.clicked.connect(
self._show_basestation_geometry_dialog)
self._is_connected = False
self._update_ui()
class LighthouseTab(Tab, lighthouse_tab_class):
"""Tab for plotting Lighthouse data"""
# Update period of log data in ms
UPDATE_PERIOD_LOG = 100
# Frame rate (updates per second)
FPS = 2
STATUS_NOT_RECEIVING = 0
STATUS_MISSING_DATA = 1
STATUS_TO_ESTIMATOR = 2
# TODO change these names to something more logical
LOG_STATUS = "lighthouse.status"
LOG_RECEIVE = "lighthouse.bsReceive"
LOG_CALIBRATION_EXISTS = "lighthouse.bsCalVal"
LOG_CALIBRATION_CONFIRMED = "lighthouse.bsCalCon"
LOG_CALIBRATION_UPDATED = "lighthouse.bsCalUd"
LOG_GEOMETERY_EXISTS = "lighthouse.bsGeoVal"
LOG_ACTIVE = "lighthouse.bsActive"
_connected_signal = pyqtSignal(str)
_disconnected_signal = pyqtSignal(str)
_log_error_signal = pyqtSignal(object, str)
_cb_param_to_detect_lighthouse_deck_signal = pyqtSignal(object, object)
_status_report_signal = pyqtSignal(int, object, object)
_new_data_geo_written_to_cf_ram_signal = pyqtSignal(bool)
_geometry_read_signal = pyqtSignal(object)
_received_location_packet_signal = pyqtSignal(object)
def __init__(self, tabWidget, helper, *args):
super(LighthouseTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Lighthouse Positioning"
self.menuName = "Lighthouse Positioning Tab"
self.tabWidget = tabWidget
self._helper = helper
# Always wrap callbacks from Crazyflie API though QT Signal/Slots
# to avoid manipulating the UI when rendering it
self._connected_signal.connect(self._connected)
self._disconnected_signal.connect(self._disconnected)
self._log_error_signal.connect(self._logging_error)
self._cb_param_to_detect_lighthouse_deck_signal.connect(
self._cb_param_to_detect_lighthouse_deck)
self._status_report_signal.connect(self._status_report_received)
self._new_data_geo_written_to_cf_ram_signal.connect(
self._new_data_geo_written_to_cf_ram_cbl)
self._received_location_packet_signal.connect(
self._received_location_packet_cb)
self._geometry_read_signal.connect(self._geometry_read_cb)
# Connect the Crazyflie API callbacks to the signals
self._helper.cf.connected.add_callback(self._connected_signal.emit)
self._helper.cf.disconnected.add_callback(
self._disconnected_signal.emit)
self._set_up_plots()
self.is_lighthouse_deck_active = False
self._lh_memory_helper = None
self._lh_geos = {}
self._bs_receives_light = set()
self._bs_calibration_data_exists = set()
self._bs_calibration_data_confirmed = set()
self._bs_calibration_data_updated = set()
self._bs_geometry_data_exists = set()
self._bs_data_to_estimator = set()
self._clear_state_indicator()
self._bs_stats = [
self._bs_receives_light, self._bs_calibration_data_exists,
self._bs_calibration_data_confirmed,
self._bs_calibration_data_updated, self._bs_geometry_data_exists,
self._bs_data_to_estimator
]
self._lh_status = self.STATUS_NOT_RECEIVING
self._graph_timer = QTimer()
self._graph_timer.setInterval(1000 / self.FPS)
self._graph_timer.timeout.connect(self._update_graphics)
self._graph_timer.start()
self._basestation_geometry_dialog = LighthouseBsGeometryDialog(self)
self._manage_estimate_geometry_button.clicked.connect(
self._show_basestation_geometry_dialog)
self._is_connected = False
self._update_ui()
def write_and_store_geometry(self, geometries):
if self._lh_memory_helper:
self._lh_memory_helper.write_geos(
geometries, self._new_data_geo_written_to_cf_ram_signal.emit)
def _new_data_geo_written_to_cf_ram_cbl(self, success):
# The new gometry data is in CF RAM, write it to persistant memory
# When done, we will get at call to _received_location_packet_cb()
self._helper.cf.loc.send_lh_persist_data_packet(list(range(16)), [])
# Reset the bit fields for calibration data status to get a fresh view on
self._helper.cf.param.set_value("lighthouse.bsCalibReset", '1')
def _received_location_packet_cb(self, packet):
# New geo data has been written and stored in the CF, read it back to update the UI
if packet.type == self._helper.cf.loc.LH_PERSIST_DATA:
self._start_read_of_geo_data()
def _show_basestation_geometry_dialog(self):
self._basestation_geometry_dialog.reset()
self._basestation_geometry_dialog.show()
def _set_up_plots(self):
self._plot_3d = Plot3dLighthouse()
self._plot_layout.addWidget(self._plot_3d.native)
def _connected(self, link_uri):
"""Callback when the Crazyflie has been connected"""
logger.info("Crazyflie connected to {}".format(link_uri))
self._request_param_to_detect_lighthouse_deck()
self._helper.cf.loc.receivedLocationPacket.add_callback(
self._received_location_packet_signal.emit)
self._basestation_geometry_dialog.reset()
self._is_connected = True
self._update_ui()
def _request_param_to_detect_lighthouse_deck(self):
"""Send a parameter request to detect if the Lighthouse deck is
installed"""
group = 'deck'
param = 'bcLighthouse4'
if self._is_in_param_toc(group, param):
logger.info("Requesting lighthouse deck parameter")
self._helper.cf.param.add_update_callback(
group=group,
name=param,
cb=self._cb_param_to_detect_lighthouse_deck_signal.emit)
def _cb_param_to_detect_lighthouse_deck(self, name, value):
"""Callback from the parameter sub system when the Lighthouse deck detection
parameter has been updated"""
if value == '1':
logger.info("Lighthouse deck installed, enabling the tab")
self._lighthouse_deck_detected()
else:
logger.info("No Lighthouse deck installed")
def _lighthouse_deck_detected(self):
"""Called when the lighthouse deck has been detected. Enables the tab,
starts logging and polling of the memory sub system as well as starts
timers for updating graphics"""
if not self.is_lighthouse_deck_active:
self.is_lighthouse_deck_active = True
try:
self._register_logblock("lhStatus", [
self.LOG_STATUS, self.LOG_RECEIVE,
self.LOG_CALIBRATION_EXISTS,
self.LOG_CALIBRATION_CONFIRMED,
self.LOG_CALIBRATION_UPDATED, self.LOG_GEOMETERY_EXISTS,
self.LOG_ACTIVE
], self._status_report_signal.emit,
self._log_error_signal.emit)
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# Now that we know we have a lighthouse deck, setup the memory helper
self._lh_memory_helper = LighthouseMemHelper(self._helper.cf)
self._start_read_of_geo_data()
self._update_ui()
def _start_read_of_geo_data(self):
self._lh_memory_helper.read_all_geos(self._geometry_read_signal.emit)
def _geometry_read_cb(self, geometries):
# Remove any geo data where the valid flag is False
self._lh_geos = dict(
filter(lambda key_value: key_value[1].valid, geometries.items()))
self._basestation_geometry_dialog.geometry_updated(self._lh_geos)
def _adjust_bitmask(self, bit_mask, bs_list):
for id in range(16):
if bit_mask & (1 << id):
bs_list.add(id)
else:
if id in bs_list:
bs_list.remove(id)
self._update_basestation_status_indicators()
def _status_report_received(self, timestamp, data, logconf):
"""Callback from the logging system when the status is updated."""
if self.LOG_RECEIVE in data:
bit_mask = data[self.LOG_RECEIVE]
self._adjust_bitmask(bit_mask, self._bs_receives_light)
if self.LOG_CALIBRATION_EXISTS in data:
bit_mask = data[self.LOG_CALIBRATION_EXISTS]
self._adjust_bitmask(bit_mask, self._bs_calibration_data_exists)
if self.LOG_CALIBRATION_CONFIRMED in data:
bit_mask = data[self.LOG_CALIBRATION_CONFIRMED]
self._adjust_bitmask(bit_mask, self._bs_calibration_data_confirmed)
if self.LOG_CALIBRATION_UPDATED in data:
bit_mask = data[self.LOG_CALIBRATION_UPDATED]
self._adjust_bitmask(bit_mask, self._bs_calibration_data_updated)
if self.LOG_GEOMETERY_EXISTS in data:
bit_mask = data[self.LOG_GEOMETERY_EXISTS]
self._adjust_bitmask(bit_mask, self._bs_geometry_data_exists)
if self.LOG_ACTIVE in data:
bit_mask = data[self.LOG_ACTIVE]
self._adjust_bitmask(bit_mask, self._bs_data_to_estimator)
if self.LOG_STATUS in data:
self._lh_status = data[self.LOG_STATUS]
def _disconnected(self, link_uri):
"""Callback for when the Crazyflie has been disconnected"""
logger.debug("Crazyflie disconnected from {}".format(link_uri))
self._clear_state()
self._update_graphics()
self._plot_3d.clear()
self._basestation_geometry_dialog.close()
self.is_lighthouse_deck_active = False
self._is_connected = False
self._update_ui()
def _register_logblock(self,
logblock_name,
variables,
data_cb,
error_cb,
update_period=UPDATE_PERIOD_LOG):
"""Register log data to listen for. One logblock can only contain a limited
number of parameters."""
lg = LogConfig(logblock_name, update_period)
for variable in variables:
if self._is_in_log_toc(variable):
lg.add_variable(variable)
self._helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(data_cb)
lg.error_cb.add_callback(error_cb)
lg.start()
return lg
def _is_in_log_toc(self, variable):
toc = self._helper.cf.log.toc
group, param = variable.split('.')
return group in toc.toc and param in toc.toc[group]
def _is_in_param_toc(self, group, param):
toc = self._helper.cf.param.toc
return bool(group in toc.toc and param in toc.toc[group])
def _logging_error(self, log_conf, msg):
"""Callback from the log layer when an error occurs"""
QMessageBox.about(self, "LighthouseTab error",
"Error when using log config",
" [{0}]: {1}".format(log_conf.name, msg))
def _update_graphics(self):
if self.is_visible() and self.is_lighthouse_deck_active:
self._plot_3d.update_cf_pose(
self._helper.pose_logger.position,
self._rpy_to_rot(self._helper.pose_logger.rpy_rad))
self._plot_3d.update_base_station_geos(self._lh_geos)
self._plot_3d.update_base_station_visibility(
self._bs_data_to_estimator)
self._update_position_label(self._helper.pose_logger.position)
self._update_status_label(self._lh_status)
def _update_ui(self):
self._manage_estimate_geometry_button.setEnabled(
self._is_connected and self.is_lighthouse_deck_active)
def _update_position_label(self, position):
if len(position) == 3:
coordinate = "({:0.2f}, {:0.2f}, {:0.2f})".format(
position[0], position[1], position[2])
else:
coordinate = '(0.00, 0.00, 0.00)'
self._status_position.setText(coordinate)
def _update_status_label(self, status):
text = ''
if status == self.STATUS_NOT_RECEIVING:
text = 'Not receiving'
elif status == self.STATUS_MISSING_DATA:
text = 'Geo or calibration data missing'
elif status == self.STATUS_TO_ESTIMATOR:
text = 'Data sent to estimator'
self._status_status.setText(text)
def _clear_state(self):
self._lh_memory_helper = None
self._lh_geos = {}
self._bs_receives_light.clear()
self._bs_calibration_data_exists.clear()
self._bs_calibration_data_confirmed.clear()
self._bs_calibration_data_updated.clear()
self._bs_geometry_data_exists.clear()
self._bs_data_to_estimator.clear()
self._update_basestation_status_indicators()
self._clear_state_indicator()
self._lh_status = self.STATUS_NOT_RECEIVING
def _clear_state_indicator(self):
container = self._basestation_stats_container
for row in range(1, 3):
for col in range(1, 5):
color_label = container.itemAtPosition(row, col).widget()
color_label.setStyleSheet(STYLE_NO_BACKGROUND)
def _rpy_to_rot(self, rpy):
# http://planning.cs.uiuc.edu/node102.html
# Pitch reversed compared to page above
roll = rpy[0]
pitch = rpy[1]
yaw = rpy[2]
cg = math.cos(roll)
cb = math.cos(-pitch)
ca = math.cos(yaw)
sg = math.sin(roll)
sb = math.sin(-pitch)
sa = math.sin(yaw)
r = [
[ca * cb, ca * sb * sg - sa * cg, ca * sb * cg + sa * sg],
[sa * cb, sa * sb * sg + ca * cg, sa * sb * cg - ca * sg],
[-sb, cb * sg, cb * cg],
]
return np.array(r)
def _update_basestation_status_indicators(self):
"""Handling the basestation status label handles to indicate
the state of received data per basestation"""
container = self._basestation_stats_container
# Ports the label number to the first index of the statistic id
stats_id_port = {1: 0, 2: 1, 3: 4, 4: 5}
for bs in range(2):
for stats_indicator_id in range(1, 5):
bs_indicator_id = bs + 1
label = container.itemAtPosition(bs_indicator_id,
stats_indicator_id).widget()
stats_id = stats_id_port.get(stats_indicator_id)
temp_set = self._bs_stats[stats_id]
if bs in temp_set:
# If the status bar for calibration data is handled, have an intermeddiate status
# else just have red or green.
if stats_indicator_id == 2:
label.setStyleSheet(STYLE_BLUE_BACKGROUND)
calib_confirm = bs in self._bs_stats[stats_id + 1]
calib_updated = bs in self._bs_stats[stats_id + 2]
if calib_confirm:
label.setStyleSheet(STYLE_GREEN_BACKGROUND)
if calib_updated:
label.setStyleSheet(STYLE_ORANGE_BACKGROUND)
else:
label.setStyleSheet(STYLE_GREEN_BACKGROUND)
else:
label.setStyleSheet(STYLE_RED_BACKGROUND)