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 and config writer
self._lh_memory_helper = LighthouseMemHelper(self._helper.cf)
self._lh_config_writer = LighthouseConfigWriter(self._helper.cf)
self._start_read_of_geo_data()
self._update_ui()
def __init__(self, cf, nr_of_base_stations=16):
self._cf = cf
self._helper = LighthouseMemHelper(cf)
self._data_stored_cb = None
self._geos_to_write = None
self._geos_to_persist = []
self._calibs_to_persist = []
self._write_failed_for_one_or_more_objects = False
self._nr_of_base_stations = nr_of_base_stations
def writeBaseStationData(self, geometryOne, geometryTwo):
geo_dict = {0: geometryOne, 1: geometryTwo}
helper = LighthouseMemHelper(self.crazyflie.cf)
writer = LighthouseConfigWriter(self.crazyflie.cf,
nr_of_base_stations=2)
self.write(lambda: helper.write_geos(geo_dict, self.writeComplete))
self.write(lambda: writer.write_and_store_config(self.writeComplete,
geos=geo_dict,
calibs=calibration.
CALIBRATION_DATA))
exceptionUtil.checkInterrupt()
def writeBaseStationData(cf, baseOne, baseTwo):
global dataWritten
dataWritten = False
mems = cf.mem.get_mems(MemoryElement.TYPE_LH)
count = len(mems)
if count != 1:
raise Exception('Unexpected nr of memories found:', count)
helper = LighthouseMemHelper(scf.cf)
geo_dict = { 0: baseOne, 1: baseTwo }
helper.write_geos(geo_dict, dataWrittenCallback)
dataWrittenEvent.wait()
def estimate(self, uri, do_write):
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(uri, cf=cf) as scf:
print("Reading sensor data...")
sweep_angle_reader = LighthouseSweepAngleAverageReader(
scf.cf, self.angles_collected_cb)
sweep_angle_reader.start_angle_collection()
self.collection_event.wait()
print("Estimating position of base stations...")
geometries = {}
estimator = LighthouseBsGeoEstimator()
for id in sorted(self.sensor_vectors_all.keys()):
average_data = self.sensor_vectors_all[id]
sensor_data = average_data[1]
rotation_bs_matrix, position_bs_vector = estimator.estimate_geometry(
sensor_data)
is_valid = estimator.sanity_check_result(position_bs_vector)
if is_valid:
geo = LighthouseBsGeometry()
geo.rotation_matrix = rotation_bs_matrix
geo.origin = position_bs_vector
geo.valid = True
geometries[id] = geo
self.print_geo(rotation_bs_matrix, position_bs_vector,
is_valid)
else:
print("Warning: could not find valid solution for " + id +
1)
print()
if do_write:
print("Uploading geo data to CF")
helper = LighthouseMemHelper(scf.cf)
helper.write_geos(geometries, self.write_done_cb)
self.write_event.wait()
def __init__(self, uri):
self._event = Event()
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
helper = LighthouseMemHelper(scf.cf)
helper.read_all_geos(self._geo_read_ready)
self._event.wait()
self._event.clear()
helper.read_all_calibs(self._calib_read_ready)
self._event.wait()
def __init__(self, uri, geo_dict, calib_dict):
self._event = Event()
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
helper = LighthouseMemHelper(scf.cf)
helper.write_geos(geo_dict, self._data_written)
self._event.wait()
self._event.clear()
helper.write_calibs(calib_dict, self._data_written)
self._event.wait()
print("\nCalib: ", type(data))
print('---- Calibration for base station', id + 1)
data.dump()
print()
readEvent.set()
with SyncCrazyflie(uri, cf=Crazyflie(ro_cache='../cache',
rw_cache='../cache')) as scf:
scf.cf.param.set_value('stabilizer.controller',
'2') # Mellinger controller
scf.cf.param.set_value('commander.enHighLevel', '1')
scf.cf.param.set_value('lighthouse.method', '0')
scf.cf.param.set_value('lighthouse.systemType', '1')
helper = LighthouseMemHelper(scf.cf)
helper.read_all_geos(geoDataReady)
readEvent.wait()
readEvent.clear()
helper.read_all_calibs(calibDataReady)
readEvent.wait()
geometryOne, geometryTwo = get_geometry()
bs1calib = LighthouseBsCalibration()
bs1calib.sweeps[0].phase = 1.0
bs1calib.sweeps[0].tilt = 2.0
bs1calib.sweeps[0].curve = 3.0
bs1calib.sweeps[0].gibmag = 4.0
bs1calib.sweeps[0].gibphase = 5.0
class LighthouseConfigWriter:
"""
This class is used to write system config data to the Crazyflie RAM and persis to permanent storage
"""
def __init__(self, cf, nr_of_base_stations=16):
self._cf = cf
self._helper = LighthouseMemHelper(cf)
self._data_stored_cb = None
self._geos_to_write = None
self._geos_to_persist = []
self._calibs_to_persist = []
self._write_failed_for_one_or_more_objects = False
self._nr_of_base_stations = nr_of_base_stations
def write_and_store_config(self,
data_stored_cb,
geos=None,
calibs=None,
system_type=None):
"""
Transfer geometry and calibration data to the Crazyflie and persist to permanent storage.
The callback is called when done.
If geos or calibs is None, no data will be written for that data type.
If geos or calibs is a dictionary, the values for the base stations in the dictionary will
transfered to the Crazyflie, data for all other base stations will be invalidated.
"""
if self._data_stored_cb is not None:
raise Exception('Write already in prgress')
self._data_stored_cb = data_stored_cb
self._cf.loc.receivedLocationPacket.add_callback(
self._received_location_packet)
self._geos_to_write = self._prepare_geos(geos)
self._calibs_to_write = self._prepare_calibs(calibs)
self._geos_to_persist = []
if self._geos_to_write is not None:
self._geos_to_persist = list(range(self._nr_of_base_stations))
self._calibs_to_persist = []
if self._calibs_to_write is not None:
self._calibs_to_persist = list(range(self._nr_of_base_stations))
self._write_failed_for_one_or_more_objects = False
if system_type is not None:
# Change system type first as this will erase calib and geo data in the CF.
# Changing system type may trigger a lengthy operation (up to 0.5 s) if the persistant memory requires
# defrag. Setting a param is an asynchronous operataion, and it is not possible to know if the system
# swich is finished before we continue.
self._cf.param.set_value('lighthouse.systemType', system_type)
# We add a sleep here to make sure the change of system type is finished. It is dirty but will have to
# do for now. A more propper solution would be to add support for Remote Procedure Calls (RPC) with
# synchronous function calls.
time.sleep(0.8)
self._next()
def write_and_store_config_from_file(self, data_stored_cb, file_name):
"""
Read system configuration data from file and write/persist to the Crazyflie.
Geometry and calibration data for base stations that are not in the config file will be invalidated.
"""
geos, calibs, system_type = LighthouseConfigFileManager.read(file_name)
self.write_and_store_config(data_stored_cb,
geos=geos,
calibs=calibs,
system_type=system_type)
def _next(self):
if self._geos_to_write is not None:
self._helper.write_geos(self._geos_to_write, self._upload_done)
self._geos_to_write = None
return
if self._calibs_to_write is not None:
self._helper.write_calibs(self._calibs_to_write, self._upload_done)
self._calibs_to_write = None
return
if len(self._geos_to_persist) > 0 or len(self._calibs_to_persist) > 0:
self._cf.loc.send_lh_persist_data_packet(self._geos_to_persist,
self._calibs_to_persist)
self._geos_to_persist = []
self._calibs_to_persist = []
return
tmp_callback = self._data_stored_cb
self._data_stored_cb = None
if tmp_callback is not None:
tmp_callback(not self._write_failed_for_one_or_more_objects)
def _upload_done(self, sucess):
if not sucess:
self._write_failed_for_one_or_more_objects = True
self._next()
def _received_location_packet(self, packet):
# New geo data has been written and stored in the CF
if packet.type == self._cf.loc.LH_PERSIST_DATA:
self._next()
def _prepare_geos(self, geos):
result = None
if geos is not None:
result = dict(geos)
# Pad for base stations without data
empty_geo = LighthouseBsGeometry()
for id in range(self._nr_of_base_stations):
if id not in result:
result[id] = empty_geo
return result
def _prepare_calibs(self, calibs):
result = None
if calibs is not None:
result = dict(calibs)
# Pad for base stations without data
empty_calib = LighthouseBsCalibration()
for id in range(self._nr_of_base_stations):
if id not in result:
result[id] = empty_calib
return result
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_system_config_written_to_cf_signal = pyqtSignal(bool)
_geometry_read_signal = pyqtSignal(object)
_calibration_read_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_system_config_written_to_cf_signal.connect(
self._new_system_config_written_to_cf)
self._geometry_read_signal.connect(self._geometry_read_cb)
self._calibration_read_signal.connect(self._calibration_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_config_writer = 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._basestation_mode_dialog = LighthouseBsModeDialog(self)
self._manage_estimate_geometry_button.clicked.connect(
self._show_basestation_geometry_dialog)
self._manage_basestation_mode_button.clicked.connect(
self._show_basestation_mode_dialog)
self._load_sys_config_button.clicked.connect(
self._load_sys_config_button_clicked)
self._save_sys_config_button.clicked.connect(
self._save_sys_config_button_clicked)
self._current_folder = os.path.expanduser('~')
self._is_connected = False
self._update_ui()
def write_and_store_geometry(self, geometries):
if self._lh_config_writer:
self._lh_config_writer.write_and_store_config(
self._new_system_config_written_to_cf_signal.emit,
geos=geometries)
def _new_system_config_written_to_cf(self, success):
# Reset the bit fields for calibration data status to get a fresh view on
self._helper.cf.param.set_value("lighthouse.bsCalibReset", '1')
# New geo data has been written and stored in the CF, read it back to update the UI
self._start_read_of_geo_data()
def _show_basestation_geometry_dialog(self):
self._basestation_geometry_dialog.reset()
self._basestation_geometry_dialog.show()
def _show_basestation_mode_dialog(self):
self._basestation_mode_dialog.reset()
self._basestation_mode_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._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 and config writer
self._lh_memory_helper = LighthouseMemHelper(self._helper.cf)
self._lh_config_writer = LighthouseConfigWriter(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):
enabled = self._is_connected and self.is_lighthouse_deck_active
self._manage_estimate_geometry_button.setEnabled(enabled)
self._load_sys_config_button.setEnabled(enabled)
self._save_sys_config_button.setEnabled(enabled)
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 = 'No geo/calib'
elif status == self.STATUS_TO_ESTIMATOR:
text = 'LH ready'
self._status_status.setText(text)
def _clear_state(self):
self._lh_memory_helper = None
self._lh_config_writer = 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)
label.setToolTip('Calibration data from cache')
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)
label.setToolTip('Calibration data verified')
if calib_updated:
label.setStyleSheet(STYLE_ORANGE_BACKGROUND)
label.setToolTip(
'Calibration data updated, the geometry probably needs to be re-estimated'
)
else:
label.setStyleSheet(STYLE_GREEN_BACKGROUND)
else:
label.setStyleSheet(STYLE_RED_BACKGROUND)
label.setToolTip('')
def _load_sys_config_button_clicked(self):
names = QFileDialog.getOpenFileName(self, 'Open file',
self._current_folder,
"*.yaml;;*.*")
if names[0] == '':
return
self._current_folder = os.path.dirname(names[0])
if self._lh_config_writer is not None:
self._lh_config_writer.write_and_store_config_from_file(
self._new_system_config_written_to_cf_signal.emit, names[0])
def _save_sys_config_button_clicked(self):
# Get calibration data from the Crazyflie to complete the system config data set
# When the data is ready we get a callback on _calibration_read
self._lh_memory_helper.read_all_calibs(
self._calibration_read_signal.emit)
def _calibration_read_cb(self, calibs):
# Got calibration data from the CF, we have the full system configuration
self._save_sys_config(self._lh_geos, calibs)
def _save_sys_config(self, geos, calibs):
names = QFileDialog.getSaveFileName(self, 'Save file',
self._current_folder,
"*.yaml;;*.*")
if names[0] == '':
return
self._current_folder = os.path.dirname(names[0])
if not names[0].endswith(".yaml") and names[0].find(".") < 0:
filename = names[0] + ".yaml"
else:
filename = names[0]
LighthouseConfigFileManager.write(filename, geos, calibs)
class LighthouseConfigWriter:
"""
This class is used to write system config data to the Crazyflie RAM and persis to permanent storage
"""
def __init__(self, cf, nr_of_base_stations=16):
self._cf = cf
self._helper = LighthouseMemHelper(cf)
self._data_stored_cb = None
self._geos_to_write = None
self._geos_to_persist = []
self._calibs_to_persist = []
self._write_failed_for_one_or_more_objects = False
self._nr_of_base_stations = nr_of_base_stations
def write_and_store_config(self, data_stored_cb, geos=None, calibs=None):
"""
Transfer geometry and calibration data to the Crazyflie and persist to permanent storage.
The callback is called when done.
If geos or calibs is None, no data will be written for that data type.
If geos or calibs is a dictionary, the values for the base stations in the dictionary will
transfered to the Crazyflie, data for all other base stations will be invalidated.
"""
if self._data_stored_cb is not None:
raise Exception('Write already in prgress')
self._data_stored_cb = data_stored_cb
self._cf.loc.receivedLocationPacket.add_callback(self._received_location_packet)
self._geos_to_write = self._prepare_geos(geos)
self._calibs_to_write = self._prepare_calibs(calibs)
self._geos_to_persist = []
if self._geos_to_write is not None:
self._geos_to_persist = list(range(self._nr_of_base_stations))
self._calibs_to_persist = []
if self._calibs_to_write is not None:
self._calibs_to_persist = list(range(self._nr_of_base_stations))
self._write_failed_for_one_or_more_objects = False
self._next()
def write_and_store_config_from_file(self, data_stored_cb, file_name):
"""
Read system configuration data from file and write/persist to the Crazyflie.
Geometry and calibration data for base stations that are not in the config file will be invalidated.
"""
geos, calibs = LighthouseConfigFileManager.read(file_name)
self.write_and_store_config(data_stored_cb, geos=geos, calibs=calibs)
def _next(self):
if self._geos_to_write is not None:
self._helper.write_geos(self._geos_to_write, self._upload_done)
self._geos_to_write = None
return
if self._calibs_to_write is not None:
self._helper.write_calibs(self._calibs_to_write, self._upload_done)
self._calibs_to_write = None
return
if len(self._geos_to_persist) > 0 or len(self._calibs_to_persist) > 0:
self._cf.loc.send_lh_persist_data_packet(self._geos_to_persist, self._calibs_to_persist)
self._geos_to_persist = []
self._calibs_to_persist = []
return
tmp_callback = self._data_stored_cb
self._data_stored_cb = None
if tmp_callback is not None:
tmp_callback(not self._write_failed_for_one_or_more_objects)
def _upload_done(self, sucess):
if not sucess:
self._write_failed_for_one_or_more_objects = True
self._next()
def _received_location_packet(self, packet):
# New geo data has been written and stored in the CF
if packet.type == self._cf.loc.LH_PERSIST_DATA:
self._next()
def _prepare_geos(self, geos):
result = None
if geos is not None:
result = dict(geos)
# Pad for base stations without data
empty_geo = LighthouseBsGeometry()
for id in range(self._nr_of_base_stations):
if id not in result:
result[id] = empty_geo
return result
def _prepare_calibs(self, calibs):
result = None
if calibs is not None:
result = dict(calibs)
# Pad for base stations without data
empty_calib = LighthouseBsCalibration()
for id in range(self._nr_of_base_stations):
if id not in result:
result[id] = empty_calib
return result