From 4ee37ffa60397654e0c514e1aa92c129c71b2092 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 22 Feb 2021 13:03:41 +0100 Subject: [PATCH] #702 Added script for uploading lighthouse system configuration --- tools/lighthouse/persist_bs_data.py | 59 ++++------------------- tools/lighthouse/upload_config.py | 73 +++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+), 49 deletions(-) mode change 100644 => 100755 tools/lighthouse/persist_bs_data.py create mode 100755 tools/lighthouse/upload_config.py diff --git a/tools/lighthouse/persist_bs_data.py b/tools/lighthouse/persist_bs_data.py old mode 100644 new mode 100755 index 5c28747618..ea5b707ce1 --- a/tools/lighthouse/persist_bs_data.py +++ b/tools/lighthouse/persist_bs_data.py @@ -34,69 +34,30 @@ import logging -import time +from threading import Event import cflib.crtp # noqa from cflib.crazyflie import Crazyflie from cflib.crazyflie.mem import LighthouseBsCalibration from cflib.crazyflie.mem import LighthouseBsGeometry -from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.localization import LighthouseConfigWriter -uri = 'radio://0/30' +uri = 'radio://0/80' class WriteMem: def __init__(self, uri, geos, calibs): - self.data_written = False - self.result_received = False + self._event = Event() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH) + writer = LighthouseConfigWriter(scf.cf, nr_of_base_stations=2) + writer.write_and_store_config(self._data_written, geos=geos, calibs=calibs) + self._event.wait() - count = len(mems) - if count != 1: - raise Exception('Unexpected nr of memories found:', count) - - lh_mem = mems[0] - - for bs, geo in geos.items(): - self.data_written = False - print('Write geoetry', bs, 'to RAM') - lh_mem.write_geo_data(bs, geo, self._data_written, write_failed_cb=self._data_failed) - - while not self.data_written: - time.sleep(0.1) - - for bs, calib in calibs.items(): - self.data_written = False - print('Write calibration', bs, 'to RAM') - lh_mem.write_calib_data(bs, calib, self._data_written, write_failed_cb=self._data_failed) - - while not self.data_written: - time.sleep(0.1) - - print('Persist data') - scf.cf.loc.receivedLocationPacket.add_callback(self._data_persisted) - scf.cf.loc.send_lh_persist_data_packet(list(range(16)), list(range(16))) - - while not self.result_received: - time.sleep(0.1) - - - def _data_written(self, mem, addr): - self.data_written = True - - def _data_failed(self, mem, addr): - raise Exception('Write to RAM failed') - - def _data_persisted(self, data): - if (data.data): - print('Data persisted') - else: - raise Exception("Write to storage failed") - - self.result_received = True + def _data_written(self, sucess): + print('Data written') + self._event.set() geo0 = LighthouseBsGeometry() diff --git a/tools/lighthouse/upload_config.py b/tools/lighthouse/upload_config.py new file mode 100755 index 0000000000..63b4af9c76 --- /dev/null +++ b/tools/lighthouse/upload_config.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--ยด | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Crazyflie control firmware +# +# Copyright (C) 2021 Bitcraze AB +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, in version 3. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +# +# +# Upload geometry and calibration data to the Crazyflie storage +# +# This script reads geometry and calibration data from a system configuration +# file, usually created from the client. The data is uploaded to a crazyflie and +# written to the data to persistant memory to make it available after +# re-boot. +# +# This functionality can also be executed from the client, but this +# script is handy when uploading configurations to a swarm for instance. + +import argparse +import logging +from threading import Event + +import cflib.crtp # noqa +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.localization import LighthouseConfigWriter + + +class WriteMem: + def __init__(self, uri, file_name): + self._event = Event() + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + writer = LighthouseConfigWriter(scf.cf) + writer.write_and_store_config_from_file(self._data_written, file_name) + self._event.wait() + + def _data_written(self, sucess): + print('Data written') + self._event.set() + + +uri = 'radio://0/80' + +parser = argparse.ArgumentParser(description='Uploads a lighthouse system conficuration to a Crazyflie') +parser.add_argument('config_file', help='the file name of the configuration file to load') +parser.add_argument('--uri', help='uri to use when connecting to the Crazyflie. Default: ' + uri) +args = parser.parse_args() +if args.uri: + uri = args.uri + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) +cflib.crtp.init_drivers(enable_debug_driver=False) + +WriteMem(uri, args.config_file)