This thread will update params through a queue to make sure that we get back values
| 280 | |
| 281 | |
| 282 | class _ParamUpdater(Thread): |
| 283 | """This thread will update params through a queue to make sure that we |
| 284 | get back values""" |
| 285 | |
| 286 | def __init__(self, cf, useV2, updated_callback): |
| 287 | """Initialize the thread""" |
| 288 | Thread.__init__(self) |
| 289 | self.setDaemon(True) |
| 290 | self.wait_lock = Lock() |
| 291 | self.cf = cf |
| 292 | self._useV2 = useV2 |
| 293 | self.updated_callback = updated_callback |
| 294 | self.request_queue = Queue() |
| 295 | self.cf.add_port_callback(CRTPPort.PARAM, self._new_packet_cb) |
| 296 | self._should_close = False |
| 297 | self._req_param = -1 |
| 298 | |
| 299 | def close(self): |
| 300 | # First empty the queue from all packets |
| 301 | while not self.request_queue.empty(): |
| 302 | self.request_queue.get() |
| 303 | # Then force an unlock of the mutex if we are waiting for a packet |
| 304 | # we didn't get back due to a disconnect for example. |
| 305 | try: |
| 306 | self.wait_lock.release() |
| 307 | except Exception: |
| 308 | pass |
| 309 | |
| 310 | def request_param_setvalue(self, pk): |
| 311 | """Place a param set value request on the queue. When this is sent to |
| 312 | the Crazyflie it will answer with the update param value. """ |
| 313 | self.request_queue.put(pk) |
| 314 | |
| 315 | def _new_packet_cb(self, pk): |
| 316 | """Callback for newly arrived packets""" |
| 317 | if pk.channel == READ_CHANNEL or pk.channel == WRITE_CHANNEL: |
| 318 | if self._useV2: |
| 319 | var_id = struct.unpack('<H', pk.data[:2])[0] |
| 320 | if pk.channel == READ_CHANNEL: |
| 321 | pk.data = pk.data[:2] + pk.data[3:] |
| 322 | else: |
| 323 | var_id = pk.data[0] |
| 324 | if (pk.channel != TOC_CHANNEL and self._req_param == var_id and |
| 325 | pk is not None): |
| 326 | self.updated_callback(pk) |
| 327 | self._req_param = -1 |
| 328 | try: |
| 329 | self.wait_lock.release() |
| 330 | except Exception: |
| 331 | pass |
| 332 | |
| 333 | def request_param_update(self, var_id): |
| 334 | """Place a param update request on the queue""" |
| 335 | self._useV2 = self.cf.platform.get_protocol_version() >= 4 |
| 336 | pk = CRTPPacket() |
| 337 | pk.set_header(CRTPPort.PARAM, READ_CHANNEL) |
| 338 | if self._useV2: |
| 339 | pk.data = struct.pack('<H', var_id) |