Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dynamic Sfusion #363

Closed
wants to merge 105 commits into from
Closed
Changes from 1 commit
Commits
Show all changes
105 commits
Select commit Hold shift + click to select a range
5cc67f2
sfusion: Use MBE
kounocom Jul 14, 2024
9c3bb73
sfusion: Tune MBE parameters
kounocom Jul 14, 2024
bfa3df9
sfusion: Unify full-scale ranges for all sensors (1000DPS/4G)
kounocom May 29, 2024
d383d45
sfusion: Perform custom motionless calibration before gyro offset
kounocom Jul 14, 2024
6b53da1
LSM6DSV: Increase accel ODR to 240Hz
kounocom Aug 21, 2024
9f52744
LSM6DSO: Increase accel ODR to 208Hz
kounocom Aug 21, 2024
ea1dfbd
BMI270: Increase accel ODR to 200Hz
kounocom Aug 21, 2024
1e0fc4a
sfusion: Adjust VQF params
kounocom Aug 21, 2024
04895aa
sfusion: Disable calibration entirely
kounocom Aug 21, 2024
9275f62
Skip loading sensor calibration
gorbit99 Aug 25, 2024
5c26f04
Put temperature into the FIFO wherever possible
gorbit99 Sep 11, 2024
28f37ef
Average temperatures and calculate change from that
gorbit99 Sep 11, 2024
52c84cc
Make biasForgettingTime temperature dependant
gorbit99 Sep 11, 2024
52ab595
Moved over some lines I missed
gorbit99 Sep 11, 2024
c627d0f
Decouple direct temperature sampling and sending
gorbit99 Sep 11, 2024
4171284
Add temperature stability constants for LSMs
kounocom Sep 11, 2024
4d992d6
Increase trust for MBE
kounocom Sep 11, 2024
e0c219b
Added per sensor VQF params
gorbit99 Sep 11, 2024
5da7022
Merge branch 'random-dev-branch-do-not-use' of https://github.com/kou…
gorbit99 Sep 11, 2024
aeb0291
There's definitely a better way to do this
kounocom Sep 12, 2024
f9caa07
Fix crash
gorbit99 Sep 12, 2024
7cfaa3e
Merge branch 'random-dev-branch-do-not-use' of https://github.com/kou…
gorbit99 Sep 12, 2024
4caaec7
Fix shaky tracker issue
gorbit99 Sep 12, 2024
4b4e055
Added non-blocking calibration
gorbit99 Sep 14, 2024
dcea19b
Don't use non-blocking calibration without USE_NONBLOCKING_CALIBRATION
gorbit99 Sep 14, 2024
a6ea9c0
Default to non-blocking calibration
kounocom Sep 16, 2024
01e97b1
Fifo overrun detection for bmi270
gorbit99 Sep 16, 2024
27b28d5
Fixed oddities during calibration
gorbit99 Sep 16, 2024
e80aaed
Fixed calibrations being calculated improperly
gorbit99 Sep 16, 2024
35344c2
Add the ability to calibrate all accel axes
gorbit99 Sep 16, 2024
7eda85a
Skip accel until next rest
gorbit99 Sep 16, 2024
fab0e3a
Calibrate gyro at 2 temperatures
gorbit99 Sep 16, 2024
7755b56
Lerp gyro offset between temperatures
gorbit99 Sep 16, 2024
5042735
Calibrate ZRO change rate
gorbit99 Sep 16, 2024
c14a0c6
make it 10x worse i guess
kounocom Sep 17, 2024
9b24282
Update more vqf params
kounocom Sep 17, 2024
498db44
Only print the calibration that was performed
gorbit99 Sep 17, 2024
60212a2
Blink LED when calibration step completed
gorbit99 Sep 17, 2024
c23ce1e
Merge branch 'random-dev-branch-do-not-use' of https://github.com/kou…
gorbit99 Sep 17, 2024
63bfa55
use logger
kounocom Sep 17, 2024
0040998
don't log that
kounocom Sep 17, 2024
68695a1
Fix BMI270 temperature reading
gorbit99 Sep 17, 2024
f7391b7
overclock 8266 by default
kounocom Sep 17, 2024
1b40787
Merge branch 'random-dev-branch-do-not-use' of https://github.com/kou…
kounocom Sep 17, 2024
c004563
reset defines
kounocom Sep 17, 2024
174e484
Implement getActiveSensorCount
gorbit99 Sep 17, 2024
c343959
Merge branch 'random-dev-branch-do-not-use' of https://github.com/kou…
gorbit99 Sep 17, 2024
cae480c
Merge branch 'random-dev-branch-do-not-use' of https://github.com/kou…
kounocom Sep 17, 2024
3fcc5d3
Cap TPS when esp8266 & aux
kounocom Sep 17, 2024
f18dfa8
overclock in platformio-tools
kounocom Sep 17, 2024
7bb5c36
Automatically expand the gyro calibration
gorbit99 Sep 18, 2024
4f026d2
Separate LSM6DS3 and LSM6DS3TR-C
kounocom Sep 18, 2024
dacbf76
Unify IMU params further
kounocom Sep 18, 2024
fc55aad
Merge branch 'dynamic-sfusion' of https://github.com/kounocom/SlimeVR…
kounocom Sep 18, 2024
54dc7a8
Refactor nonblocking calibration code
gorbit99 Sep 18, 2024
0dbd452
Merge branch 'dynamic-sfusion' of https://github.com/kounocom/SlimeVR…
gorbit99 Sep 18, 2024
8a63326
In theory this works?
gorbit99 Sep 25, 2024
9c2055c
Actually allow the calibration to be cancelled
gorbit99 Sep 29, 2024
8500fee
Add delay into calibration steps
gorbit99 Sep 29, 2024
9798738
Make rest detection more sensitive
kounocom Sep 29, 2024
f219cc3
Always limit TPS with aux (now to 90)
kounocom Sep 29, 2024
db71a05
change to 4g, fill in missing stuff elsewhere
kounocom Sep 30, 2024
44e2aaf
This works better
kounocom Oct 1, 2024
eec8ac6
Actually enable FIFO this time
gorbit99 Oct 1, 2024
bc65001
Use clock
kounocom Oct 1, 2024
1aae55a
Use 8KB FIFO
kounocom Oct 2, 2024
73faa41
Sleep more
kounocom Oct 2, 2024
6515014
Hires mode maybe?
gorbit99 Oct 2, 2024
b84096c
Merge branch 'icm45686-firmware' of https://github.com/gorbit99/Slime…
gorbit99 Oct 2, 2024
6eb1ef9
Use correct sensitivity
kounocom Oct 2, 2024
6f74905
Merge branch 'icm45686-firmware' into dynamic-sfusion
gorbit99 Oct 2, 2024
099fd9a
Stuffs for dynamic
kounocom Oct 2, 2024
1e47ff7
Increase sensitivity when gyro is calibrated + skip accel calibration…
gorbit99 Oct 8, 2024
ea5ea1a
Merge branch 'dynamic-sfusion' of https://github.com/kounocom/SlimeVR…
gorbit99 Oct 8, 2024
5222085
ACTUALLY skip accel calibration
gorbit99 Oct 8, 2024
3d9ad76
Zero out accel calibration
gorbit99 Oct 8, 2024
afb4d32
Whoops
gorbit99 Oct 8, 2024
2f2c248
Adjust VQF params more
kounocom Oct 11, 2024
1faff26
...and some more
kounocom Oct 11, 2024
4950a17
...and then make them actually work per-sensor
kounocom Oct 11, 2024
2ef6306
...and then light the LED when rest is detected, as a debug step
kounocom Oct 11, 2024
f1ac5aa
...and finally, change whatever these do
kounocom Oct 11, 2024
fede1e3
Merge commit 'b278bcfcf4d5bcc4a71d25d4485510c11bd8d530' into dynamic-…
gorbit99 Nov 12, 2024
5457a0d
Merge commit '2946a6a7a69ce2b0052cc5ba44ba020429bce2f3' into dynamic-…
gorbit99 Nov 12, 2024
325a5ce
Merge commit '85dead25f339bc3a6d79a147b6ca0e9e55741f1d' into dynamic-…
gorbit99 Nov 12, 2024
9ab9ee7
Merge commit 'a9f5b1ae8c65e13856d14dacdef5dc341e42f7b5' into dynamic-…
gorbit99 Nov 12, 2024
56a3507
Merge commit 'a4a9778f62d0b962c46898d03e9ce70171a1ca12' into dynamic-…
gorbit99 Nov 12, 2024
35cc25b
Merge commit '50fa801653022f5b2604cfdb06915b6211b28f43' into dynamic-…
gorbit99 Nov 12, 2024
ff5836c
Merge commit '0b882db74f6565fc8b63f6ae638f951dec4441d4' into dynamic-…
gorbit99 Nov 12, 2024
14e3891
Merge commit '628fe209609956c8d5ba8af83cb54d52fdf7e71b' into dynamic-…
gorbit99 Nov 12, 2024
9d96d19
ICM45 implementation
gorbit99 Nov 12, 2024
4ba3b5b
Make sure it builds even without nonblocking calibration
gorbit99 Nov 12, 2024
69951cb
Fix clangformat errors
gorbit99 Nov 12, 2024
ca68392
Apply suggestions from code review
gorbit99 Nov 12, 2024
c7fb10c
define USE_NONBLOCKING_CALIBRATION as true instead
gorbit99 Nov 12, 2024
4ff6e1b
Increase clang format version to 18
gorbit99 Nov 12, 2024
b9aad36
Apply formatting
gorbit99 Nov 12, 2024
7a8d980
Apply even more formatting
gorbit99 Nov 13, 2024
e20c0d2
Even more linting
gorbit99 Nov 13, 2024
49ef1ef
Linting test
gorbit99 Nov 13, 2024
75fc842
Base formatting
gorbit99 Nov 13, 2024
b7e05ca
Manually clear up issues
gorbit99 Nov 13, 2024
8a10e2b
Remove unnecessary change to includes
gorbit99 Nov 13, 2024
f388d8f
fuck
kounocom Dec 11, 2024
3e6ef0d
Make ICM45 rest detection more lenient
kounocom Jan 1, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
ICM45 implementation
gorbit99 committed Nov 12, 2024
commit 9d96d196849cdab7c64cc62f8ab02e3460df2f81
1 change: 0 additions & 1 deletion platformio.ini
Original file line number Diff line number Diff line change
@@ -66,7 +66,6 @@ board = esp12e
; Comment out this line below if you have any trouble uploading the firmware
; and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed
upload_speed = 921600
board_build.f_cpu = 160000000L

; Uncomment below if you want to build for ESP-01
;[env:esp01_1m]
2 changes: 2 additions & 0 deletions src/consts.h
Original file line number Diff line number Diff line change
@@ -44,6 +44,7 @@ enum class ImuID {
LSM6DSR,
LSM6DS3,
ICM45686,
ICM45605,
Empty = 255
};

@@ -66,6 +67,7 @@ enum class ImuID {
#define IMU_MPU6050_SF SoftFusionMPU6050
#define IMU_LSM6DS3 SoftFusionLSM6DS3
#define IMU_ICM45686 SoftFusionICM45686
#define IMU_ICM45605 SoftFusionICM45605

#define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware

204 changes: 185 additions & 19 deletions src/defines.h
Original file line number Diff line number Diff line change
@@ -20,45 +20,211 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
// ================================================
// See docs for configuration options and examples:
// https://docs.slimevr.dev/firmware/configuring-project.html#2-configuring-definesh
// ================================================

#define IMU IMU_LSM6DSV
#define BOARD BOARD_CUSTOM
#define IMU_ROTATION DEG_90
// Set parameters of IMU and board used
#define IMU IMU_BNO085
#define SECOND_IMU IMU
#define BOARD BOARD_SLIMEVR
#define IMU_ROTATION DEG_270
#define SECOND_IMU_ROTATION DEG_270

#define PRIMARY_IMU_OPTIONAL false
#define SECONDARY_IMU_OPTIONAL true

#define MAX_IMU_COUNT 1
#define MAX_IMU_COUNT 2

#define ON_OFF_BUTTON 1
// Axis mapping example
/*
#include "sensors/axisremap.h"
#define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN,
AXIS_REMAP_USE_Z, \ AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)

IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL,
PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
*/

#ifndef IMU_DESC_LIST
#define IMU_DESC_LIST \
IMU_DESC_ENTRY( \
IMU, \
PRIMARY_IMU_ADDRESS_ONE, \
IMU_ROTATION, \
PIN_IMU_SCL, \
PIN_IMU_SDA, \
PRIMARY_IMU_OPTIONAL, \
PIN_IMU_INT \
#define IMU_DESC_LIST \
IMU_DESC_ENTRY( \
IMU, \
PRIMARY_IMU_ADDRESS_ONE, \
IMU_ROTATION, \
PIN_IMU_SCL, \
PIN_IMU_SDA, \
PRIMARY_IMU_OPTIONAL, \
PIN_IMU_INT \
) \
IMU_DESC_ENTRY( \
SECOND_IMU, \
SECONDARY_IMU_ADDRESS_TWO, \
SECOND_IMU_ROTATION, \
PIN_IMU_SCL, \
PIN_IMU_SDA, \
SECONDARY_IMU_OPTIONAL, \
PIN_IMU_INT_2 \
)
#endif

// Battery monitoring options (comment to disable):
// BAT_EXTERNAL for ADC pin,
// BAT_INTERNAL for internal - can detect only low battery,
// BAT_MCP3021 for external ADC connected over I2C
#define BATTERY_MONITOR BAT_EXTERNAL

#define PIN_IMU_SDA 5
#define PIN_IMU_SCL 6
// BAT_EXTERNAL definition override
// D1 Mini boards with ESP8266 have internal resistors. For these boards you only have
// to adjust BATTERY_SHIELD_RESISTANCE. For other boards you can now adjust the other
// resistor values. The diagram looks like this:
// (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2]
// ---(ESP32_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
// #define BATTERY_SHIELD_RESISTANCE 180 //130k BatteryShield, 180k SlimeVR or fill in
// external resistor value in kOhm #define BATTERY_SHIELD_R1 100 // Board voltage
// divider resistor Ain to GND in kOhm #define BATTERY_SHIELD_R2 220 // Board voltage
// divider resistor Ain to INPUT_BOARD in kOhm

// LED configuration:
// Configuration Priority 1 = Highest:
// 1. LED_PIN
// 2. LED_BUILTIN
//
// LED_PIN
// - Number or Symbol (D1,..) of the Output
// - To turn off the LED, set LED_PIN to LED_OFF
// LED_INVERTED
// - false for output 3.3V on high
// - true for pull down to GND on high

// Board-specific configurations
#if BOARD == BOARD_SLIMEVR
#define PIN_IMU_SDA 14
#define PIN_IMU_SCL 12
#define PIN_IMU_INT 16
#define PIN_IMU_INT_2 13
#define PIN_BATTERY_LEVEL 17
#define LED_PIN 2
#define LED_INVERTED true
#ifndef BATTERY_SHIELD_RESISTANCE
#define BATTERY_SHIELD_RESISTANCE 0
#endif
#ifndef BATTERY_SHIELD_R1
#define BATTERY_SHIELD_R1 10
#endif
#ifndef BATTERY_SHIELD_R2
#define BATTERY_SHIELD_R2 40.2
#endif
#elif BOARD == BOARD_SLIMEVR_LEGACY || BOARD == BOARD_SLIMEVR_DEV
#define PIN_IMU_SDA 4
#define PIN_IMU_SCL 5
#define PIN_IMU_INT 10
#define PIN_IMU_INT_2 13
#define PIN_BATTERY_LEVEL 17
#define LED_PIN 2
#define LED_INVERTED true
#ifndef BATTERY_SHIELD_RESISTANCE
#define BATTERY_SHIELD_RESISTANCE 0
#endif
#ifndef BATTERY_SHIELD_R1
#define BATTERY_SHIELD_R1 10
#endif
#ifndef BATTERY_SHIELD_R2
#define BATTERY_SHIELD_R2 40.2
#endif
#elif BOARD == BOARD_NODEMCU || BOARD == BOARD_WEMOSD1MINI
#define PIN_IMU_SDA D2
#define PIN_IMU_SCL D1
#define PIN_IMU_INT D5
#define PIN_IMU_INT_2 D6
#define PIN_BATTERY_LEVEL A0
// #define LED_PIN 2
// #define LED_INVERTED true
#ifndef BATTERY_SHIELD_RESISTANCE
#define BATTERY_SHIELD_RESISTANCE 180
#endif
#ifndef BATTERY_SHIELD_R1
#define BATTERY_SHIELD_R1 100
#endif
#ifndef BATTERY_SHIELD_R2
#define BATTERY_SHIELD_R2 220
#endif
#elif BOARD == BOARD_ESP01
#define PIN_IMU_SDA 2
#define PIN_IMU_SCL 0
#define PIN_IMU_INT 255
#define PIN_IMU_INT_2 255
#define PIN_BATTERY_LEVEL 255
#define LED_PIN LED_OFF
#define LED_INVERTED false
#elif BOARD == BOARD_TTGO_TBASE
#define PIN_IMU_SDA 5
#define PIN_IMU_SCL 4
#define PIN_IMU_INT 14
#define PIN_IMU_INT_2 13
#define PIN_BATTERY_LEVEL A0
// #define LED_PIN 2
// #define LED_INVERTED false
#elif BOARD == BOARD_CUSTOM
// Define pins by the examples above
#elif BOARD == BOARD_WROOM32
#define PIN_IMU_SDA 21
#define PIN_IMU_SCL 22
#define PIN_IMU_INT 23
#define PIN_IMU_INT_2 25
#define PIN_BATTERY_LEVEL 36
// #define LED_PIN 2
// #define LED_INVERTED false
#elif BOARD == BOARD_LOLIN_C3_MINI
#define PIN_IMU_SDA 5
#define PIN_IMU_SCL 4
#define PIN_IMU_INT 6
#define PIN_IMU_INT_2 8
#define PIN_BATTERY_LEVEL 3
#define LED_PIN 0
#define LED_PIN 7
// #define LED_INVERTED false
#elif BOARD == BOARD_BEETLE32C3
#define PIN_IMU_SDA 8
#define PIN_IMU_SCL 9
#define PIN_IMU_INT 6
#define PIN_IMU_INT_2 7
#define PIN_BATTERY_LEVEL 3
#define LED_PIN 10
#define LED_INVERTED false
#elif BOARD == BOARD_ES32C3DEVKITM1
#define PIN_IMU_SDA 5
#define PIN_IMU_SCL 4
#define PIN_IMU_INT 6
#define PIN_IMU_INT_2 7
#define PIN_BATTERY_LEVEL 3
#define LED_PIN \
LED_OFF // RGB LED Protocol would need to be implementetet did not brother for the
// test, because the board ideal for tracker ifself
// #define LED_INVERTED false
#elif BOARD == BOARD_WEMOSWROOM02
#define PIN_IMU_SDA 2
#define PIN_IMU_SCL 14
#define PIN_IMU_INT 0
#define PIN_IMU_INT_2 4
#define PIN_BATTERY_LEVEL A0
#define LED_PIN 16
#define LED_INVERTED true
#elif BOARD == BOARD_XIAO_ESP32C3
#define PIN_IMU_SDA 6 // D4
#define PIN_IMU_SCL 7 // D5
#define PIN_IMU_INT 5 // D3
#define PIN_IMU_INT_2 10 // D10
#define LED_PIN 4 // D2
#define LED_INVERTED false
#define PIN_BATTERY_LEVEL 2 // D0 / A0
#ifndef BATTERY_SHIELD_RESISTANCE
#define BATTERY_SHIELD_RESISTANCE 0
#endif
#ifndef BATTERY_SHIELD_R1
#define BATTERY_SHIELD_R1 150
#define BATTERY_SHIELD_R1 100
#endif
#ifndef BATTERY_SHIELD_R2
#define BATTERY_SHIELD_R2 150
#define BATTERY_SHIELD_R2 100
#endif
#endif
17 changes: 9 additions & 8 deletions src/network/wifihandler.cpp
Original file line number Diff line number Diff line change
@@ -150,7 +150,8 @@ void WiFiNetwork::upkeep() {
return;
case SLIME_WIFI_SAVED_ATTEMPT: // Couldn't connect with first set of
// credentials
#if ESP8266 Try again but with 11G
#if ESP8266
// Try again but with 11G
// But only if there are credentials, otherwise we just waste time
// before switching to hardcoded credentials.
if (WiFi.SSID().length() > 0) {
@@ -178,8 +179,8 @@ void WiFiNetwork::upkeep() {
return;
case SLIME_WIFI_SAVED_G_ATTEMPT: // Couldn't connect with first set of
// credentials with PHY Mode G
#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) Try
// hardcoded credentials now
#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD)
// Try hardcoded credentials now
#if ESP8266
#if USE_ATTENUATION
WiFi.setOutputPower(20.0 - ATTENUATION_N);
@@ -199,9 +200,9 @@ void WiFiNetwork::upkeep() {
return;
case SLIME_WIFI_HARDCODE_ATTEMPT: // Couldn't connect with second set
// of credentials
#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266 Try hardcoded
// credentials again, but with PHY
// Mode G
#if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) && ESP8266
// Try hardcoded credentials again, but with PHY
// Mode G
#if USE_ATTENUATION
WiFi.setOutputPower(20.0 - ATTENUATION_G);
#endif
@@ -221,8 +222,8 @@ void WiFiNetwork::upkeep() {
return;
case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with
// server-sent credentials.
#if ESP8266 Try
// again silently but with 11G
#if ESP8266
// Try again silently but with 11G
#if USE_ATTENUATION
WiFi.setOutputPower(20.0 - ATTENUATION_G);
#endif
3 changes: 3 additions & 0 deletions src/sensors/SensorManager.cpp
Original file line number Diff line number Diff line change
@@ -32,6 +32,7 @@
#include "sensoraddresses.h"
#include "softfusion/drivers/bmi270.h"
#include "softfusion/drivers/icm42688.h"
#include "softfusion/drivers/icm45605.h"
#include "softfusion/drivers/icm45686.h"
#include "softfusion/drivers/lsm6ds3.h"
#include "softfusion/drivers/lsm6ds3trc.h"
@@ -66,6 +67,8 @@ using SoftFusionLSM6DS3
= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3, SoftFusion::I2CImpl>;
using SoftFusionICM45686
= SoftFusionSensor<SoftFusion::Drivers::ICM45686, SoftFusion::I2CImpl>;
using SoftFusionICM45605
= SoftFusionSensor<SoftFusion::Drivers::ICM45605, SoftFusion::I2CImpl>;

// TODO Make it more generic in the future and move another place (abstract sensor
// interface)
2 changes: 2 additions & 0 deletions src/sensors/sensor.cpp
Original file line number Diff line number Diff line change
@@ -132,6 +132,8 @@ const char* getIMUNameByType(ImuID imuType) {
return "LSM6DS3";
case ImuID::ICM45686:
return "ICM45686";
case ImuID::ICM45605:
return "ICM45605";
case ImuID::Unknown:
case ImuID::Empty:
return "UNKNOWN";
35 changes: 27 additions & 8 deletions src/sensors/softfusion/drivers/icm42688.h
Original file line number Diff line number Diff line change
@@ -49,6 +49,8 @@ struct ICM42688 {
static constexpr float GyroSensitivity = 32.8f;
static constexpr float AccelSensitivity = 8192.0f;

static constexpr bool Uses32BitSensorData = true;

static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 2.07f;

@@ -99,8 +101,8 @@ struct ICM42688 {
struct FifoConfig1 {
static constexpr uint8_t reg = 0x5f;
static constexpr uint8_t value
= 0b1 | (0b1 << 1)
| (0b1 << 2); // fifo accel en=1, gyro=1, temp=0 todo: fsync, hires
= 0b1 | (0b1 << 1) | (0b0 << 2)
| (0b0 << 4); // fifo accel en=1, gyro=1, temp=0, hires=1
};
struct GyroConfig {
static constexpr uint8_t reg = 0x4f;
@@ -132,15 +134,18 @@ struct ICM42688 {
struct {
int16_t accel[3];
int16_t gyro[3];
uint8_t temp;
uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned
uint16_t temp;
uint16_t timestamp;
uint8_t xlsb;
uint8_t ylsb;
uint8_t zlsb;
} part;
uint8_t raw[15];
uint8_t raw[19];
};
};
#pragma pack(pop)

static constexpr size_t FullFifoEntrySize = 16;
static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;

bool initialize() {
// perform initialization step
@@ -180,10 +185,24 @@ struct ICM42688 {
&read_buffer[i + 0x1],
sizeof(FifoEntryAligned)
); // skip fifo header
processGyroSample(entry.part.gyro, GyrTs);

const int32_t gyroData[3]{
static_cast<int32_t>(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf),
static_cast<int32_t>(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf),
static_cast<int32_t>(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf),
};
processGyroSample(gyroData, GyrTs);

if (entry.part.accel[0] != -32768) {
processAccelSample(entry.part.accel, AccTs);
const int32_t accelData[3]{
static_cast<int32_t>(entry.part.accel[0]) << 4
| (static_cast<int32_t>(entry.part.xlsb) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[1]) << 4
| (static_cast<int32_t>(entry.part.ylsb) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[2]) << 4
| (static_cast<int32_t>(entry.part.zlsb) & 0xf0 >> 4),
};
processAccelSample(accelData, AccTs);
}

processTemperatureSample(static_cast<int16_t>(entry.part.temp), TempTs);
Loading