From 418a33a1fc514dcd88e0fad6191732d8121e624c Mon Sep 17 00:00:00 2001 From: Externet Date: Tue, 25 Oct 2022 02:54:59 +0200 Subject: [PATCH 1/7] Fix for I2C startup issues on BNO055 and ICM20948 (#206) --- src/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index df8022863..768e6eb8f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -70,9 +70,9 @@ void setup() SerialCommands::setUp(); -#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 +#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down - // Do it only for MPU, cause reaction of BNO to this is not investigated yet + // Fixes I2C issues for certain IMUs. Only has been tested on IMUs above. Testing advised when adding other IMUs. #endif // join I2C bus From 77cb8ac4899bb7048ee9b4408a4952f43dee618d Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Thu, 27 Oct 2022 15:22:11 +0200 Subject: [PATCH 2/7] Icm20948 magnetometer fix (#202) --- src/sensors/bmi160sensor.cpp | 8 ++-- src/sensors/icm20948sensor.cpp | 88 ++++++++++++++++++++++++---------- src/sensors/icm20948sensor.h | 1 + 3 files changed, 68 insertions(+), 29 deletions(-) diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 1af750f71..85b4eb7ef 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -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); diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 3c558b7a4..3569392c6 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -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 { diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index ff4284757..74dd1355d 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -45,6 +45,7 @@ class ICM20948Sensor : public Sensor void load_bias(); private: + void calculateAcceleration(Quat *quaternion); unsigned long lastData = 0; int bias_save_counter = 0; bool newTap; From 3fa60cc73563d4f05a6f10a9689a19da8fc7acb0 Mon Sep 17 00:00:00 2001 From: Externet Date: Thu, 27 Oct 2022 15:22:59 +0200 Subject: [PATCH 3/7] Fix BNO055 not working at all (#205) --- src/debug.h | 2 +- src/sensors/bno055sensor.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/debug.h b/src/debug.h index c4addf7ae..950c07ed6 100644 --- a/src/debug.h +++ b/src/debug.h @@ -28,7 +28,7 @@ #define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration #define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs #define BNO_USE_MAGNETOMETER_CORRECTION false // Set to true to enable magnetometer correction for BNO08x IMUs. Only works with USE_6_AXIS set to true. -#define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO08x currently) +#define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently) #define LOAD_BIAS 1 // Loads the bias values from NVS on start (ESP32 Only) #define SAVE_BIAS 1 // Periodically saves bias calibration data to NVS (ESP32 Only) #define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only) diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index ebcd1e51f..2ef7a85ed 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -28,7 +28,11 @@ void BNO055Sensor::motionSetup() { imu = Adafruit_BNO055(sensorId, addr); delay(3000); +#if USE_6_AXIS if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_IMUPLUS)) +#else + if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_NDOF)) +#endif { m_Logger.fatal("Can't connect to BNO055 at address 0x%02x", addr); ledManager.pattern(50, 50, 200); @@ -36,9 +40,11 @@ void BNO055Sensor::motionSetup() { } delay(1000); + imu.setExtCrystalUse(true); //Adafruit BNO055's use external crystal. Enable it, otherwise it does not work. imu.setAxisRemap(Adafruit_BNO055::REMAP_CONFIG_P0); imu.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P0); m_Logger.info("Connected to BNO055 at address 0x%02x", addr); + working = true; configured = true; } From faa3e4c0f198f4569822abe3adebb936f0b9c992 Mon Sep 17 00:00:00 2001 From: nullstalgia Date: Fri, 28 Oct 2022 09:14:54 -0700 Subject: [PATCH 4/7] Workaround for connecting to some ASUS+? routers (#207) --- src/network/wifihandler.cpp | 65 +++++++++++++++++++++++++++++++++---- src/network/wifihandler.h | 13 +++++++- 2 files changed, 70 insertions(+), 8 deletions(-) diff --git a/src/network/wifihandler.cpp b/src/network/wifihandler.cpp index 469b8cff4..8c20c6090 100644 --- a/src/network/wifihandler.cpp +++ b/src/network/wifihandler.cpp @@ -31,7 +31,7 @@ unsigned long lastWifiReportTime = 0; unsigned long wifiConnectionTimeout = millis(); bool isWifiConnected = false; -uint8_t wifiState = 0; +uint8_t wifiState = SLIME_WIFI_NOT_SETUP; bool hadWifi = false; unsigned long last_rssi_sample = 0; @@ -64,7 +64,7 @@ void WiFiNetwork::setWiFiCredentials(const char * SSID, const char * pass) { WiFi.begin(SSID, pass); // Reset state, will get back into provisioning if can't connect hadWifi = false; - wifiState = 2; + wifiState = SLIME_WIFI_SERVER_CRED_ATTEMPT; wifiConnectionTimeout = millis(); } @@ -76,12 +76,15 @@ void WiFiNetwork::setUp() { wifiHandlerLogger.info("Setting up WiFi"); WiFi.persistent(true); WiFi.mode(WIFI_STA); + #if ESP8266 + WiFi.setPhyMode(WIFI_PHY_MODE_11N); + #endif WiFi.hostname("SlimeVR FBT Tracker"); wifiHandlerLogger.info("Loaded credentials for SSID %s and pass length %d", WiFi.SSID().c_str(), WiFi.psk().length()); setStaticIPIfDefined(); wl_status_t status = WiFi.begin(); // Should connect to last used access point, see https://arduino-esp8266.readthedocs.io/en/latest/esp8266wifi/station-class.html#begin wifiHandlerLogger.debug("Status: %d", status); - wifiState = 1; + wifiState = SLIME_WIFI_SAVED_ATTEMPT; wifiConnectionTimeout = millis(); #if ESP8266 @@ -135,26 +138,74 @@ void WiFiNetwork::upkeep() { reportWifiError(); if(wifiConnectionTimeout + 11000 < millis()) { switch(wifiState) { - case 0: // Wasn't set up + case SLIME_WIFI_NOT_SETUP: // Wasn't set up return; - case 1: // Couldn't connect with first set of credentials + case SLIME_WIFI_SAVED_ATTEMPT: // Couldn't connect with first set of credentials + #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) { + WiFi.setPhyMode(WIFI_PHY_MODE_11G); + setStaticIPIfDefined(); + WiFi.begin(); + wifiConnectionTimeout = millis(); + wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status()); + wifiHandlerLogger.debug("Trying saved credentials with PHY Mode G..."); + } else { + wifiHandlerLogger.debug("Skipping PHY Mode G attempt on 0-length SSID..."); + } + #endif + wifiState = SLIME_WIFI_SAVED_G_ATTEMPT; + 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 ESP8266 + WiFi.setPhyMode(WIFI_PHY_MODE_11N); + #endif setStaticIPIfDefined(); WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD); wifiConnectionTimeout = millis(); wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status()); wifiHandlerLogger.debug("Trying hardcoded credentials..."); #endif - wifiState = 2; + wifiState = SLIME_WIFI_HARDCODE_ATTEMPT; + 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 + WiFi.setPhyMode(WIFI_PHY_MODE_11G); + setStaticIPIfDefined(); + WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD); + wifiConnectionTimeout = millis(); + wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status()); + wifiHandlerLogger.debug("Trying hardcoded credentials with WiFi PHY Mode G..."); + #endif + wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT; return; - case 2: // Couldn't connect with second set of credentials + case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with server-sent credentials. + #if ESP8266 + // Try again silently but with 11G + WiFi.setPhyMode(WIFI_PHY_MODE_11G); + setStaticIPIfDefined(); + WiFi.begin(); + wifiConnectionTimeout = millis(); + wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT; + #endif + return; + case SLIME_WIFI_HARDCODE_G_ATTEMPT: // Couldn't connect with second set of credentials with PHY Mode G. + case SLIME_WIFI_SERVER_CRED_G_ATTEMPT: // Or if couldn't connect with server-sent credentials // Start smart config if(!hadWifi && !WiFi.smartConfigDone() && wifiConnectionTimeout + 11000 < millis()) { if(WiFi.status() != WL_IDLE_STATUS) { wifiHandlerLogger.error("Can't connect from any credentials, status: %d.", WiFi.status()); wifiConnectionTimeout = millis(); } + // Return to the default PHY Mode N. + #if ESP8266 + WiFi.setPhyMode(WIFI_PHY_MODE_11N); + #endif startProvisioning(); } return; diff --git a/src/network/wifihandler.h b/src/network/wifihandler.h index 0c2cd4cff..0deb5c82d 100644 --- a/src/network/wifihandler.h +++ b/src/network/wifihandler.h @@ -37,4 +37,15 @@ namespace WiFiNetwork { IPAddress getAddress(); } -#endif // SLIMEVR_WIFI_H_ \ No newline at end of file +/** Wifi Reconnection Statuses **/ +typedef enum { + SLIME_WIFI_NOT_SETUP = 0, + SLIME_WIFI_SAVED_ATTEMPT, + SLIME_WIFI_SAVED_G_ATTEMPT, + SLIME_WIFI_HARDCODE_ATTEMPT, + SLIME_WIFI_HARDCODE_G_ATTEMPT, + SLIME_WIFI_SERVER_CRED_ATTEMPT, + SLIME_WIFI_SERVER_CRED_G_ATTEMPT +} wifi_reconnection_statuses; + +#endif // SLIMEVR_WIFI_H_ From f907db049ff956711bdb8fb72eaf6d0674d8c397 Mon Sep 17 00:00:00 2001 From: lucas lelievre Date: Fri, 11 Nov 2022 20:42:19 +0100 Subject: [PATCH 5/7] Release workflow test (#209) --- .github/workflows/release.yml | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 .github/workflows/release.yml diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 000000000..4aab4a714 --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,20 @@ +name: Releases + +on: + push: + tags: + - '*' + +jobs: + + build: + runs-on: ubuntu-latest + permissions: + contents: write + steps: + - uses: actions/checkout@v2 + - uses: ncipollo/release-action@v1 + with: + artifacts: "./build/*.bin" + draft: true + token: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file From 093d99acea94fa94b8061f6f267e728c8eac717b Mon Sep 17 00:00:00 2001 From: ThreadOfFate <37180802+ThreadOfFate@users.noreply.github.com> Date: Fri, 11 Nov 2022 19:42:58 +0000 Subject: [PATCH 6/7] Refactored ICM20948 and improved rotation data. (#208) --- src/debug.h | 4 +- src/sensors/icm20948sensor.cpp | 639 ++++++++++++++++----------------- src/sensors/icm20948sensor.h | 19 +- 3 files changed, 326 insertions(+), 336 deletions(-) diff --git a/src/debug.h b/src/debug.h index 950c07ed6..8f1ade967 100644 --- a/src/debug.h +++ b/src/debug.h @@ -29,8 +29,8 @@ #define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs #define BNO_USE_MAGNETOMETER_CORRECTION false // Set to true to enable magnetometer correction for BNO08x IMUs. Only works with USE_6_AXIS set to true. #define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently) -#define LOAD_BIAS 1 // Loads the bias values from NVS on start (ESP32 Only) -#define SAVE_BIAS 1 // Periodically saves bias calibration data to NVS (ESP32 Only) +#define LOAD_BIAS true // Loads the bias values from NVS on start +#define SAVE_BIAS true // Periodically saves bias calibration data to NVS #define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only) #define ENABLE_TAP false // monitor accel for (triple) tap events and send them. Uses more cpu, disable if problems. Server does nothing with value so disabled atm #define SEND_ACCELERATION true // send linear acceleration to the server diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 3569392c6..6516441be 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -34,204 +34,101 @@ int bias_save_periods[] = { 120, 180, 300, 600, 600 }; // 2min + 3min + 5min + 1 // Accel scale conversion steps: LSB/G -> G -> m/s^2 constexpr float ASCALE_4G = ((32768. / ACCEL_SENSITIVITY_4G) / 32768.) * EARTH_GRAVITY; -// #ifndef ENABLE_TAP -// #define ENABLE_TAP false -// #endif - -void ICM20948Sensor::save_bias(bool repeat) { -#if defined(SAVE_BIAS) && SAVE_BIAS - #ifdef DEBUG_SENSOR - m_Logger.trace("Saving Bias"); - #endif - - imu.GetBiasGyroX(&m_Calibration.G[0]); - imu.GetBiasGyroY(&m_Calibration.G[1]); - imu.GetBiasGyroZ(&m_Calibration.G[2]); +void ICM20948Sensor::motionSetup() +{ + connectSensor(); + startDMP(); + loadCalibration(); + startMotionLoop(); + startCalibrationAutoSave(); +} - imu.GetBiasAccelX(&m_Calibration.A[0]); - imu.GetBiasAccelY(&m_Calibration.A[1]); - imu.GetBiasAccelZ(&m_Calibration.A[2]); +void ICM20948Sensor::motionLoop() +{ +#if ENABLE_INSPECTION + { + (void)imu.getAGMT(); - #if !USE_6_AXIS - imu.GetBiasCPassX(&m_Calibration.C[0]); - imu.GetBiasCPassY(&m_Calibration.C[1]); - imu.GetBiasCPassZ(&m_Calibration.C[2]); - #endif + float rX = imu.gyrX(); + float rY = imu.gyrY(); + float rZ = imu.gyrZ(); - #ifdef DEBUG_SENSOR - m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.G)); - m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.A)); - #if !USE_6_AXIS - m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); - #endif - #endif + float aX = imu.accX(); + float aY = imu.accY(); + float aZ = imu.accZ(); - SlimeVR::Configuration::CalibrationConfig calibration; - calibration.type = SlimeVR::Configuration::CalibrationConfigType::ICM20948; - calibration.data.icm20948 = m_Calibration; - configuration.setCalibration(sensorId, calibration); - configuration.save(); + float mX = imu.magX(); + float mY = imu.magY(); + float mZ = imu.magZ(); - if (repeat) { - bias_save_counter++; - // Possible: Could make it repeat the final timer value if any of the biases are still 0. Save strategy could be improved. - if (sizeof(bias_save_periods) != bias_save_counter) { - timer.in( - bias_save_periods[bias_save_counter] * 1000, - [](void* arg) -> bool { - ((ICM20948Sensor*)arg)->save_bias(true); - return false; - }, - this); - } + Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); } #endif -} -void ICM20948Sensor::load_bias() { -#ifdef DEBUG_SENSOR - m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.G)); - m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.A)); - #if !USE_6_AXIS - m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); - #endif -#endif + timer.tick(); -#if defined(LOAD_BIAS) && LOAD_BIAS - imu.SetBiasGyroX(m_Calibration.G[0]); - imu.SetBiasGyroY(m_Calibration.G[1]); - imu.SetBiasGyroZ(m_Calibration.G[2]); + readFIFOToEnd(); + readRotation(); + checkSensorTimeout(); +} - imu.SetBiasAccelX(m_Calibration.A[0]); - imu.SetBiasAccelY(m_Calibration.A[1]); - imu.SetBiasAccelZ(m_Calibration.A[2]); +void ICM20948Sensor::readFIFOToEnd() +{ + ICM_20948_Status_e readStatus = imu.readDMPdataFromFIFO(&dmpDataTemp); - #if !USE_6_AXIS - imu.SetBiasCPassX(m_Calibration.C[0]); - imu.SetBiasCPassY(m_Calibration.C[1]); - imu.SetBiasCPassZ(m_Calibration.C[2]); - #endif -#else - #if BIAS_DEBUG - // Sets all bias to 90 - imu.SetBiasGyroX(90); - imu.SetBiasGyroY(90); - imu.SetBiasGyroZ(90); - - imu.SetBiasAccelX(90); - imu.SetBiasAccelY(90); - imu.SetBiasAccelZ(90); - - imu.SetBiasCPassX(90); - imu.SetBiasCPassY(90); - imu.SetBiasCPassZ(90); - - int32_t bias_gyro[3], bias_accel[3], bias_compass[3]; - - // Reloads all bias from memory - imu.GetBiasGyroX(&bias_gyro[0]); - imu.GetBiasGyroY(&bias_gyro[1]); - imu.GetBiasGyroZ(&bias_gyro[2]); - - imu.GetBiasAccelX(&bias_accel[0]); - imu.GetBiasAccelY(&bias_accel[1]); - imu.GetBiasAccelZ(&bias_accel[2]); - - imu.GetBiasCPassX(&bias_compass[0]); - imu.GetBiasCPassY(&bias_compass[1]); - imu.GetBiasCPassZ(&bias_compass[2]); - - #ifdef DEBUG_SENSOR - m_Logger.trace("All set bias should be 90"); - - m_Logger.trace("Gyrometer bias : [%d, %d, %d]", UNPACK_VECTOR_ARRAY(bias_gyro)); - m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(bias_accel)); - m_Logger.trace("Compass bias : [%d, %d, %d]", UNPACK_VECTOR_ARRAY(bias_compass)); - #endif + #ifdef DEBUG_SENSOR + { + m_Logger.trace("e0x%02x", readStatus); + } #endif -#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; + if(readStatus == ICM_20948_Stat_Ok) + { + dmpData = dmpDataTemp; + readFIFOToEnd(); + } +} -/* - 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; +void ICM20948Sensor::sendData() +{ + if(newData && lastDataSent + 7 < millis()) + { + lastDataSent = millis(); + newData = false; - // finally scale the acceleration values to mps2 - this->acceleration[0] *= ASCALE_4G; - this->acceleration[1] *= ASCALE_4G; - this->acceleration[2] *= ASCALE_4G; + #if(USE_6_AXIS) + { + Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, 0, sensorId); + } + #else + { + Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy, sensorId); + } + #endif -/* - 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; -*/ + #if SEND_ACCELERATION + { + Network::sendAccel(acceleration, sensorId); } -#endif + #endif + } } -void ICM20948Sensor::motionSetup() { - #ifdef DEBUG_SENSOR - imu.enableDebugging(Serial); +void ICM20948Sensor::startCalibration(int calibrationType) +{ + // 20948 does continuous calibration + saveCalibration(false); +} + +void ICM20948Sensor::startCalibrationAutoSave() +{ + #if SAVE_BIAS + timer.in(bias_save_periods[0] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->saveCalibration(true); return false; }, this); #endif - // SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if something else throw a error - boolean tracker = false; - - if (addr == 0x68) { - tracker = false; - } else if (addr == 0x69) - { - tracker = true; - } else { - m_Logger.fatal("I2C Address not supported by ICM20948 library: 0x%02x", addr); - return; - } - //m_Logger.debug("Start Init with addr = %s", tracker ? "true" : "false"); - ICM_20948_Status_e imu_err = imu.begin(Wire, tracker); - if (imu_err != ICM_20948_Stat_Ok) { - m_Logger.fatal("Can't connect to ICM20948 at address 0x%02x, error code: 0x%02x", addr, imu_err); - ledManager.pattern(50, 50, 200); - return; - } +} - // Configure imu setup and load any stored bias values +void ICM20948Sensor::startDMP() +{ if(imu.initializeDMP() == ICM_20948_Stat_Ok) { m_Logger.debug("DMP initialized"); @@ -242,7 +139,7 @@ void ICM20948Sensor::motionSetup() { return; } - if (USE_6_AXIS) + #if(USE_6_AXIS) { m_Logger.debug("Using 6 axis configuration"); if(imu.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok) @@ -252,10 +149,10 @@ void ICM20948Sensor::motionSetup() { else { m_Logger.fatal("Failed to enable DMP sensor for game rotation vector"); - return; + return; } } - else + #else { m_Logger.debug("Using 9 axis configuration"); if(imu.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok) @@ -265,10 +162,12 @@ void ICM20948Sensor::motionSetup() { else { m_Logger.fatal("Failed to enable DMP sensor orientation"); - return; + return; } } + #endif + #if(SEND_ACCELERATION) if (imu.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok) { m_Logger.debug("Enabled DMP sensor for accelerometer"); @@ -276,14 +175,15 @@ void ICM20948Sensor::motionSetup() { else { m_Logger.fatal("Failed to enable DMP sensor for accelerometer"); - return; + return; } + #endif // Might need to set up other DMP functions later, just Quad6/Quad9/Accel for now - if (USE_6_AXIS) + #if(USE_6_AXIS) { - if(imu.setDMPODRrate(DMP_ODR_Reg_Quat6, 1.25) == ICM_20948_Stat_Ok) + if(imu.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok) { m_Logger.debug("Set Quat6 to 100Hz frequency"); } @@ -293,9 +193,9 @@ void ICM20948Sensor::motionSetup() { return; } } - else + #else { - if(imu.setDMPODRrate(DMP_ODR_Reg_Quat9, 1.25) == ICM_20948_Stat_Ok) + if(imu.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok) { m_Logger.debug("Set Quat9 to 100Hz frequency"); } @@ -305,8 +205,10 @@ void ICM20948Sensor::motionSetup() { return; } } + #endif - if (this->imu.setDMPODRrate(DMP_ODR_Reg_Accel, 1.25) == ICM_20948_Stat_Ok) + #if(SEND_ACCELERATION) + if (this->imu.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok) { this->m_Logger.debug("Set Accel to 100Hz frequency"); } @@ -315,6 +217,7 @@ void ICM20948Sensor::motionSetup() { this->m_Logger.fatal("Failed to set Accel to 100Hz frequency"); return; } + #endif // Enable the FIFO if(imu.enableFIFO() == ICM_20948_Stat_Ok) @@ -359,177 +262,253 @@ void ICM20948Sensor::motionSetup() { m_Logger.fatal("Failed to reset FIFO"); return; } +} -#if LOAD_BIAS - // Initialize the configuration - { - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::ICM20948: - m_Calibration = sensorCalibration.data.icm20948; - break; - - case SlimeVR::Configuration::CalibrationConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } +void ICM20948Sensor::connectSensor() +{ + #ifdef DEBUG_SENSOR + imu.enableDebugging(Serial); + #endif + // SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if something else throw a error + boolean isOnSecondAddress = false; + + if (addr == 0x68) { + isOnSecondAddress = false; + } else if (addr == 0x69){ + isOnSecondAddress = true; + } else { + m_Logger.fatal("I2C Address not supported by ICM20948 library: 0x%02x", addr); + return; } -#endif - load_bias(); + ICM_20948_Status_e imu_err = imu.begin(Wire, isOnSecondAddress); + if (imu_err != ICM_20948_Stat_Ok) { + m_Logger.fatal("Can't connect to ICM20948 at address 0x%02x, error code: 0x%02x", addr, imu_err); + ledManager.pattern(50, 50, 200); + return; + } +} +void ICM20948Sensor::startMotionLoop() +{ lastData = millis(); working = true; +} - #if defined(SAVE_BIAS) && SAVE_BIAS - timer.in(bias_save_periods[0] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->save_bias(true); return false; }, this); - #endif +void ICM20948Sensor::checkSensorTimeout() +{ + if(lastData + 1000 < millis()) { + working = false; + lastData = millis(); + m_Logger.error("Sensor timeout I2C Address 0x%02x", addr); + Network::sendError(1, this->sensorId); + } } -void ICM20948Sensor::motionLoop() { -#if ENABLE_INSPECTION +void ICM20948Sensor::readRotation() +{ + #if(USE_6_AXIS) { - (void)imu.getAGMT(); - - float rX = imu.gyrX(); - float rY = imu.gyrY(); - float rZ = imu.gyrZ(); + if ((dmpData.header & DMP_header_bitmap_Quat6) > 0) + { + // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. + // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values. + // The quaternion data is scaled by 2^30. + // Scale to +/- 1 + double q1 = ((double)dmpData.Quat6.Data.Q1) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q2 = ((double)dmpData.Quat6.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q3 = ((double)dmpData.Quat6.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); + quaternion.w = q0; + quaternion.x = q1; + quaternion.y = q2; + quaternion.z = q3; + + #if SEND_ACCELERATION + calculateAccelerationWithoutGravity(&quaternion); + #endif + + quaternion *= sensorOffset; //imu rotation + + #if ENABLE_INSPECTION + { + Network::sendInspectionFusedIMUData(sensorId, quaternion); + } + #endif - float aX = imu.accX(); - float aY = imu.accY(); - float aZ = imu.accZ(); + newData = true; + lastData = millis(); + } + } + #else + { + if((dmpData.header & DMP_header_bitmap_Quat9) > 0) + { + // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. + // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values. + // The quaternion data is scaled by 2^30. + // Scale to +/- 1 + double q1 = ((double)dmpData.Quat9.Data.Q1) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q2 = ((double)dmpData.Quat9.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q3 = ((double)dmpData.Quat9.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 + double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); + quaternion.w = q0; + quaternion.x = q1; + quaternion.y = q2; + quaternion.z = q3; + + #if SEND_ACCELERATION + calculateAccelerationWithoutGravity(&quaternion); + #endif + + quaternion *= sensorOffset; //imu rotation + + #if ENABLE_INSPECTION + { + Network::sendInspectionFusedIMUData(sensorId, quaternion); + } + #endif - float mX = imu.magX(); - float mY = imu.magY(); - float mZ = imu.magZ(); + newData = true; + lastData = millis(); + } + } + #endif +} - Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); +void ICM20948Sensor::saveCalibration(bool repeat) +{ + #if(!SAVE_BIAS) + { + return; } -#endif + #endif + #ifdef DEBUG_SENSOR + m_Logger.trace("Saving Bias"); + #endif - timer.tick(); + imu.GetBiasGyroX(&m_Calibration.G[0]); + imu.GetBiasGyroY(&m_Calibration.G[1]); + imu.GetBiasGyroZ(&m_Calibration.G[2]); - bool dataavaliable = true; - while (dataavaliable) { - ICM_20948_Status_e readStatus = imu.readDMPdataFromFIFO(&dmpData); - if(readStatus == ICM_20948_Stat_Ok) - { - if(USE_6_AXIS) - { - if ((dmpData.header & DMP_header_bitmap_Quat6) > 0) - { - // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. - // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values. - // The quaternion data is scaled by 2^30. - // Scale to +/- 1 - double q1 = ((double)dmpData.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 - double q2 = ((double)dmpData.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 - double q3 = ((double)dmpData.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 - double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - quaternion.w = q0; - quaternion.x = q1; - quaternion.y = q2; - quaternion.z = q3; -#if SEND_ACCELERATION - calculateAcceleration(&quaternion); -#endif - quaternion *= sensorOffset; //imu rotation + imu.GetBiasAccelX(&m_Calibration.A[0]); + imu.GetBiasAccelY(&m_Calibration.A[1]); + imu.GetBiasAccelZ(&m_Calibration.A[2]); -#if ENABLE_INSPECTION - { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } -#endif + #if !USE_6_AXIS + imu.GetBiasCPassX(&m_Calibration.C[0]); + imu.GetBiasCPassY(&m_Calibration.C[1]); + imu.GetBiasCPassZ(&m_Calibration.C[2]); + #endif - newData = true; - lastData = millis(); - } - } - else - { - if((dmpData.header & DMP_header_bitmap_Quat9) > 0) - { - // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1. - // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values. - // The quaternion data is scaled by 2^30. - // Scale to +/- 1 - double q1 = ((double)dmpData.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 - double q2 = ((double)dmpData.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 - double q3 = ((double)dmpData.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 - double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - quaternion.w = q0; - quaternion.x = q1; - quaternion.y = q2; - quaternion.z = q3; -#if SEND_ACCELERATION - calculateAcceleration(&quaternion); -#endif - quaternion *= sensorOffset; //imu rotation + #ifdef DEBUG_SENSOR + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.A)); + #if !USE_6_AXIS + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); + #endif + #endif -#if ENABLE_INSPECTION - { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } -#endif + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::ICM20948; + calibration.data.icm20948 = m_Calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); - newData = true; - lastData = millis(); - } - } - } - else - { - if (readStatus == ICM_20948_Stat_FIFONoDataAvail || lastData + 1000 < millis()) { - dataavaliable = false; - } else if (readStatus == ICM_20948_Stat_FIFOMoreDataAvail) { - dataavaliable = true; - } - // Sorry for this horrible formatting -#ifdef DEBUG_SENSOR - else { - m_Logger.trace("e0x%02x", readStatus); - } -#endif + if (repeat) { + bias_save_counter++; + // Possible: Could make it repeat the final timer value if any of the biases are still 0. Save strategy could be improved. + if (sizeof(bias_save_periods) != bias_save_counter) { + timer.in( + bias_save_periods[bias_save_counter] * 1000, + [](void* arg) -> bool { + ((ICM20948Sensor*)arg)->saveCalibration(true); + return false; + }, + this); } } - if(lastData + 1000 < millis()) { - working = false; - lastData = millis(); - m_Logger.error("Sensor timeout I2C Address 0x%02x", addr); - Network::sendError(1, this->sensorId); - } } -void ICM20948Sensor::sendData() { - if(newData) { - newData = false; - if (USE_6_AXIS) { - Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, 0, sensorId); - } else { - Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy, sensorId); - } +void ICM20948Sensor::loadCalibration() +{ + #if(!LOAD_BIAS) + { + return; + } + #endif -#if SEND_ACCELERATION - { - Network::sendAccel(acceleration, sensorId); - } -#endif + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::ICM20948: + m_Calibration = sensorCalibration.data.icm20948; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); } + + #ifdef DEBUG_SENSOR + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.A)); + #if !USE_6_AXIS + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); + #endif + #endif + + imu.SetBiasGyroX(m_Calibration.G[0]); + imu.SetBiasGyroY(m_Calibration.G[1]); + imu.SetBiasGyroZ(m_Calibration.G[2]); + + imu.SetBiasAccelX(m_Calibration.A[0]); + imu.SetBiasAccelY(m_Calibration.A[1]); + imu.SetBiasAccelZ(m_Calibration.A[2]); + + #if !USE_6_AXIS + imu.SetBiasCPassX(m_Calibration.C[0]); + imu.SetBiasCPassY(m_Calibration.C[1]); + imu.SetBiasCPassZ(m_Calibration.C[2]); + #endif } -void ICM20948Sensor::startCalibration(int calibrationType) { - // 20948 does continuous calibration +void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) +{ + #if SEND_ACCELERATION + { + if((dmpData.header & DMP_header_bitmap_Accel) > 0) + { + 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 * ((-quaternion->x) * (-quaternion->z) - quaternion->w * quaternion->y); + gravity[1] = -2 * (quaternion->w * (-quaternion->x) + quaternion->y * (-quaternion->z)); + gravity[2] = quaternion->w * quaternion->w - quaternion->x * quaternion->x - quaternion->y * quaternion->y + quaternion->z * 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; - save_bias(false); + // finally scale the acceleration values to mps2 + this->acceleration[0] *= ASCALE_4G; + this->acceleration[1] *= ASCALE_4G; + this->acceleration[2] *= ASCALE_4G; + } + } + #endif } -//You need to override the library's initializeDMP to change some settings +//You need to override the library's initializeDMP to change some settings #if OVERRIDEDMPSETUP // initializeDMP is a weak function. Let's overwrite it so we can increase the sample rate ICM_20948_Status_e ICM_20948::initializeDMP(void) @@ -596,7 +575,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) // You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case. result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3 uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz - result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register + result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register // Configure clock source through PWR_MGMT_1 // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator @@ -663,8 +642,8 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) //mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13). mySmplrt.g = 4; // 225Hz mySmplrt.a = 4; // 225Hz - //mySmplrt.g = 8; // 112Hz - //mySmplrt.a = 8; // 112Hz + // mySmplrt.g = 8; // 112Hz + // mySmplrt.a = 8; // 112Hz result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result; // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 74dd1355d..afa33b972 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -38,25 +38,36 @@ class ICM20948Sensor : public Sensor } void motionLoop() override final; - void sendData() override final; void startCalibration(int calibrationType) override final; - void save_bias(bool repeat); - void load_bias(); private: - void calculateAcceleration(Quat *quaternion); + void calculateAccelerationWithoutGravity(Quat *quaternion); unsigned long lastData = 0; + unsigned long lastDataSent = 0; int bias_save_counter = 0; bool newTap; int16_t rawAccel[3]; + + #define DMPNUMBERTODOUBLECONVERTER 1073741824.0; ICM_20948_I2C imu; ICM_20948_Device_t pdev; icm_20948_DMP_data_t dmpData{}; + icm_20948_DMP_data_t dmpDataTemp{}; SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration; + void saveCalibration(bool repeat); + void loadCalibration(); + void startCalibrationAutoSave(); + void startDMP(); + void connectSensor(); + void startMotionLoop(); + void checkSensorTimeout(); + void readRotation(); + void readFIFOToEnd(); + #define OVERRIDEDMPSETUP true Timer<> timer = timer_create_default(); From 83550d21ef748678704b3a11c0f7afb6f44258e4 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Fri, 11 Nov 2022 22:45:05 +0300 Subject: [PATCH 7/7] Bump version to 0.3.0 (13) --- src/debug.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/debug.h b/src/debug.h index 8f1ade967..e540ea96b 100644 --- a/src/debug.h +++ b/src/debug.h @@ -78,7 +78,7 @@ // Not recommended for production #define ENABLE_INSPECTION false -#define FIRMWARE_BUILD_NUMBER 12 -#define FIRMWARE_VERSION "0.2.3" +#define FIRMWARE_BUILD_NUMBER 13 +#define FIRMWARE_VERSION "0.3.0" #endif // SLIMEVR_DEBUG_H_