Handle localization-related data communication with the Crazyflie
| 48 | |
| 49 | |
| 50 | class Localization(): |
| 51 | """ |
| 52 | Handle localization-related data communication with the Crazyflie |
| 53 | """ |
| 54 | |
| 55 | # Implemented channels |
| 56 | POSITION_CH = 0 |
| 57 | GENERIC_CH = 1 |
| 58 | |
| 59 | # Location message types for generig channel |
| 60 | RANGE_STREAM_REPORT = 0 |
| 61 | RANGE_STREAM_REPORT_FP16 = 1 |
| 62 | LPS_SHORT_LPP_PACKET = 2 |
| 63 | EMERGENCY_STOP = 3 |
| 64 | EMERGENCY_STOP_WATCHDOG = 4 |
| 65 | COMM_GNSS_NMEA = 6 |
| 66 | COMM_GNSS_PROPRIETARY = 7 |
| 67 | EXT_POSE = 8 |
| 68 | EXT_POSE_PACKED = 9 |
| 69 | LH_ANGLE_STREAM = 10 |
| 70 | LH_PERSIST_DATA = 11 |
| 71 | |
| 72 | def __init__(self, crazyflie=None): |
| 73 | """ |
| 74 | Initialize the Extpos object. |
| 75 | """ |
| 76 | self._cf = crazyflie |
| 77 | |
| 78 | self.receivedLocationPacket = Caller() |
| 79 | self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming) |
| 80 | |
| 81 | def _incoming(self, packet): |
| 82 | """ |
| 83 | Callback for data received from the copter. |
| 84 | """ |
| 85 | if len(packet.data) < 1: |
| 86 | logger.warning('Localization packet received with incorrect' + |
| 87 | 'length (length is {})'.format(len(packet.data))) |
| 88 | return |
| 89 | |
| 90 | pk_type = struct.unpack('<B', packet.data[:1])[0] |
| 91 | data = packet.data[1:] |
| 92 | |
| 93 | # Decoding the known packet types |
| 94 | # TODO: more generic decoding scheme? |
| 95 | decoded_data = None |
| 96 | if pk_type == self.RANGE_STREAM_REPORT: |
| 97 | if len(data) % 5 != 0: |
| 98 | logger.error('Wrong range stream report data lenght') |
| 99 | return |
| 100 | decoded_data = {} |
| 101 | raw_data = data |
| 102 | for i in range(int(len(data) / 5)): |
| 103 | anchor_id, distance = struct.unpack('<Bf', raw_data[:5]) |
| 104 | decoded_data[anchor_id] = distance |
| 105 | raw_data = raw_data[5:] |
| 106 | elif pk_type == self.LH_PERSIST_DATA: |
| 107 | decoded_data = bool(data[0]) |