diff --git a/src/configuration/CalibrationConfig.cpp b/src/configuration/CalibrationConfig.cpp index 9aeb3eec7..057fdeab8 100644 --- a/src/configuration/CalibrationConfig.cpp +++ b/src/configuration/CalibrationConfig.cpp @@ -39,6 +39,8 @@ namespace SlimeVR { return "ICM20948"; case ICM42688: return "ICM42688"; + case BNO0XX: + return "BNO0XX"; default: return "UNKNOWN"; } diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 0fe4d91f8..47c3827a7 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -89,7 +89,11 @@ namespace SlimeVR { float G_off[3]; }; - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, ICM42688 }; + struct BNO0XXCalibrationConfig { + bool magEnabled; + }; + + enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, ICM42688, BNO0XX }; const char* calibrationConfigTypeToString(CalibrationConfigType type); @@ -102,6 +106,7 @@ namespace SlimeVR { MPU9250CalibrationConfig mpu9250; ICM20948CalibrationConfig icm20948; ICM42688CalibrationConfig icm42688; + BNO0XXCalibrationConfig bno0XX; } data; }; } diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 773d2e637..ccb9f16ed 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -459,6 +459,10 @@ namespace SlimeVR { m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.G_off)); + break; + case CalibrationConfigType::BNO0XX: + m_Logger.info(" Mag enabled : %s", c.data.bno0XX.magEnabled ? "true" : "false"); + break; } } diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 1f60ad876..a502fd416 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -381,6 +381,21 @@ void Connection::sendFeatureFlags() { MUST(endPacket()); } +// PACKET_ACKNOWLEDGE_CONFIG_CHANGE 23 + +void Connection::sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_ACKNOWLEDGE_CONFIG_CHANGE)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendShort(configType)); + + MUST(endPacket()); +} + void Connection::sendTrackerDiscovery() { MUST(!m_Connected); @@ -710,6 +725,24 @@ void Connection::update() { } break; + case PACKET_SET_CONFIG_FLAG: + // Packet type (4) + Packet number (8) + sensor_id(1) + flag_id (2) + state (1) + if (len < 16) { + m_Logger.warn("Invalid sensor config flag packet: too short"); + break; + } + uint8_t sensorId = m_Packet[12]; + uint16_t flagId = m_Packet[13] << 8 | m_Packet[14]; + bool newState = m_Packet[15] > 0; + if(sensorId == 255) { + // Apply the flag to the whole device + } else { + std::vector & sensors = sensorManager.getSensors(); + if(sensorId < sensors.size()) { + Sensor * sensor = sensors[sensorId]; + } + } + sendAcknowledgeConfigChange(sensorId, flagId); } } diff --git a/src/network/connection.h b/src/network/connection.h index 7b8f7b6a2..c65ed9082 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -158,6 +158,8 @@ class Connection { // PACKET_SENSOR_INFO 15 void sendSensorInfo(Sensor* sensor); + void sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType); + bool m_Connected = false; SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection"); diff --git a/src/network/featureflags.h b/src/network/featureflags.h index b0820be40..7f2164a7b 100644 --- a/src/network/featureflags.h +++ b/src/network/featureflags.h @@ -74,7 +74,7 @@ class FirmwareFeatures { enum EFirmwareFeatureFlags: uint32_t { // EXAMPLE_FEATURE, B64_WIFI_SCANNING = 1, - + SENSOR_CONFIG = 2, // Add new flags here BITS_TOTAL, @@ -84,7 +84,7 @@ class FirmwareFeatures { static constexpr const std::initializer_list flagsEnabled = { // EXAMPLE_FEATURE, B64_WIFI_SCANNING, - + SENSOR_CONFIG // Add enabled flags here }; diff --git a/src/network/packets.h b/src/network/packets.h index e962a6af0..184c56ccc 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -47,6 +47,8 @@ #define PACKET_TEMPERATURE 20 // #define PACKET_USER_ACTION 21 // Joycon buttons only currently #define PACKET_FEATURE_FLAGS 22 +#define PACKET_ACKNOWLEDGE_CONFIG_CHANGE 23 +#define PACKET_SET_CONFIG_FLAG 24 #define PACKET_BUNDLE 100 diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 0694643a0..65f14292a 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -40,6 +40,7 @@ class BNO055Sensor : public Sensor private: Adafruit_BNO055 imu; + SlimeVR::Configuration::BNO0XXCalibrationConfig m_Calibration = {}; }; #endif diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 2b9717164..a8545c9b5 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -53,23 +53,34 @@ void BNO080Sensor::motionSetup() this->imu.enableLinearAccelerometer(10); -#if USE_6_AXIS - 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 ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) { - imu.enableARVRStabilizedRotationVector(10); + 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::BNO0XX: + m_Calibration = sensorCalibration.data.bno0XX; + m_magEnabled = m_Calibration.magEnabled; + break; + default: + // Ignore lack of config for BNO, byt default use from FW build + } + + if(!m_magEnabled) { + 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 { - imu.enableRotationVector(10); + if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedRotationVector(10); + } else { + imu.enableRotationVector(10); + } } -#endif #if ENABLE_INSPECTION imu.enableRawGyro(10); @@ -113,51 +124,51 @@ void BNO080Sensor::motionLoop() lastReset = 0; lastData = millis(); -#if USE_6_AXIS - if (imu.hasNewGameQuat()) // New quaternion if context - { - imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); - fusedRotation *= sensorOffset; + if(!m_magEnabled) { + if (imu.hasNewGameQuat()) // New quaternion if context + { + imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); + fusedRotation *= sensorOffset; - setFusedRotationReady(); - // Leave new quaternion if context open, it's closed later + setFusedRotationReady(); + // Leave new quaternion if context open, it's closed later -#else // USE_6_AXIS + } else { - if (imu.hasNewQuat()) // New quaternion if context - { - imu.getQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, magneticAccuracyEstimate, calibrationAccuracy); - fusedRotation *= sensorOffset; + if (imu.hasNewQuat()) // New quaternion if context + { + imu.getQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, magneticAccuracyEstimate, calibrationAccuracy); + fusedRotation *= sensorOffset; - setFusedRotationReady(); - // Leave new quaternion if context open, it's closed later -#endif // USE_6_AXIS + setFusedRotationReady(); + // Leave new quaternion if context open, it's closed later + } - // Continuation of the new quaternion if context, used for both 6 and 9 axis + // Continuation of the new quaternion if context, used for both 6 and 9 axis #if SEND_ACCELERATION - { - uint8_t acc; - imu.getLinAccel(acceleration.x, acceleration.y, acceleration.z, acc); - setAccelerationReady(); - } + { + uint8_t acc; + imu.getLinAccel(acceleration.x, acceleration.y, acceleration.z, acc); + setAccelerationReady(); + } #endif // SEND_ACCELERATION } // Closing new quaternion if context -#if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION - if (imu.hasNewMagQuat()) - { - imu.getMagQuat(magQuaternion.x, magQuaternion.y, magQuaternion.z, magQuaternion.w, magneticAccuracyEstimate, magCalibrationAccuracy); - magQuaternion *= sensorOffset; + if(BNO_USE_MAGNETOMETER_CORRECTION && !m_magEnabled) { + if (imu.hasNewMagQuat()) + { + imu.getMagQuat(magQuaternion.x, magQuaternion.y, magQuaternion.z, magQuaternion.w, magneticAccuracyEstimate, magCalibrationAccuracy); + magQuaternion *= sensorOffset; - #if ENABLE_INSPECTION - { - networkConnection.sendInspectionCorrectionData(sensorId, quaternion); - } - #endif // ENABLE_INSPECTION + #if ENABLE_INSPECTION + { + networkConnection.sendInspectionCorrectionData(sensorId, quaternion); + } + #endif // ENABLE_INSPECTION - newMagData = true; + newMagData = true; + } } -#endif // USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION if (imu.getTapDetected()) { @@ -216,18 +227,18 @@ void BNO080Sensor::sendData() #endif } -#if !USE_6_AXIS + if(m_magEnabled) { networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); -#endif + } -#if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION - if (newMagData) - { - newMagData = false; - networkConnection.sendRotationData(sensorId, &magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy); - networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); + if(BNO_USE_MAGNETOMETER_CORRECTION && !m_magEnabled) { + if (newMagData) + { + newMagData = false; + networkConnection.sendRotationData(sensorId, &magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy); + networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); + } } -#endif if (tap != 0) { @@ -236,6 +247,32 @@ void BNO080Sensor::sendData() } } +void BNO080Sensor::setFlag(uint16_t flagId, bool state) { + if(flagId == FLAG_SENSOR_BNO0XX_MAG_ENABLED) { + m_Calibration.magEnabled = state; + m_magEnabled = state; + configuration.setCalibration(sensorId, m_Calibration); + if(state) { + if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedRotationVector(10); + } else { + imu.enableRotationVector(10); + } + } else { + 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 + } + imu.softReset(); + } +} + void BNO080Sensor::startCalibration(int calibrationType) { // BNO does automatic calibration, diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 54b413f78..1fde719cc 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -27,6 +27,8 @@ #include "sensor.h" #include +#define FLAG_SENSOR_BNO0XX_MAG_ENABLED 1 + class BNO080Sensor : public Sensor { public: @@ -42,7 +44,7 @@ class BNO080Sensor : public Sensor void sendData() override final; void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; - + void setFlag(uint16_t flagId, bool state) override final; private: BNO080 imu{}; @@ -52,6 +54,8 @@ class BNO080Sensor : public Sensor unsigned long lastData = 0; uint8_t lastReset = 0; BNO080Error lastError{}; + bool m_magEnabled = !USE_6_AXIS; + SlimeVR::Configuration::BNO0XXCalibrationConfig m_Calibration = {}; // Magnetometer specific members Quat magQuaternion{}; diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index da6ce90b4..992c71c89 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -66,6 +66,7 @@ class Sensor virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); virtual void saveTemperatureCalibration(); + virtual void setFlag(uint16_t flagId, bool state){}; bool isWorking() { return working; };