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