Skip to content

Commit

Permalink
Untested magnetometer toggle feature for BNO08X and overal packet str…
Browse files Browse the repository at this point in the history 
…ucture for setting flags from the server
  • Loading branch information
@Eirenliel @TheDevMinerTV
Eirenliel authored and TheDevMinerTV committed Mar 26, 2024
1 parent 230859d commit b89ab7d
Show file tree
Hide file tree
Showing 11 changed files with 152 additions and 61 deletions.
2 changes: 2 additions & 0 deletions src/configuration/CalibrationConfig.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@ namespace SlimeVR {
return "ICM20948";
case ICM42688:
return "ICM42688";
case BNO0XX:
return "BNO0XX";
default:
return "UNKNOWN";
}
Expand Down
7 changes: 6 additions & 1 deletion src/configuration/CalibrationConfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -102,6 +106,7 @@ namespace SlimeVR {
MPU9250CalibrationConfig mpu9250;
ICM20948CalibrationConfig icm20948;
ICM42688CalibrationConfig icm42688;
BNO0XXCalibrationConfig bno0XX;
} data;
};
}
Expand Down
4 changes: 4 additions & 0 deletions src/configuration/Configuration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -321,6 +321,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;
}
}
Expand Down
33 changes: 33 additions & 0 deletions src/network/connection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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<Sensor *> & sensors = sensorManager.getSensors();
if(sensorId < sensors.size()) {
Sensor * sensor = sensors[sensorId];
}
}
sendAcknowledgeConfigChange(sensorId, flagId);
}
}

Expand Down
2 changes: 2 additions & 0 deletions src/network/connection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");

Expand Down
4 changes: 2 additions & 2 deletions src/network/featureflags.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -84,7 +84,7 @@ class FirmwareFeatures {
static constexpr const std::initializer_list<EFirmwareFeatureFlags> flagsEnabled = {
// EXAMPLE_FEATURE,
B64_WIFI_SCANNING,

SENSOR_CONFIG
// Add enabled flags here
};

Expand Down
2 changes: 2 additions & 0 deletions src/network/packets.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
1 change: 1 addition & 0 deletions src/sensors/bno055sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ class BNO055Sensor : public Sensor

private:
Adafruit_BNO055 imu;
SlimeVR::Configuration::BNO0XXCalibrationConfig m_Calibration = {};
};

#endif
151 changes: 94 additions & 57 deletions src/sensors/bno080sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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())
{
Expand Down Expand Up @@ -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)
{
Expand All @@ -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,
Expand Down
6 changes: 5 additions & 1 deletion src/sensors/bno080sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
#include "sensor.h"
#include <BNO080.h>

#define FLAG_SENSOR_BNO0XX_MAG_ENABLED 1

class BNO080Sensor : public Sensor
{
public:
Expand All @@ -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{};

Expand All @@ -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{};
Expand Down
1 change: 1 addition & 0 deletions src/sensors/sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down

0 comments on commit b89ab7d

Please sign in to comment.