github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-06 02:01:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Icm20948 magnetometer fix (#202)

Browse Source
This commit is contained in:
unlogisch04
2022-10-27 15:22:11 +02:00
committed by GitHub
parent 418a33a1fc
commit 77cb8ac489
3 changed files with 68 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/sensors/bmi160sensor.cpp
View File

@@ -134,12 +134,12 @@ void BMI160Sensor::motionLoop() {
mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat * 1.0e-6f);
quaternion.set(-q[2], q[1], q[3], q[0]);
quaternion *= sensorOffset;
#if SEND_ACCELERATION
{
this->acceleration[0] = Axyz[0];
this->acceleration[1] = Axyz[1];
// Use the same mapping as in quaternion.set(-q[2], q[1], q[3], q[0]);
this->acceleration[0] = -Axyz[1];
this->acceleration[1] = Axyz[0];
this->acceleration[2] = Axyz[2];
// get the component of the acceleration that is gravity
@@ -160,6 +160,8 @@ void BMI160Sensor::motionLoop() {
}
#endif
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);

88
src/sensors/icm20948sensor.cpp
View File

@@ -152,6 +152,61 @@ void ICM20948Sensor::load_bias() {
#endif
}
void ICM20948Sensor::calculateAcceleration(Quat *quaternion) {
#if SEND_ACCELERATION
{
/*
Quat quat_test{};
quat_test.w = quaternion->w;
quat_test.x = -quaternion->x;
quat_test.y = quaternion->y;
quat_test.z = -quaternion->z;
//{Quat(Vector3(0, 0, 1), rotation)}
*/
this->acceleration[0] = (float)this->dmpData.Raw_Accel.Data.X;
this->acceleration[1] = (float)this->dmpData.Raw_Accel.Data.Y;
this->acceleration[2] = (float)this->dmpData.Raw_Accel.Data.Z;
// get the component of the acceleration that is gravity
float gravity[3];
// gravity[0] = 2 * (quat_test.x * quat_test.z - quat_test.w * quat_test.y);
gravity[0] = 2 * ((-quaternion->x) * (-quaternion->z) - quaternion->w * quaternion->y);
// gravity[1] = -2 * (quat_test.w * quat_test.x + quat_test.y * quat_test.z);
gravity[1] = -2 * (quaternion->w * (-quaternion->x) + quaternion->y * (-quaternion->z));
// gravity[2] = quat_test.w * quat_test.w - quat_test.x * quat_test.x - quat_test.y * quat_test.y + quat_test.z * quat_test.z;
gravity[2] = quaternion->w * quaternion->w - quaternion->x * quaternion->x - quaternion->y * quaternion->y + quaternion->z * quaternion->z;
/*
m_Logger.debug("Gravity x:%+0.3f y:%+0.3f z:%+0.3f, Accel x:%+6.0f y:%+6.0f z:%+6.0f, quat_test x:%+0.3f y:%+0.3f z:%+0.3f",
gravity[0], gravity[1], gravity[2],
this->acceleration[0], this->acceleration[1], this->acceleration[2],
quat_test.x, quat_test.y, quat_test.z);
*/
/*
m_Logger.debug("Gravity x:%+0.3f y:%+0.3f z:%+0.3f, Accel x:%+7.1f y:%+7.1f z:%+7.1f",
gravity[0], gravity[1], gravity[2],
this->acceleration[0], this->acceleration[1], this->acceleration[2]);
*/
// subtract gravity from the acceleration vector
this->acceleration[0] -= gravity[0] * ACCEL_SENSITIVITY_4G;
this->acceleration[1] -= gravity[1] * ACCEL_SENSITIVITY_4G;
this->acceleration[2] -= gravity[2] * ACCEL_SENSITIVITY_4G;
// finally scale the acceleration values to mps2
this->acceleration[0] *= ASCALE_4G;
this->acceleration[1] *= ASCALE_4G;
this->acceleration[2] *= ASCALE_4G;
/*
imu.getAGMT();
this->acceleration[0] = imu.accX()/1000*9.81;
this->acceleration[1] = imu.accY()/1000*9.81;
this->acceleration[2] = imu.accZ()/1000*9.81;
*/
}
#endif
}
void ICM20948Sensor::motionSetup() {
#ifdef DEBUG_SENSOR
imu.enableDebugging(Serial);
@@ -381,6 +436,9 @@ void ICM20948Sensor::motionLoop() {
quaternion.x = q1;
quaternion.y = q2;
quaternion.z = q3;
#if SEND_ACCELERATION
calculateAcceleration(&quaternion);
#endif
quaternion *= sensorOffset; //imu rotation
#if ENABLE_INSPECTION
@@ -407,8 +465,11 @@ void ICM20948Sensor::motionLoop() {
double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
quaternion.w = q0;
quaternion.x = q1;
quaternion.y = -q2; // make the acceleration work correctly
quaternion.y = q2;
quaternion.z = q3;
#if SEND_ACCELERATION
calculateAcceleration(&quaternion);
#endif
quaternion *= sensorOffset; //imu rotation
#if ENABLE_INSPECTION
@@ -421,31 +482,6 @@ void ICM20948Sensor::motionLoop() {
lastData = millis();
}
}
#if SEND_ACCELERATION
{
this->acceleration[0] = (float)this->dmpData.Raw_Accel.Data.X;
this->acceleration[1] = (float)this->dmpData.Raw_Accel.Data.Y;
this->acceleration[2] = (float)this->dmpData.Raw_Accel.Data.Z;
// get the component of the acceleration that is gravity
float gravity[3];
gravity[0] = 2 * (this->quaternion.x * this->quaternion.z - this->quaternion.w * this->quaternion.y);
gravity[1] = 2 * (this->quaternion.w * this->quaternion.x + this->quaternion.y * this->quaternion.z);
gravity[2] = this->quaternion.w * this->quaternion.w - this->quaternion.x * this->quaternion.x - this->quaternion.y * this->quaternion.y + this->quaternion.z * this->quaternion.z;
// subtract gravity from the acceleration vector
this->acceleration[0] -= gravity[0] * ACCEL_SENSITIVITY_4G;
this->acceleration[1] -= gravity[1] * ACCEL_SENSITIVITY_4G;
this->acceleration[2] -= gravity[2] * ACCEL_SENSITIVITY_4G;
// finally scale the acceleration values to mps2
this->acceleration[0] *= ASCALE_4G;
this->acceleration[1] *= ASCALE_4G;
this->acceleration[2] *= ASCALE_4G;
}
#endif
}
else
{

1
src/sensors/icm20948sensor.h
View File

@@ -45,6 +45,7 @@ public:
void load_bias();
private:
void calculateAcceleration(Quat *quaternion);
unsigned long lastData = 0;
int bias_save_counter = 0;
bool newTap;