def test_conversion_to_from_cartesian(self):
# Fixture
horiz = 0.123
vert = -0.987
v1 = LighthouseBsVector(horiz, vert)
# Test
actual = LighthouseBsVector.from_cart(v1.cart)
# Assert
self.assertAlmostEqual(horiz, actual.lh_v1_horiz_angle)
self.assertAlmostEqual(vert, actual.lh_v1_vert_angle)
def test_that_initial_yaw_guess_is_correct_ba_is_front(self):
# Fixture 1, 3, 2, 0
bs_vectors = [
LighthouseBsVector(3, 0),
LighthouseBsVector(0, 0),
LighthouseBsVector(2, 0),
LighthouseBsVector(1, 0),
]
# Test
actual = self.sut._find_initial_yaw_guess(bs_vectors)
# Assert
self.assertEqual(math.radians(180), actual)
def test_that_initial_yaw_guess_is_correct_bs_left_behind(self):
# Fixture
bs_vectors = [
LighthouseBsVector(1.0, 0),
LighthouseBsVector(-0.5, 0),
LighthouseBsVector(0.5, 0),
LighthouseBsVector(-1.0, 0),
]
# Test
actual = self.sut._find_initial_yaw_guess(bs_vectors)
# Assert
self.assertEqual(math.radians(155), actual)
def test_that_initial_yaw_guess_is_correct_ba_is_behind(self):
# Fixture
bs_vectors = [
LighthouseBsVector(1, 0),
LighthouseBsVector(2, 0),
LighthouseBsVector(0, 0),
LighthouseBsVector(3, 0),
]
# Test
actual = self.sut._find_initial_yaw_guess(bs_vectors)
# Assert
self.assertEqual(0.0, actual)
def setUp(self):
self.vec0 = LighthouseBsVector(0.0, 0.0)
self.vec1 = LighthouseBsVector(0.1, 0.1)
self.vec2 = LighthouseBsVector(0.2, 0.2)
self.vec3 = LighthouseBsVector(0.3, 0.3)
self.samples = [
LhMeasurement(timestamp=1.000, base_station_id=0,
angles=self.vec0),
LhMeasurement(timestamp=1.015, base_station_id=1,
angles=self.vec1),
LhMeasurement(timestamp=1.020, base_station_id=0,
angles=self.vec2),
LhMeasurement(timestamp=1.035, base_station_id=1,
angles=self.vec3),
]
def test_conversion_to_lh2_angles_are_equal_with_vert_zero(self):
# Fixture
horiz = 1.0
vert = 0.0
# Test
actual = LighthouseBsVector(horiz, vert)
# Assert
self.assertEqual(actual.lh_v2_angle_1, actual.lh_v2_angle_2)
def _average_sample_list(self, sample_list):
sum_horiz = 0.0
sum_vert = 0.0
for bs_vector in sample_list:
sum_horiz += bs_vector.lh_v1_horiz_angle
sum_vert += bs_vector.lh_v1_vert_angle
count = len(sample_list)
return LighthouseBsVector(sum_horiz / count, sum_vert / count)
def test_conversion_to_lh2_angles_are_zero_straight_forward(self):
# Fixture
horiz = 0
vert = 0
# Test
actual = LighthouseBsVector(horiz, vert)
# Assert
self.assertEqual(0.0, actual.lh_v2_angle_1)
self.assertEqual(0.0, actual.lh_v2_angle_2)
def test_init_from_lh1_angles(self):
# Fixture
horiz = 0.123
vert = -1.23
# Test
actual = LighthouseBsVector(horiz, vert)
# Assert
self.assertEqual(horiz, actual.lh_v1_horiz_angle)
self.assertEqual(vert, actual.lh_v1_vert_angle)
def test_cartesian_is_normalized(self):
# Fixture
horiz = 0.123
vert = 0.456
vector = LighthouseBsVector(horiz, vert)
# Test
actual = np.linalg.norm(vector.cart)
# Assert
self.assertAlmostEqual(1.0, actual)
def test_conversion_to_cartesian_straight_forward(self):
# Fixture
horiz = 0.0
vert = 0.0
vector = LighthouseBsVector(horiz, vert)
# Test
actual = vector.cart
# Assert
self.assertAlmostEqual(1.0, actual[0])
self.assertAlmostEqual(0.0, actual[1])
self.assertAlmostEqual(0.0, actual[2])
def _packet_received_cb(self, packet):
if packet.type != self._cf.loc.LH_ANGLE_STREAM:
return
if self._cb:
base_station_id = packet.data["basestation"]
horiz_angles = packet.data['x']
vert_angles = packet.data['y']
result = []
for i in range(self.NR_OF_SENSORS):
result.append(
LighthouseBsVector(horiz_angles[i], vert_angles[i]))
self._cb(base_station_id, result)
def read_sensors(scf: SyncCrazyflie):
# Mutext released by the callback function when enough measurements have
# been accumulated
reading_finished = Semaphore(0)
measurements = {}
# Callback accumulating measurements
def _loc_callback(pk):
if pk.type != scf.cf.loc.LH_ANGLE_STREAM:
return
print(pk.data["basestation"], end='')
sys.stdout.flush()
if not pk.data["basestation"] in measurements:
measurements[pk.data["basestation"]] = {
"count": 1,
"sensors": [
pk.data["x"],
pk.data["y"]
]
}
else:
for axis, axis_name in enumerate(['x', 'y']):
for sensor in range(4):
measurements[pk.data["basestation"]]["sensors"][axis][sensor] += \
pk.data[axis_name][sensor]
measurements[pk.data["basestation"]]["count"] += 1
if measurements[pk.data["basestation"]]["count"] > N_SAMPLE:
print()
reading_finished.release()
# Setup callback function and start streaming
scf.cf.loc.receivedLocationPacket.add_callback(_loc_callback)
scf.cf.param.set_value("locSrv.enLhAngleStream", 1)
# Wait for enough sample to be acquired
reading_finished.acquire()
# Stop streaming and disable callback function
scf.cf.param.set_value("locSrv.enLhAngleStream", 0)
scf.cf.loc.receivedLocationPacket.remove_callback(_loc_callback)
# Average the sum of measurements and generate t
for basestation in measurements:
for axis, axis_name in enumerate(['x', 'y']):
for sensor in range(4):
measurements[basestation]["sensors"][axis][sensor] /= \
measurements[basestation]["count"]
# Reorganize data to be used by the geometry functions
sensor_vectors = {}
for bs in measurements:
sensor_vectors[bs] = ([None, None, None, None])
for sensor in range(4):
horiz_angle = measurements[bs]["sensors"][0][sensor]
vertical_angle = measurements[bs]["sensors"][1][sensor]
bs_vector = LighthouseBsVector(horiz_angle, vertical_angle)
sensor_vectors[bs][sensor] = bs_vector
return sensor_vectors