diff --git a/cflib/crazyflie/param.py b/cflib/crazyflie/param.py index 605b543d8..f4bc7d38c 100644 --- a/cflib/crazyflie/param.py +++ b/cflib/crazyflie/param.py @@ -69,6 +69,8 @@ MISC_PERSISTENT_GET_STATE = 4 MISC_PERSISTENT_CLEAR = 5 MISC_GET_DEFAULT_VALUE = 6 +MISC_GET_EXTENDED_TYPE_V2 = 7 +MISC_GET_DEFAULT_VALUE_V2 = 8 PersistentParamState = namedtuple('PersistentParamState', 'is_stored default_value stored_value') @@ -395,15 +397,29 @@ def get_default_value(self, complete_name, callback): @param callback The callback should take `complete_name` and default value as argument """ element = self.toc.get_element_by_complete_name(complete_name) + # Protocol 11+ uses V2 command with unambiguous status byte + use_v2 = self.cf.platform.get_protocol_version() >= 11 + cmd = MISC_GET_DEFAULT_VALUE_V2 if use_v2 else MISC_GET_DEFAULT_VALUE def new_packet_cb(pk): - if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_GET_DEFAULT_VALUE: - if pk.data[3] == errno.ENOENT: - callback(complete_name, None) - self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) - return + if pk.channel == MISC_CHANNEL and pk.data[0] == cmd: + if use_v2: + # V2: [CMD, ID_L, ID_H, STATUS, VALUE...] on success + # [CMD, ID_L, ID_H, ERROR_CODE] on error + if pk.data[3] != 0: + callback(complete_name, None) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + return + default_value, = struct.unpack(element.pytype, pk.data[4:]) + else: + # V1: [CMD, ID_L, ID_H, VALUE...] + # Ambiguous: ENOENT (0x02) indistinguishable from u8 value 2 + if pk.data[3] == errno.ENOENT: + callback(complete_name, None) + self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) + return + default_value, = struct.unpack(element.pytype, pk.data[3:]) - default_value, = struct.unpack(element.pytype, pk.data[3:]) callback(complete_name, default_value) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) @@ -411,7 +427,7 @@ def new_packet_cb(pk): pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) - pk.data = struct.pack('= 11 + self._cmd = MISC_GET_EXTENDED_TYPE_V2 if self._use_v2 else MISC_GET_EXTENDED_TYPE def _new_packet_cb(self, pk): """Callback for newly arrived packets""" - if pk.channel == MISC_CHANNEL: + if pk.channel == MISC_CHANNEL and pk.data[0] == self._cmd: var_id = struct.unpack('