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.

Sign up for GitHub

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

fix Imu icm42688 #290

Merged
merged 8 commits into from
Sep 22, 2023
Merged

fix Imu icm42688 #290

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
d516bbf
add icm only
sctanf May 13, 2023
18da4d3
add mmc, cleanup
sctanf May 14, 2023
3375fb3
todo, mag axes
sctanf May 14, 2023
99b3c05
make it work
sctanf May 15, 2023
79e7486
Merge branch 'main' into imu-icm-mmc
sctanf Sep 22, 2023
edf9ff9
Update icm42688sensor.h
sctanf Sep 22, 2023
7e9947f
Merge branch 'main' into imu-icm-mmc
sctanf Sep 22, 2023
1f13fcd
more fixes, use vqf maybe
sctanf Sep 22, 2023
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
34 changes: 15 additions & 19 deletions src/sensors/icm42688sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
*/

#include "icm42688sensor.h"
#include "network/network.h"
#include "globals.h"
#include "helper_3dmath.h"
#include <i2cscan.h>
Expand All @@ -44,7 +43,7 @@ void ICM42688Sensor::motionSetup() {

m_Logger.info("Connected to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr);

if (I2CSCAN::isI2CExist(addr_mag)) {
if (I2CSCAN::hasDevOnBus(addr_mag)) {
I2Cdev::readByte(addr_mag, MMC5983MA_PRODUCT_ID, &temp);
if(!(temp == 0x30)) {
m_Logger.fatal("Can't connect to MMC5983MA (reported device ID 0x%02x) at address 0x%02x", temp, addr_mag);
Expand All @@ -54,10 +53,11 @@ void ICM42688Sensor::motionSetup() {
m_Logger.info("Connected to MMC5983MA (reported device ID 0x%02x) at address 0x%02x", temp, addr_mag);
magExists = true;
}
} else {
m_Logger.info("Magnetometer unavailable!");
magExists = false;
}

deltat = 1.0 / 1000.0; // gyro fifo 1khz

if (magExists) {
I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_1, 0x80); // Reset MMC now
}
Expand Down Expand Up @@ -136,7 +136,6 @@ void ICM42688Sensor::motionLoop() {
count += 32; // Add a few read buffer packets (4 ms)
uint16_t packets = count / 8; // Packet size 8 bytes
uint8_t rawData[2080];
uint16_t stco = 0;
I2Cdev::readBytes(addr, ICM42688_FIFO_DATA, count, &rawData[0]); // Read buffer

accel_read();
Expand Down Expand Up @@ -164,22 +163,20 @@ void ICM42688Sensor::motionLoop() {
Gxyz[2] = raw2 * gscale; //gres
parseGyroData();

// TODO: mag axes will be different, make sure to change them here
#if defined(_MAHONY_H_)
mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);
#elif defined(_MADGWICK_H_)
madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);
#endif
// TODO: mag axes will be different, make sure to change them???
sfusion.updateGyro(Gxyz);
}

quaternion.set(-q[2], q[1], q[3], q[0]);

quaternion *= sensorOffset;
sfusion.updateAcc(Axyz);

if(!lastQuatSent.equalsWithEpsilon(quaternion)) {
newData = true;
lastQuatSent = quaternion;
}
if (magExists)
sfusion.updateMag(Mxyz);

fusedRotation = sfusion.getQuaternionQuat();
fusedRotation *= sensorOffset;
acceleration = sfusion.getLinearAccVec();
setFusedRotationReady();
setAccelerationReady();
}

void ICM42688Sensor::accel_read() {
Expand Down Expand Up @@ -310,7 +307,6 @@ void ICM42688Sensor::startCalibration(int calibrationType) {
configuration.save();

ledManager.off();
Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0);
m_Logger.debug("Saved the calibration data");

m_Logger.info("Calibration data gathered");
Expand Down
12 changes: 7 additions & 5 deletions src/sensors/icm42688sensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,14 @@
#include <MMC5983MA.h>
#include "I2Cdev.h"

#include "SensorFusion.h"

class ICM42688Sensor : public Sensor
{
public:
ICM42688Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation){};
ICM42688Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin)
: Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation, sclPin, sdaPin),
sfusion(0.001f, 0.01f, 0.01f){};
~ICM42688Sensor(){};
void motionSetup() override final;
void motionLoop() override final;
Expand All @@ -45,13 +49,11 @@ class ICM42688Sensor : public Sensor
bool magExists = false;

// raw data and scaled as vector
float q[4]{1.0f, 0.0f, 0.0f, 0.0f}; // for raw filter
float Axyz[3]{};
float Gxyz[3]{};
float Mxyz[3]{};
Quat correction{0, 0, 0, 0};
// Loop timing globals
float deltat = 0; // sample time in seconds

SlimeVR::Sensors::SensorFusion sfusion;

SlimeVR::Configuration::ICM42688CalibrationConfig m_Calibration;

Expand Down
Loading