Skip to content

Commit 97ea4fd

Browse files
NeutreeNeutree
committed
add IMU AHRS attitude estimation code and doc
1 parent 9ff7329 commit 97ea4fd

File tree

16 files changed

+747
-205
lines changed

16 files changed

+747
-205
lines changed

‎components/maix/component.py‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,5 +39,14 @@ def add_requirements(platform : str, component_dirs : list):
3939
if os.path.exists(py_paty) or os.path.exists(cmake_path):
4040
requires.append(name)
4141
break
42+
if "algo" in names:
43+
names = os.listdir(os.path.join(dir, "algo"))
44+
for name in names:
45+
path = os.path.join(dir, "algo", name)
46+
py_paty = os.path.join(path, "component.py")
47+
cmake_path = os.path.join(path, "CMakeLists.txt")
48+
if os.path.exists(py_paty) or os.path.exists(cmake_path):
49+
requires.append(name)
50+
break
4251
return requires
4352

‎docs/doc/en/modules/acc.md‎

Lines changed: 1 addition & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -2,42 +2,5 @@
22
title: Reading the Accelerometer and Attitude Calculation with MaixCAM MaixPy
33
---
44

5-
## Introduction to IMU
6-
7-
For the MaixCAM-Pro, it has an onboard QMI8658 chip that integrates a three-axis gyroscope and a three-axis accelerometer. This chip can provide high-precision data on attitude, motion, and position, making it suitable for various applications that require accurate motion detection, such as drones, robots, game controllers, and virtual reality devices. The QMI8658 features low power consumption, high stability, and high sensitivity. Below is an introduction to using the IMU module to obtain attitude data.
8-
9-
> MaixCAM does not have an onboard accelerometer, but you can connect one externally using an IIC driver.
10-
11-
12-
## Using IMU in MaixPy
13-
14-
Example:
15-
16-
```python
17-
from maix.ext_dev import imu
18-
19-
i = imu.IMU("qmi8658", mode=imu.Mode.DUAL,
20-
acc_scale=imu.AccScale.ACC_SCALE_2G,
21-
acc_odr=imu.AccOdr.ACC_ODR_8000,
22-
gyro_scale=imu.GyroScale.GYRO_SCALE_16DPS,
23-
gyro_odr=imu.GyroOdr.GYRO_ODR_8000)
24-
25-
while True:
26-
data = i.read()
27-
print("\n-------------------")
28-
print(f"acc x: {data[0]}")
29-
print(f"acc y: {data[1]}")
30-
print(f"acc z: {data[2]}")
31-
print(f"gyro x: {data[3]}")
32-
print(f"gyro y: {data[4]}")
33-
print(f"gyro z: {data[5]}")
34-
print(f"temp: {data[6]}")
35-
print("-------------------\n")
36-
```
37-
38-
Initialize the IMU object according to your needs, and then call `read()` to get the raw data read from the IMU.
39-
40-
**If the `mode` parameter is set to `DUAL`, the data returned by `read()` will be `[acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, temp]`. If `mode` is set to only one of ACC or GYRO, it will return only the corresponding `[x, y, z, temp]`. For example, if ACC is selected, `read()` will return `[acc_x, acc_y, acc_z, temp]`.**
41-
42-
For detailed information on the BM8653 API, please refer to the [BM8653 API Documentation](../../../api/maix/ext_dev/imu.md)
5+
Please refer to [AHRS Documentation](./ahrs.md)
436

‎docs/doc/en/modules/ahrs.md‎

Lines changed: 219 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,219 @@
1+
---
2+
title: MaixCAM MaixPy Read IMU Accelerometer and Gyroscope for Attitude Estimation
3+
update:
4+
- date: 2025-07-08
5+
author: neucrack
6+
version: 1.0.0
7+
content: Added attitude estimation code support and documentatio
8+
---
9+
10+
11+
## IMU Introduction
12+
13+
IMU (Inertial Measurement Unit) typically consists of several parts:
14+
15+
* Accelerometer: Measures linear acceleration, including gravitational acceleration.
16+
* Gyroscope: Measures angular velocity (rotation around an axis).
17+
* Magnetometer (optional): Measures magnetic field direction, used to assist in yaw calculation.
18+
19+
Using data from these sensors, we can calculate the device's attitude, the angles of rotation along its three axes, also known as Euler angles.
20+
For example, in the following diagram, the `z` axis is up, the `y` axis is forward, and the `x` axis is right. This is a right-handed coordinate system:
21+
22+
```
23+
^z / y(front)
24+
| /
25+
| /
26+
. ————————> x(right)
27+
```
28+
29+
The rotation angle around the `x` axis is called `pitch`.
30+
The rotation angle around the `y` axis is called `roll`.
31+
The rotation angle around the `z` axis is called `yaw`.
32+
33+
A system of sensor + reading + attitude estimation + output can be called `AHRS` (Attitude and Heading Reference System).
34+
35+
With these three angles, we can determine the current orientation of our device, which can be used in many scenarios, such as:
36+
37+
* Drones
38+
* Self-balancing vehicles
39+
* Robots
40+
* Motion control
41+
* Anti-shake
42+
* Direction and vibration detection
43+
* Motion judgment, gesture recognition, behavior analysis (can be combined with AI models)
44+
45+
## Hardware Support
46+
47+
Some devices have a built-in IMU, as shown below:
48+
49+
| Device Name | Sensor Model | Accelerometer | Gyroscope | Magnetometer | Interface | Features |
50+
| ----------- | ------------ | ------------- | --------- | ------------ | -------------------- | ----------------------------------------------- |
51+
| MaixCAM-Pro | QMI8658 | ✅ | ✅ | ❌ | IIC4<br>Address 0x6B | Low power<br>High stability<br>High sensitivity |
52+
| MaixCAM | None ❌ | ❌ | ❌ | ❌ | | |
53+
54+
Besides using the built-in IMU, you can also connect an external IMU sensor, such as the classic `MPU6050/MPU9150`. Search for the latest and suitable sensors yourself.
55+
56+
In MaixPy, attitude estimation and IMU drivers are separated, so you can use your own driver for external sensors and still call the attitude estimation algorithm.
57+
58+
## MaixPy Read IMU Data
59+
60+
Using the built-in `QMI8658` of MaixCAM-Pro as an example, use `maix.imu.IMU` to read:
61+
62+
```python
63+
from maix.ext_dev import imu
64+
65+
# Force calibrate first
66+
calibrate_first = False
67+
68+
# default config: acc +-2g 1KHz, gyro +-256rad/s 8KHz
69+
sensor = imu.IMU("qmi8658", mode=imu.Mode.DUAL,
70+
acc_scale=imu.AccScale.ACC_SCALE_2G,
71+
acc_odr=imu.AccOdr.ACC_ODR_1000,
72+
gyro_scale=imu.GyroScale.GYRO_SCALE_256DPS,
73+
gyro_odr=imu.GyroOdr.GYRO_ODR_8000)
74+
75+
# for gyro have bias, we need to calibrate first
76+
if calibrate_first or not sensor.calib_gyro_exists():
77+
print("\n\nNeed calibrate fisrt")
78+
print("now calibrate, please !! don't move !! device, wait for 10 seconds")
79+
# calib_gyro will auto calculate and save bias,
80+
# the next time use load_calib_gyro load
81+
sensor.calib_gyro(10000)
82+
else:
83+
sensor.load_calib_gyro()
84+
85+
while True:
86+
data = sensor.read_all(calib_gryo = True, radian = False)
87+
msg = "acc: {:10.4f}, {:10.4f}, {:10.4f}, gyro: {:10.4f}, {:10.4f}, {:10.4f}, temp: {:4.1f}".format(
88+
data.acc.x, data.acc.y, data.acc.z,
89+
data.gyro.x, data.gyro.y, data.gyro.z,
90+
data.temp
91+
)
92+
print(msg)
93+
```
94+
95+
Here, an `IMU` object is constructed, then `read_all` is called to read and print the data.
96+
Since `qmi8658` has no magnetometer, it's not printed.
97+
98+
However, note that calibration is needed, as explained below.
99+
100+
## Data Calibration
101+
102+
### Gyroscope Calibration
103+
104+
Why gyroscope calibration is needed:
105+
If not calibrated, we assume the gyroscope reads `x, y, z = 0` when stationary, but in practice, there's an offset—known as "zero drift".
106+
107+
As shown in the code, we need to perform `calib_gyro` once, which calibrates the gyroscope data.
108+
The principle is simple: collect multiple samples over time and average them.
109+
Here, `calib_gyro(10000)` samples for 10s and saves the result in `/maixapp/share/misc`.
110+
Next time, use `load_calib_gyro` to load the file, skipping recalibration.
111+
112+
When reading data, `read_all(calib_gryo = True)` will subtract the bias automatically. You can also set `calib_gyro = False` and handle it manually.
113+
114+
### Accelerometer Calibration
115+
116+
In theory, the accelerometer should also be calibrated, but its effect is less significant than the gyroscope. Not covered here—please refer to other resources.
117+
118+
### Magnetometer Calibration
119+
120+
Similarly, magnetometers need calibration, such as the common ellipsoid calibration. Not covered here—please search and learn.
121+
122+
## Attitude Estimation
123+
124+
Once we have sensor values, we can use an attitude estimation algorithm to obtain Euler angles.
125+
126+
### Basic Principles
127+
128+
* Theoretically, gyroscopes can tell how much rotation occurred since the last moment: `angular velocity * dt = rotation angle`. But due to drift, this is only reliable short-term.
129+
* Accelerometers measure gravity. Since gravity is constant, when the device is still or moving at constant speed, we can use this to determine absolute orientation relative to the Earth.
130+
So we trust the accelerometer long-term and gyroscope short-term—they complement each other.
131+
* However, if the device rotates around an axis perpendicular to gravity, the accelerometer can't detect it. In this case, only the gyroscope helps, but drift may occur over time.
132+
* The magnetometer helps here since it points to Earth's magnetic north, making up for the accelerometer's blind spot.
133+
134+
### Attitude Estimation Algorithm
135+
136+
There are many algorithms. MaixPy includes the **Mahony complementary filter**—lightweight and fast.
137+
Full code in [MaixPy/examples/ext\_dev/sensors/imu/imu\_ahrs\_mahony.py](https://github.com/sipeed/MaixPy/tree/main/examples/ext_dev/sensors/imu)
138+
139+
```python
140+
'''
141+
Mahony AHRS get device euler angle demo.
142+
Full Application code see:
143+
https://github.com/sipeed/MaixPy/tree/main/projects/app_imu_ahrs
144+
'''
145+
146+
from maix import ahrs, app, time
147+
from maix.ext_dev import imu
148+
149+
# P of PI controller, a larger P (proportional gain) leads to faster response,
150+
# but it increases the risk of overshoot and oscillation.
151+
kp = 2
152+
153+
# I of PI controller, a larger I (integral gain) helps to eliminate steady-state errors more quickly,
154+
# but it can accumulate error over time, potentially causing instability or slow drift.
155+
ki = 0.01
156+
157+
# Force calibrate first
158+
calibrate_first = False
159+
160+
# default config: acc +-2g 1KHz, gyro +-256rad/s 8KHz
161+
sensor = imu.IMU("qmi8658", mode=imu.Mode.DUAL,
162+
acc_scale=imu.AccScale.ACC_SCALE_2G,
163+
acc_odr=imu.AccOdr.ACC_ODR_1000,
164+
gyro_scale=imu.GyroScale.GYRO_SCALE_256DPS,
165+
gyro_odr=imu.GyroOdr.GYRO_ODR_8000)
166+
ahrs_filter = ahrs.MahonyAHRS(kp, ki)
167+
168+
# for gyro have bias, we need to calibrate first
169+
if calibrate_first or not sensor.calib_gyro_exists():
170+
print("\n\nNeed calibrate fisrt")
171+
print("now calibrate, please !! don't move !! device, wait for 10 seconds")
172+
# calib_gyro will auto calculate and save bias,
173+
# the next time use load_calib_gyro load
174+
sensor.calib_gyro(10000)
175+
else:
176+
sensor.load_calib_gyro()
177+
178+
# time.sleep(3)
179+
last_time = time.ticks_s()
180+
while not app.need_exit():
181+
# get imu data
182+
data = sensor.read_all(calib_gryo = True, radian = True)
183+
184+
# calculate angles
185+
t = time.ticks_s()
186+
dt = t - last_time
187+
last_time = t
188+
# print(f"{data.mag.x:8.2f}, {data.mag.y:8.2f}, {data.mag.z:8.2f}, {data.gyro.z:8.2f}")
189+
angle = ahrs_filter.get_angle(data.acc, data.gyro, data.mag, dt, radian = False)
190+
191+
# ^z / y(front)
192+
# | /
193+
# | /
194+
# . ————————> x(right)
195+
# this demo's axis
196+
197+
# x axis same with camera
198+
# angle.y -= 90
199+
200+
print(f"pitch: {angle.x:8.2f}, roll: {angle.y:8.2f}, yaw: {angle.z:8.2f}, dt: {int(dt*1000):3d}ms, temp: {data.temp:.1f}")
201+
202+
# time.sleep_ms(1)
203+
```
204+
205+
As shown, use `get_angle` to input raw sensor data and get Euler angles. If there's no magnetometer, set it to all zero.
206+
207+
You may have noticed the use of a `PI` controller to adjust sensitivity. You can experiment with different values (learn about PID tuning):
208+
209+
* `kp`: Proportional gain. Higher values respond faster but risk overshoot.
210+
* `ki`: Integral gain. Higher values eliminate errors faster but may cause instability or drift.
211+
212+
Other parameters, such as the gyroscope's default range of `[-256degree/s, 256degree/s]`, must be tuned for your scenario.
213+
Exceeding the range (e.g., rotating 100° but detecting only 60°) causes errors.
214+
Also, different settings affect sensitivity and noise levels.
215+
216+
## API Documentation
217+
218+
For details on IMU APIs, see [IMU API Documentation](../../../api/maix/ext_dev/imu.md).
219+
For details on AHRS APIs, see [AHRS Documentation](../../../api/maix/ahrs.md).

‎docs/doc/en/modules/qmi8658.md‎

Lines changed: 1 addition & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -7,54 +7,5 @@ update:
77
content: Initial document
88
---
99

10-
## Introduction to QMI8658
11-
12-
QMI8658 is an Inertial Measurement Unit (IMU) chip that integrates a three-axis gyroscope and a three-axis accelerometer. It provides high-precision attitude, motion, and position data, making it suitable for various applications requiring accurate motion detection, such as drones, robots, game controllers, and virtual reality devices. QMI8658 features low power consumption, high stability, and high sensitivity.
13-
14-
## Using QMI8658 in MaixPy
15-
16-
Using QMI8658 in MaixPy is straightforward; you only need to know which I2C bus your platform's QMI8658 is mounted on. The onboard QMI8658 on the MaixCAM Pro is mounted on I2C-4.
17-
18-
Example:
19-
20-
```python
21-
from maix import ext_dev, pinmap, err, time
22-
23-
### Enable I2C
24-
# ret = pinmap.set_pin_function("PIN_NAME", "I2Cx_SCL")
25-
# if ret != err.Err.ERR_NONE:
26-
# print("Failed in function pinmap...")
27-
# exit(-1)
28-
# ret = pinmap.set_pin_function("PIN_NAME", "I2Cx_SDA")
29-
# if ret != err.Err.ERR_NONE:
30-
# print("Failed in function pinmap...")
31-
# exit(-1)
32-
33-
QMI8658_I2CBUS_NUM = 4
34-
35-
imu = ext_dev.qmi8658.QMI8658(QMI8658_I2CBUS_NUM,
36-
mode=ext_dev.imu.Mode.DUAL,
37-
acc_scale=ext_dev.imu.AccScale.ACC_SCALE_2G,
38-
acc_odr=ext_dev.imu.AccOdr.ACC_ODR_8000,
39-
gyro_scale=ext_dev.imu.GyroScale.GYRO_SCALE_16DPS,
40-
gyro_odr=ext_dev.imu.GyroOdr.GYRO_ODR_8000)
41-
42-
while True:
43-
data = imu.read()
44-
print("\n-------------------")
45-
print(f"acc x: {data[0]}")
46-
print(f"acc y: {data[1]}")
47-
print(f"acc z: {data[2]}")
48-
print(f"gyro x: {data[3]}")
49-
print(f"gyro y: {data[4]}")
50-
print(f"gyro z: {data[5]}")
51-
print(f"temp: {data[6]}")
52-
print("-------------------\n")
53-
```
54-
55-
Initialize the QMI8658 object according to your needs, and then call `read()` to get the raw data read from the QMI8658.
56-
57-
**If the `mode` parameter is set to `DUAL`, the data returned by `read()` will be `[acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, temp]`. If `mode` is set to only one of ACC or GYRO, it will return only the corresponding `[x, y, z, temp]`. For example, if ACC is selected, `read()` will return `[acc_x, acc_y, acc_z, temp]`.**
58-
59-
For detailed information on the QMI8658 API, please refer to the [QMI8658 API Documentation](../../../api/maix/ext_dev/qmi8658.md)
10+
Please refer to [AHRS Documentation](./ahrs.md)
6011

‎docs/doc/en/sidebar.yaml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -201,10 +201,10 @@ items:
201201
items:
202202
- file: moudles/microscope.md
203203
label: Microscope kit
204+
- file: modules/ahrs.md
205+
label: IMU AHRS Attitude Estimation
204206
- file: modules/temp_humi.md
205207
label: Temperature and humidity
206-
- file: modules/acc.md
207-
label: Accelerometer
208208
- file: modules/rtc.md
209209
label: RTC usage
210210
- file: modules/tmc2209.md

‎docs/doc/zh/modules/acc.md‎

Lines changed: 1 addition & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -2,42 +2,5 @@
22
title: MaixCAM MaixPy 读取加速度计和姿态解算
33
---
44

5+
请看 [AHRS 文档](./ahrs.md)
56

6-
## IMU 简介
7-
8-
对于 MaixCAM-Pro，板载了一款集成了三轴陀螺仪和三轴加速度计的QMI8658芯片, 它能够提供高精度的姿态、运动和位置数据，适用于各种需要精确运动检测的应用场景，如无人机、机器人、游戏控制器和虚拟现实设备等。QMI8658具有低功耗、高稳定性和高灵敏度的特点, 下面将介绍使用IMU模块来获取姿态数据。
9-
> MaixCAM 无板载加速度计，可自行外接使用 iic 驱动。
10-
11-
## MaixPy 中使用 IMU
12-
13-
使用 IMU 模块从 QMI8658 读取数据.
14-
15-
示例代码:
16-
17-
```python
18-
from maix.ext_dev import imu
19-
20-
i = imu.IMU("qmi8658", mode=imu.Mode.DUAL,
21-
acc_scale=imu.AccScale.ACC_SCALE_2G,
22-
acc_odr=imu.AccOdr.ACC_ODR_8000,
23-
gyro_scale=imu.GyroScale.GYRO_SCALE_16DPS,
24-
gyro_odr=imu.GyroOdr.GYRO_ODR_8000)
25-
26-
while True:
27-
data = i.read()
28-
print("\n-------------------")
29-
print(f"acc x: {data[0]}")
30-
print(f"acc y: {data[1]}")
31-
print(f"acc z: {data[2]}")
32-
print(f"gyro x: {data[3]}")
33-
print(f"gyro y: {data[4]}")
34-
print(f"gyro z: {data[5]}")
35-
print(f"temp: {data[6]}")
36-
print("-------------------\n")
37-
```
38-
39-
按照您的需求初始化 IMU 对象, 然后调用 `read()` 即可. `read()` 返回的是从 IMU 中读出的原始数据.
40-
41-
**如果 `mode` 参数选择 `DUAL`, 则 `read()`返回的数据为 `[acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, temp]`, 如果 `mode` 只选择 ACC/GYRO 中的一个, 只会返回对应的 `[x, y, z, temp]`, 例如选择 ACC, `read()` 会返回 `[acc_x, acc_y, acc_z, temp]`.**
42-
43-
有关 IMU API 的详细说明请看 [IMU API 文档](../../../api/maix/ext_dev/imu.md)

0 commit comments

Comments
 (0)