SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations #322

l0ud · 2024-03-29T14:44:45Z

Hi
This change is a is a collaborative effort among developers from Slime Discord to:

Reduce the amount of copy-pasting when implementing new IMUs.
Make adding new IMUs much quicker and simpler.
Support several new, promising IMUs in firmware.

Sfusion is based on a simplified version of 0.dev's BMI160 driver, providing a generic sensor framework with several calibration methods for very slim drivers focused on the low-level handling of particular IMUs.

This change adds support for the following IMUs:

BMI270 (good price/performance)
ICM42688 (similar to BMI270)
LSM6DS3TR-C (cheapest one)
LSM6DSV (best performing one)
LSM6DSR
LSM6DSO
MPU6050, without using its flaky onboard fusion.

All drivers were tested to be functional, some are already being used in many sets (LSM6DSV, partially BMI270).
One exception: MPU6050 sfusion driver may underperform until we find a way to change "restThGyr and restThAcc" in VQF based on the sensor. I would prefer to make this separate change, as this PR is already pretty large.
Driver code is written from scratch for Slime, so I don't expect any licensing problems. Required Bosch BMI firmware is provided in separated file and it's BSD-licensed.

Existing drivers were not affected except for the current ICM42688 implementation, which was removed as it didn't work.

Drivers are as minimal as possible to reduce code size not incorporating any external libraries and to have full control over IMU programming.

Other changes:

Significant SensorManager refactoring to reduce boilerplate code (like switch-cases for every IMU, ifdef with sensor addresses) and to reduce code size by not linking unused sensors.
Sensors' constructors were standardized to support SensorManager change.
The "Had data" flag was removed as I considered it not really useful and not supported by most sensors anyway.
Platform/Arduino versions were upgraded to have a toolchain with C++20, and the compiler was switched to use it. We checked functionality on ESP8266 and ESP32C3."

… working

…onal

…cal yet

… equivalent to the old driver

…lution

…re v3

Bump Arduino Core to v3.0 to Match GCC12

Bring back fast pin swapping

Fix accel timescale for LSM6DSV (calibration no longer takes forever)

…iver

Resolves an issue where the soft watchdog would trigger.

Reset the soft watchdog while eating or collecting calibration samples

Log FIFO overruns on LSMs

Fix missing word in comment, switch to constexpr

Change add BNO085 to exeption; update esp32/esp8266

ImUrX

BNO seems to work fine

…ations (SlimeVR#322) * Update readme to mention BMI270 support. * Soft fusion sensor initial code, wip * Soft fusion ICM-42688-P lazy WIP implementation. * sfusion: Cleanup, implemented sensor frequency calibration * icm42688: add more comments, basic driver (no hw filtering) should be working * sfustion: compilation fix * sfusion: start calibration when upside down * cleanup: remove confusing had data flag * sensor manager: use unique_ptr instead of raw pointers * sfusion: big refactoring wip * sfusion: make aux work, at least sfusion sensors should now be functional * sfusion: lightweight implementation of BMI270 sensor, no sensitivity cal yet * sfusion: BMI270: added CRT and gyro zx factor. should be functionally equivalent to the old driver * Added lsm6dsv * Trying to work around esp32c3 compilation problem, not liking that solution * sfusion: fix problems found after rebase * Update README.md * Bump Arduino core to 3.0 to match GCC12 * Remove fast pin swapping that is no longer compatible with arduino core v3 * Bring back fast pin swapping * Update platformio-tools.ini * Fix accel timescale (calibration no longer takes forever) * Fix non-sfusion sensors * Added LSM6DSO and DSR support and refactored DSV support * Removed template float param from the implementation * sfusion: port MPU6050 driver wip, not expecting to be functional yet * sfusion: add headers specifying main code owners * connection: fix warning * update README.md * fshelper: fixed ESP8266 regression caused by abstracting FS access * sfusion: fix error on merge * bno080: differentiate bno080, bno085, bno086 again * sfusion: final touches * restore hadData functionality, implementing it in every sensor, made configured flag bno-only * fix address supplement in non-sfusion sensors, do i2c bus reset for all sensors * sfusion: make MPU6050 driver use normal MPU6050 ImuID, change eatSamplesAndReturn function to take ms instead of seconds * sfusion: hotfix, don't apply sensorOffset, it's applied in sensor base * Log FIFO overruns on LSMs * Reset the soft watchdog while eating or collecting calibration samples Resolves an issue where the soft watchdog would trigger. * Fix missing word in comment, switch to constexpr * Update esp32/esp8266 --------- Co-authored-by: Gorbit99 <gorbitgames@gmail.com> Co-authored-by: nekomona <nekomona@nekomona.com> Co-authored-by: nekomona <nekomona@163.com> Co-authored-by: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Co-authored-by: kounocom <meia@kouno.xyz> Co-authored-by: Kubuxu <oss@kubuxu.com>

Issue:: SlimeVR#40 Discussion: SlimeVR/SlimeVR-Tracker-ESP#322 (comment) Discussion: SlimeVR/SlimeVR-Server#1100

Discussion: SlimeVR/SlimeVR-Tracker-ESP#322 (comment) Discussion: SlimeVR/SlimeVR-Server#1100

commit 56c3bbb Author: Timo Witte <timo.witte@gmail.com> Date: Sat Jun 29 00:26:42 2024 +0200 more change neutrality commit c8b2b17 Author: Timo Witte <timo.witte@gmail.com> Date: Sat Jun 29 00:24:28 2024 +0200 make it change neutral commit 9fa57ec Author: Timo Witte <timo.witte@gmail.com> Date: Sat Jun 29 00:23:47 2024 +0200 remove 2s wait in main.cpp it´s not required commit 69b07bc Author: Timo Witte <timo.witte@gmail.com> Date: Sat Jun 29 00:21:17 2024 +0200 remove change that does nothing commit f6cd9a8 Author: Timo Witte <timo.witte@gmail.com> Date: Sat Jun 29 00:19:27 2024 +0200 serial over USB commit 6d37c3d Author: Timo Witte <timo.witte@gmail.com> Date: Fri Jun 28 23:45:18 2024 +0200 update to latest tasmota tools for ESP32C6 commit 11c7a60 Merge: 230d98b fef504e Author: Spacefish <timo.witte@gmail.com> Date: Fri Jun 28 23:42:52 2024 +0200 Merge branch 'main' into fix-compiler-error commit 230d98b Author: Timo Witte <timo.witte@gmail.com> Date: Fri Jun 28 23:41:24 2024 +0200 add custom portmap for ESP32C6 commit 6914db2 Author: Timo Witte <timo.witte@gmail.com> Date: Fri Jun 28 19:47:07 2024 +0200 dont double scan i2c address on bus for ESP32C6 commit 233b256 Author: JovannMC <jovannmc@femboyfurry.net> Date: Tue Jun 25 14:36:31 2024 +0300 Add Haritora to consts (SlimeVR#333) Add haritora consts, fix misspelling commit 02bb9d3 Author: Przemyslaw Romaniak <loudpl@gmail.com> Date: Tue Jun 25 12:57:18 2024 +0200 SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations (SlimeVR#322) * Update readme to mention BMI270 support. * Soft fusion sensor initial code, wip * Soft fusion ICM-42688-P lazy WIP implementation. * sfusion: Cleanup, implemented sensor frequency calibration * icm42688: add more comments, basic driver (no hw filtering) should be working * sfustion: compilation fix * sfusion: start calibration when upside down * cleanup: remove confusing had data flag * sensor manager: use unique_ptr instead of raw pointers * sfusion: big refactoring wip * sfusion: make aux work, at least sfusion sensors should now be functional * sfusion: lightweight implementation of BMI270 sensor, no sensitivity cal yet * sfusion: BMI270: added CRT and gyro zx factor. should be functionally equivalent to the old driver * Added lsm6dsv * Trying to work around esp32c3 compilation problem, not liking that solution * sfusion: fix problems found after rebase * Update README.md * Bump Arduino core to 3.0 to match GCC12 * Remove fast pin swapping that is no longer compatible with arduino core v3 * Bring back fast pin swapping * Update platformio-tools.ini * Fix accel timescale (calibration no longer takes forever) * Fix non-sfusion sensors * Added LSM6DSO and DSR support and refactored DSV support * Removed template float param from the implementation * sfusion: port MPU6050 driver wip, not expecting to be functional yet * sfusion: add headers specifying main code owners * connection: fix warning * update README.md * fshelper: fixed ESP8266 regression caused by abstracting FS access * sfusion: fix error on merge * bno080: differentiate bno080, bno085, bno086 again * sfusion: final touches * restore hadData functionality, implementing it in every sensor, made configured flag bno-only * fix address supplement in non-sfusion sensors, do i2c bus reset for all sensors * sfusion: make MPU6050 driver use normal MPU6050 ImuID, change eatSamplesAndReturn function to take ms instead of seconds * sfusion: hotfix, don't apply sensorOffset, it's applied in sensor base * Log FIFO overruns on LSMs * Reset the soft watchdog while eating or collecting calibration samples Resolves an issue where the soft watchdog would trigger. * Fix missing word in comment, switch to constexpr * Update esp32/esp8266 --------- Co-authored-by: Gorbit99 <gorbitgames@gmail.com> Co-authored-by: nekomona <nekomona@nekomona.com> Co-authored-by: nekomona <nekomona@163.com> Co-authored-by: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Co-authored-by: kounocom <meia@kouno.xyz> Co-authored-by: Kubuxu <oss@kubuxu.com> commit e04d269 Author: Fredrik Hatletvedt <32248439+Pespiri@users.noreply.github.com> Date: Thu Jun 20 01:35:00 2024 +0200 add macro for calculating radians (SlimeVR#317) * feat: add macro for calculating radians * style: silence unused variable warning * remove unnecessary float cast in macro commit 2d2b21f Author: Timo Witte <timo.witte@gmail.com> Date: Sun May 26 20:05:48 2024 +0200 fix pre-processor warning commit 8cdd036 Author: Meia Kouno <71262281+kounocom@users.noreply.github.com> Date: Sun May 5 02:35:22 2024 +0300 Fix enabling motion bias estimation (SlimeVR#325) commit 572c9be Author: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Sat May 4 19:04:08 2024 +0200 fshelper: fixed ESP8266 regression caused by abstracting FS access SlimeVR#321 (SlimeVR#328) * fshelper: fixed ESP8266 regression caused by abstracting FS access SlimeVR#321 * Removing not needed ifdef l0ud spotted that this is not need. Co-Authored-By: Przemyslaw Romaniak <przemyslaw.romaniak@intel.com> --------- Co-authored-by: Przemyslaw Romaniak <przemyslaw.romaniak@intel.com> commit b70eb62 Author: Timo Witte <timo.witte@gmail.com> Date: Thu May 2 00:51:05 2024 +0200 ESP32C6 support commit 8b43c3d Author: Timo Witte <timo.witte@gmail.com> Date: Wed May 1 20:10:44 2024 +0200 Designate all initializer clauses to fix compiler errors with newer compilers

* Designate all initializer clauses to fix compiler errors with newer compilers * ESP32C6 support * fshelper: fixed ESP8266 regression caused by abstracting FS access #321 (#328) * fshelper: fixed ESP8266 regression caused by abstracting FS access #321 * Removing not needed ifdef l0ud spotted that this is not need. Co-Authored-By: Przemyslaw Romaniak <przemyslaw.romaniak@intel.com> --------- Co-authored-by: Przemyslaw Romaniak <przemyslaw.romaniak@intel.com> * Fix enabling motion bias estimation (#325) * fix pre-processor warning * add macro for calculating radians (#317) * feat: add macro for calculating radians * style: silence unused variable warning * remove unnecessary float cast in macro * SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations (#322) * Update readme to mention BMI270 support. * Soft fusion sensor initial code, wip * Soft fusion ICM-42688-P lazy WIP implementation. * sfusion: Cleanup, implemented sensor frequency calibration * icm42688: add more comments, basic driver (no hw filtering) should be working * sfustion: compilation fix * sfusion: start calibration when upside down * cleanup: remove confusing had data flag * sensor manager: use unique_ptr instead of raw pointers * sfusion: big refactoring wip * sfusion: make aux work, at least sfusion sensors should now be functional * sfusion: lightweight implementation of BMI270 sensor, no sensitivity cal yet * sfusion: BMI270: added CRT and gyro zx factor. should be functionally equivalent to the old driver * Added lsm6dsv * Trying to work around esp32c3 compilation problem, not liking that solution * sfusion: fix problems found after rebase * Update README.md * Bump Arduino core to 3.0 to match GCC12 * Remove fast pin swapping that is no longer compatible with arduino core v3 * Bring back fast pin swapping * Update platformio-tools.ini * Fix accel timescale (calibration no longer takes forever) * Fix non-sfusion sensors * Added LSM6DSO and DSR support and refactored DSV support * Removed template float param from the implementation * sfusion: port MPU6050 driver wip, not expecting to be functional yet * sfusion: add headers specifying main code owners * connection: fix warning * update README.md * fshelper: fixed ESP8266 regression caused by abstracting FS access * sfusion: fix error on merge * bno080: differentiate bno080, bno085, bno086 again * sfusion: final touches * restore hadData functionality, implementing it in every sensor, made configured flag bno-only * fix address supplement in non-sfusion sensors, do i2c bus reset for all sensors * sfusion: make MPU6050 driver use normal MPU6050 ImuID, change eatSamplesAndReturn function to take ms instead of seconds * sfusion: hotfix, don't apply sensorOffset, it's applied in sensor base * Log FIFO overruns on LSMs * Reset the soft watchdog while eating or collecting calibration samples Resolves an issue where the soft watchdog would trigger. * Fix missing word in comment, switch to constexpr * Update esp32/esp8266 --------- Co-authored-by: Gorbit99 <gorbitgames@gmail.com> Co-authored-by: nekomona <nekomona@nekomona.com> Co-authored-by: nekomona <nekomona@163.com> Co-authored-by: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Co-authored-by: kounocom <meia@kouno.xyz> Co-authored-by: Kubuxu <oss@kubuxu.com> * Add Haritora to consts (#333) Add haritora consts, fix misspelling * dont double scan i2c address on bus for ESP32C6 * add custom portmap for ESP32C6 * update to latest tasmota tools for ESP32C6 * serial over USB * remove change that does nothing * remove 2s wait in main.cpp it´s not required * make it change neutral * more change neutrality --------- Co-authored-by: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Co-authored-by: Przemyslaw Romaniak <przemyslaw.romaniak@intel.com> Co-authored-by: Meia Kouno <71262281+kounocom@users.noreply.github.com> Co-authored-by: Fredrik Hatletvedt <32248439+Pespiri@users.noreply.github.com> Co-authored-by: Przemyslaw Romaniak <loudpl@gmail.com> Co-authored-by: Gorbit99 <gorbitgames@gmail.com> Co-authored-by: nekomona <nekomona@nekomona.com> Co-authored-by: nekomona <nekomona@163.com> Co-authored-by: kounocom <meia@kouno.xyz> Co-authored-by: Kubuxu <oss@kubuxu.com> Co-authored-by: JovannMC <jovannmc@femboyfurry.net> Co-authored-by: Eiren Rain <Eirenliel@users.noreply.github.com>

l0ud and others added 30 commits March 13, 2024 00:27

Update readme to mention BMI270 support.

04b7208

Soft fusion sensor initial code, wip

60f6eef

Soft fusion ICM-42688-P lazy WIP implementation.

93bebd3

sfusion: Cleanup, implemented sensor frequency calibration

eb48391

icm42688: add more comments, basic driver (no hw filtering) should be…

9b5e4a7

… working

sfustion: compilation fix

8e47f34

sfusion: start calibration when upside down

961af2f

cleanup: remove confusing had data flag

73578f1

sensor manager: use unique_ptr instead of raw pointers

597f472

sfusion: big refactoring wip

4e24586

sfusion: make aux work, at least sfusion sensors should now be functi…

3fc522d

…onal

sfusion: lightweight implementation of BMI270 sensor, no sensitivity …

e9d27f2

…cal yet

sfusion: BMI270: added CRT and gyro zx factor. should be functionally…

b2497a1

… equivalent to the old driver

Added lsm6dsv

7e84c91

Trying to work around esp32c3 compilation problem, not liking that so…

e299c3c

…lution

sfusion: fix problems found after rebase

60eae4c

Update README.md

5083f90

Bump Arduino core to 3.0 to match GCC12

98f1cdf

Remove fast pin swapping that is no longer compatible with arduino co…

ee44f19

…re v3

Merge pull request #4 from nekomona/sfusion

7c8bab2

Bump Arduino Core to v3.0 to Match GCC12

Bring back fast pin swapping

ccf3c20

Merge pull request #5 from nekomona/sfusion-fastswap

8e40776

Bring back fast pin swapping

Update platformio-tools.ini

811e5d4

Fix accel timescale (calibration no longer takes forever)

37ca5d7

Merge pull request #7 from wigwagwent/lsm6dsv-sfusion-merge

28d6fc2

Fix accel timescale for LSM6DSV (calibration no longer takes forever)

Fix non-sfusion sensors

27be085

Added LSM6DSO and DSR support and refactored DSV support

cb48a2b

Removed template float param from the implementation

da8736f

sfusion: port MPU6050 driver wip, not expecting to be functional yet

239ee30

Merge remote-tracking branch 'tailsy/sfusion' into sfusion-lsm6dsx-dr…

a5a291c

…iver

Merge remote-tracking branch 'origin/main' into sfusion

10f0fb1

unlogisch04 mentioned this pull request May 4, 2024

Support LSM6DSV IMU #308

Closed

unlogisch04 added this to the 0.5.0 milestone May 4, 2024

unlogisch04 mentioned this pull request May 4, 2024

BMI160 sens calibration #281

Closed

unlogisch04 requested a review from TheDevMinerTV May 31, 2024 21:49

Kubuxu and others added 6 commits June 3, 2024 21:18

Log FIFO overruns on LSMs

ddaebfb

Reset the soft watchdog while eating or collecting calibration samples

701f63b

Resolves an issue where the soft watchdog would trigger.

Merge pull request #10 from Kubuxu/fix/swtd-calibration

22ae545

Reset the soft watchdog while eating or collecting calibration samples

Merge pull request #9 from Kubuxu/misc/lsm-overflow-warn

73d80b8

Log FIFO overruns on LSMs

Fix missing word in comment, switch to constexpr

6ac4e83

Merge pull request #11 from Kubuxu/misc/comment-cleanup

0111adf

Fix missing word in comment, switch to constexpr

unlogisch04 approved these changes Jun 19, 2024

View reviewed changes

l0ud and others added 3 commits June 21, 2024 20:31

Merge branch 'main' into sfusion

316b223

Update esp32/esp8266

0b7afd1

Merge pull request #12 from unlogisch04/l0ud_fixforpr

a9b1c74

Change add BNO085 to exeption; update esp32/esp8266

l0ud requested a review from Eirenliel June 22, 2024 09:59

unlogisch04 approved these changes Jun 22, 2024

View reviewed changes

ImUrX approved these changes Jun 25, 2024

View reviewed changes

Eirenliel approved these changes Jun 25, 2024

View reviewed changes

Eirenliel merged commit ea00beb into SlimeVR:main Jun 25, 2024
1 check passed

kounocom mentioned this pull request Jun 25, 2024

Remove warning about custom firmware for BMI270 SlimeVR/SlimeVR-Docs-Site#287

Merged

This was referenced Jul 6, 2024

Updated IMU Constants SlimeVR/SlimeVR-Server#1100

Merged

Not a IMU SlimeVR/slimevr-node#40

Closed

ShineBrightMeow added a commit to Shine-Bright-Meow/slimevr-node that referenced this pull request Jul 6, 2024

MPU6050_SF not an actual iMU

b438810

Issue:: SlimeVR#40 Discussion: SlimeVR/SlimeVR-Tracker-ESP#322 (comment) Discussion: SlimeVR/SlimeVR-Server#1100

ShineBrightMeow mentioned this pull request Jul 6, 2024

MPU6050_SF is not an actual IMU SlimeVR/slimevr-node#41

Merged

ShineBrightMeow added a commit to Shine-Bright-Meow/SolarXR-Protocol that referenced this pull request Jul 6, 2024

MPU6050_SF is not an actual IMU

2aa122d

Discussion: SlimeVR/SlimeVR-Tracker-ESP#322 (comment) Discussion: SlimeVR/SlimeVR-Server#1100

Eirenliel mentioned this pull request Dec 20, 2024

Reverse IMU I2C address from supplement to full #372

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations #322

SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations #322

l0ud commented Mar 29, 2024 •

edited

Loading

ImUrX left a comment

SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations #322

SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations #322

Conversation

l0ud commented Mar 29, 2024 • edited Loading

ImUrX left a comment

Choose a reason for hiding this comment

l0ud commented Mar 29, 2024 •

edited

Loading