def decode(self, src): """ Decodes data from src. Expects header and footer bytes at the start and end of packet respectively. Unescapes any header, footer or escape bytes within the packet warning: Does not support messages over 255. :param src: Data to decode :type src: bytes :return: decoded data :rtype: bytes """ if src[0:1] != PACKET_HEAD_BYTES: raise Exception( "First byte of the message to decode is not a header byte") if src[-1:] != PACKET_FOOT_BYTES: raise Exception( "Last byte of the message to decode is not a footer byte") dest = b"" src_idx = 0 # Loop through all bytes except footer while src_idx < len(src) - 1: char = src[src_idx:src_idx + 1] if char == PACKET_HEAD_BYTES: pass elif char == PACKET_ESC_BYTES: src_idx += 1 # Advance to the byte after the escape character char = src[src_idx:src_idx + 1] c = unpack("<B", char)[0] # convert byts to int dest += int_to_bytes(c ^ PACKET_XOR) else: dest += char src_idx += 1 # Crc check data = dest[0:-2] crc = self.crc16_ccitt(data) try: rx_crc = unpack("<H", dest[-2:])[0] except Exception: as_hex = ''.join(format(x, '02x') for x in src) raise Exception("Failed to extract CRC: {}".format(as_hex)) if crc != rx_crc: as_hex = ''.join(format(x, '02x') for x in src) raise Exception("CRC Fail: {}".format(as_hex)) else: return data
def set_angle_filter(self, minimum, maximum): """ Sets the angle filter to apply to the readings (in degrees). 0 degrees is center, negative angles are to the left of the sensor, positive angles are to the right. :param minimum: The minimum angle (-60 to +60) :type minimum: int :param maximum: The maximum angle (-60 to +60) :type maximum: int """ if not (isinstance(minimum, int) and -55 <= minimum <= 55): raise ValueError( "Angle filter minimum must be an integer between -55 and +55") if not (isinstance(maximum, int) and -55 <= maximum <= 55): raise ValueError( "Angle filter maximum must be an integer between -55 and +55") if maximum < minimum: raise ValueError( "Angle filter maximum must be greater than the minimum") try: self._send(pack("<BBbb", 0x07, 0x02, minimum, maximum)) res = unpack("<BBbb", self._read()) if res[0] == 0x07 and res[1] == 0x01: return True else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get angle filter")
def set_frame_rate(self, frame_rate): """ Sets the frequency with which to capture frames of data. :param frame_rate: The frame rate frames/second) :type frame_rate: int """ if not isinstance(frame_rate, int): raise ValueError("Frame rate must be an integer") if not 0 <= frame_rate <= 20: raise ValueError("Frame rate must be between 0 and 20 fps") try: self._send(pack("<BBB", 0x04, 0x02, frame_rate)) res = unpack("<BBB", self._read()) if res[0] == 0x04 and res[1] == 0x01: if res[2] == frame_rate: return True else: raise Exception("Frame rate did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set frame rate")
def get_height_filter(self): """ Gets the height filter applied to the readings. The units of this filter are those set by :meth:`set_units` :return: The height filter .. code-block:: python {"minimum": minimum, "maximum": maximum} :rtype: dict """ try: self._send(pack("<BB", 0x12, 0x00)) res = unpack("<BBhh", self._read()) if res[0] == 0x12 and res[1] == 0x01: minimum = units.convert_distance_from_si( self.distance_units, res[2] / 1000) maximum = units.convert_distance_from_si( self.distance_units, res[3] / 1000) return {"minimum": minimum, "maximum": maximum} else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get height filter")
def set_auto_start(self, auto_start): """ Sets the auto start settings. Auto start immediately starts the RadarIQ sensor on power on. :param auto_start: The auto start setting (True or False) :type auto_start: bool """ if not isinstance(auto_start, bool): raise ValueError("Auto start must be a boolean") try: self._send(pack("<BBB", 0x17, 0x02, int(auto_start))) res = unpack("<BBB", self._read()) if res[0] == 0x17 and res[1] == 0x01: if res[2] == int(auto_start): return True else: raise Exception("Auto start did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set auto start")
def _process_statistics(self, packet_type, frame): """ Process the statistics packet into a dictionary. :param packet_type: The type of statistics packet to process :type packet_type: int :param frame: Frame of binary data :type frame: bytes """ if packet_type == 0x68: # Core core = {} ( core['active_frame_cpu'], core['inter_frame_cpu'], core['inter_frame_proc_time'], core['transmit_output_time'], core['inter_frame_proc_margin'], core['inter_chirp_proc_margin'], core['packet_transmit_time'], core['temperature_sensor_0'], core['temperature_sensor_1'], core['temperature_power_management'], core['temperature_rx_0'], core['temperature_rx_1'], core['temperature_rx_2'], core['temperature_rx_3'], core['temperature_tx_0'], core['temperature_tx_1'], core['temperature_tx_2'], ) = unpack("<7L10h", frame[2:]) self.statistics['core'] = core elif packet_type == 0x70: # Point Cloud ptcld = {} (ptcld['points_aggregation_time'], ptcld['intensity_sort_time'], ptcld['nearest_neighbours_time'], ptcld['uart_transmission_time'], ptcld['filter_points_removed'], ptcld['num_transmitted_points'], ptcld['input_points_truncated_flag'], ptcld['output_points_truncated_flag']) = unpack( "<6L2B", frame[2:]) self.statistics['point_cloud'] = ptcld self.statistics['rx_buffer_length'] = len(self.connection.rx_buffer) self.statistics['rx_packet_queue'] = self.get_queue_size()
def get_radar_application_versions(self): """ Gets the version of the radar applications. :return: The radar application versions :rtype: dict """ try: data = { 'controller': ['', 0, 0, 0], 'application_1': ['', 0, 0, 0], 'application_2': ['', 0, 0, 0], 'application_3': ['', 0, 0, 0], } self._send(pack("<BBB", 0x14, 0x00, 0x01)) res = unpack("<BBB20sBBH", self._read()) # Slot 1 if res[0] == 0x14 and res[1] == 0x01 and res[2] == 1: data['controller'] = [ res[3].decode().rstrip('\x00'), res[4], res[5], res[6] ] self._send(pack("<BBB", 0x14, 0x00, 0x02)) res = unpack("<BBB20sBBH", self._read()) # Slot 2 if res[0] == 0x14 and res[1] == 0x01 and res[2] == 2: data['application_1'] = [ res[3].decode().rstrip('\x00'), res[4], res[5], res[6] ] self._send(pack("<BBB", 0x14, 0x00, 0x03)) res = unpack("<BBB20sBBH", self._read()) # Slot 3 if res[0] == 0x14 and res[1] == 0x01 and res[2] == 3: data['application_2'] = [ res[3].decode().rstrip('\x00'), res[4], res[5], res[6] ] self._send(pack("<BBB", 0x14, 0x00, 0x04)) res = unpack("<BBB20sBBH", self._read()) # Slot 4 if res[0] == 0x14 and res[1] == 0x01 and res[2] == 4: data['application_3'] = [ res[3].decode().rstrip('\x00'), res[4], res[5], res[6] ] return data except Exception as err: raise Exception("Failed to get radar versions")
def save(self): """ Saves the settings to the sensor. """ try: self._send(pack("<BB", 0x09, 0x02)) res = unpack("<BB", self._read()) if res[0] == 0x09 and res[1] == 0x01: return True else: raise Exception("Invalid response") except Exception: raise Exception("Failed to save settings")
def get_frame_rate(self): """ Gets the frequency with which to capture frames of data (frames/second) from the sensor. :return: The frame rate as it is set in the sensor :rtype: int """ try: self._send(pack("<BB", 0x04, 0x00)) res = unpack("<BBB", self._read()) if res[0] == 0x04 and res[1] == 0x01: return res[2] else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get frame rate")
def get_auto_start(self): """ Gets the autostart setting. :return: True or False :rtype: bool """ try: self._send(pack("<BB", 0x17, 0x00)) res = unpack("<BBB", self._read()) if res[0] == 0x17 and res[1] == 0x01: return bool(res[2]) else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get auto start")
def get_sensitivity(self): """ Get the point sensitivity setting. :return: Sensitivity setting. See `Sensitivity values`_. :rtype: int """ try: self._send(pack("<BB", 0x11, 0x00)) res = unpack("<BBB", self._read()) if res[0] == 0x11 and res[1] == 0x01: return res[2] else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get sensitivity setting")
def get_object_type_mode(self): """ Gets the object type mode from the sensor. :return: The object type mode. See `Object Type Modes`_ :rtype: int """ try: self._send(pack("<BB", 0x16, 0x00)) res = unpack("<BBB", self._read()) if res[0] == 0x16 and res[1] == 0x01: return res[2] else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get object type mode")
def get_mode(self): """ Gets the capture mode from the sensor. :return: The capture mode. See `Capture Modes`_ :rtype: int """ try: self._send(pack("<BB", 0x05, 0x00)) res = unpack("<BBB", self._read()) if res[0] == 0x05 and res[1] == 0x01: return res[2] else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get mode")
def get_point_density(self): """ Gets the point density setting. :return: Point density. See `Point Density Options`_ :rtype: int """ try: self._send(pack("<BB", 0x10, 0x00)) res = unpack("<BBB", self._read()) if res[0] == 0x10 and res[1] == 0x01: return res[2] else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get point density setting")
def get_moving_filter(self): """ Gets the moving objects filter applied to the readings. :return: moving filter. See `Moving filter`_ :rtype: str """ try: self._send(pack("<BB", 0x08, 0x00)) res = unpack("<BBB", self._read()) if res[0] == 0x08 and res[1] == 0x01: return res[2] else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get moving filter")
def get_version(self): """ Gets the version of the hardware and firmware. :return: The sensor version (firmware and hardware) :rtype: dict """ try: self._send(pack("<BB", 0x01, 0x00)) res = unpack("<BBBBHBBH", self._read()) if res[0] == 0x01 and res[1] == 0x01: return {"firmware": list(res[2:5]), "hardware": list(res[5:8])} else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get version")
def get_serial_number(self): """ Gets the serial of the sensor. :return: The sensors serial number :rtype: str """ try: self._send(pack("<BB", 0x02, 0x00)) res = unpack("<BBLL", self._read()) if res[0] == 0x02 and res[1] == 0x01: return "{}-{}".format(res[2], res[3]) else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get serial")
def reset(self, code): """ Resets the sensor. :param code: The reset code. See `Reset codes`_ :type code: int """ if not 0 <= code <= 1: raise ValueError("Invalid reset code") try: self._send(pack("<BBB", 0x03, 0x02, code)) res = unpack("<BB", self._read()) if res[0] == 0x03 and res[1] == 0x01: return True else: raise Exception("Invalid response") except Exception: raise Exception("Failed to reset sensor")
def set_distance_filter(self, minimum, maximum): """ Sets the distance filter applied to the readings. :param minimum: The minimum distance (in units as specified by :meth:`set_units`) :type minimum: number :param maximum: The maximum distance (in units as specified by :meth:`set_units`) :type maximum: number """ minimum = int( units.convert_distance_to_si(self.distance_units, minimum) * 1000) maximum = int( units.convert_distance_to_si(self.distance_units, maximum) * 1000) if not (isinstance(minimum, int) and 0 <= minimum <= 10000): raise ValueError( "Distance filter minimum must be a number between 0 and 10000mm" ) if not (isinstance(maximum, int) and 0 <= maximum <= 10000): raise ValueError( "Distance filter maximum must be a number between 0 and 10000mm" ) if maximum < minimum: raise ValueError( "Distance filter maximum must be greater than the minimum") try: self._send(pack("<BBHH", 0x06, 0x02, minimum, maximum)) res = unpack("<BBHH", self._read()) if res[0] == 0x06 and res[1] == 0x01: if res[2] == minimum and res[3] == maximum: return True else: raise Exception("Distance filter did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set distance filter")
def set_moving_filter(self, moving): """ Sets the moving filter to apply to the readings. :param moving: One of MOVING_BOTH, MOVING_OBJECTS_ONLY :type moving: int """ if not (isinstance(moving, int) and 0 <= moving <= 1): raise ValueError("Moving filter value is invalid") try: self._send(pack("<BBB", 0x08, 0x02, moving)) res = unpack("<BBB", self._read()) if res[0] == 0x08 and res[1] == 0x01: if res[2] == moving: return True else: raise Exception("Moving filter did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set moving filter")
def set_point_density(self, density): """ Sets the point density setting. :param density: The point density to set. See `Point Density Options`_ :type density: int """ if not 0 <= density <= 2: raise ValueError("Invalid point density setting") try: self._send(pack("<BBB", 0x10, 0x02, density)) res = unpack("<BBB", self._read()) if res[0] == 0x10 and res[1] == 0x01: if res[2] == density: return True else: raise Exception("Point density did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set the point density")
def set_mode(self, mode): """ Sets the capture mode for the sensor. :param mode: See `Capture Modes`_ :type mode: int :return: None """ if not 0 <= mode <= 1: raise ValueError("Invalid mode") try: self._send(pack("<BBB", 0x05, 0x02, mode)) res = unpack("<BBB", self._read()) if res[0] == 0x05 and res[1] == 0x01: if res[2] == mode: return True else: raise Exception("Mode did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set mode")
def set_sensitivity(self, sensitivity): """ Sets the the sensitivity setting to apply to the readings. :param sensitivity: The sensitivity setting to set. See `Sensitivity values`_. :type sensitivity: int """ if not (isinstance(sensitivity, int) and 0 <= sensitivity <= 9): raise ValueError("Sensitivity must be an integer between 0 and 9") try: self._send(pack("<BBB", 0x11, 0x02, sensitivity)) res = unpack("<BBB", self._read()) if res[0] == 0x11 and res[1] == 0x01: if res[2] == sensitivity: return True else: raise Exception( "Sensitivity setting did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set the sensitivity setting")
def get_angle_filter(self): """ Gets the angle filter applied to the readings (in degrees). 0 degrees is center, negative angles are to the left of the sensor, positive angles are to the right. :return: The angle filter. .. code-block:: python {"minimum": minimum, "maximum": maximum} :rtype: dict """ try: self._send(pack("<BB", 0x07, 0x00)) res = unpack("<BBbb", self._read()) if res[0] == 0x07 and res[1] == 0x01: return {"minimum": res[2], "maximum": res[3]} else: raise Exception("Invalid response") except Exception: raise Exception("Failed to get angle filter")
def _process_message(self, msg): """ Process a message onto the python log. """ try: message_length = len(msg) - 4 packing = "<BBBB{}s".format(message_length) res = unpack(packing, msg) if res[0] == 0x00 and res[1] == 0x01: message_type = res[2] message_code = res[3] message = res[4].decode().rstrip('\x00') if message_type in [0, 1]: log.debug('{} {}'.format(message_code, message)) elif message_type in [2, 5]: log.info('{} {}'.format(message_code, message)) elif message_type in [3]: log.warning('{} {}'.format(message_code, message)) elif message_type in [4]: log.error('{} {}'.format(message_code, message)) except Exception: raise Exception( "Failed to process message from the RadarIQ sensor")
def set_object_type_mode(self, mode): """ Sets the object type mode for the sensor. :param mode: See `Object Type Modes`_ :type mode: int :return: None """ if not 0 <= mode <= 1: raise ValueError("Invalid object type mode") try: self._send(pack("<BBB", 0x16, 0x02, mode)) res = unpack("<BBB", self._read()) if res[0] == 0x16 and res[1] == 0x01: if res[2] == mode: return True else: raise Exception( "Object Tracking mode did not set correctly") else: raise Exception("Invalid response") except Exception: raise Exception("Failed to set object tracking mode")
def _get_data_thread(self): mirror = 1 if self.mirror is False else -1 # multiplier for mirroring x-data rx_frame = [] while self.is_capturing is True: try: subframe = self.connection.read_from_queue() if subframe is not None: (command, variant) = unpack("<BB", subframe[:2]) if command in [ 0x68, 0x70 ] and variant == 0x01: # is statistics packet self._process_statistics(command, subframe) elif command == 0x66 and variant == 0x01: # is a point cloud packet (subframe_type, count) = unpack("<BB", subframe[2:4]) unpacking = "<" + "hhhBh" * count unpacked = unpack(unpacking, subframe[4:]) idx = 0 for cnt in range(count): # SI units are needed so convert mm to m rx_frame.append([ mirror * units.convert_distance_from_si( self.distance_units, unpacked[idx] / 1000), units.convert_distance_from_si( self.distance_units, unpacked[idx + 1] / 1000), units.convert_distance_from_si( self.distance_units, unpacked[idx + 2] / 1000), unpacked[idx + 3], units.convert_speed_from_si( self.speed_units, unpacked[idx + 4] / 1000) ]) idx += 5 if subframe_type == 0x02: # End of frame self.capture_count += 1 if self.output_format == OUTPUT_LIST: data = rx_frame elif self.output_format == OUTPUT_NUMPY: data = self._convert_pointcloud_to_numpy( rx_frame) else: data = None try: self.data_queue.put_nowait(data) except queue.Full: pass rx_frame = [ ] # clear the buffer now the frame has been sent if 0 < self.capture_max == self.capture_count: break elif command == 0x67 and variant == 0x01: # is an object packet (subframe_type, count) = unpack("<BB", subframe[2:4]) unpacking = "<" + "b9h" * count unpacked = unpack(unpacking, subframe[4:]) idx = 0 for cnt in range(count): # SI units are needed so convert mm to m rx_frame.append({ 'tracking_id': unpacked[idx], 'x_pos': mirror * units.convert_distance_from_si( self.distance_units, unpacked[idx + 1] / 1000), 'y_pos': units.convert_distance_from_si( self.distance_units, unpacked[idx + 2] / 1000), 'z_pos': units.convert_distance_from_si( self.distance_units, unpacked[idx + 3] / 1000), 'x_vel': mirror * units.convert_speed_from_si( self.speed_units, unpacked[idx + 4] / 1000), 'y_vel': units.convert_speed_from_si( self.speed_units, unpacked[idx + 5] / 1000), 'z_vel': units.convert_speed_from_si( self.speed_units, unpacked[idx + 6] / 1000), 'x_acc': mirror * units.convert_acceleration_from_si( self.acceleration_units, unpacked[idx + 7] / 1000), 'y_acc': units.convert_acceleration_from_si( self.acceleration_units, unpacked[idx + 8] / 1000), 'z_acc': units.convert_acceleration_from_si( self.acceleration_units, unpacked[idx + 9] / 1000) }) idx += 10 if subframe_type == 0x02: # End of frame self.capture_count += 1 if self.output_format == OUTPUT_LIST: data = rx_frame elif self.output_format == OUTPUT_NUMPY: data = self._convert_object_tracking_to_numpy( rx_frame) else: data = None try: self.data_queue.put_nowait(data) except queue.Full: pass rx_frame = [ ] # clear the buffer now the frame has been sent if 0 < self.capture_max == self.capture_count: break else: pass else: time.sleep(0.01) except ValueError: pass self.stop()