Skip to content

roverc.imu_api

Minimal implementation of the Xsens API.

roverc.imu_api.IMUData

Bases: NamedTuple

IMU orientation, angular velocity, acceleration data.

Attributes:

Name Type Description
rot Float32[ndarray, 3]

rotation in Euler angles (roll, pitch, yaw) in radians.
acc Float32[ndarray, 3]

acceleration in m/s^2.
avel Float32[ndarray, 3]

angular velocity in rad/s.
Source code in collect/roverc/imu_api.py 
18
19
20
21
22
23
24
25
26
27
28
29
class IMUData(NamedTuple):
    """IMU orientation, angular velocity, acceleration data.

    Attributes:
        rot: rotation in Euler angles (roll, pitch, yaw) in radians.
        acc: acceleration in m/s^2.
        avel: angular velocity in rad/s.
    """

    rot: Float32[np.ndarray, "3"]
    acc: Float32[np.ndarray, "3"]
    avel: Float32[np.ndarray, "3"]

roverc.imu_api.InvalidChecksum

Bases: Exception

Non-fatal IMU error message.

Source code in collect/roverc/imu_api.py 
12
13
14
15
class InvalidChecksum(Exception):
    """Non-fatal IMU error message."""

    pass

roverc.imu_api.XsensIMU

Xsens MTi-3 IMU API.

The IMU should be set up using MT Manager with the following:

  • Orientation: Euler Angles; Floating Point 32-bit; 100Hz
  • Inertial data: Rate of Turn, Acceleration; Floating Point 32-bit; 100Hz

Parameters:

Name Type Description Default
port str

serial port to use. Must have read/write permissions, e.g. sudo chmod 666 /dev/ttyUSB0.

 '/dev/ttyUSB0'
baudrate int

serial baudrate.

 115200
name str

human readable name.

 'Xsens.MTi-3'
Source code in collect/roverc/imu_api.py 
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
class XsensIMU:
    """Xsens MTi-3 IMU API.

    The IMU should be set up using MT Manager with the following:

    - Orientation: Euler Angles; Floating Point 32-bit; 100Hz
    - Inertial data: Rate of Turn, Acceleration; Floating Point 32-bit; 100Hz

    Args:
        port: serial port to use. Must have read/write permissions, e.g.
            `sudo chmod 666 /dev/ttyUSB0`.
        baudrate: serial baudrate.
        name: human readable name.
    """

    PRE = 0xfa
    BID = 0xff
    MTData2 = 0x36

    def __init__(
        self, port: str = "/dev/ttyUSB0", baudrate: int = 115200,
        name: str = "Xsens.MTi-3"
    ) -> None:
        self.log = logging.getLogger(name=name)
        self.port = serial.Serial(port, baudrate, timeout=1)
        self.port.set_low_latency_mode(True)
        self.port.reset_input_buffer()

    def read(self) -> Optional[IMUData]:
        """Read a single data packet.

        Only handles MTData2 messages; [see the Xsens MT Low Level
        Documentation.](
        https://www.xsens.com/hubfs/Downloads/Manuals/MT_Low-Level_Documentation.pdf)

        Returns:
            IMUData on success; otherwise, returns None.
        """
        # Fast forward until we get to the preamble start bytes `fa ff``
        prev, current = None, None
        while (prev, current) != (self.PRE, self.BID):
            prev = current
            current = struct.unpack('>B', self.port.read(1))[0]

        mid, packet_len = struct.unpack('>BB', self.port.read(2))
        payload = self.port.read(packet_len)
        checksum = self.port.read(1)

        summation = self.BID + mid + packet_len + sum(payload + checksum)
        if (summation & 0xff) != 0:
            raise InvalidChecksum(summation & 0xff)

        # We only handle MTData2 messages.
        if mid != self.MTData2:
            return None

        rot, acc, avel = None, None, None
        while len(payload) > 0:
            data_id, data_len = struct.unpack('>HB', payload[:3])
            payload = payload[3:]

            data = payload[:data_len]
            payload = payload[data_len:]

            if data_id & 0xfff0 == 0x2030:
                rot = np.frombuffer(data, dtype='>f4').astype(np.dtype('f4'))
            elif data_id & 0xfff0 == 0x4020:
                acc = np.frombuffer(data, dtype='>f4').astype(np.dtype('f4'))
            elif data_id & 0xfff0 == 0x8020:
                avel = np.frombuffer(data, dtype='>f4').astype(np.dtype('f4'))

        return IMUData(rot=rot, acc=acc, avel=avel)  # type: ignore

read 

read() -> Optional[IMUData]

Read a single data packet.

Only handles MTData2 messages; see the Xsens MT Low Level Documentation.

Returns:

Type Description
Optional[IMUData]

IMUData on success; otherwise, returns None.
Source code in collect/roverc/imu_api.py 
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
def read(self) -> Optional[IMUData]:
    """Read a single data packet.

    Only handles MTData2 messages; [see the Xsens MT Low Level
    Documentation.](
    https://www.xsens.com/hubfs/Downloads/Manuals/MT_Low-Level_Documentation.pdf)

    Returns:
        IMUData on success; otherwise, returns None.
    """
    # Fast forward until we get to the preamble start bytes `fa ff``
    prev, current = None, None
    while (prev, current) != (self.PRE, self.BID):
        prev = current
        current = struct.unpack('>B', self.port.read(1))[0]

    mid, packet_len = struct.unpack('>BB', self.port.read(2))
    payload = self.port.read(packet_len)
    checksum = self.port.read(1)

    summation = self.BID + mid + packet_len + sum(payload + checksum)
    if (summation & 0xff) != 0:
        raise InvalidChecksum(summation & 0xff)

    # We only handle MTData2 messages.
    if mid != self.MTData2:
        return None

    rot, acc, avel = None, None, None
    while len(payload) > 0:
        data_id, data_len = struct.unpack('>HB', payload[:3])
        payload = payload[3:]

        data = payload[:data_len]
        payload = payload[data_len:]

        if data_id & 0xfff0 == 0x2030:
            rot = np.frombuffer(data, dtype='>f4').astype(np.dtype('f4'))
        elif data_id & 0xfff0 == 0x4020:
            acc = np.frombuffer(data, dtype='>f4').astype(np.dtype('f4'))
        elif data_id & 0xfff0 == 0x8020:
            avel = np.frombuffer(data, dtype='>f4').astype(np.dtype('f4'))

    return IMUData(rot=rot, acc=acc, avel=avel)  # type: ignore