| 26 | |
| 27 | |
| 28 | class Multiranger: |
| 29 | FRONT = 'range.front' |
| 30 | BACK = 'range.back' |
| 31 | LEFT = 'range.left' |
| 32 | RIGHT = 'range.right' |
| 33 | UP = 'range.up' |
| 34 | DOWN = 'range.zrange' |
| 35 | |
| 36 | def __init__(self, crazyflie, rate_ms=100, zranger=False): |
| 37 | if isinstance(crazyflie, SyncCrazyflie): |
| 38 | self._cf = crazyflie.cf |
| 39 | else: |
| 40 | self._cf = crazyflie |
| 41 | self._log_config = self._create_log_config(rate_ms) |
| 42 | |
| 43 | self._up_distance = None |
| 44 | self._front_distance = None |
| 45 | self._back_distance = None |
| 46 | self._left_distance = None |
| 47 | self._right_distance = None |
| 48 | self._down_distance = None |
| 49 | |
| 50 | def _create_log_config(self, rate_ms): |
| 51 | log_config = LogConfig('multiranger', rate_ms) |
| 52 | log_config.add_variable(self.FRONT) |
| 53 | log_config.add_variable(self.BACK) |
| 54 | log_config.add_variable(self.LEFT) |
| 55 | log_config.add_variable(self.RIGHT) |
| 56 | log_config.add_variable(self.UP) |
| 57 | log_config.add_variable(self.DOWN) |
| 58 | |
| 59 | log_config.data_received_cb.add_callback(self._data_received) |
| 60 | |
| 61 | return log_config |
| 62 | |
| 63 | def start(self): |
| 64 | self._cf.log.add_config(self._log_config) |
| 65 | self._log_config.start() |
| 66 | |
| 67 | def _convert_log_to_distance(self, data): |
| 68 | if data >= 8000: |
| 69 | return None |
| 70 | else: |
| 71 | return data / 1000.0 |
| 72 | |
| 73 | def _data_received(self, timestamp, data, logconf): |
| 74 | self._up_distance = self._convert_log_to_distance(data[self.UP]) |
| 75 | self._front_distance = self._convert_log_to_distance(data[self.FRONT]) |
| 76 | self._back_distance = self._convert_log_to_distance(data[self.BACK]) |
| 77 | self._left_distance = self._convert_log_to_distance(data[self.LEFT]) |
| 78 | self._right_distance = self._convert_log_to_distance(data[self.RIGHT]) |
| 79 | if self.DOWN in data: |
| 80 | self._down_distance = self._convert_log_to_distance(data[self.DOWN] |
| 81 | ) |
| 82 | |
| 83 | def stop(self): |
| 84 | self._log_config.delete() |
| 85 |
no outgoing calls