Rework the lighthouse geometry estimator

cflib/crazyflie/localization.py

@@ -66,6 +66,7 @@ class Localization():
     EXT_POSE_PACKED = 9
     LH_ANGLE_STREAM = 10
     LH_PERSIST_DATA = 11
+    LH_MATCHED_ANGLE_STREAM = 12
 
     def __init__(self, crazyflie=None):
         """
@@ -105,6 +106,8 @@ def _incoming(self, packet):
             decoded_data = bool(data[0])
         elif pk_type == self.LH_ANGLE_STREAM:
             decoded_data = self._decode_lh_angle(data)
+        elif pk_type == self.LH_MATCHED_ANGLE_STREAM:
+            decoded_data = self._decode_matched_lh_angle(data)
 
         pk = LocalizationPacket(pk_type, data, decoded_data)
         self.receivedLocationPacket.call(pk)
@@ -128,6 +131,27 @@ def _decode_lh_angle(self, data):
 
         return decoded_data
 
+    def _decode_matched_lh_angle(self, data):
+        decoded_data = {}
+
+        raw_data = struct.unpack('<BfhhhfhhhB', data)
+
+        decoded_data['basestation'] = raw_data[0]
+        decoded_data['x'] = [0, 0, 0, 0]
+        decoded_data['x'][0] = raw_data[1]
+        decoded_data['x'][1] = raw_data[1] - fp16_to_float(raw_data[2])
+        decoded_data['x'][2] = raw_data[1] - fp16_to_float(raw_data[3])
+        decoded_data['x'][3] = raw_data[1] - fp16_to_float(raw_data[4])
+        decoded_data['y'] = [0, 0, 0, 0]
+        decoded_data['y'][0] = raw_data[5]
+        decoded_data['y'][1] = raw_data[5] - fp16_to_float(raw_data[6])
+        decoded_data['y'][2] = raw_data[5] - fp16_to_float(raw_data[7])
+        decoded_data['y'][3] = raw_data[5] - fp16_to_float(raw_data[8])
+        decoded_data['group_id'] = raw_data[9] >> 4
+        decoded_data['bs_count'] = raw_data[9] & 0x0F
+
+        return decoded_data
+
     def send_extpos(self, pos):
         """
         Send the current Crazyflie X, Y, Z position. This is going to be

cflib/crazyflie/platformservice.py

@@ -42,6 +42,7 @@
 PLATFORM_SET_CONT_WAVE = 0
 PLATFORM_REQUEST_ARMING = 1
 PLATFORM_REQUEST_CRASH_RECOVERY = 2
+PLATFORM_REQUEST_USER_NOTIFICATION = 3
 
 VERSION_GET_PROTOCOL = 0
 VERSION_GET_FIRMWARE = 1
@@ -110,6 +111,17 @@ def send_crash_recovery_request(self):
         pk.data = (PLATFORM_REQUEST_CRASH_RECOVERY, )
         self._cf.send_packet(pk)
 
+    def send_user_notification(self, success: bool = True):
+        """
+        Send a user notification to the Crazyflie. This is used to notify a user of some sort of event by using the
+        means available on the Crazyflie.
+        """
+        pk = CRTPPacket()
+        pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
+        notification_type = 1 if success else 0
+        pk.data = (PLATFORM_REQUEST_USER_NOTIFICATION, notification_type)
+        self._cf.send_packet(pk)
+
     def get_protocol_version(self):
         """
         Return version of the CRTP protocol

cflib/localization/__init__.py

@@ -21,17 +21,25 @@
 # along with this program. If not, see <https://www.gnu.org/licenses/>.
 from .lighthouse_bs_geo import LighthouseBsGeoEstimator
 from .lighthouse_bs_vector import LighthouseBsVector
+from .lighthouse_cf_pose_sample import LhCfPoseSampleType
 from .lighthouse_config_manager import LighthouseConfigFileManager
 from .lighthouse_config_manager import LighthouseConfigWriter
+from .lighthouse_geometry_solution import LighthouseGeometrySolution
+from .lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
+from .lighthouse_utils import LighthouseCrossingBeam
 from .param_io import ParamFileManager
 
 __all__ = [
     'LighthouseBsGeoEstimator',
     'LighthouseBsVector',
     'LighthouseSweepAngleAverageReader',
     'LighthouseSweepAngleReader',
+    'LighthouseMatchedSweepAngleReader',
     'LighthouseConfigFileManager',
     'LighthouseConfigWriter',
-    'ParamFileManager']
+    'ParamFileManager',
+    'LighthouseCrossingBeam',
+    'LighthouseGeometrySolution',
+    'LhCfPoseSampleType']

cflib/localization/ippe_cf.py

@@ -65,7 +65,7 @@ def solve(U_cf: npt.ArrayLike, Q_cf: npt.ArrayLike) -> list[Solution]:
 
                      First param: Y (positive to the left)
                      Second param: Z (positive up)
-        :return: A list that contains 2 sets of pose solution from IPPE including rotation matrix
+        :return: A list that contains 2 sets of pose solutions from IPPE including rotation matrix
                  translation matrix, and reprojection error. The first solution in the list has
                  the smallest reprojection error.
         """

cflib/localization/lighthouse_bs_vector.py

@@ -25,6 +25,7 @@
 
 import numpy as np
 import numpy.typing as npt
+import yaml
 
 
 class LighthouseBsVector:
@@ -137,14 +138,40 @@ def projection(self) -> npt.NDArray[np.float32]:
     def _q(self):
         return math.tan(self._lh_v1_vert_angle) / math.sqrt(1 + math.tan(self._lh_v1_horiz_angle) ** 2)
 
+    def __eq__(self, other):
+        if not isinstance(other, LighthouseBsVector):
+            return NotImplemented
+
+        return (self._lh_v1_horiz_angle == other._lh_v1_horiz_angle and
+                self._lh_v1_vert_angle == other._lh_v1_vert_angle)
+
+    @staticmethod
+    def yaml_representer(dumper, data: 'LighthouseBsVector'):
+        return dumper.represent_mapping('!LighthouseBsVector', {
+            'lh_v1_angles': [data.lh_v1_horiz_angle, data.lh_v1_vert_angle],
+        })
+
+    @staticmethod
+    def yaml_constructor(loader, node):
+        values = loader.construct_mapping(node, deep=True)
+        lh_v1_angles = values.get('lh_v1_angles', [0.0, 0.0])
+        if len(lh_v1_angles) != 2:
+            raise ValueError('lh_v1_angles must be a list of two angles')
+        lh_v1_horiz_angle, lh_v1_vert_angle = lh_v1_angles
+        return LighthouseBsVector(lh_v1_horiz_angle, lh_v1_vert_angle)
+
+
+yaml.add_representer(LighthouseBsVector, LighthouseBsVector.yaml_representer)
+yaml.add_constructor('!LighthouseBsVector', LighthouseBsVector.yaml_constructor)
+
 
 class LighthouseBsVectors(list):
     """A list of 4 LighthouseBsVector, one for each sensor.
        LighthouseBsVectors is essentially the same as list[LighthouseBsVector]"""
 
     def projection_pair_list(self) -> npt.NDArray:
         """
-        Genereate a list of projection pairs for all vectors
+        Generate a list of projection pairs for all vectors
         """
         result = np.empty((len(self), 2), dtype=float)
         for i, vector in enumerate(self):
@@ -154,11 +181,26 @@ def projection_pair_list(self) -> npt.NDArray:
 
     def angle_list(self) -> npt.NDArray:
         """
-        Genereate a list of angles for all vectors, the order is horizontal, vertical, horizontal, vertical...
+        Generate a list of angles for all vectors, the order is horizontal, vertical, horizontal, vertical...
         """
         result = np.empty((len(self) * 2), dtype=float)
         for i, vector in enumerate(self):
             result[i * 2] = vector.lh_v1_horiz_angle
             result[i * 2 + 1] = vector.lh_v1_vert_angle
 
         return result
+
+    @staticmethod
+    def yaml_representer(dumper, data: 'LighthouseBsVectors'):
+        # Instead of using a sequence of LighthouseBsVector, we represent it as a sequence of lists to make it more
+        # compact
+        return dumper.represent_sequence('!LighthouseBsVectors', [list(vector.lh_v1_angle_pair) for vector in data])
+
+    @staticmethod
+    def yaml_constructor(loader, node):
+        values = loader.construct_sequence(node, deep=True)
+        return LighthouseBsVectors([LighthouseBsVector(pair[0], pair[1]) for pair in values])
+
+
+yaml.add_representer(LighthouseBsVectors, LighthouseBsVectors.yaml_representer)
+yaml.add_constructor('!LighthouseBsVectors', LighthouseBsVectors.yaml_constructor)