Wheeltec N100 IMU Debug Code for Qmini Robot

Key Features

1. Continuous Output : Unlike the original that returns data once, this script continuously dumps IMU data to the terminal

2. Rate Control : Added --rate parameter to control output frequency (default 10 Hz)

3. Formatted Display : Clean, timestamped output showing:

- Accelerometer data (X, Y, Z in m/s²)

- Angular velocity (Roll, Pitch, Heading in rad/s)

- Orientation angles (Roll, Pitch, Heading in rad)

- Quaternion values (w, x, y, z)

4. Error Handling : Proper exception handling for serial port issues and graceful shutdown on Ctrl+C

5. Status Information : Shows connection status, baud rate, and output rate on startup

To determine the maximum output rate for the IMU dump script with a baud rate of 921600 bps, I need to calculate the theoretical limits based on the data frame structure.

Calculation Analysis

Data Frame Structure

- Frame header: 1 byte (0xFC)
- Type: 1 byte (0x40 for IMU, 0x41 for AHRS)
- Length: 1 byte
- Sequence: 1 byte
- CRC8: 1 byte
- CRC16: 2 bytes
- Total header overhead: 7 bytes per frame

Data Payload Sizes

- IMU data: 56 bytes (0x38 hex)
- AHRS data: 48 bytes (0x30 hex)
- Total data per complete reading: 104 bytes

Complete Frame Size

- IMU frame: 7 + 56 = 63 bytes
- AHRS frame: 7 + 48 = 55 bytes
- Total per complete IMU+AHRS reading: 118 bytes

Theoretical Maximum Rate

Baud rate: 921,600 bps = 115,200 bytes/second
Bytes per reading: 118 bytes
Max theoretical rate: 115,200 ÷ 118 = ~976 Hz

Practical Considerations

Recommended Maximum Rates

- Conservative: 100-200 Hz - Accounts for processing overhead, serial buffer management, and system latency
- Aggressive: 300-500 Hz - For high-performance systems with minimal other processing
- Theoretical limit: ~976 Hz - Only achievable under perfect conditions

Factors that reduce actual maximum rate

1. Processing overhead - Python parsing and struct unpacking
2. Serial buffer delays - OS-level buffering and timing
3. System load - Other processes competing for CPU
4. Print/display overhead - Terminal output takes time
5. Frame synchronization - Time spent finding valid frame headers

Recommendations

For your IMU dump script:

- Start with 50-100 Hz for reliable operation
- Test incrementally up to 200-300 Hz
- Monitor for dropped frames or timing issues
- Consider removing print statements for maximum throughput if just logging data

Example usage

# Conservative rate

1python3 imu_dump.py --rate 100

# Higher rate for testing

1python3 imu_dump.py --rate 300

# Maximum practical rate

1python3 imu_dump.py --rate 500

AHRS

AHRS_DATA 是从IMU传感器接收到的姿态和航向参考系统（Attitude and Heading Reference System）数据。

AHRS_DATA 数据结构

AHRS_DATA 是通过以下方式解析的：

1AHRS_DATA = struct.unpack('10f ii', data_s[0:48])

这表示AHRS_DATA包含：

- 10个浮点数 (10f)
- 2个整数 (ii)
- 总共48字节的数据

AHRS_DATA 各索引的含义

根据代码中的注释和赋值，AHRS_DATA的各个索引代表：

索引数据类型含义单位
AHRS_DATA[0] / float / PitchSpeed（俯仰角速度） / rad/s
AHRS_DATA[1] / float / RollSpeed（横滚角速度） / rad/s
AHRS_DATA[2] / float / HeadingSpeed（偏航角速度） / rad/s
AHRS_DATA[3] / float / Pitch（俯仰角） rad
AHRS_DATA[4] / float / Roll（横滚角） rad
AHRS_DATA[5] / float / Heading（偏航角/航向角） rad
AHRS_DATA[6] / float / qw（四元数w分量） -
AHRS_DATA[7] / float / qx（四元数x分量） -
AHRS_DATA[8] / float / qy（四元数y分量） -
AHRS_DATA[9] / float / qz（四元数z分量） -
AHRS_DATA[10] / int / 时间戳或状态信息 - AHRS_DATA[11] int 时间戳或状态信息 -

AHRS系统的作用

AHRS（姿态和航向参考系统）提供：

1. 姿态信息：横滚角(Roll)、俯仰角(Pitch)、偏航角(Heading)
2. 角速度信息：三轴角速度
3. 四元数表示：更稳定的姿态表示方法，避免万向锁问题

代码

1import time
2import struct
3import argparse
4import serial
5import serial.tools.list_ports
6from serial import EIGHTBITS, PARITY_NONE, STOPBITS_ONE
7
8# Macro definition parameters
9PI = 3.1415926
10FRAME_HEAD = str('fc')
11FRAME_END = str('fd')
12TYPE_IMU = str('40')
13TYPE_AHRS = str('41')
14TYPE_INSGPS = str('42')
15TYPE_GEODETIC_POS = str('5c')
16TYPE_GROUND = str('f0')
17TYPE_SYS_STATE = str('50')
18TYPE_BODY_ACCELERATION = str('62')
19TYPE_ACCELERATION = str('61')
20TYPE_MSG_BODY_VEL = str('60')
21IMU_LEN = str('38')  # //56
22AHRS_LEN = str('30')  # //48
23INSGPS_LEN = str('48')  # //72
24GEODETIC_POS_LEN = str('20')  # //32
25SYS_STATE_LEN = str('64')  # // 100
26BODY_ACCELERATION_LEN = str('10') #// 16
27ACCELERATION_LEN = str('0c')  # 12
28PI = 3.141592653589793
29DEG_TO_RAD = 0.017453292519943295
30isrun = True
31
32
33# Get command line input parameters
34def parse_opt(known=False):
35    parser = argparse.ArgumentParser()
36    parser.add_argument('--port', type=str, default='/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0', help='Serial port receive data')
37    parser.add_argument('--bps', type=int, default=921600, help='the models baud rate set; default: 921600')
38    parser.add_argument('--timeout', type=int, default=20, help='set the serial port timeout; default: 20')
39    parser.add_argument('--rate', type=float, default=10.0, help='output rate in Hz; default: 10.0')
40
41    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
42    return receive_params
43
44
45def dump_imu_data(port='/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0', baudrate=921600, timeout=1, output_rate=10.0):
46    try:
47        serial_ = serial.Serial(port=port, baudrate=baudrate, bytesize=EIGHTBITS, parity=PARITY_NONE, stopbits=STOPBITS_ONE, timeout=timeout)
48        print(f"Connected to IMU on port: {port}")
49        print(f"Baud rate: {serial_.baudrate}")
50        print(f"Output rate: {output_rate} Hz")
51        print("=" * 80)
52        print("Starting IMU data dump... (Press Ctrl+C to stop)")
53        print("=" * 80)
54    except Exception as e:
55        print(f"Error: Unable to open port {port}. {e}")
56        exit(1)
57
58    temp1 = False
59    temp2 = False
60    last_output_time = 0
61    output_interval = 1.0 / output_rate
62
63    result = {
64        "Accelerometer_X": 0,
65        "Accelerometer_Y": 0,
66        "Accelerometer_Z": 0,
67        "RollSpeed": 0,
68        "PitchSpeed": 0,
69        "HeadingSpeed": 0,
70        "Roll": 0,
71        "Pitch": 0,
72        "Heading": 0,
73        "qw": 0,
74        "qx": 0,
75        "qy": 0,
76        "qz": 0,
77    }
78
79    try:
80        while serial_.isOpen():
81            check_head = serial_.read().hex()
82            # Verify frame header
83            if check_head != FRAME_HEAD:
84                continue
85            head_type = serial_.read().hex()
86            # Verify data type
87            if (head_type != TYPE_IMU and head_type != TYPE_AHRS and head_type != TYPE_INSGPS and
88                    head_type != TYPE_GEODETIC_POS and head_type != 0x50 and head_type != TYPE_GROUND and
89                    head_type != TYPE_SYS_STATE and  head_type!=TYPE_MSG_BODY_VEL and head_type!=TYPE_BODY_ACCELERATION and head_type!=TYPE_ACCELERATION):
90                continue
91            check_len = serial_.read().hex()
92            # Verify data type length
93            if head_type == TYPE_IMU and check_len != IMU_LEN:
94                continue
95            elif head_type == TYPE_AHRS and check_len != AHRS_LEN:
96                continue
97            elif head_type == TYPE_INSGPS and check_len != INSGPS_LEN:
98                continue
99            elif head_type == TYPE_GEODETIC_POS and check_len != GEODETIC_POS_LEN:
100                continue
101            elif head_type == TYPE_SYS_STATE and check_len != SYS_STATE_LEN:
102                continue
103            elif head_type == TYPE_GROUND or head_type == 0x50:
104                continue
105            elif head_type == TYPE_MSG_BODY_VEL and check_len != ACCELERATION_LEN:
106                print("check head type "+str(TYPE_MSG_BODY_VEL)+" failed;"+" check_LEN:"+str(check_len))
107                continue
108            elif head_type == TYPE_BODY_ACCELERATION and check_len != BODY_ACCELERATION_LEN:
109                print("check head type "+str(TYPE_BODY_ACCELERATION)+" failed;"+" check_LEN:"+str(check_len))
110                continue
111            elif head_type == TYPE_ACCELERATION and check_len != ACCELERATION_LEN:
112                print("check head type "+str(TYPE_ACCELERATION)+" failed;"+" ckeck_LEN:"+str(check_len))
113                continue
114            check_sn = serial_.read().hex()
115            head_crc8 = serial_.read().hex()
116            crc16_H_s = serial_.read().hex()
117            crc16_L_s = serial_.read().hex()
118
119            # Read and parse IMU data
120            if head_type == TYPE_IMU:
121                data_s = serial_.read(int(IMU_LEN, 16))
122                IMU_DATA = struct.unpack('12f ii',data_s[0:56])
123                result["Accelerometer_X"] = IMU_DATA[3]
124                result["Accelerometer_Y"] = IMU_DATA[4]
125                result["Accelerometer_Z"] = IMU_DATA[5]
126                temp1 = True
127
128            # Read and parse AHRS data
129            elif head_type == TYPE_AHRS:
130                data_s = serial_.read(int(AHRS_LEN, 16))
131                AHRS_DATA = struct.unpack('10f ii',data_s[0:48])
132                # Coordinate transformation
133                result["RollSpeed"] = AHRS_DATA[1]
134                result["PitchSpeed"] = AHRS_DATA[0] * -1
135                result["HeadingSpeed"] = AHRS_DATA[2]
136                r = AHRS_DATA[4]
137                p = AHRS_DATA[3] * -1
138                h = AHRS_DATA[5]
139                result["Roll"] = r
140                result["Pitch"] = p
141                result["Heading"] = h
142                result["qw"] = AHRS_DATA[6]
143                result["qx"] = AHRS_DATA[7]
144                result["qy"] = AHRS_DATA[8]
145                result["qz"] = AHRS_DATA[9]
146                temp2 = True
147
148            # When complete IMU and AHRS data is collected, output to terminal
149            if temp1 and temp2:
150                current_time = time.time()
151                if current_time - last_output_time >= output_interval:
152                    print(f"\n[{time.strftime('%H:%M:%S', time.localtime(current_time))}] IMU Data:")
153                    print(f"  Accelerometer (m/s²): X={result['Accelerometer_X']:8.4f}, Y={result['Accelerometer_Y']:8.4f}, Z={result['Accelerometer_Z']:8.4f}")
154                    print(f"  Angular Velocity (rad/s): Roll={result['RollSpeed']:8.4f}, Pitch={result['PitchSpeed']:8.4f}, Heading={result['HeadingSpeed']:8.4f}")
155                    print(f"  Orientation (rad): Roll={result['Roll']:8.4f}, Pitch={result['Pitch']:8.4f}, Heading={result['Heading']:8.4f}")
156                    print(f"  Quaternion: w={result['qw']:8.4f}, x={result['qx']:8.4f}, y={result['qy']:8.4f}, z={result['qz']:8.4f}")
157                    
158                    last_output_time = current_time
159                
160                temp1 = False
161                temp2 = False
162
163    except KeyboardInterrupt:
164        print("\n\nIMU data dump stopped by user.")
165    except Exception as e:
166        print(f"\nError during data reading: {e}")
167    finally:
168        if serial_.isOpen():
169            serial_.close()
170            print("Serial port closed.")
171
172
173if __name__ == "__main__":
174    args = parse_opt()
175    dump_imu_data(port=args.port, baudrate=args.bps, timeout=args.timeout, output_rate=args.rate)