Skip to content

Commit ef64aca

Browse files
finger563finger563
authored
fix(imu): Cleanup ICM20948 and ICM42607 to ensure proper mutex protection (#461)
1 parent 8af407d commit ef64aca

File tree

8 files changed

+193
-135
lines changed

8 files changed

+193
-135
lines changed

‎components/i2c/example/main/Kconfig.projbuild

Lines changed: 54 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -1,67 +1,67 @@
11
menu "Example Configuration"
22

3-
choice EXAMPLE_HARDWARE
4-
prompt "Hardware"
5-
default EXAMPLE_HARDWARE_QTPYPICO
6-
help
7-
Select the hardware to run this example on.
3+
choice EXAMPLE_HARDWARE
4+
prompt "Hardware"
5+
default EXAMPLE_HARDWARE_QTPYPICO
6+
help
7+
Select the hardware to run this example on.
88

9-
config EXAMPLE_HARDWARE_QTPYPICO
10-
depends on IDF_TARGET_ESP32
11-
bool "Qt Py PICO"
9+
config EXAMPLE_HARDWARE_QTPYPICO
10+
depends on IDF_TARGET_ESP32
11+
bool "Qt Py PICO"
1212

13-
config EXAMPLE_HARDWARE_QTPYS3
14-
depends on IDF_TARGET_ESP32S3
15-
bool "Qt Py S3"
13+
config EXAMPLE_HARDWARE_QTPYS3
14+
depends on IDF_TARGET_ESP32S3
15+
bool "Qt Py S3"
1616

17-
config EXAMPLE_HARDWARE_CUSTOM
18-
bool "Custom"
19-
endchoice
17+
config EXAMPLE_HARDWARE_CUSTOM
18+
bool "Custom"
19+
endchoice
2020

21-
config EXAMPLE_I2C_SCL_GPIO
22-
int "SCL GPIO Num"
23-
range 0 50
24-
default 19 if EXAMPLE_HARDWARE_QTPYPICO
25-
default 40 if EXAMPLE_HARDWARE_QTPYS3
26-
default 19 if EXAMPLE_HARDWARE_CUSTOM
27-
help
28-
GPIO number for I2C Master clock line.
21+
config EXAMPLE_I2C_SCL_GPIO
22+
int "SCL GPIO Num"
23+
range 0 50
24+
default 19 if EXAMPLE_HARDWARE_QTPYPICO
25+
default 40 if EXAMPLE_HARDWARE_QTPYS3
26+
default 19 if EXAMPLE_HARDWARE_CUSTOM
27+
help
28+
GPIO number for I2C Master clock line.
2929

30-
config EXAMPLE_I2C_SDA_GPIO
31-
int "SDA GPIO Num"
32-
range 0 50
33-
default 22 if EXAMPLE_HARDWARE_QTPYPICO
34-
default 41 if EXAMPLE_HARDWARE_QTPYS3
35-
default 22 if EXAMPLE_HARDWARE_CUSTOM
36-
help
37-
GPIO number for I2C Master data line.
30+
config EXAMPLE_I2C_SDA_GPIO
31+
int "SDA GPIO Num"
32+
range 0 50
33+
default 22 if EXAMPLE_HARDWARE_QTPYPICO
34+
default 41 if EXAMPLE_HARDWARE_QTPYS3
35+
default 22 if EXAMPLE_HARDWARE_CUSTOM
36+
help
37+
GPIO number for I2C Master data line.
3838

39-
config EXAMPLE_I2C_CLOCK_SPEED_HZ
40-
int "I2C Clock Speed"
41-
range 100 1000000
42-
default 400000
43-
help
44-
I2C clock speed in Hz.
39+
config EXAMPLE_I2C_CLOCK_SPEED_HZ
40+
int "I2C Clock Speed"
41+
range 100 1000000
42+
default 400000
43+
help
44+
I2C clock speed in Hz.
4545

46-
config EXAMPLE_I2C_DEVICE_ADDR
47-
hex "I2C Device Address"
48-
range 0x00 0x7F
49-
default 0x3C
50-
help
51-
I2C device address of the device.
46+
config EXAMPLE_I2C_DEVICE_ADDR
47+
hex "I2C Device Address"
48+
range 0x00 0x7F
49+
default 0x3C
50+
help
51+
I2C device address of the device.
5252

53-
config EXAMPLE_I2C_DEVICE_REG_ADDR
54-
hex "I2C Device Register Address"
55-
range 0x00 0xFF
56-
default 0x00
57-
help
58-
I2C device register address of the device.
53+
config EXAMPLE_I2C_DEVICE_REG_ADDR
54+
hex "I2C Device Register Address"
55+
range 0x00 0xFF
56+
default 0x00
57+
help
58+
I2C device register address of the device.
5959

60-
config EXAMPLE_I2C_DEVICE_REG_SIZE
61-
int "I2C Device Register Size"
62-
range 1 100
63-
default 1
64-
help
65-
I2C device register size of the device.
60+
config EXAMPLE_I2C_DEVICE_REG_SIZE
61+
int "I2C Device Register Size"
62+
range 1 100
63+
default 1
64+
help
65+
I2C device register size of the device.
6666

6767
endmenu

‎components/icm20948/example/main/Kconfig.projbuild

Lines changed: 32 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,41 +1,41 @@
11
menu "Example Configuration"
22

3-
choice EXAMPLE_HARDWARE
4-
prompt "Hardware"
5-
default EXAMPLE_HARDWARE_QTPY
6-
help
7-
Select the hardware to run this example on.
3+
choice EXAMPLE_HARDWARE
4+
prompt "Hardware"
5+
default EXAMPLE_HARDWARE_QTPY
6+
help
7+
Select the hardware to run this example on.
88

9-
config EXAMPLE_HARDWARE_QTPY
10-
bool "QtPy ESP32 Pico or QtPy ESP32-S3"
9+
config EXAMPLE_HARDWARE_QTPY
10+
bool "QtPy ESP32 Pico or QtPy ESP32-S3"
1111

12-
config EXAMPLE_HARDWARE_CUSTOM
13-
bool "Custom"
14-
endchoice
12+
config EXAMPLE_HARDWARE_CUSTOM
13+
bool "Custom"
14+
endchoice
1515

16-
config EXAMPLE_I2C_SCL_GPIO
17-
int "SCL GPIO Num"
18-
range 0 50
19-
default 40 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32S3
20-
default 19 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32
21-
default 19 if EXAMPLE_HARDWARE_CUSTOM
22-
help
23-
GPIO number for I2C Master clock line.
16+
config EXAMPLE_I2C_SCL_GPIO
17+
int "SCL GPIO Num"
18+
range 0 50
19+
default 40 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32S3
20+
default 19 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32
21+
default 19 if EXAMPLE_HARDWARE_CUSTOM
22+
help
23+
GPIO number for I2C Master clock line.
2424

25-
config EXAMPLE_I2C_SDA_GPIO
26-
int "SDA GPIO Num"
27-
range 0 50
28-
default 41 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32S3
29-
default 22 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32
30-
default 22 if EXAMPLE_HARDWARE_CUSTOM
31-
help
32-
GPIO number for I2C Master data line.
25+
config EXAMPLE_I2C_SDA_GPIO
26+
int "SDA GPIO Num"
27+
range 0 50
28+
default 41 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32S3
29+
default 22 if EXAMPLE_HARDWARE_QTPY && IDF_TARGET_ESP32
30+
default 22 if EXAMPLE_HARDWARE_CUSTOM
31+
help
32+
GPIO number for I2C Master data line.
3333

34-
config EXAMPLE_I2C_CLOCK_SPEED_HZ
35-
int "I2C Clock Speed"
36-
range 100 1000000
37-
default 400000
38-
help
39-
I2C clock speed in Hz.
34+
config EXAMPLE_I2C_CLOCK_SPEED_HZ
35+
int "I2C Clock Speed"
36+
range 100 1000000
37+
default 400000
38+
help
39+
I2C clock speed in Hz.
4040

4141
endmenu

‎components/icm20948/example/main/icm20948_example.cpp

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,6 @@ extern "C" void app_main(void) {
4949
static espp::KalmanFilter<3> kf;
5050
kf.set_process_noise(rate_noise);
5151
kf.set_measurement_noise(angle_noise);
52-
static constexpr float beta = 0.1f; // higher = more accelerometer, lower = more gyro
53-
static espp::MadgwickFilter f(beta);
54-
5552
auto kalman_filter_fn = [](float dt, const Imu::Value &accel, const Imu::Value &gyro,
5653
const Imu::Value &mag) -> Imu::Value {
5754
// Apply Kalman filter
@@ -71,13 +68,15 @@ extern "C" void app_main(void) {
7168
return orientation;
7269
};
7370

71+
static constexpr float beta = 0.1f; // higher = more accelerometer, lower = more gyro
72+
static espp::MadgwickFilter madgwick(beta);
7473
auto madgwick_filter_fn = [](float dt, const Imu::Value &accel, const Imu::Value &gyro,
7574
const Imu::Value &mag) -> Imu::Value {
7675
// Apply Madgwick filter
77-
f.update(dt, accel.x, accel.y, accel.z, espp::deg_to_rad(gyro.x), espp::deg_to_rad(gyro.y),
78-
espp::deg_to_rad(gyro.z), mag.x, mag.y, mag.z);
76+
madgwick.update(dt, accel.x, accel.y, accel.z, espp::deg_to_rad(gyro.x),
77+
espp::deg_to_rad(gyro.y), espp::deg_to_rad(gyro.z), mag.x, mag.y, mag.z);
7978
float roll, pitch, yaw;
80-
f.get_euler(roll, pitch, yaw);
79+
madgwick.get_euler(roll, pitch, yaw);
8180
// return the computed orientation
8281
Imu::Value orientation{};
8382
orientation.pitch = espp::deg_to_rad(pitch);

‎components/icm20948/include/icm20948.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ class Icm20948 : public espp::BasePeripheral<uint8_t, Interface == icm20948::Int
8888
using BasePeripheral<uint8_t,
8989
Interface == icm20948::Interface::I2C>::set_bits_in_register_by_mask;
9090
using BasePeripheral<uint8_t, Interface == icm20948::Interface::I2C>::read;
91+
using BasePeripheral<uint8_t, Interface == icm20948::Interface::I2C>::base_mutex_;
9192
using BasePeripheral<uint8_t, Interface == icm20948::Interface::I2C>::logger_;
9293

9394
public:

0 commit comments

Comments
 (0)