Пример #1
0
    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
Пример #2
0
    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")
Пример #3
0
    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")
Пример #4
0
    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")
Пример #5
0
    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")
Пример #6
0
    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()
Пример #7
0
    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")
Пример #8
0
 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")
Пример #9
0
    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")
Пример #10
0
    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")
Пример #11
0
    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")
Пример #12
0
    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")
Пример #13
0
    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")
Пример #14
0
    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")
Пример #15
0
    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")
Пример #16
0
    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")
Пример #17
0
    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")
Пример #18
0
    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")
Пример #19
0
    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")
Пример #20
0
    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")
Пример #21
0
    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")
Пример #22
0
    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")
Пример #23
0
    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")
Пример #24
0
    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")
Пример #25
0
 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")
Пример #26
0
    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")
Пример #27
0
    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()