def test_bs_calibration_file_format(self):
# Fixture
calib = LighthouseBsCalibration()
calib.uid = 1234
calib.sweeps[0].curve = 1.0
calib.sweeps[0].phase = 2.0
calib.sweeps[0].tilt = 3.0
calib.sweeps[0].gibmag = 4.0
calib.sweeps[0].gibphase = 5.0
calib.sweeps[0].ogeemag = 6.0
calib.sweeps[0].ogeephase = 7.0
calib.sweeps[1].curve = 8.0
# Test
actual = calib.as_file_object()
# Assert
self.assertEqual(1234, actual['uid'])
self.assertEqual(1.0, actual['sweeps'][0]['curve'])
self.assertEqual(2.0, actual['sweeps'][0]['phase'])
self.assertEqual(3.0, actual['sweeps'][0]['tilt'])
self.assertEqual(4.0, actual['sweeps'][0]['gibmag'])
self.assertEqual(5.0, actual['sweeps'][0]['gibphase'])
self.assertEqual(6.0, actual['sweeps'][0]['ogeemag'])
self.assertEqual(7.0, actual['sweeps'][0]['ogeephase'])
self.assertEqual(8.0, actual['sweeps'][1]['curve'])
def read(file_name):
file = open(file_name, 'r')
with file:
data = yaml.safe_load(file)
if LighthouseConfigFileManager.TYPE_ID not in data:
raise Exception('Type field missing')
if data[LighthouseConfigFileManager.TYPE_ID] != LighthouseConfigFileManager.TYPE:
raise Exception('Unsupported file type')
if LighthouseConfigFileManager.VERSION_ID not in data:
raise Exception('Version field missing')
if data[LighthouseConfigFileManager.VERSION_ID] != LighthouseConfigFileManager.VERSION:
raise Exception('Unsupported file version')
result_geos = {}
result_calibs = {}
if LighthouseConfigFileManager.GEOS_ID in data:
for id, geo in data[LighthouseConfigFileManager.GEOS_ID].items():
result_geos[id] = LighthouseBsGeometry.from_file_object(geo)
if LighthouseConfigFileManager.CALIBS_ID in data:
for id, calib in data[LighthouseConfigFileManager.CALIBS_ID].items():
result_calibs[id] = LighthouseBsCalibration.from_file_object(calib)
return result_geos, result_calibs
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
def test_bs_calibration_file_write_read(self):
# Fixture
calib = LighthouseBsCalibration()
calib.uid = 1234
calib.sweeps[0].curve = 1.0
calib.sweeps[0].phase = 2.0
calib.sweeps[0].tilt = 3.0
calib.sweeps[0].gibmag = 4.0
calib.sweeps[0].gibphase = 5.0
calib.sweeps[0].ogeemag = 6.0
calib.sweeps[0].ogeephase = 7.0
calib.sweeps[1].curve = 8.0
file_object = calib.as_file_object()
# Test
actual = LighthouseBsCalibration.from_file_object(file_object)
# Assert
actual_file_object = actual.as_file_object()
self.assertEqual(file_object, actual_file_object)
self.assertTrue(actual.valid)
'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
bs1calib.sweeps[0].ogeemag = 6.0
bs1calib.sweeps[0].ogeephase = 7.0
bs1calib.sweeps[1].phase = 1.1
bs1calib.sweeps[1].tilt = 2.1
bs1calib.sweeps[1].curve = 3.1
bs1calib.sweeps[1].gibmag = 4.1
bs1calib.sweeps[1].gibphase = 5.1
bs1calib.sweeps[1].ogeemag = 6.1
bs1calib.sweeps[1].ogeephase = 7.1
bs1calib.uid = 1234
geo0 = LighthouseBsGeometry()
geo0.origin = [-1.01977998, -0.19424433, 1.97086964]
geo0.rotation_matrix = [
[0.66385385, -0.26347329, 0.6999142],
[0.18206993, 0.96466617, 0.19044612],
[-0.72536102, 0.00100494, 0.68836792],
]
geo0.valid = True
geo1 = LighthouseBsGeometry()
geo1.origin = [0, 0, 0]
geo1.rotation_matrix = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
geo1.valid = False
calib0 = LighthouseBsCalibration()
calib0.sweeps[0].tilt = -0.047353
calib0.sweeps[0].phase = 0.000000
calib0.sweeps[0].curve = 0.478887
calib0.sweeps[0].gibphase = 1.023093
calib0.sweeps[0].gibmag = 0.005071
calib0.sweeps[0].ogeephase = 1.136886
calib0.sweeps[0].ogeemag = -0.520102
calib0.sweeps[1].tilt = 0.049104
calib0.sweeps[1].phase = -0.006642
calib0.sweeps[1].curve = 0.675827
calib0.sweeps[1].gibphase = 2.367835
calib0.sweeps[1].gibmag = 0.004907
calib0.sweeps[1].ogeephase = 1.900456
calib0.sweeps[1].ogeemag = -0.457289
calib0.valid = True
geo0 = LighthouseBsGeometry()
geo0.origin = [-1.01977998, -0.19424433, 1.97086964]
geo0.rotation_matrix = [
[0.66385385, -0.26347329, 0.6999142],
[0.18206993, 0.96466617, 0.19044612],
[-0.72536102, 0.00100494, 0.68836792],
]
geo0.valid = True
geo1 = LighthouseBsGeometry()
geo1.origin = [0, 0, 0]
geo1.rotation_matrix = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
geo1.valid = False
calib0 = LighthouseBsCalibration()
calib0.sweeps[0].tilt = -0.047353
calib0.sweeps[0].phase = 0.000000
calib0.sweeps[0].curve = 0.478887
calib0.sweeps[0].gibphase = 1.023093
calib0.sweeps[0].gibmag = 0.005071
calib0.sweeps[0].ogeephase = 1.136886
calib0.sweeps[0].ogeemag = -0.520102
calib0.sweeps[1].tilt = 0.049104
calib0.sweeps[1].phase = -0.006642
calib0.sweeps[1].curve = 0.675827
calib0.sweeps[1].gibphase = 2.367835
calib0.sweeps[1].gibmag = 0.004907
calib0.sweeps[1].ogeephase = 1.900456
calib0.sweeps[1].ogeemag = -0.457289
calib0.uid = 0x3C65D22F
from cflib.crazyflie.mem import LighthouseBsCalibration CalibrationOne = LighthouseBsCalibration() CalibrationOne.sweeps[0].phase = 1.0 CalibrationOne.sweeps[0].tilt = 2.0 CalibrationOne.sweeps[0].curve = 3.0 CalibrationOne.sweeps[0].gibmag = 4.0 CalibrationOne.sweeps[0].gibphase = 5.0 CalibrationOne.sweeps[0].ogeemag = 6.0 CalibrationOne.sweeps[0].ogeephase = 7.0 CalibrationOne.sweeps[1].phase = 1.1 CalibrationOne.sweeps[1].tilt = 2.1 CalibrationOne.sweeps[1].curve = 3.1 CalibrationOne.sweeps[1].gibmag = 4.1 CalibrationOne.sweeps[1].gibphase = 5.1 CalibrationOne.sweeps[1].ogeemag = 6.1 CalibrationOne.sweeps[1].ogeephase = 7.1 CalibrationOne.uid = 1234 CalibrationOne.valid = True CalibrationTwo = LighthouseBsCalibration() CalibrationTwo.sweeps[0].phase = 1.5 CalibrationTwo.sweeps[0].tilt = 2.5 CalibrationTwo.sweeps[0].curve = 3.5 CalibrationTwo.sweeps[0].gibmag = 4.5 CalibrationTwo.sweeps[0].gibphase = 5.5 CalibrationTwo.sweeps[0].ogeemag = 6.5 CalibrationTwo.sweeps[0].ogeephase = 7.5 CalibrationTwo.sweeps[1].phase = 1.51 CalibrationTwo.sweeps[1].tilt = 2.51 CalibrationTwo.sweeps[1].curve = 3.51
[4.0, 5.0, 6.0],
[7.0, 8.0, 9.0],
[10.0, 11.0, 12.0],
]
bs1geo.valid = True
bs2geo = LighthouseBsGeometry()
bs2geo.origin = [21.0, 22.0, 23.0]
bs2geo.rotation_matrix = [
[24.0, 25.0, 26.0],
[27.0, 28.0, 29.0],
[30.0, 31.0, 32.0],
]
bs2geo.valid = True
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
bs1calib.sweeps[0].ogeemag = 6.0
bs1calib.sweeps[0].ogeephase = 7.0
bs1calib.sweeps[1].phase = 1.1
bs1calib.sweeps[1].tilt = 2.1
bs1calib.sweeps[1].curve = 3.1
bs1calib.sweeps[1].gibmag = 4.1
bs1calib.sweeps[1].gibphase = 5.1
bs1calib.sweeps[1].ogeemag = 6.1
bs1calib.sweeps[1].ogeephase = 7.1
bs1calib.valid = True