From b7e87bd5434f75e6b6b0c015f2e88aaa9f11aeb8 Mon Sep 17 00:00:00 2001 From: Przemyslaw Romaniak Date: Mon, 18 Sep 2023 16:03:10 +0200 Subject: [PATCH 01/23] ESP32C3 stability improvements (#265) --- lib/i2cscan/i2cscan.cpp | 38 ++++++++++++++++++----------- lib/i2cscan/i2cscan.h | 2 +- src/batterymonitor.cpp | 2 +- src/configuration/Configuration.cpp | 6 ++--- src/configuration/Configuration.h | 2 -- src/sensors/SensorManager.cpp | 2 +- src/sensors/bmi160sensor.h | 4 +-- src/sensors/icm20948sensor.h | 2 +- src/sensors/mpu6050sensor.h | 2 +- src/sensors/mpu9250sensor.h | 4 +-- 10 files changed, 35 insertions(+), 29 deletions(-) diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp index 649dbc7c9..ae94247fa 100644 --- a/lib/i2cscan/i2cscan.cpp +++ b/lib/i2cscan/i2cscan.cpp @@ -20,18 +20,19 @@ namespace I2CSCAN { uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound) { - if(I2CSCAN::isI2CExist(addr1)) + if(I2CSCAN::hasDevOnBus(addr1)) { return addr1; - if(!I2CSCAN::isI2CExist(addr2)) { - if(scanIfNotFound) { - Serial.println("[ERR] I2C: Can't find I2C device on provided addresses, scanning for all I2C devices and returning"); - I2CSCAN::scani2cports(); - } else { - Serial.println("[ERR] I2C: Can't find I2C device on provided addresses"); - } - return 0; } - return addr2; + if(I2CSCAN::hasDevOnBus(addr2)) { + return addr2; + } + if (scanIfNotFound) { + Serial.println("[ERR] I2C: Can't find I2C device on provided addresses, scanning for all I2C devices and returning"); + I2CSCAN::scani2cports(); + } else { + Serial.println("[ERR] I2C: Can't find I2C device on provided addresses"); + } + return 0; } void scani2cports() @@ -103,16 +104,25 @@ namespace I2CSCAN } else if (error == 4) { - Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknow error at address 0x%02x\n", + Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n", portMap[i].c_str(), portArray[i], portMap[j].c_str(), portArray[j], address); } } return found; } - bool isI2CExist(uint8_t addr) { - Wire.beginTransmission(addr); - byte error = Wire.endTransmission(); + bool hasDevOnBus(uint8_t addr) { + byte error; +#if ESP32C3 + int retries = 1; + do { +#endif + Wire.beginTransmission(addr); + error = Wire.endTransmission(); +#if ESP32C3 + } + while (error == 5 && retries--); +#endif if(error == 0) return true; return false; diff --git a/lib/i2cscan/i2cscan.h b/lib/i2cscan/i2cscan.h index 0ace23455..a0234db34 100644 --- a/lib/i2cscan/i2cscan.h +++ b/lib/i2cscan/i2cscan.h @@ -7,7 +7,7 @@ namespace I2CSCAN { void scani2cports(); bool checkI2C(uint8_t i, uint8_t j); - bool isI2CExist(uint8_t addr); + bool hasDevOnBus(uint8_t addr); uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound); int clearBus(uint8_t SDA, uint8_t SCL); boolean inArray(uint8_t value, uint8_t* arr, size_t arrSize); diff --git a/src/batterymonitor.cpp b/src/batterymonitor.cpp index b7ff913d5..42ec85c01 100644 --- a/src/batterymonitor.cpp +++ b/src/batterymonitor.cpp @@ -32,7 +32,7 @@ void BatteryMonitor::Setup() #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021 for (uint8_t i = 0x48; i < 0x4F; i++) { - if (I2CSCAN::isI2CExist(i)) + if (I2CSCAN::hasDevOnBus(i)) { address = i; break; diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index a0264d4b5..74431a063 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -32,8 +32,6 @@ namespace SlimeVR { namespace Configuration { - CalibrationConfig Configuration::m_EmptyCalibration = {NONE}; - void Configuration::setup() { if (m_Loaded) { return; @@ -140,7 +138,7 @@ namespace SlimeVR { CalibrationConfig Configuration::getCalibration(size_t sensorID) const { if (sensorID >= m_Calibrations.size()) { - return m_EmptyCalibration; + return {}; } return m_Calibrations.at(sensorID); @@ -150,7 +148,7 @@ namespace SlimeVR { size_t currentCalibrations = m_Calibrations.size(); if (sensorID >= currentCalibrations) { - m_Calibrations.resize(sensorID + 1, m_EmptyCalibration); + m_Calibrations.resize(sensorID + 1); } m_Calibrations[sensorID] = config; diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h index 7b191bdb5..4a0eb69e3 100644 --- a/src/configuration/Configuration.h +++ b/src/configuration/Configuration.h @@ -60,8 +60,6 @@ namespace SlimeVR { std::vector m_Calibrations; Logging::Logger m_Logger = Logging::Logger("Configuration"); - - static CalibrationConfig m_EmptyCalibration; }; } } diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index b3b46f0e9..9818291b1 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -57,7 +57,7 @@ namespace SlimeVR I2CSCAN::clearBus(sdaPin, sclPin); swapI2C(sclPin, sdaPin); - if (I2CSCAN::isI2CExist(address)) { + if (I2CSCAN::hasDevOnBus(address)) { m_Logger.trace("IMU %d found at address 0x%02X", sensorID, address); } else { sensor = new ErroneousSensor(sensorID, imuType); diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 3c8643a86..9ebfa7850 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -217,13 +217,13 @@ class BMI160Sensor : public Sensor { double gscaleY = BMI160_GSCALE; double gscaleZ = BMI160_GSCALE; - double GOxyzStaticTempCompensated[3]; + double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0}; bool isGyroCalibrated = false; bool isAccelCalibrated = false; bool isMagCalibrated = false; - SlimeVR::Configuration::BMI160CalibrationConfig m_Calibration; + SlimeVR::Configuration::BMI160CalibrationConfig m_Calibration = {}; }; #endif diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index dff47a4af..f3e5c36bc 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -55,7 +55,7 @@ class ICM20948Sensor : public Sensor icm_20948_DMP_data_t dmpData{}; icm_20948_DMP_data_t dmpDataTemp{}; - SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration; + SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration = {}; SlimeVR::Sensors::SensorFusionDMP sfusion; diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index 2ae02543a..d7a7b6d90 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -53,7 +53,7 @@ class MPU6050Sensor : public Sensor SlimeVR::Sensors::SensorFusionDMP sfusion; #ifndef IMU_MPU6050_RUNTIME_CALIBRATION - SlimeVR::Configuration::MPU6050CalibrationConfig m_Calibration; + SlimeVR::Configuration::MPU6050CalibrationConfig m_Calibration = {}; #endif }; diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index a666b340c..612657974 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -75,8 +75,8 @@ class MPU9250Sensor : public Sensor float Mxyz[3]{}; VectorInt16 rawAccel{}; Quat correction{0, 0, 0, 0}; - - SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration; + + SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration = {}; // outputs to respective member variables void parseAccelData(int16_t data[3]); From 6144f01799e72ee3208aa3242be5ab85279d7f10 Mon Sep 17 00:00:00 2001 From: Przemyslaw Romaniak Date: Mon, 18 Sep 2023 16:04:18 +0200 Subject: [PATCH 02/23] BNO080: Try to handle reset better (#268) --- lib/bno080/BNO080.cpp | 22 ++++++++++++++-------- lib/bno080/BNO080.h | 2 +- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/lib/bno080/BNO080.cpp b/lib/bno080/BNO080.cpp index c2d5c39fb..c21dc765d 100644 --- a/lib/bno080/BNO080.cpp +++ b/lib/bno080/BNO080.cpp @@ -159,6 +159,7 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I if (receivePacket() == true) { if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) + { if (_printDebug == true) { _debugPort->print(F("SW Version Major: 0x")); @@ -176,6 +177,7 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I _debugPort->println(SW_Version_Patch, HEX); } return (true); + } } return (false); //Something went wrong @@ -1036,7 +1038,7 @@ bool BNO080::readFRSdata(uint16_t recordID, uint8_t startLocation, uint8_t words // See 5.2.1 on BNO08X datasheet. // Wait For both of these packets specifically up to a max time and exit // after both packets are read or max waiting time is reached. -void BNO080::waitForCompletedReset(void) +void BNO080::waitForCompletedReset(uint32_t timeout) { uint32_t tInitialResetTimeMS = millis(); bool tResetCompleteReceived = false; @@ -1044,9 +1046,12 @@ void BNO080::waitForCompletedReset(void) shtpHeader[2] = 0; // Make sure we aren't reading old data. shtpData[0] = 0; - // Wait max 5s for the two packets. OR Until we get both reset responses. - while (millis() - tInitialResetTimeMS < 5000 && + // Wait requested timeout for the two packets. OR Until we get both reset responses. + while (millis() - tInitialResetTimeMS < timeout && (!tResetCompleteReceived || !tUnsolicitedResponseReceived)) { + #ifdef ESP8266 + ESP.wdtFeed(); + #endif receivePacket(); if (shtpHeader[2] == 1 && shtpData[0] == 0x01) { tResetCompleteReceived = true; @@ -1059,17 +1064,18 @@ void BNO080::waitForCompletedReset(void) //Send command to reset IC //Read all advertisement packets from sensor -//The sensor has been seen to reset twice if we attempt too much too quickly. -//This seems to work reliably. void BNO080::softReset(void) { + // after power-on sensor is resetting by itself + // let's handle that POTENTIAL reset with shorter timeout + // in case sensor was not resetted - like after issuing RESET command + waitForCompletedReset(1000); shtpData[0] = 1; //Reset //Attempt to start communication with sensor sendPacket(CHANNEL_EXECUTABLE, 1); //Transmit packet on channel 1, 1 byte - - waitForCompletedReset(); - waitForCompletedReset(); + // now that reset should occur for sure, so let's try with longer timeout + waitForCompletedReset(5000); } //Set the operating mode to "On" diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index eee52a71b..11660aa84 100644 --- a/lib/bno080/BNO080.h +++ b/lib/bno080/BNO080.h @@ -156,7 +156,7 @@ class BNO080 void enableDebugging(Stream &debugPort = Serial); //Turn on debug printing. If user doesn't specify then Serial will be used. void softReset(); //Try to reset the IMU via software - void waitForCompletedReset(); + void waitForCompletedReset(uint32_t timeout); uint8_t resetReason(); //Query the IMU for the reason it last reset void modeOn(); //Use the executable channel to turn the BNO on void modeSleep(); //Use the executable channel to put the BNO to sleep From 3a27447f163bad1fd852bfc6ab85724534d8cf2f Mon Sep 17 00:00:00 2001 From: 0forks <114709761+0forks@users.noreply.github.com> Date: Mon, 18 Sep 2023 17:05:55 +0300 Subject: [PATCH 03/23] Fix sending ErroneousSensor if not found (#266) Co-authored-by: Eiren Rain --- src/sensors/SensorManager.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 9818291b1..1ce8a37bf 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -58,9 +58,10 @@ namespace SlimeVR swapI2C(sclPin, sdaPin); if (I2CSCAN::hasDevOnBus(address)) { - m_Logger.trace("IMU %d found at address 0x%02X", sensorID, address); + m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address); } else { - sensor = new ErroneousSensor(sensorID, imuType); + m_Logger.debug("Sensor %d not found at address 0x%02X", sensorID + 1, address); + sensor = new EmptySensor(sensorID); return sensor; } From 71120ac0a85915a36aa6e9877029f9d704f52f39 Mon Sep 17 00:00:00 2001 From: 0forks <114709761+0forks@users.noreply.github.com> Date: Mon, 18 Sep 2023 17:08:03 +0300 Subject: [PATCH 04/23] Fix OPTIMIZE_UPDATES logic for acceleration (#269) --- src/network/connection.cpp | 8 ++++---- src/network/connection.h | 2 +- src/sensors/SensorFusion.cpp | 6 ++++++ src/sensors/SensorFusion.h | 1 + src/sensors/SensorFusionDMP.cpp | 7 ++++++- src/sensors/SensorFusionDMP.h | 1 + src/sensors/bmi160sensor.cpp | 11 +++-------- src/sensors/bno055sensor.cpp | 13 +++---------- src/sensors/bno080sensor.cpp | 28 ++++++++++------------------ src/sensors/icm20948sensor.cpp | 12 ++++++------ src/sensors/mpu6050sensor.cpp | 15 +++++---------- src/sensors/mpu9250sensor.cpp | 18 +++++++----------- src/sensors/sensor.cpp | 24 ++++++++++++++++++------ src/sensors/sensor.h | 7 ++++++- 14 files changed, 77 insertions(+), 76 deletions(-) diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 907427365..1f60ad876 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -230,16 +230,16 @@ void Connection::sendHeartbeat() { } // PACKET_ACCEL 4 -void Connection::sendSensorAcceleration(uint8_t sensorId, float* vector) { +void Connection::sendSensorAcceleration(uint8_t sensorId, Vector3 vector) { MUST(m_Connected); MUST(beginPacket()); MUST(sendPacketType(PACKET_ACCEL)); MUST(sendPacketNumber()); - MUST(sendFloat(vector[0])); - MUST(sendFloat(vector[1])); - MUST(sendFloat(vector[2])); + MUST(sendFloat(vector.x)); + MUST(sendFloat(vector.y)); + MUST(sendFloat(vector.z)); MUST(sendByte(sensorId)); MUST(endPacket()); diff --git a/src/network/connection.h b/src/network/connection.h index 505a30d29..1231ee247 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -43,7 +43,7 @@ class Connection { bool isConnected() const { return m_Connected; } // PACKET_ACCEL 4 - void sendSensorAcceleration(uint8_t sensorId, float* vector); + void sendSensorAcceleration(uint8_t sensorId, Vector3 vector); // PACKET_BATTERY_LEVEL 12 void sendBatteryLevel(float batteryVoltage, float batteryPercentage); diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index 809dbf10d..106339a25 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -156,6 +156,12 @@ namespace SlimeVR std::copy(linAccel, linAccel+3, outLinAccel); } + Vector3 SensorFusion::getLinearAccVec() + { + getLinearAcc(); + return Vector3(linAccel[0], linAccel[1], linAccel[2]); + } + void SensorFusion::calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3]) { gravVec[0] = 2 * (qwxyz[1] * qwxyz[3] - qwxyz[0] * qwxyz[2]); diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index a41fe6c33..b8c739f18 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -75,6 +75,7 @@ namespace SlimeVR sensor_real_t const * getGravityVec(); sensor_real_t const * getLinearAcc(); void getLinearAcc(sensor_real_t outLinAccel[3]); + Vector3 getLinearAccVec(); static void calcGravityVec(const sensor_real_t qwxyz[4], sensor_real_t gravVec[3]); static void calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3]); diff --git a/src/sensors/SensorFusionDMP.cpp b/src/sensors/SensorFusionDMP.cpp index e8759df8a..831195d71 100644 --- a/src/sensors/SensorFusionDMP.cpp +++ b/src/sensors/SensorFusionDMP.cpp @@ -103,6 +103,11 @@ namespace SlimeVR getLinearAcc(); std::copy(linAccel, linAccel+3, outLinAccel); } - + + Vector3 SensorFusionDMP::getLinearAccVec() + { + getLinearAcc(); + return Vector3(linAccel[0], linAccel[1], linAccel[2]); + } } } diff --git a/src/sensors/SensorFusionDMP.h b/src/sensors/SensorFusionDMP.h index b230cc5b4..ce7e745bd 100644 --- a/src/sensors/SensorFusionDMP.h +++ b/src/sensors/SensorFusionDMP.h @@ -24,6 +24,7 @@ namespace SlimeVR sensor_real_t const * getGravityVec(); sensor_real_t const * getLinearAcc(); void getLinearAcc(sensor_real_t outLinAccel[3]); + Vector3 getLinearAccVec(); protected: DMPMag dmpmag; diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 78c21d633..70b925e6b 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -376,18 +376,13 @@ void BMI160Sensor::motionLoop() { lastRotationPacketSent = now - (elapsed - sendInterval); fusedRotation = sfusion.getQuaternionQuat(); + setFusedRotationReady(); - sfusion.getLinearAcc(this->acceleration); - this->newAcceleration = true; + acceleration = sfusion.getLinearAccVec(); + setAccelerationReady(); fusedRotation *= sensorOffset; - if (!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) - { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } - optimistic_yield(100); } } diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index 97787b31c..9161511e9 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -63,21 +63,14 @@ void BNO055Sensor::motionLoop() { Quat quat = imu.getQuat(); fusedRotation.set(quat.x, quat.y, quat.z, quat.w); fusedRotation *= sensorOffset; + setFusedRotationReady(); #if SEND_ACCELERATION { - Vector3 accel = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - this->acceleration[0] = accel.x; - this->acceleration[1] = accel.y; - this->acceleration[2] = accel.z; - this->newAcceleration = true; + acceleration = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + setAccelerationReady(); } #endif - - if(!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } } void BNO055Sensor::startCalibration(int calibrationType) { diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 3772d206d..c0f6873b5 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -119,11 +119,7 @@ void BNO080Sensor::motionLoop() imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); fusedRotation *= sensorOffset; - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) - { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } + setFusedRotationReady(); // Leave new quaternion if context open, it's closed later #else // USE_6_AXIS @@ -133,11 +129,7 @@ void BNO080Sensor::motionLoop() imu.getQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, magneticAccuracyEstimate, calibrationAccuracy); fusedRotation *= sensorOffset; - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) - { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } + setFusedRotationReady(); // Leave new quaternion if context open, it's closed later #endif // USE_6_AXIS @@ -145,8 +137,8 @@ void BNO080Sensor::motionLoop() #if SEND_ACCELERATION { uint8_t acc; - this->imu.getLinAccel(this->acceleration[0], this->acceleration[1], this->acceleration[2], acc); - this->newAcceleration = true; + imu.getLinAccel(acceleration.x, acceleration.y, acceleration.z, acc); + setAccelerationReady(); } #endif // SEND_ACCELERATION } // Closing new quaternion if context @@ -214,15 +206,15 @@ void BNO080Sensor::sendData() #ifdef DEBUG_SENSOR m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); #endif - } #if SEND_ACCELERATION - if(newAcceleration) - { - newAcceleration = false; - networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration); - } + if (newAcceleration) + { + newAcceleration = false; + networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration); + } #endif + } #if !USE_6_AXIS networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index f2ccaf812..6bd978296 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -102,14 +102,14 @@ void ICM20948Sensor::sendData() networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy); } #endif - } #if SEND_ACCELERATION - if(newAcceleration) { - newAcceleration = false; - networkConnection.sendSensorAcceleration(sensorId, acceleration); - } + if (newAcceleration) { + newAcceleration = false; + networkConnection.sendSensorAcceleration(sensorId, acceleration); + } #endif + } } void ICM20948Sensor::startCalibration(int calibrationType) @@ -478,7 +478,7 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) }; sfusion.updateAcc(Axyz); - sfusion.getLinearAcc(this->acceleration); + acceleration = sfusion.getLinearAccVec(); this->newAcceleration = true; } } diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index a6ae04856..e327ebd88 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -141,8 +141,9 @@ void MPU6050Sensor::motionLoop() fusedRotation = sfusion.getQuaternionQuat(); fusedRotation *= sensorOffset; + setFusedRotationReady(); - #if SEND_ACCELERATION + #if SEND_ACCELERATION { this->imu.dmpGetAccel(&this->rawAccel, this->fifoBuffer); @@ -152,16 +153,10 @@ void MPU6050Sensor::motionLoop() sfusion.updateAcc(Axyz); - sfusion.getLinearAcc(this->acceleration); - this->newAcceleration = true; - } - #endif - - if (!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) - { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; + acceleration = sfusion.getLinearAccVec(); + setAccelerationReady(); } + #endif } } diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 0ff4ec5f6..3f9f44878 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -165,7 +165,7 @@ void MPU9250Sensor::motionLoop() { fusedRotation = sfusion.getQuaternionQuat(); -#if SEND_ACCELERATION + #if SEND_ACCELERATION { int16_t atemp[3]; this->imu.dmpGetAccel(atemp, dmpPacket); @@ -173,10 +173,10 @@ void MPU9250Sensor::motionLoop() { sfusion.updateAcc(Axyz); - sfusion.getLinearAcc(this->acceleration); - this->newAcceleration = true; + acceleration = sfusion.getLinearAccVec(); + setAccelerationReady(); } -#endif + #endif #else union fifo_sample_raw buf; @@ -200,16 +200,12 @@ void MPU9250Sensor::motionLoop() { fusedRotation = sfusion.getQuaternionQuat(); #if SEND_ACCELERATION - sfusion.getLinearAcc(this->acceleration); - this->newAcceleration = true; + acceleration = sfusion.getLinearAccVec(); + setAccelerationReady(); #endif #endif fusedRotation *= sensorOffset; - - if(!lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } + setFusedRotationReady(); } void MPU9250Sensor::startCalibration(int calibrationType) { diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index aaad8b9b4..33b95e48d 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -29,22 +29,34 @@ SensorStatus Sensor::getSensorState() { return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } +void Sensor::setAccelerationReady() { + newAcceleration = true; +} + +void Sensor::setFusedRotationReady() { + bool changed = OPTIMIZE_UPDATES ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) : true; + if (ENABLE_INSPECTION || changed) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } +} + void Sensor::sendData() { - if(newFusedRotation) { + if (newFusedRotation) { newFusedRotation = false; networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); #ifdef DEBUG_SENSOR m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); #endif - } #if SEND_ACCELERATION - if(newAcceleration) { - newAcceleration = false; - networkConnection.sendSensorAcceleration(sensorId, acceleration); - } + if (newAcceleration) { + newAcceleration = false; + networkConnection.sendSensorAcceleration(sensorId, acceleration); + } #endif + } } void Sensor::printTemperatureCalibrationUnsupported() { diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index b736756d9..da6ce90b4 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -58,6 +58,8 @@ class Sensor virtual void postSetup(){}; virtual void motionLoop(){}; virtual void sendData(); + virtual void setAccelerationReady(); + virtual void setFusedRotationReady(); virtual void startCalibration(int calibrationType){}; virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState(); @@ -76,6 +78,9 @@ class Sensor uint8_t getSensorType() { return sensorType; }; + const Vector3& getAcceleration() { + return acceleration; + }; const Quat& getFusedRotation() { return fusedRotation; }; @@ -98,7 +103,7 @@ class Sensor Quat lastFusedRotationSent{}; bool newAcceleration = false; - float acceleration[3]{}; + Vector3 acceleration{}; SlimeVR::Logging::Logger m_Logger; From 54e5167f1563099777d626ccbdfaa32ffea8ac04 Mon Sep 17 00:00:00 2001 From: Przemyslaw Romaniak Date: Mon, 18 Sep 2023 21:34:39 +0200 Subject: [PATCH 05/23] Ability to set sensor to be mandatory or not. (#282) Ability to set sensor to be mandatory or not. Adjusted ESP32C3 workaround. --- lib/i2cscan/i2cscan.cpp | 4 ++-- src/defines.h | 7 +++++-- src/sensors/ErroneousSensor.h | 2 +- src/sensors/SensorManager.cpp | 16 +++++++++++----- src/sensors/SensorManager.h | 2 +- 5 files changed, 20 insertions(+), 11 deletions(-) diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp index ae94247fa..a252caed7 100644 --- a/lib/i2cscan/i2cscan.cpp +++ b/lib/i2cscan/i2cscan.cpp @@ -114,14 +114,14 @@ namespace I2CSCAN bool hasDevOnBus(uint8_t addr) { byte error; #if ESP32C3 - int retries = 1; + int retries = 2; do { #endif Wire.beginTransmission(addr); error = Wire.endTransmission(); #if ESP32C3 } - while (error == 5 && retries--); + while (error != 0 && retries--); #endif if(error == 0) return true; diff --git a/src/defines.h b/src/defines.h index ad27b61cc..3763dfff6 100644 --- a/src/defines.h +++ b/src/defines.h @@ -32,6 +32,9 @@ #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 2 // Axis mapping example @@ -45,8 +48,8 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P #ifndef IMU_DESC_LIST #define IMU_DESC_LIST \ - IMU_DESC_ENTRY(IMU, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PIN_IMU_INT ) \ - IMU_DESC_ENTRY(SECOND_IMU, SECONDARY_IMU_ADDRESS_TWO, SECOND_IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PIN_IMU_INT_2) + 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): diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h index c298b7fbc..4dc1d5310 100644 --- a/src/sensors/ErroneousSensor.h +++ b/src/sensors/ErroneousSensor.h @@ -40,7 +40,7 @@ namespace SlimeVR void motionLoop() override final{}; void sendData() override{}; void startCalibration(int calibrationType) override final{}; - SensorStatus getSensorState() override; + SensorStatus getSensorState() override final; private: uint8_t m_ExpectedType; diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 1ce8a37bf..b2c4931ba 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -41,14 +41,14 @@ namespace SlimeVR { namespace Sensors { - Sensor* SensorManager::buildSensor(uint8_t sensorID, uint8_t imuType, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, int extraParam) + Sensor* SensorManager::buildSensor(uint8_t sensorID, uint8_t imuType, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, bool optional, int extraParam) { m_Logger.trace("Building IMU with: id=%d,\n\ imuType=0x%02X, address=0x%02X, rotation=%f,\n\ - sclPin=%d, sdaPin=%d, extraParam=%d", + sclPin=%d, sdaPin=%d, extraParam=%d, optional=%d", sensorID, imuType, address, rotation, - sclPin, sdaPin, extraParam); + sclPin, sdaPin, extraParam, optional); // Now start detecting and building the IMU Sensor* sensor = nullptr; @@ -60,8 +60,14 @@ namespace SlimeVR if (I2CSCAN::hasDevOnBus(address)) { m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address); } else { - m_Logger.debug("Sensor %d not found at address 0x%02X", sensorID + 1, address); - sensor = new EmptySensor(sensorID); + if (!optional) { + m_Logger.error("Mandatory sensor %d not found at address 0x%02X", sensorID + 1, address); + sensor = new ErroneousSensor(sensorID, imuType); + } + else { + m_Logger.debug("Optional sensor %d not found at address 0x%02X", sensorID + 1, address); + sensor = new EmptySensor(sensorID); + } return sensor; } diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 1eacb8f8c..e68033ee7 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -63,7 +63,7 @@ namespace SlimeVR SlimeVR::Logging::Logger m_Logger; std::vector m_Sensors; - Sensor* buildSensor(uint8_t sensorID, uint8_t imuType, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, int extraParam = 0); + Sensor* buildSensor(uint8_t sensorID, uint8_t imuType, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, bool optional = false, int extraParam = 0); uint8_t activeSCL = 0; uint8_t activeSDA = 0; From 63d25dafc6aaa75b88c5d4f56c69bab49208721f Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Thu, 21 Sep 2023 16:44:19 +0200 Subject: [PATCH 06/23] ICM20948 no timeout detected fix (#287) * ICM20948 no timeout detected fix * Change ODR because we pull double of the data. --- src/sensors/icm20948sensor.cpp | 39 +++++++++++++++++++++++++++++----- src/sensors/icm20948sensor.h | 7 ++++++ 2 files changed, 41 insertions(+), 5 deletions(-) diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 6bd978296..6cae5ba1e 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -64,9 +64,22 @@ void ICM20948Sensor::motionLoop() } #endif + hasdata = false; readFIFOToEnd(); readRotation(); checkSensorTimeout(); +// Performance Test +/* + if (hasdata) cntrounds ++; + + if ((lastData2 + 2000) <= millis()) + { + lastData2 = millis(); + printf("Data worked/2sec: %d, Dataframes: %d\n", cntrounds,cntbuf); + cntbuf = 0; + cntrounds = 0; + } +*/ } void ICM20948Sensor::readFIFOToEnd() @@ -82,6 +95,9 @@ void ICM20948Sensor::readFIFOToEnd() if(readStatus == ICM_20948_Stat_Ok) { dmpData = dmpDataTemp; +// Performance Test +// cntbuf ++; + hasdata = true; readFIFOToEnd(); } } @@ -127,6 +143,19 @@ void ICM20948Sensor::startCalibrationAutoSave() void ICM20948Sensor::startDMP() { +#ifdef ESP32 + #if ESP32C3 + #define ICM20948_ODRGYR 1 + #define ICM20948_ODRAXL 1 + #else + #define ICM20948_ODRGYR 1 + #define ICM20948_ODRAXL 1 + #endif +#else + #define ICM20948_ODRGYR 1 + #define ICM20948_ODRAXL 1 +#endif + if(imu.initializeDMP() == ICM_20948_Stat_Ok) { m_Logger.debug("DMP initialized"); @@ -181,7 +210,7 @@ void ICM20948Sensor::startDMP() #if(USE_6_AXIS) { - if(imu.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok) + if(imu.setDMPODRrate(DMP_ODR_Reg_Quat6, ICM20948_ODRGYR) == ICM_20948_Stat_Ok) { m_Logger.debug("Set Quat6 to 100Hz frequency"); } @@ -193,7 +222,7 @@ void ICM20948Sensor::startDMP() } #else { - if(imu.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok) + if(imu.setDMPODRrate(DMP_ODR_Reg_Quat9, ICM20948_ODRGYR) == ICM_20948_Stat_Ok) { m_Logger.debug("Set Quat9 to 100Hz frequency"); } @@ -206,7 +235,7 @@ void ICM20948Sensor::startDMP() #endif #if(SEND_ACCELERATION) - if (this->imu.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok) + if (this->imu.setDMPODRrate(DMP_ODR_Reg_Accel, ICM20948_ODRAXL) == ICM_20948_Stat_Ok) { this->m_Logger.debug("Set Accel to 100Hz frequency"); } @@ -307,7 +336,7 @@ void ICM20948Sensor::readRotation() { #if(USE_6_AXIS) { - if ((dmpData.header & DMP_header_bitmap_Quat6) > 0) + if (((dmpData.header & DMP_header_bitmap_Quat6) > 0) && hasdata) { // 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. @@ -334,7 +363,7 @@ void ICM20948Sensor::readRotation() } #else { - if((dmpData.header & DMP_header_bitmap_Quat9) > 0) + if(((dmpData.header & DMP_header_bitmap_Quat9) > 0) && hasdata) { // 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. diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index f3e5c36bc..ca116ba21 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -47,6 +47,13 @@ class ICM20948Sensor : public Sensor unsigned long lastData = 0; unsigned long lastDataSent = 0; int bias_save_counter = 0; + bool hasdata = false; +// Performance test +/* + uint8_t cntbuf = 0; + int32_t cntrounds = 0; + unsigned long lastData2 = 0; +*/ #define DMPNUMBERTODOUBLECONVERTER 1073741824.0; From ffebf5fbb18f079c1282e9c6a3f2ad40fbfbcdbc Mon Sep 17 00:00:00 2001 From: Przemyslaw Romaniak Date: Thu, 21 Sep 2023 16:45:24 +0200 Subject: [PATCH 07/23] Report IMU errors (#288) * Make not working sensors report error by default * Set IMU error flag if any sensor reported error --- src/sensors/EmptySensor.h | 4 ++++ src/sensors/SensorManager.cpp | 7 +++++++ src/sensors/sensor.cpp | 2 +- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/sensors/EmptySensor.h b/src/sensors/EmptySensor.h index dff269315..1263178a9 100644 --- a/src/sensors/EmptySensor.h +++ b/src/sensors/EmptySensor.h @@ -40,6 +40,10 @@ namespace SlimeVR void motionLoop() override final{}; void sendData() override final{}; void startCalibration(int calibrationType) override final{}; + SensorStatus getSensorState() override final + { + return SensorStatus::SENSOR_OFFLINE; + }; }; } } diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index b2c4931ba..6995147bb 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -187,13 +187,20 @@ namespace SlimeVR void SensorManager::update() { // Gather IMU data + bool allIMUGood = true; for (auto sensor : m_Sensors) { if (sensor->isWorking()) { swapI2C(sensor->sclPin, sensor->sdaPin); sensor->motionLoop(); } + if (sensor->getSensorState() == SensorStatus::SENSOR_ERROR) + { + allIMUGood = false; + } } + statusManager.setStatus(SlimeVR::Status::IMU_ERROR, !allIMUGood); + if (!networkConnection.isConnected()) { return; } diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 33b95e48d..730ea68d8 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -26,7 +26,7 @@ #include "calibration.h" SensorStatus Sensor::getSensorState() { - return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; + return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR; } void Sensor::setAccelerationReady() { From d66ebd0d97bcef6910a200aeb1bc785cdedc5ac3 Mon Sep 17 00:00:00 2001 From: Castle McCloud <31018599+castlemccloud@users.noreply.github.com> Date: Thu, 21 Sep 2023 11:04:13 -0400 Subject: [PATCH 08/23] Fix 9250 loop (#246) Co-authored-by: Kitlith --- src/sensors/mpu9250sensor.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 3f9f44878..4419e5fda 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -360,6 +360,9 @@ void MPU9250Sensor::parseMagData(int16_t data[3]) { //apply offsets and scale factors from Magneto for (unsigned i = 0; i < 3; i++) { temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); + } + + for (unsigned i = 0; i < 3; i++) { #if useFullCalibrationMatrix == true Mxyz[i] = m_Calibration.M_Ainv[i][0] * temp[0] + m_Calibration.M_Ainv[i][1] * temp[1] + m_Calibration.M_Ainv[i][2] * temp[2]; #else @@ -382,6 +385,9 @@ void MPU9250Sensor::parseAccelData(int16_t data[3]) { for (unsigned i = 0; i < 3; i++) { #if !MPU_USE_DMPMAG temp[i] = (Axyz[i] - m_Calibration.A_B[i]); + } + + for (unsigned i = 0; i < 3; i++) { #if useFullCalibrationMatrix == true Axyz[i] = m_Calibration.A_Ainv[i][0] * temp[0] + m_Calibration.A_Ainv[i][1] * temp[1] + m_Calibration.A_Ainv[i][2] * temp[2]; #else From 69523f2a035f1b2c5d01cd79171efb4ace830659 Mon Sep 17 00:00:00 2001 From: 0forks <114709761+0forks@users.noreply.github.com> Date: Fri, 22 Sep 2023 16:10:13 +0300 Subject: [PATCH 09/23] Fix mahony/madgwick updates (#283) --- src/sensors/SensorFusion.cpp | 50 +++++++++++++++++++----------------- 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index 106339a25..e36380a40 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -57,34 +57,36 @@ namespace SlimeVR { if (deltat < 0) deltat = gyrTs; - #if SENSOR_USE_MAHONY + #if SENSOR_USE_MAHONY || SENSOR_USE_MADGWICK + sensor_real_t Axyz[3] {0.0f, 0.0f, 0.0f}; if (accelUpdated) { - if (magExist) { - mahony.update(qwxyz, bAxyz[0], bAxyz[1], bAxyz[2], - Gxyz[0], Gxyz[1], Gxyz[2], - deltat); - } else { - mahony.update(qwxyz, bAxyz[0], bAxyz[1], bAxyz[2], - Gxyz[0], Gxyz[1], Gxyz[2], - bMxyz[0], bMxyz[1], bMxyz[2], - deltat); - } + std::copy(bAxyz, bAxyz+3, Axyz); + accelUpdated = false; + } + #endif + + #if SENSOR_USE_MAHONY + if (!magExist) { + mahony.update(qwxyz, Axyz[0], Axyz[1], Axyz[2], + Gxyz[0], Gxyz[1], Gxyz[2], + deltat); + } else { + mahony.update(qwxyz, Axyz[0], Axyz[1], Axyz[2], + Gxyz[0], Gxyz[1], Gxyz[2], + bMxyz[0], bMxyz[1], bMxyz[2], + deltat); } - accelUpdated = false; #elif SENSOR_USE_MADGWICK - if (accelUpdated) { - if (magExist) { - madgwick.update(qwxyz, bAxyz[0], bAxyz[1], bAxyz[2], - Gxyz[0], Gxyz[1], Gxyz[2], - deltat); - } else { - madgwick.update(qwxyz, bAxyz[0], bAxyz[1], bAxyz[2], - Gxyz[0], Gxyz[1], Gxyz[2], - bMxyz[0], bMxyz[1], bMxyz[2], - deltat); - } + if (!magExist) { + madgwick.update(qwxyz, Axyz[0], Axyz[1], Axyz[2], + Gxyz[0], Gxyz[1], Gxyz[2], + deltat); + } else { + madgwick.update(qwxyz, Axyz[0], Axyz[1], Axyz[2], + Gxyz[0], Gxyz[1], Gxyz[2], + bMxyz[0], bMxyz[1], bMxyz[2], + deltat); } - accelUpdated = false; #elif SENSOR_USE_BASICVQF basicvqf.updateGyr(Gxyz, deltat); #elif SENSOR_USE_VQF From afca9b295763ca127a940d448800482065acb5af Mon Sep 17 00:00:00 2001 From: 0forks <114709761+0forks@users.noreply.github.com> Date: Fri, 22 Sep 2023 16:10:41 +0300 Subject: [PATCH 10/23] BMI160: Print fusion algo name in debug log (#284) Print fusion algo name in BMI160 debug log --- src/sensors/SensorFusion.h | 10 ++++++++++ src/sensors/bmi160sensor.cpp | 4 +--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index b8c739f18..09e475f34 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -13,6 +13,16 @@ #define SENSOR_FUSION_BASICVQF 3 #define SENSOR_FUSION_VQF 4 +#if SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY + #define SENSOR_FUSION_TYPE_STRING "mahony" +#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK + #define SENSOR_FUSION_TYPE_STRING "madgwick" +#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF + #define SENSOR_FUSION_TYPE_STRING "bvqf" +#elif SENSOR_FUSION_TYPE == SENSOR_FUSION_VQF + #define SENSOR_FUSION_TYPE_STRING "vqf" +#endif + #define SENSOR_USE_MAHONY (SENSOR_FUSION_TYPE == SENSOR_FUSION_MAHONY) #define SENSOR_USE_MADGWICK (SENSOR_FUSION_TYPE == SENSOR_FUSION_MADGWICK) #define SENSOR_USE_BASICVQF (SENSOR_FUSION_TYPE == SENSOR_FUSION_BASICVQF) diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 70b925e6b..0a1352e76 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -323,9 +323,7 @@ void BMI160Sensor::motionLoop() { if (end - lastCpuUsagePrinted > 1e6) { bool restDetected = sfusion.getRestDetected(); - #define BMI160_FUSION_TYPE "sfusion" - - m_Logger.debug("readFIFO took %0.4f ms, read gyr %i acc %i mag %i rest %i resets %i readerrs %i type " BMI160_FUSION_TYPE, + m_Logger.debug("readFIFO took %0.4f ms, read gyr %i acc %i mag %i rest %i resets %i readerrs %i type " SENSOR_FUSION_TYPE_STRING, ((float)cpuUsageMicros / 1e3f), gyrReads, accReads, From 41f57bce5b95b7e9b64a41aec0a2ee517dba13f1 Mon Sep 17 00:00:00 2001 From: 0forks <114709761+0forks@users.noreply.github.com> Date: Fri, 22 Sep 2023 16:11:23 +0300 Subject: [PATCH 11/23] BMI160: Fix magnetometer error check (#285) Fix BMI160 error register checks --- lib/bmi160/BMI160.cpp | 8 +------- lib/bmi160/BMI160.h | 15 ++++++++------- src/sensors/bmi160sensor.cpp | 12 ------------ 3 files changed, 9 insertions(+), 26 deletions(-) diff --git a/lib/bmi160/BMI160.cpp b/lib/bmi160/BMI160.cpp index 8313de50a..218a05f1a 100644 --- a/lib/bmi160/BMI160.cpp +++ b/lib/bmi160/BMI160.cpp @@ -121,16 +121,10 @@ void BMI160::setMagDeviceAddress(uint8_t addr) { setRegister(BMI160_RA_MAG_IF_0_DEVADDR, addr << 1); // 0 bit of address is reserved and needs to be shifted } -bool BMI160::setMagRegister(uint8_t addr, uint8_t value) { +void BMI160::setMagRegister(uint8_t addr, uint8_t value) { setRegister(BMI160_RA_MAG_IF_4_WRITE_VALUE, value); setRegister(BMI160_RA_MAG_IF_3_WRITE_RA, addr); delay(3); - I2CdevMod::readByte(devAddr, BMI160_RA_ERR, buffer); - if (buffer[0] & BMI160_ERR_MASK_I2C_FAIL) { - printf("BMI160: mag register proxy write error\n"); - return false; - } - return true; } /** Get Device ID. diff --git a/lib/bmi160/BMI160.h b/lib/bmi160/BMI160.h index dc7701a55..228f9256e 100644 --- a/lib/bmi160/BMI160.h +++ b/lib/bmi160/BMI160.h @@ -52,7 +52,7 @@ THE SOFTWARE. #define BMI160_RA_MAG_CONF 0x44 #define BMI160_RA_MAG_IF_0_DEVADDR 0x4B #define BMI160_RA_MAG_IF_1_MODE 0x4C -#define BMI160_RA_MAG_IF_2_READ_RA 0x4D +#define BMI160_RA_MAG_IF_2_READ_RA 0x4D #define BMI160_RA_MAG_IF_3_WRITE_RA 0x4E #define BMI160_RA_MAG_IF_4_WRITE_VALUE 0x4F #define BMI160_RA_IF_CONF 0x6B @@ -343,11 +343,12 @@ typedef enum { #define BMI160_FIFO_A_LEN 6 #define BMI160_RA_ERR 0x02 -#define BMI160_ERR_MASK_MAG_DRDY_ERR 0x10000000 -#define BMI160_ERR_MASK_DROP_CMD_ERR 0x01000000 -#define BMI160_ERR_MASK_I2C_FAIL 0x00100000 -#define BMI160_ERR_MASK_ERROR_CODE 0x00011110 -#define BMI160_ERR_MASK_CHIP_NOT_OPERABLE 0x00000001 +#define BMI160_ERR_MASK_MAG_DRDY_ERR 0b10000000 +#define BMI160_ERR_MASK_DROP_CMD_ERR 0b01000000 +// datasheet is unclear - reserved or i2c_fail_err? +// #define BMI160_ERR_MASK_I2C_FAIL 0b00100000 +#define BMI160_ERR_MASK_ERROR_CODE 0b00011110 +#define BMI160_ERR_MASK_CHIP_NOT_OPERABLE 0b00000001 /** * Interrupt Latch Mode options @@ -755,7 +756,7 @@ class BMI160 { void waitForMagDrdy(); bool getSensorTime(uint32_t *v_sensor_time_u32); void setMagDeviceAddress(uint8_t addr); - bool setMagRegister(uint8_t addr, uint8_t value); + void setMagRegister(uint8_t addr, uint8_t value); bool getErrReg(uint8_t* out); private: diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 0a1352e76..a94036ff8 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -33,12 +33,6 @@ void BMI160Sensor::initHMC(BMI160MagRate magRate) { imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); delay(60); - imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_1); - imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_2); - imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_3); - imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_4); - imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_5); - /* Enable MAG interface */ imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); delay(1); @@ -65,12 +59,6 @@ void BMI160Sensor::initQMC(BMI160MagRate magRate) { imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); delay(60); - imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_1); - imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_2); - imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_3); - imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_4); - imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_5); - /* Enable MAG interface */ imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); delay(1); From 3b4ca6e627ae17029288a1d44b827e6fc4f7a508 Mon Sep 17 00:00:00 2001 From: 0forks <114709761+0forks@users.noreply.github.com> Date: Fri, 22 Sep 2023 16:12:19 +0300 Subject: [PATCH 12/23] BMI160: Fix double rest detection (#286) Fix double rest detection with vqf --- src/sensors/SensorFusion.h | 2 +- src/sensors/SensorFusionRestDetect.cpp | 16 +++++++--------- src/sensors/SensorFusionRestDetect.h | 8 ++++---- 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index 09e475f34..f5c3da420 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -57,7 +57,7 @@ namespace SlimeVR class SensorFusion { public: - SensorFusion(float gyrTs, float accTs=-1.0, float magTs=-1.0) + SensorFusion(sensor_real_t gyrTs, sensor_real_t accTs=-1.0, sensor_real_t magTs=-1.0) : gyrTs(gyrTs), accTs( (accTs<0) ? gyrTs : accTs ), magTs( (magTs<0) ? gyrTs : magTs ) diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index f7c47fc5f..2e6630d37 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/src/sensors/SensorFusionRestDetect.cpp @@ -4,27 +4,25 @@ namespace SlimeVR { namespace Sensors { + #if !SENSOR_FUSION_WITH_RESTDETECT void SensorFusionRestDetect::updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat) { - #if !SENSOR_WITH_REST_DETECT - if (deltat < 0) deltat = accTs; - restDetection.updateAcc(deltat, Axyz); - #endif + if (deltat < 0) deltat = accTs; + restDetection.updateAcc(deltat, Axyz); SensorFusion::updateAcc(Axyz, deltat); } void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat) { - #if !SENSOR_WITH_REST_DETECT - if (deltat < 0) deltat = accTs; - restDetection.updateGyr(deltat, Gxyz); - #endif + if (deltat < 0) deltat = gyrTs; + restDetection.updateGyr(deltat, Gxyz); SensorFusion::updateGyro(Gxyz, deltat); } + #endif bool SensorFusionRestDetect::getRestDetected() { - #if !SENSOR_WITH_REST_DETECT + #if !SENSOR_FUSION_WITH_RESTDETECT return restDetection.getRestDetected(); #elif SENSOR_USE_VQF return vqf.getRestDetected(); diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index 684d8e640..e38d351c0 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/src/sensors/SensorFusionRestDetect.h @@ -15,7 +15,7 @@ namespace SlimeVR { namespace Sensors { - #if !SENSOR_WITH_REST_DETECT + #if !SENSOR_FUSION_WITH_RESTDETECT struct SensorRestDetectionParams: RestDetectionParams { SensorRestDetectionParams() : RestDetectionParams() { restMinTimeMicros = 2.0f * 1e6; @@ -30,7 +30,7 @@ namespace SlimeVR public: SensorFusionRestDetect(float gyrTs, float accTs=-1.0, float magTs=-1.0) : SensorFusion(gyrTs, accTs, magTs) - #if !SENSOR_WITH_REST_DETECT + #if !SENSOR_FUSION_WITH_RESTDETECT , restDetection(restDetectionParams, gyrTs, (accTs<0) ? gyrTs : accTs) #endif @@ -38,12 +38,12 @@ namespace SlimeVR bool getRestDetected(); - #if !SENSOR_WITH_REST_DETECT + #if !SENSOR_FUSION_WITH_RESTDETECT void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat); void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat); #endif protected: - #if !SENSOR_WITH_REST_DETECT + #if !SENSOR_FUSION_WITH_RESTDETECT SensorRestDetectionParams restDetectionParams {}; RestDetection restDetection; #endif From 77d9d3229e71512ff06d8359636d0b0aeaccc203 Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Fri, 22 Sep 2023 15:12:59 +0200 Subject: [PATCH 13/23] icm20948 timeout correction because of wifi set (#289) --- src/sensors/icm20948sensor.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 6cae5ba1e..9e60914d8 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -324,11 +324,12 @@ void ICM20948Sensor::startMotionLoop() void ICM20948Sensor::checkSensorTimeout() { - if(lastData + 1000 < millis()) { + unsigned long currenttime = millis(); + if(lastData + 2000 < currenttime) { working = false; - lastData = millis(); - m_Logger.error("Sensor timeout I2C Address 0x%02x", addr); + m_Logger.error("Sensor timeout I2C Address 0x%02x delaytime: %d ms", addr, currenttime-lastData ); networkConnection.sendSensorError(this->sensorId, 1); + lastData = currenttime; } } From 3789a4cdb89ad4d5bcfec4b46e745992385f4879 Mon Sep 17 00:00:00 2001 From: nekomona Date: Fri, 22 Sep 2023 21:14:28 +0800 Subject: [PATCH 14/23] Add `GET WIFISCAN` and base64 WiFi credential commands (#262) --- src/network/featureflags.h | 16 +++++---- src/serial/serialcommands.cpp | 65 +++++++++++++++++++++++++++++------ 2 files changed, 64 insertions(+), 17 deletions(-) diff --git a/src/network/featureflags.h b/src/network/featureflags.h index b1be622a2..b0820be40 100644 --- a/src/network/featureflags.h +++ b/src/network/featureflags.h @@ -29,7 +29,7 @@ /** * Bit packed flags, enum values start with 0 and indicate which bit it is. - * + * * Change the enums and `flagsEnabled` inside to extend. */ struct ServerFeatures { @@ -37,9 +37,9 @@ struct ServerFeatures { enum EServerFeatureFlags: uint32_t { // Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). PROTOCOL_BUNDLE_SUPPORT, - + // Add new flags here - + BITS_TOTAL, }; @@ -73,15 +73,17 @@ class FirmwareFeatures { public: enum EFirmwareFeatureFlags: uint32_t { // EXAMPLE_FEATURE, - + B64_WIFI_SCANNING = 1, + // Add new flags here - + BITS_TOTAL, }; // Flags to send static constexpr const std::initializer_list flagsEnabled = { // EXAMPLE_FEATURE, + B64_WIFI_SCANNING, // Add enabled flags here }; @@ -89,7 +91,7 @@ class FirmwareFeatures { static constexpr auto flags = []{ constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1; std::array packed{}; - + for (uint32_t bit : flagsEnabled) { packed[bit / 8] |= 1 << (bit % 8); } @@ -98,4 +100,4 @@ class FirmwareFeatures { }(); }; -#endif \ No newline at end of file +#endif diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 2158ec469..8030b93e3 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -24,6 +24,7 @@ #include "serialcommands.h" #include "logging/Logger.h" #include +#include #include "GlobalVars.h" #include "batterymonitor.h" #include "utils.h" @@ -37,19 +38,37 @@ namespace SerialCommands { CmdCallback<6> cmdCallbacks; CmdParser cmdParser; - CmdBuffer<64> cmdBuffer; + CmdBuffer<256> cmdBuffer; void cmdSet(CmdParser * parser) { - if(parser->getParamCount() != 1 && parser->equalCmdParam(1, "WIFI") ) { - if(parser->getParamCount() < 3) { - logger.error("CMD SET WIFI ERROR: Too few arguments"); - logger.info("Syntax: SET WIFI \"\" \"\""); - } else { - WiFiNetwork::setWiFiCredentials(parser->getCmdParam(2), parser->getCmdParam(3)); - logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting"); - } + if(parser->getParamCount() != 1) { + if (parser->equalCmdParam(1, "WIFI")) { + if(parser->getParamCount() < 3) { + logger.error("CMD SET WIFI ERROR: Too few arguments"); + logger.info("Syntax: SET WIFI \"\" \"\""); + } else { + WiFiNetwork::setWiFiCredentials(parser->getCmdParam(2), parser->getCmdParam(3)); + logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting"); + } + } else if (parser->equalCmdParam(1, "BWIFI")) { + if(parser->getParamCount() < 3) { + logger.error("CMD SET BWIFI ERROR: Too few arguments"); + logger.info("Syntax: SET BWIFI "); + } else { + char ssid[33]; + char pass[65]; + const char * b64ssid = parser->getCmdParam(2); + const char * b64pass = parser->getCmdParam(3); + base64_decode_chars(b64ssid, strlen_P(b64ssid), ssid); + base64_decode_chars(b64pass, strlen_P(b64pass), pass); + WiFiNetwork::setWiFiCredentials(ssid, pass); + logger.info("CMD SET BWIFI OK: New wifi credentials set, reconnecting"); + } + } else { + logger.error("CMD SET ERROR: Unrecognized variable to set"); + } } else { - logger.error("CMD SET ERROR: Unrecognized variable to set"); + logger.error("CMD SET ERROR: No variable to set"); } } @@ -153,6 +172,32 @@ namespace SerialCommands { logger.info("[TEST] Sensor[0] sent some data, looks working."); } } + + if (parser->equalCmdParam(1, "WIFISCAN")) { + logger.info("[WSCAN] Scanning for WiFi networks..."); + + // Scan would fail if connecting, stop connecting before scan + if (WiFi.status() != WL_CONNECTED) WiFi.disconnect(); + + WiFi.scanNetworks(); + + int scanRes = WiFi.scanComplete(); + if (scanRes >= 0) { + logger.info("[WSCAN] Found %d networks:", scanRes); + for (int i = 0; i < scanRes; i++) { + logger.info("[WSCAN] %d:\t%02d\t%s\t(%d)\t%s", + i, WiFi.SSID(i).length(), WiFi.SSID(i).c_str(), WiFi.RSSI(i), + ((WiFi.encryptionType(i) == 0) ? "OPEN" : "PASS") + ); + } + WiFi.scanDelete(); + } else { + logger.info("[WSCAN] Scan failed!"); + } + + // Restore conencting state + if (WiFi.status() != WL_CONNECTED) WiFi.begin(); + } } void cmdReboot(CmdParser * parser) { From 1ed8d63e65b2eca6ce0de3adedd46db9ebc0f87e Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Fri, 22 Sep 2023 09:30:25 -0500 Subject: [PATCH 15/23] Add ICM-42688 imu and MMC5983MA magnetometer (#242) --- lib/ICM42688/ICM42688.h | 192 +++++++++++++ lib/ICM42688/MMC5983MA.h | 63 +++++ src/configuration/CalibrationConfig.cpp | 2 + src/configuration/CalibrationConfig.h | 16 +- src/configuration/Configuration.cpp | 19 ++ src/consts.h | 1 + src/main.cpp | 2 +- src/sensors/SensorManager.cpp | 4 + src/sensors/icm42688sensor.cpp | 352 ++++++++++++++++++++++++ src/sensors/icm42688sensor.h | 67 +++++ src/sensors/sensor.cpp | 2 + src/sensors/sensoraddresses.h | 4 +- 12 files changed, 720 insertions(+), 4 deletions(-) create mode 100644 lib/ICM42688/ICM42688.h create mode 100644 lib/ICM42688/MMC5983MA.h create mode 100644 src/sensors/icm42688sensor.cpp create mode 100644 src/sensors/icm42688sensor.h diff --git a/lib/ICM42688/ICM42688.h b/lib/ICM42688/ICM42688.h new file mode 100644 index 000000000..115879390 --- /dev/null +++ b/lib/ICM42688/ICM42688.h @@ -0,0 +1,192 @@ +/* 01/14/2022 Copyright Tlera Corporation + + Created by Kris Winer + + This sketch uses SDA/SCL on pins 21/20 (ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board. + The ICM42688 is a combo sensor with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution. + + Library may be used freely and without limit with attribution. + +*/ + +#ifndef ICM42688_h +#define ICM42688_h + +/* ICM42688 registers +https://media.digikey.com/pdf/Data%20Sheets/TDK%20PDFs/ICM-42688-P_DS_Rev1.2.pdf +*/ +// User Bank 0 +#define ICM42688_DEVICE_CONFIG 0x11 +#define ICM42688_DRIVE_CONFIG 0x13 +#define ICM42688_INT_CONFIG 0x14 +#define ICM42688_FIFO_CONFIG 0x16 +#define ICM42688_TEMP_DATA1 0x1D +#define ICM42688_TEMP_DATA0 0x1E +#define ICM42688_ACCEL_DATA_X1 0x1F +#define ICM42688_ACCEL_DATA_X0 0x20 +#define ICM42688_ACCEL_DATA_Y1 0x21 +#define ICM42688_ACCEL_DATA_Y0 0x22 +#define ICM42688_ACCEL_DATA_Z1 0x23 +#define ICM42688_ACCEL_DATA_Z0 0x24 +#define ICM42688_GYRO_DATA_X1 0x25 +#define ICM42688_GYRO_DATA_X0 0x26 +#define ICM42688_GYRO_DATA_Y1 0x27 +#define ICM42688_GYRO_DATA_Y0 0x28 +#define ICM42688_GYRO_DATA_Z1 0x29 +#define ICM42688_GYRO_DATA_Z0 0x2A +#define ICM42688_TMST_FSYNCH 0x2B +#define ICM42688_TMST_FSYNCL 0x2C +#define ICM42688_INT_STATUS 0x2D +#define ICM42688_FIFO_COUNTH 0x2E +#define ICM42688_FIFO_COUNTL 0x2F +#define ICM42688_FIFO_DATA 0x30 +#define ICM42688_APEX_DATA0 0x31 +#define ICM42688_APEX_DATA1 0x32 +#define ICM42688_APEX_DATA2 0x33 +#define ICM42688_APEX_DATA3 0x34 +#define ICM42688_APEX_DATA4 0x35 +#define ICM42688_APEX_DATA5 0x36 +#define ICM42688_INT_STATUS2 0x37 +#define ICM42688_INT_STATUS3 0x38 +#define ICM42688_SIGNAL_PATH_RESET 0x4B +#define ICM42688_INTF_CONFIG0 0x4C +#define ICM42688_INTF_CONFIG1 0x4D +#define ICM42688_PWR_MGMT0 0x4E +#define ICM42688_GYRO_CONFIG0 0x4F +#define ICM42688_ACCEL_CONFIG0 0x50 +#define ICM42688_GYRO_CONFIG1 0x51 +#define ICM42688_GYRO_ACCEL_CONFIG0 0x52 +#define ICM42688_ACCEL_CONFIG1 0x53 +#define ICM42688_TMST_CONFIG 0x54 +#define ICM42688_APEX_CONFIG0 0x56 +#define ICM42688_SMD_CONFIG 0x57 +#define ICM42688_FIFO_CONFIG1 0x5F +#define ICM42688_FIFO_CONFIG2 0x60 +#define ICM42688_FIFO_CONFIG3 0x61 +#define ICM42688_FSYNC_CONFIG 0x62 +#define ICM42688_INT_CONFIG0 0x63 +#define ICM42688_INT_CONFIG1 0x64 +#define ICM42688_INT_SOURCE0 0x65 +#define ICM42688_INT_SOURCE1 0x66 +#define ICM42688_INT_SOURCE3 0x68 +#define ICM42688_INT_SOURCE4 0x69 +#define ICM42688_FIFO_LOST_PKT0 0x6C +#define ICM42688_FIFO_LOST_PKT1 0x6D +#define ICM42688_SELF_TEST_CONFIG 0x70 +#define ICM42688_WHO_AM_I 0x75 // should return 0x47 +#define ICM42688_REG_BANK_SEL 0x76 + +// User Bank 1 +#define ICM42688_SENSOR_CONFIG0 0x03 +#define ICM42688_GYRO_CONFIG_STATIC2 0x0B +#define ICM42688_GYRO_CONFIG_STATIC3 0x0C +#define ICM42688_GYRO_CONFIG_STATIC4 0x0D +#define ICM42688_GYRO_CONFIG_STATIC5 0x0E +#define ICM42688_GYRO_CONFIG_STATIC6 0x0F +#define ICM42688_GYRO_CONFIG_STATIC7 0x10 +#define ICM42688_GYRO_CONFIG_STATIC8 0x11 +#define ICM42688_GYRO_CONFIG_STATIC9 0x12 +#define ICM42688_GYRO_CONFIG_STATIC10 0x13 +#define ICM42688_XG_ST_DATA 0x5F +#define ICM42688_YG_ST_DATA 0x60 +#define ICM42688_ZG_ST_DATA 0x61 +#define ICM42688_TMSTAL0 0x63 +#define ICM42688_TMSTAL1 0x64 +#define ICM42688_TMSTAL2 0x62 +#define ICM42688_INTF_CONFIG4 0x7A +#define ICM42688_INTF_CONFIG5 0x7B +#define ICM42688_INTF_CONFIG6 0x7C + +// User Bank 2 +#define ICM42688_ACCEL_CONFIG_STATIC2 0x03 +#define ICM42688_ACCEL_CONFIG_STATIC3 0x04 +#define ICM42688_ACCEL_CONFIG_STATIC4 0x05 +#define ICM42688_XA_ST_DATA 0x3B +#define ICM42688_YA_ST_DATA 0x3C +#define ICM42688_ZA_ST_DATA 0x3D + +// User Bank 4 +#define ICM42688_APEX_CONFIG1 0x40 +#define ICM42688_APEX_CONFIG2 0x41 +#define ICM42688_APEX_CONFIG3 0x42 +#define ICM42688_APEX_CONFIG4 0x43 +#define ICM42688_APEX_CONFIG5 0x44 +#define ICM42688_APEX_CONFIG6 0x45 +#define ICM42688_APEX_CONFIG7 0x46 +#define ICM42688_APEX_CONFIG8 0x47 +#define ICM42688_APEX_CONFIG9 0x48 +#define ICM42688_ACCEL_WOM_X_THR 0x4A +#define ICM42688_ACCEL_WOM_Y_THR 0x4B +#define ICM42688_ACCEL_WOM_Z_THR 0x4C +#define ICM42688_INT_SOURCE6 0x4D +#define ICM42688_INT_SOURCE7 0x4E +#define ICM42688_INT_SOURCE8 0x4F +#define ICM42688_INT_SOURCE9 0x50 +#define ICM42688_INT_SOURCE10 0x51 +#define ICM42688_OFFSET_USER0 0x77 +#define ICM42688_OFFSET_USER1 0x78 +#define ICM42688_OFFSET_USER2 0x79 +#define ICM42688_OFFSET_USER3 0x7A +#define ICM42688_OFFSET_USER4 0x7B +#define ICM42688_OFFSET_USER5 0x7C +#define ICM42688_OFFSET_USER6 0x7D +#define ICM42688_OFFSET_USER7 0x7E +#define ICM42688_OFFSET_USER8 0x7F + +#define ICM42688_ADDRESS 0x68 // Address of ICM42688 accel/gyro when ADO = 0 + +#define AFS_2G 0x03 +#define AFS_4G 0x02 +#define AFS_8G 0x01 +#define AFS_16G 0x00 // default + +#define GFS_2000DPS 0x00 // default +#define GFS_1000DPS 0x01 +#define GFS_500DPS 0x02 +#define GFS_250DPS 0x03 +#define GFS_125DPS 0x04 +#define GFS_62_50DPS 0x05 +#define GFS_31_25DPS 0x06 +#define GFS_15_625DPS 0x07 + +// Low Noise mode +#define AODR_32kHz 0x01 +#define AODR_16kHz 0x02 +#define AODR_8kHz 0x03 +#define AODR_4kHz 0x04 +#define AODR_2kHz 0x05 +#define AODR_1kHz 0x06 // default +//Low Noise or Low Power modes +#define AODR_500Hz 0x0F +#define AODR_200Hz 0x07 +#define AODR_100Hz 0x08 +#define AODR_50Hz 0x09 +#define AODR_25Hz 0x0A +#define AODR_12_5Hz 0x0B +// Low Power mode +#define AODR_6_25Hz 0x0C +#define AODR_3_125Hz 0x0D +#define AODR_1_5625Hz 0x0E + +#define GODR_32kHz 0x01 +#define GODR_16kHz 0x02 +#define GODR_8kHz 0x03 +#define GODR_4kHz 0x04 +#define GODR_2kHz 0x05 +#define GODR_1kHz 0x06 // default +#define GODR_500Hz 0x0F +#define GODR_200Hz 0x07 +#define GODR_100Hz 0x08 +#define GODR_50Hz 0x09 +#define GODR_25Hz 0x0A +#define GODR_12_5Hz 0x0B + +#define aMode_OFF 0x01 +#define aMode_LP 0x02 +#define aMode_LN 0x03 + +#define gMode_OFF 0x00 +#define gMode_SBY 0x01 +#define gMode_LN 0x03 + +#endif diff --git a/lib/ICM42688/MMC5983MA.h b/lib/ICM42688/MMC5983MA.h new file mode 100644 index 000000000..bdec5b77e --- /dev/null +++ b/lib/ICM42688/MMC5983MA.h @@ -0,0 +1,63 @@ +/* 06/14/2020 Copyright Tlera Corporation + + Created by Kris Winer + + This sketch uses SDA/SCL on pins 21/20 (Ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board. + The MMC5983MA is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution. + + Library may be used freely and without limit with attribution. + +*/ + +#ifndef MMC5983MA_h +#define MMC5983MA_h + +//Register map for MMC5983MA' +//http://www.memsic.com/userfiles/files/DataSheets/Magnetic-Sensors-Datasheets/MMC5983MA_Datasheet.pdf +#define MMC5983MA_XOUT_0 0x00 +#define MMC5983MA_XOUT_1 0x01 +#define MMC5983MA_YOUT_0 0x02 +#define MMC5983MA_YOUT_1 0x03 +#define MMC5983MA_ZOUT_0 0x04 +#define MMC5983MA_ZOUT_1 0x05 +#define MMC5983MA_XYZOUT_2 0x06 +#define MMC5983MA_TOUT 0x07 +#define MMC5983MA_STATUS 0x08 +#define MMC5983MA_CONTROL_0 0x09 +#define MMC5983MA_CONTROL_1 0x0A +#define MMC5983MA_CONTROL_2 0x0B +#define MMC5983MA_CONTROL_3 0x0C +#define MMC5983MA_PRODUCT_ID 0x2F + +#define MMC5983MA_ADDRESS 0x30 + +// Sample rates +#define MODR_ONESHOT 0x00 +#define MODR_1Hz 0x01 +#define MODR_10Hz 0x02 +#define MODR_20Hz 0x03 +#define MODR_50Hz 0x04 +#define MODR_100Hz 0x05 +#define MODR_200Hz 0x06 // BW = 0x01 only +#define MODR_1000Hz 0x07 // BW = 0x11 only + +//Bandwidths +#define MBW_100Hz 0x00 // 8 ms measurement time +#define MBW_200Hz 0x01 // 4 ms +#define MBW_400Hz 0x02 // 2 ms +#define MBW_800Hz 0x03 // 0.5 ms + +// Set/Reset as a function of measurements +#define MSET_1 0x00 // Set/Reset each data measurement +#define MSET_25 0x01 // each 25 data measurements +#define MSET_75 0x02 +#define MSET_100 0x03 +#define MSET_250 0x04 +#define MSET_500 0x05 +#define MSET_1000 0x06 +#define MSET_2000 0x07 + +#define MMC5983MA_mRes (1.0f / 16384.0f) // mag sensitivity if using 18 bit data +#define MMC5983MA_offset 131072.0f // mag range unsigned to signed + +#endif diff --git a/src/configuration/CalibrationConfig.cpp b/src/configuration/CalibrationConfig.cpp index 75555ad34..9aeb3eec7 100644 --- a/src/configuration/CalibrationConfig.cpp +++ b/src/configuration/CalibrationConfig.cpp @@ -37,6 +37,8 @@ namespace SlimeVR { return "MPU9250"; case ICM20948: return "ICM20948"; + case ICM42688: + return "ICM42688"; default: return "UNKNOWN"; } diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 5b2a3859f..0fe4d91f8 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -76,7 +76,20 @@ namespace SlimeVR { int32_t C[3]; }; - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948 }; + struct ICM42688CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; + + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; + }; + + enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, ICM42688 }; const char* calibrationConfigTypeToString(CalibrationConfigType type); @@ -88,6 +101,7 @@ namespace SlimeVR { MPU6050CalibrationConfig mpu6050; MPU9250CalibrationConfig mpu9250; ICM20948CalibrationConfig icm20948; + ICM42688CalibrationConfig icm42688; } data; }; } diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 74431a063..773d2e637 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -440,6 +440,25 @@ namespace SlimeVR { m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)); m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)); + break; + + case CalibrationConfigType::ICM42688: + m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.A_B)); + + m_Logger.info(" A_Ainv:"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.A_Ainv[i])); + } + + m_Logger.info(" M_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.M_B)); + + m_Logger.info(" M_Ainv:"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.M_Ainv[i])); + } + + m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.G_off)); + break; } } diff --git a/src/consts.h b/src/consts.h index 15f804e46..3e60a68bf 100644 --- a/src/consts.h +++ b/src/consts.h @@ -34,6 +34,7 @@ #define IMU_BNO086 7 #define IMU_BMI160 8 #define IMU_ICM20948 9 +#define IMU_ICM42688 10 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BOARD_UNKNOWN 0 diff --git a/src/main.cpp b/src/main.cpp index bc6148d8f..b4a4dd8ee 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -71,7 +71,7 @@ void setup() SerialCommands::setUp(); -#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 || IMU == IMU_BMI160 +#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 || IMU == IMU_BMI160|| IMU == IMU_ICM42688 I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down // Fixes I2C issues for certain IMUs. Only has been tested on IMUs above. Testing advised when adding other IMUs. #endif diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 6995147bb..434f8a5f7 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -29,6 +29,7 @@ #include "mpu6050sensor.h" #include "bmi160sensor.h" #include "icm20948sensor.h" +#include "icm42688sensor.h" #include "ErroneousSensor.h" #include "sensoraddresses.h" #include "GlobalVars.h" @@ -104,6 +105,9 @@ namespace SlimeVR case IMU_ICM20948: sensor = new ICM20948Sensor(sensorID, address, rotation, sclPin, sdaPin); break; + case IMU_ICM42688: + sensor = new ICM42688Sensor(sensorID, address, rotation, sclPin, sdaPin); + break; default: sensor = new ErroneousSensor(sensorID, imuType); break; diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp new file mode 100644 index 000000000..ce698d3a7 --- /dev/null +++ b/src/sensors/icm42688sensor.cpp @@ -0,0 +1,352 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain, S.J. Remington & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "icm42688sensor.h" +#include "network/network.h" +#include "globals.h" +#include "helper_3dmath.h" +#include +#include "calibration.h" +#include "magneto1.4.h" +#include "GlobalVars.h" +#include "mahony.h" +// #include "madgwick.h" + +constexpr float gscale = (2000. / 32768.0) * (PI / 180.0); // gyro LSB/d/s -> rad/s +constexpr float ascale = (8. / 32768.) * CONST_EARTH_GRAVITY; // accel LSB/G -> m/s^2 + +void ICM42688Sensor::motionSetup() { + // initialize device + uint8_t temp; + I2Cdev::readByte(addr, ICM42688_WHO_AM_I, &temp); + if(!(temp == 0x47 || temp == 0xDB)) { + m_Logger.fatal("Can't connect to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr); + return; + } + + m_Logger.info("Connected to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr); + + if (I2CSCAN::isI2CExist(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); + m_Logger.info("Magnetometer unavailable!"); + magExists = false; + } else { + m_Logger.info("Connected to MMC5983MA (reported device ID 0x%02x) at address 0x%02x", temp, addr_mag); + magExists = true; + } + } + + deltat = 1.0 / 1000.0; // gyro fifo 1khz + + if (magExists) { + I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_1, 0x80); // Reset MMC now + } + + I2Cdev::writeByte(addr, ICM42688_DEVICE_CONFIG, 1); // reset + delay(2); // wait 1ms for reset + I2Cdev::readByte(addr, ICM42688_INT_STATUS, &temp); // clear reset done int flag + I2Cdev::writeByte(addr, ICM42688_INT_SOURCE0, 0); // disable ints + I2Cdev::writeByte(addr, ICM42688_REG_BANK_SEL, 0x00); // select register bank 0 + I2Cdev::writeByte(addr, ICM42688_PWR_MGMT0, gMode_LN << 2 | aMode_LN); // set accel and gyro modes (low noise) + delay(1); // wait >200us (datasheet 14.36) + I2Cdev::writeByte(addr, ICM42688_ACCEL_CONFIG0, AFS_8G << 5 | AODR_200Hz); // set accel ODR and FS (200hz, 8g) + I2Cdev::writeByte(addr, ICM42688_GYRO_CONFIG0, GFS_2000DPS << 5 | GODR_1kHz); // set gyro ODR and FS (1khz, 2000dps) + I2Cdev::writeByte(addr, ICM42688_GYRO_ACCEL_CONFIG0, 0x44); // set gyro and accel bandwidth to ODR/10 + delay(50); // 10ms Accel, 30ms Gyro startup + + if (magExists) { + I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_0, 0x08); // SET + delayMicroseconds(1); // auto clear after 500ns + I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_0, 0x20); // auto SET/RESET + I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_1, MBW_400Hz); // set mag BW (400Hz or ~50% duty cycle with 200Hz ODR) + I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_2, 0x80 | (MSET_2000 << 4) | 0x08 | MODR_200Hz); // continuous measurement mode, set sample rate, auto SET/RESET, set SET/RESET rate (200Hz ODR, 2000 samples between SET/RESET) + } + + // turn on while flip back to calibrate. then, flip again after 5 seconds. + // TODO: Move calibration invoke after calibrate button on slimeVR server available + accel_read(); + if(Gxyz[2] < -0.75f) { + ledManager.on(); + m_Logger.info("Flip front to confirm start calibration"); + delay(5000); + ledManager.off(); + + accel_read(); + if(Gxyz[2] > 0.75f) { + m_Logger.debug("Starting calibration..."); + startCalibration(0); + } + } + + // 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::ICM42688: + m_Calibration = sensorCalibration.data.icm42688; + 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"); + } + } + + I2Cdev::writeByte(addr, ICM42688_FIFO_CONFIG, 0x00); // FIFO bypass mode + I2Cdev::writeByte(addr, ICM42688_FSYNC_CONFIG, 0x00); // disable FSYNC + I2Cdev::readByte(addr, ICM42688_TMST_CONFIG, &temp); // disable FSYNC + I2Cdev::writeByte(addr, ICM42688_TMST_CONFIG, temp & 0xfd); // disable FSYNC + I2Cdev::writeByte(addr, ICM42688_FIFO_CONFIG1, 0x02); // enable FIFO gyro only + I2Cdev::writeByte(addr, ICM42688_FIFO_CONFIG, 1<<6); // begin FIFO stream + + working = true; + configured = true; +} + +void ICM42688Sensor::motionLoop() { + uint8_t rawCount[2]; + I2Cdev::readBytes(addr, ICM42688_FIFO_COUNTH, 2, &rawCount[0]); + uint16_t count = (uint16_t)(rawCount[0] << 8 | rawCount[1]); // Turn the 16 bits into a unsigned 16-bit value + 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(); + parseAccelData(); + + if (magExists) { + mag_read(); + parseMagData(); + } + + for (uint16_t i = 0; i < packets; i++) { + uint16_t index = i * 8; // Packet size 8 bytes + if ((rawData[index] & 0x80) == 0x80) { + continue; // Skip empty packets + } + // combine into 16 bit values + float raw0 = (int16_t)((((int16_t)rawData[index + 1]) << 8) | rawData[index + 2]); // gx + float raw1 = (int16_t)((((int16_t)rawData[index + 3]) << 8) | rawData[index + 4]); // gy + float raw2 = (int16_t)((((int16_t)rawData[index + 5]) << 8) | rawData[index + 6]); // gz + if (raw0 < -32766 || raw1 < -32766 || raw2 < -32766) { + continue; // Skip invalid data + } + Gxyz[0] = raw0 * gscale; //gres + Gxyz[1] = raw1 * gscale; //gres + 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 + } + + quaternion.set(-q[2], q[1], q[3], q[0]); + + quaternion *= sensorOffset; + + if(!lastQuatSent.equalsWithEpsilon(quaternion)) { + newData = true; + lastQuatSent = quaternion; + } +} + +void ICM42688Sensor::accel_read() { + uint8_t rawAccel[6]; + I2Cdev::readBytes(addr, ICM42688_ACCEL_DATA_X1, 6, &rawAccel[0]); + float raw0 = (int16_t)((((int16_t)rawAccel[0]) << 8) | rawAccel[1]); + float raw1 = (int16_t)((((int16_t)rawAccel[2]) << 8) | rawAccel[3]); + float raw2 = (int16_t)((((int16_t)rawAccel[4]) << 8) | rawAccel[5]); + Axyz[0] = raw0 * ascale; + Axyz[1] = raw1 * ascale; + Axyz[2] = raw2 * ascale; +} + +void ICM42688Sensor::gyro_read() { + uint8_t rawGyro[6]; + I2Cdev::readBytes(addr, ICM42688_GYRO_DATA_X1, 6, &rawGyro[0]); + float raw0 = (int16_t)((((int16_t)rawGyro[0]) << 8) | rawGyro[1]); + float raw1 = (int16_t)((((int16_t)rawGyro[2]) << 8) | rawGyro[3]); + float raw2 = (int16_t)((((int16_t)rawGyro[4]) << 8) | rawGyro[5]); + Gxyz[0] = raw0 * gscale; + Gxyz[1] = raw1 * gscale; + Gxyz[2] = raw2 * gscale; +} + +void ICM42688Sensor::mag_read() { + if (!magExists) return; + uint8_t rawMag[7]; + I2Cdev::readBytes(addr_mag, MMC5983MA_XOUT_0, 7, &rawMag[0]); + double raw0 = (uint32_t)(rawMag[0] << 10 | rawMag[1] << 2 | (rawMag[6] & 0xC0) >> 6); + double raw1 = (uint32_t)(rawMag[2] << 10 | rawMag[3] << 2 | (rawMag[6] & 0x30) >> 4); + double raw2 = (uint32_t)(rawMag[4] << 10 | rawMag[5] << 2 | (rawMag[6] & 0x0C) >> 2); + Mxyz[0] = (raw0 - MMC5983MA_offset) * MMC5983MA_mRes; + Mxyz[1] = (raw1 - MMC5983MA_offset) * MMC5983MA_mRes; + Mxyz[2] = (raw2 - MMC5983MA_offset) * MMC5983MA_mRes; +} + +void ICM42688Sensor::startCalibration(int calibrationType) { + ledManager.on(); + m_Logger.debug("Gathering raw data for device calibration..."); + constexpr int calibrationSamples = 500; + double GxyzC[3] = {0, 0, 0}; + + // Wait for sensor to calm down before calibration + m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); + delay(2000); + + for (int i = 0; i < calibrationSamples; i++) { + delay(5); + gyro_read(); + GxyzC[0] += Gxyz[0]; + GxyzC[1] += Gxyz[1]; + GxyzC[2] += Gxyz[2]; + } + GxyzC[0] /= calibrationSamples; + GxyzC[1] /= calibrationSamples; + GxyzC[2] /= calibrationSamples; + +#ifdef DEBUG_SENSOR + m_Logger.trace("Gyro calibration results: %f %f %f", GxyzC[0], GxyzC[1], GxyzC[2]); +#endif + + // TODO: use offset registers? + m_Calibration.G_off[0] = GxyzC[0]; + m_Calibration.G_off[1] = GxyzC[1]; + m_Calibration.G_off[2] = GxyzC[2]; + + // Blink calibrating led before user should rotate the sensor + m_Logger.info("Gently rotate the device while it's gathering accelerometer and magnetometer data"); + ledManager.pattern(15, 300, 3000/310); + + MagnetoCalibration *magneto_acc = new MagnetoCalibration(); + MagnetoCalibration *magneto_mag = new MagnetoCalibration(); + + // NOTE: we don't use the FIFO here on *purpose*. This makes the difference between a calibration that takes a second or three and a calibration that takes much longer. + for (int i = 0; i < calibrationSamples; i++) { + ledManager.on(); + accel_read(); + magneto_acc->sample(Axyz[0], Axyz[1], Axyz[2]); + mag_read(); + magneto_mag->sample(Mxyz[0], Mxyz[1], Mxyz[2]); + + ledManager.off(); + delay(50); + } + m_Logger.debug("Calculating calibration data..."); + + float A_BAinv[4][3]; + magneto_acc->current_calibration(A_BAinv); + delete magneto_acc; + + float M_BAinv[4][3]; + if (magExists) { + magneto_mag->current_calibration(M_BAinv); + } + delete magneto_mag; + + m_Logger.debug("Finished Calculate Calibration data"); + m_Logger.debug("Accelerometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) + { + m_Calibration.A_B[i] = A_BAinv[0][i]; + m_Calibration.A_Ainv[0][i] = A_BAinv[1][i]; + m_Calibration.A_Ainv[1][i] = A_BAinv[2][i]; + m_Calibration.A_Ainv[2][i] = A_BAinv[3][i]; + m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); + } + m_Logger.debug("}"); + if (magExists) { + m_Logger.debug("[INFO] Magnetometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + m_Calibration.M_B[i] = M_BAinv[0][i]; + m_Calibration.M_Ainv[0][i] = M_BAinv[1][i]; + m_Calibration.M_Ainv[1][i] = M_BAinv[2][i]; + m_Calibration.M_Ainv[2][i] = M_BAinv[3][i]; + m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); + } + m_Logger.debug("}"); + } + + m_Logger.debug("Saving the calibration data"); + + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::ICM42688; + calibration.data.icm42688 = m_Calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + + ledManager.off(); + Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0); + m_Logger.debug("Saved the calibration data"); + + m_Logger.info("Calibration data gathered"); + I2Cdev::writeByte(addr, ICM42688_SIGNAL_PATH_RESET, 0x02); // flush FIFO before return +} + +void ICM42688Sensor::parseMagData() { + float temp[3]; + + //apply offsets and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { + temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); + #if useFullCalibrationMatrix == true + Mxyz[i] = m_Calibration.M_Ainv[i][0] * temp[0] + m_Calibration.M_Ainv[i][1] * temp[1] + m_Calibration.M_Ainv[i][2] * temp[2]; + #else + Mxyz[i] = temp[i]; + #endif + } +} + +void ICM42688Sensor::parseAccelData() { + float temp[3]; + + //apply offsets (bias) and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { + temp[i] = (Axyz[i] - m_Calibration.A_B[i]); + #if useFullCalibrationMatrix == true + Axyz[i] = m_Calibration.A_Ainv[i][0] * temp[0] + m_Calibration.A_Ainv[i][1] * temp[1] + m_Calibration.A_Ainv[i][2] * temp[2]; + #else + Axyz[i] = temp[i]; + #endif + } +} + +void ICM42688Sensor::parseGyroData() { + Gxyz[0] = (Gxyz[0] - m_Calibration.G_off[0]); + Gxyz[1] = (Gxyz[1] - m_Calibration.G_off[1]); + Gxyz[2] = (Gxyz[2] - m_Calibration.G_off[2]); +} diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h new file mode 100644 index 000000000..f33a70a23 --- /dev/null +++ b/src/sensors/icm42688sensor.h @@ -0,0 +1,67 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef SENSORS_ICM42688SENSOR_H +#define SENSORS_ICM42688SENSOR_H + +#include "sensor.h" +#include "logging/Logger.h" + +#include +#include +#include "I2Cdev.h" + +class ICM42688Sensor : public Sensor +{ +public: + ICM42688Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation){}; + ~ICM42688Sensor(){}; + void motionSetup() override final; + void motionLoop() override final; + void startCalibration(int calibrationType) override final; + +private: + uint8_t addr_mag = 0x30; + 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::Configuration::ICM42688CalibrationConfig m_Calibration; + + void accel_read(); + void gyro_read(); + void mag_read(); + + void parseAccelData(); + void parseGyroData(); + void parseMagData(); +}; + +#endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 730ea68d8..011fe93ac 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -87,6 +87,8 @@ const char * getIMUNameByType(int imuType) { return "BMI160"; case IMU_ICM20948: return "ICM20948"; + case IMU_ICM42688: + return "ICM42688"; } return "Unknown"; } diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h index b8cb8f0f3..567120d45 100644 --- a/src/sensors/sensoraddresses.h +++ b/src/sensors/sensoraddresses.h @@ -10,7 +10,7 @@ #elif IMU == IMU_BNO055 #define PRIMARY_IMU_ADDRESS_ONE 0x29 #define PRIMARY_IMU_ADDRESS_TWO 0x28 - #elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 + #elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 || IMU == IMU_ICM42688 #define PRIMARY_IMU_ADDRESS_ONE 0x68 #define PRIMARY_IMU_ADDRESS_TWO 0x69 #endif @@ -21,7 +21,7 @@ #elif SECOND_IMU == IMU_BNO055 #define SECONDARY_IMU_ADDRESS_ONE 0x29 #define SECONDARY_IMU_ADDRESS_TWO 0x28 - #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 + #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 || SECOND_IMU == IMU_ICM42688 #define SECONDARY_IMU_ADDRESS_ONE 0x68 #define SECONDARY_IMU_ADDRESS_TWO 0x69 #endif From 07785c8fc8c08f0bf9fa087d60dad57ab6505e61 Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Fri, 22 Sep 2023 12:06:14 -0500 Subject: [PATCH 16/23] fix Imu icm42688 (#290) --- src/sensors/icm42688sensor.cpp | 34 +++++++++++++++------------------- src/sensors/icm42688sensor.h | 12 +++++++----- 2 files changed, 22 insertions(+), 24 deletions(-) diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp index ce698d3a7..6884ba2a4 100644 --- a/src/sensors/icm42688sensor.cpp +++ b/src/sensors/icm42688sensor.cpp @@ -20,7 +20,6 @@ */ #include "icm42688sensor.h" -#include "network/network.h" #include "globals.h" #include "helper_3dmath.h" #include @@ -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); @@ -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 } @@ -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(); @@ -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() { @@ -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"); diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h index f33a70a23..dc3b936cb 100644 --- a/src/sensors/icm42688sensor.h +++ b/src/sensors/icm42688sensor.h @@ -31,10 +31,14 @@ #include #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; @@ -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; From 9968f152fc323249f4a8fe8be7d6c5bb14a35598 Mon Sep 17 00:00:00 2001 From: Yao Wei Date: Mon, 2 Oct 2023 06:52:51 +0800 Subject: [PATCH 17/23] Add BOARD_WEMOSWROOM02 (#279) Co-authored-by: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> --- platformio-tools.ini | 4 ++++ src/consts.h | 1 + src/defines.h | 8 ++++++++ 3 files changed, 13 insertions(+) diff --git a/platformio-tools.ini b/platformio-tools.ini index 9ebc16c81..ca97fc8fd 100644 --- a/platformio-tools.ini +++ b/platformio-tools.ini @@ -26,6 +26,10 @@ board = esp12e platform = espressif8266 @ 4.2.0 board = esp12e +[env:BOARD_WEMOSWROOM02] +platform = espressif8266 @ 4.2.0 +board = esp12e + [env:BOARD_WROOM32] platform = espressif32 @ 6.1.0 board = esp32dev diff --git a/src/consts.h b/src/consts.h index 3e60a68bf..3c09d1d26 100644 --- a/src/consts.h +++ b/src/consts.h @@ -53,6 +53,7 @@ #define BOARD_OWOTRACK 13 // Only used by owoTrack mobile app #define BOARD_WRANGLER 14 // Only used by wrangler app #define BOARD_MOCOPI 15 // Used by mocopi/moslime +#define BOARD_WEMOSWROOM02 16 #define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BAT_EXTERNAL 1 diff --git a/src/defines.h b/src/defines.h index 3763dfff6..79f4d94b3 100644 --- a/src/defines.h +++ b/src/defines.h @@ -181,4 +181,12 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P #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 #endif From 26f53ae5e54878c048d0146b5adc9365bd644dc6 Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Fri, 13 Oct 2023 14:54:50 +0200 Subject: [PATCH 18/23] Fix serial wifi and bwifi. Crash bwifi when no ... (#298) Co-authored-by: DevMiner --- .editorconfig | 4 ++ platformio-tools.ini | 1 + platformio.ini | 1 + src/serial/serialcommands.cpp | 81 +++++++++++++++++++++++++++++------ src/serial/serialcommands.h | 2 +- 5 files changed, 74 insertions(+), 15 deletions(-) diff --git a/.editorconfig b/.editorconfig index 980ef210d..5ae073093 100644 --- a/.editorconfig +++ b/.editorconfig @@ -10,3 +10,7 @@ end_of_line = lf charset = utf-8 trim_trailing_whitespace = true insert_final_newline = true + +# the files need the trailing withe space at the end of = else it does not work +[{platformio.ini,platformio-tools.ini}] +trim_trailing_whitespace = false diff --git a/platformio-tools.ini b/platformio-tools.ini index ca97fc8fd..e5a268caa 100644 --- a/platformio-tools.ini +++ b/platformio-tools.ini @@ -1,6 +1,7 @@ [env] lib_deps= https://github.com/SlimeVR/CmdParser.git + https://github.com/SlimeVR/base64_arduino.git monitor_speed = 115200 framework = arduino build_flags = -O2 -std=gnu++17 diff --git a/platformio.ini b/platformio.ini index dec2c2307..6d4d67407 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,6 +14,7 @@ [env] lib_deps= https://github.com/SlimeVR/CmdParser.git + https://github.com/SlimeVR/base64_arduino.git monitor_speed = 115200 monitor_echo = yes monitor_filters = colorize diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 8030b93e3..cb61fae82 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -24,7 +24,7 @@ #include "serialcommands.h" #include "logging/Logger.h" #include -#include +#include "base64.hpp" #include "GlobalVars.h" #include "batterymonitor.h" #include "utils.h" @@ -40,14 +40,42 @@ namespace SerialCommands { CmdParser cmdParser; CmdBuffer<256> cmdBuffer; - void cmdSet(CmdParser * parser) { - if(parser->getParamCount() != 1) { + + bool lengthCheck (const char* const text, unsigned int length, const char* const cmd, const char* const name) + { + size_t l = text != nullptr ? strlen(text) : 0; + if ((l > length)) { + logger.error("%s ERROR: %s is longer than %d bytes / Characters", cmd, name, length); + return false; + } + return true; + } + + unsigned int decode_base64_length_null(const char* const b64char, unsigned int* b64ssidlength) + { + if (b64char==NULL) { + return 0; + } + *b64ssidlength = (unsigned int)strlen(b64char); + return decode_base64_length((unsigned char *)b64char, *b64ssidlength); + } + + void cmdSet(CmdParser * parser) { + if(parser->getParamCount() != 1) { if (parser->equalCmdParam(1, "WIFI")) { if(parser->getParamCount() < 3) { logger.error("CMD SET WIFI ERROR: Too few arguments"); logger.info("Syntax: SET WIFI \"\" \"\""); } else { - WiFiNetwork::setWiFiCredentials(parser->getCmdParam(2), parser->getCmdParam(3)); + const char *sc_ssid = parser->getCmdParam(2); + const char *sc_pw = parser->getCmdParam(3); + + if (!lengthCheck(sc_ssid, 32, "CMD SET WIFI", "SSID") && + !lengthCheck(sc_pw, 64, "CMD SET WIFI", "Password")) { + return; + } + + WiFiNetwork::setWiFiCredentials(sc_ssid, sc_pw); logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting"); } } else if (parser->equalCmdParam(1, "BWIFI")) { @@ -55,22 +83,44 @@ namespace SerialCommands { logger.error("CMD SET BWIFI ERROR: Too few arguments"); logger.info("Syntax: SET BWIFI "); } else { - char ssid[33]; - char pass[65]; const char * b64ssid = parser->getCmdParam(2); const char * b64pass = parser->getCmdParam(3); - base64_decode_chars(b64ssid, strlen_P(b64ssid), ssid); - base64_decode_chars(b64pass, strlen_P(b64pass), pass); - WiFiNetwork::setWiFiCredentials(ssid, pass); + unsigned int b64ssidlength = 0; + unsigned int b64passlength = 0; + unsigned int ssidlength = decode_base64_length_null(b64ssid, &b64ssidlength); + unsigned int passlength = decode_base64_length_null(b64pass, &b64passlength); + + // alloc the strings and set them to 0 (null terminating) + char ssid[ssidlength+1]; + memset(ssid, 0, ssidlength+1); + char pass[passlength+1]; + memset(pass, 0, passlength+1); + // make a pointer to pass + char *ppass = pass; + decode_base64((const unsigned char *)b64ssid, b64ssidlength, (unsigned char*)ssid); + if (!lengthCheck(ssid, 32, "CMD SET BWIFI", "SSID")) { + return; + } + + if ((b64pass!=NULL) && (b64passlength > 0)) { + decode_base64((const unsigned char *)b64pass, b64passlength, (unsigned char*)pass); + if (!lengthCheck(pass, 64, "CMD SET BWIFI", "Password")) { + return; + } + } else { + // set the pointer for pass to null for no password + ppass = NULL; + } + WiFiNetwork::setWiFiCredentials(ssid, ppass); logger.info("CMD SET BWIFI OK: New wifi credentials set, reconnecting"); } } else { - logger.error("CMD SET ERROR: Unrecognized variable to set"); + logger.error("CMD SET ERROR: Unrecognized variable to set"); } - } else { - logger.error("CMD SET ERROR: No variable to set"); - } - } + } else { + logger.error("CMD SET ERROR: No variable to set"); + } + } void printState() { logger.info( @@ -178,6 +228,9 @@ namespace SerialCommands { // Scan would fail if connecting, stop connecting before scan if (WiFi.status() != WL_CONNECTED) WiFi.disconnect(); + if (WiFiNetwork::isProvisioning()) { + WiFiNetwork::stopProvisioning(); + } WiFi.scanNetworks(); diff --git a/src/serial/serialcommands.h b/src/serial/serialcommands.h index 21cbb0cd3..a07f06cb7 100644 --- a/src/serial/serialcommands.h +++ b/src/serial/serialcommands.h @@ -30,4 +30,4 @@ namespace SerialCommands { void printState(); } -#endif // SLIMEVR_SERIALCOMMANDS_H_ \ No newline at end of file +#endif // SLIMEVR_SERIALCOMMANDS_H_ From 10125c72533bc0b79fabf69645b677ab584e2a17 Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Fri, 13 Oct 2023 14:55:55 +0200 Subject: [PATCH 19/23] Fix WiFiscan not working when not connected (#293) Co-authored-by: DevMiner --- src/serial/serialcommands.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index cb61fae82..ce2fbadb6 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -227,7 +227,9 @@ namespace SerialCommands { logger.info("[WSCAN] Scanning for WiFi networks..."); // Scan would fail if connecting, stop connecting before scan - if (WiFi.status() != WL_CONNECTED) WiFi.disconnect(); + if (WiFi.status() != WL_CONNECTED) { + WiFi.disconnect(); + } if (WiFiNetwork::isProvisioning()) { WiFiNetwork::stopProvisioning(); } @@ -249,7 +251,9 @@ namespace SerialCommands { } // Restore conencting state - if (WiFi.status() != WL_CONNECTED) WiFi.begin(); + if (WiFi.status() != WL_CONNECTED) { + WiFi.begin(); + } } } From a3d4321a896543d8cf6684871240aa99be4b6ed0 Mon Sep 17 00:00:00 2001 From: wigwagwent <66268000+wigwagwent@users.noreply.github.com> Date: Fri, 13 Oct 2023 05:56:30 -0700 Subject: [PATCH 20/23] Use sensorType instead of IMU define (#297) Co-authored-by: DevMiner --- src/sensors/bno080sensor.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index c0f6873b5..2b9717164 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -54,21 +54,21 @@ void BNO080Sensor::motionSetup() this->imu.enableLinearAccelerometer(10); #if USE_6_AXIS - #if (IMU == IMU_BNO085 || IMU == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION - imu.enableARVRStabilizedGameRotationVector(10); - #else - imu.enableGameRotationVector(10); - #endif + if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedGameRotationVector(10); + } else { + imu.enableGameRotationVector(10); + } #if BNO_USE_MAGNETOMETER_CORRECTION imu.enableRotationVector(1000); #endif #else - #if (IMU == IMU_BNO085 || IMU == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION - imu.enableARVRStabilizedRotationVector(10); - #else - imu.enableRotationVector(10); - #endif + if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedRotationVector(10); + } else { + imu.enableRotationVector(10); + } #endif #if ENABLE_INSPECTION From 14f2752d4d0515023f5f649067f0e774b80ad866 Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Fri, 13 Oct 2023 14:57:33 +0200 Subject: [PATCH 21/23] feat: commit hash (#228) Co-authored-by: DevMiner Co-authored-by: nekomona --- platformio-tools.ini | 9 +++++++-- platformio.ini | 3 ++- scripts/get_git_commit.py | 19 +++++++++++++++++++ src/serial/serialcommands.cpp | 3 +++ 4 files changed, 31 insertions(+), 3 deletions(-) create mode 100644 scripts/get_git_commit.py diff --git a/platformio-tools.ini b/platformio-tools.ini index e5a268caa..f3d1bb3d2 100644 --- a/platformio-tools.ini +++ b/platformio-tools.ini @@ -4,8 +4,13 @@ lib_deps= https://github.com/SlimeVR/base64_arduino.git monitor_speed = 115200 framework = arduino -build_flags = -O2 -std=gnu++17 -build_unflags = -Os -std=gnu++11 +build_flags = + !python scripts/get_git_commit.py + -O2 + -std=gnu++17 +build_unflags = + -Os + -std=gnu++11 [env:BOARD_SLIMEVR] platform = espressif8266 @ 4.2.0 diff --git a/platformio.ini b/platformio.ini index 6d4d67407..01e6fdd65 100644 --- a/platformio.ini +++ b/platformio.ini @@ -21,7 +21,8 @@ monitor_filters = colorize ;monitor_rts = 0 ;monitor_dtr = 0 framework = arduino -build_flags = +build_flags = + !python scripts/get_git_commit.py ;If you want to set hardcoded WiFi SSID and password, uncomment and edit the lines below ;To uncomment, only remove ";" and leave the two spaces in front of the tags ; '" - quotes are necessary! diff --git a/scripts/get_git_commit.py b/scripts/get_git_commit.py new file mode 100644 index 000000000..ba482183a --- /dev/null +++ b/scripts/get_git_commit.py @@ -0,0 +1,19 @@ +import subprocess +import os + +revision = "" + +env_rev = os.environ.get("GIT_REV") +if not env_rev is None and env_rev != "": + revision = env_rev +else: + try: + revision = ( + subprocess.check_output(["git", "rev-parse", "HEAD"]) + .strip() + .decode("utf-8") + ) + except Exception: + revision = "NOT_GIT" + +print(f"'-DGIT_REV=\"{revision}\"'") diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index ce2fbadb6..a85469502 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -153,6 +153,9 @@ namespace SerialCommands { if (parser->equalCmdParam(1, "INFO")) { printState(); + + // We don't want to print this on every timed state output + logger.info("Git commit: %s", GIT_REV); } if (parser->equalCmdParam(1, "CONFIG")) { From 8a0037620094786ff264f4a25a05441391178c39 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Wed, 8 Nov 2023 17:57:41 +0100 Subject: [PATCH 22/23] Add compliance mode to limit trasmitter power to FCC certified values --- src/debug.h | 6 ++++++ src/network/wifihandler.cpp | 18 ++++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/src/debug.h b/src/debug.h index 1e5075628..e41e0bca3 100644 --- a/src/debug.h +++ b/src/debug.h @@ -80,6 +80,12 @@ #define I2C_SPEED 400000 +#define COMPLIANCE_MODE true +#define USE_ATTENUATION COMPLIANCE_MODE && ESP8266 +#define ATTENUATION_N 10.0 / 4.0 +#define ATTENUATION_G 14.0 / 4.0 +#define ATTENUATION_B 40.0 / 4.0 + // Send inspection packets over the network to a profiler // Not recommended for production #define ENABLE_INSPECTION false diff --git a/src/network/wifihandler.cpp b/src/network/wifihandler.cpp index e701a6afc..4119c93c9 100644 --- a/src/network/wifihandler.cpp +++ b/src/network/wifihandler.cpp @@ -76,6 +76,9 @@ void WiFiNetwork::setUp() { WiFi.persistent(true); WiFi.mode(WIFI_STA); #if ESP8266 + #if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_N); + #endif WiFi.setPhyMode(WIFI_PHY_MODE_11N); #endif WiFi.hostname("SlimeVR FBT Tracker"); @@ -149,6 +152,9 @@ void WiFiNetwork::upkeep() { // But only if there are credentials, otherwise we just waste time before // switching to hardcoded credentials. if (WiFi.SSID().length() > 0) { + #if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_G); + #endif WiFi.setPhyMode(WIFI_PHY_MODE_11G); setStaticIPIfDefined(); WiFi.begin(); @@ -165,6 +171,9 @@ void WiFiNetwork::upkeep() { #if defined(WIFI_CREDS_SSID) && defined(WIFI_CREDS_PASSWD) // Try hardcoded credentials now #if ESP8266 + #if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_N); + #endif WiFi.setPhyMode(WIFI_PHY_MODE_11N); #endif setStaticIPIfDefined(); @@ -178,6 +187,9 @@ void WiFiNetwork::upkeep() { 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 USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_G); + #endif WiFi.setPhyMode(WIFI_PHY_MODE_11G); setStaticIPIfDefined(); WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD); @@ -190,6 +202,9 @@ void WiFiNetwork::upkeep() { case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with server-sent credentials. #if ESP8266 // Try again silently but with 11G + #if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_G); + #endif WiFi.setPhyMode(WIFI_PHY_MODE_11G); setStaticIPIfDefined(); WiFi.begin(); @@ -201,6 +216,9 @@ void WiFiNetwork::upkeep() { case SLIME_WIFI_SERVER_CRED_G_ATTEMPT: // Or if couldn't connect with server-sent credentials // Return to the default PHY Mode N. #if ESP8266 + #if USE_ATTENUATION + WiFi.setOutputPower(20.0 - ATTENUATION_N); + #endif WiFi.setPhyMode(WIFI_PHY_MODE_11N); #endif // Start smart config From b744c536764559c71b3d062552d159511fe8b69a Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Wed, 8 Nov 2023 17:57:51 +0100 Subject: [PATCH 23/23] Bump version to 0.4.0 --- src/debug.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/debug.h b/src/debug.h index e41e0bca3..b2c4968d5 100644 --- a/src/debug.h +++ b/src/debug.h @@ -90,7 +90,7 @@ // Not recommended for production #define ENABLE_INSPECTION false -#define FIRMWARE_BUILD_NUMBER 16 -#define FIRMWARE_VERSION "0.3.3" +#define FIRMWARE_BUILD_NUMBER 17 +#define FIRMWARE_VERSION "0.4.0" #endif // SLIMEVR_DEBUG_H_