Skip to content

Commit

Permalink
Merge branch 'icm45686-firmware' of https://github.com/gorbit99/Slime…
Browse files Browse the repository at this point in the history 
…VR-Tracker-ESP into icm45686-firmware
  • Loading branch information
@kounocom
kounocom committed Oct 11, 2024
2 parents f7fc1ce + 86915b1 commit e2e1880
Show file tree
Hide file tree
Showing 2 changed files with 151 additions and 168 deletions.
2 changes: 1 addition & 1 deletion src/sensors/softfusion/drivers/icm45686.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
template <typename I2CImpl>
struct ICM45686 {
static constexpr uint8_t Address = 0x68;
static constexpr auto Name = "ICM-45688";
static constexpr auto Name = "ICM-45686";
static constexpr auto Type = ImuID::ICM45686;

static constexpr float GyrTs = 1.0 / 409.6;
Expand Down
317 changes: 150 additions & 167 deletions src/sensors/softfusion/softfusionsensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,28 +26,30 @@
#include <array>
#include <type_traits>

#include <array>
#include <type_traits>

#include "../SensorFusionRestDetect.h"
#include "../sensor.h"
#include "GlobalVars.h"

namespace SlimeVR::Sensors {

template <template <typename I2CImpl> typename T, typename I2CImpl>
class SoftFusionSensor : public Sensor {
using imu = T<I2CImpl>;
template <template<typename I2CImpl> typename T, typename I2CImpl>
class SoftFusionSensor : public Sensor
{
using imu = T<I2CImpl>;

static constexpr bool Uses32BitSensorData
= requires(imu& i) { i.Uses32BitSensorData; };
static constexpr bool Uses32BitSensorData = requires(imu& i){ i.Uses32BitSensorData; };

using RawSensorT =
typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
using RawVectorT = std::array<RawSensorT, 3>;
using RawSensorT = typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
using RawVectorT = std::array<RawSensorT, 3>;

static constexpr auto UpsideDownCalibrationInit = true;
static constexpr auto GyroCalibDelaySeconds = 5;
static constexpr auto GyroCalibSeconds = 5;
static constexpr auto SampleRateCalibDelaySeconds = 1;
static constexpr auto SampleRateCalibSeconds = 5;
static constexpr auto UpsideDownCalibrationInit = true;
static constexpr auto GyroCalibDelaySeconds = 5;
static constexpr auto GyroCalibSeconds = 5;
static constexpr auto SampleRateCalibDelaySeconds = 1;
static constexpr auto SampleRateCalibSeconds = 5;

static constexpr auto AccelCalibDelaySeconds = 3;
static constexpr auto AccelCalibRestSeconds = 3;
Expand Down Expand Up @@ -101,11 +103,12 @@ class SoftFusionSensor : public Sensor {
);
}

void processAccelSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) {
sensor_real_t accelData[]
= {static_cast<sensor_real_t>(xyz[0]),
static_cast<sensor_real_t>(xyz[1]),
static_cast<sensor_real_t>(xyz[2])};
void processAccelSample(const RawSensorT xyz[3], const sensor_real_t timeDelta)
{
sensor_real_t accelData[] = {
static_cast<sensor_real_t>(xyz[0]),
static_cast<sensor_real_t>(xyz[1]),
static_cast<sensor_real_t>(xyz[2]) };

float tmp[3];
for (uint8_t i = 0; i < 3; i++) {
Expand All @@ -128,61 +131,58 @@ class SoftFusionSensor : public Sensor {
m_fusion.updateAcc(accelData, m_calibration.A_Ts);
}

void processGyroSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) {
const sensor_real_t scaledData[] = {
static_cast<sensor_real_t>(
GScale * (static_cast<sensor_real_t>(xyz[0]) - m_calibration.G_off[0])
),
static_cast<sensor_real_t>(
GScale * (static_cast<sensor_real_t>(xyz[1]) - m_calibration.G_off[1])
),
static_cast<sensor_real_t>(
GScale * (static_cast<sensor_real_t>(xyz[2]) - m_calibration.G_off[2])
)};
m_fusion.updateGyro(scaledData, m_calibration.G_Ts);
}

void eatSamplesForSeconds(const uint32_t seconds) {
const auto targetDelay = millis() + 1000 * seconds;
auto lastSecondsRemaining = seconds;
while (millis() < targetDelay) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
auto currentSecondsRemaining = (targetDelay - millis()) / 1000;
if (currentSecondsRemaining != lastSecondsRemaining) {
m_Logger.info("%d...", currentSecondsRemaining + 1);
lastSecondsRemaining = currentSecondsRemaining;
}

m_sensor.bulkRead(
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}
);
}
}

std::pair<RawVectorT, RawVectorT> eatSamplesReturnLast(const uint32_t milliseconds
) {
RawVectorT accel = {0};
RawVectorT gyro = {0};
const auto targetDelay = millis() + milliseconds;
while (millis() < targetDelay) {
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
accel[0] = xyz[0];
accel[1] = xyz[1];
accel[2] = xyz[2];
},
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
gyro[0] = xyz[0];
gyro[1] = xyz[1];
gyro[2] = xyz[2];
}
);
}
return std::make_pair(accel, gyro);
}
void processGyroSample(const RawSensorT xyz[3], const sensor_real_t timeDelta)
{
const sensor_real_t scaledData[] = {
static_cast<sensor_real_t>(GScale * (static_cast<sensor_real_t>(xyz[0]) - m_calibration.G_off[0])),
static_cast<sensor_real_t>(GScale * (static_cast<sensor_real_t>(xyz[1]) - m_calibration.G_off[1])),
static_cast<sensor_real_t>(GScale * (static_cast<sensor_real_t>(xyz[2]) - m_calibration.G_off[2]))};
m_fusion.updateGyro(scaledData, m_calibration.G_Ts);
}

void eatSamplesForSeconds(const uint32_t seconds) {
const auto targetDelay = millis() + 1000 * seconds;
auto lastSecondsRemaining = seconds;
while (millis() < targetDelay)
{
#ifdef ESP8266
ESP.wdtFeed();
#endif
auto currentSecondsRemaining = (targetDelay - millis()) / 1000;
if (currentSecondsRemaining != lastSecondsRemaining) {
m_Logger.info("%d...", currentSecondsRemaining + 1);
lastSecondsRemaining = currentSecondsRemaining;
}

m_sensor.bulkRead(
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}
);
}
}

std::pair<RawVectorT, RawVectorT> eatSamplesReturnLast(const uint32_t milliseconds)
{
RawVectorT accel = {0};
RawVectorT gyro = {0};
const auto targetDelay = millis() + milliseconds;
while (millis() < targetDelay)
{
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
accel[0] = xyz[0];
accel[1] = xyz[1];
accel[2] = xyz[2];
},
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
gyro[0] = xyz[0];
gyro[1] = xyz[1];
gyro[2] = xyz[2];
}
);
}
return std::make_pair(accel, gyro);
}

public:
static constexpr auto TypeID = imu::Type;
Expand Down Expand Up @@ -212,27 +212,22 @@ class SoftFusionSensor : public Sensor {
void motionLoop() override final {
sendTempIfNeeded();

// read fifo updating fusion
uint32_t now = micros();
constexpr uint32_t targetPollIntervalMicros = 6000;
uint32_t elapsed = now - m_lastPollTime;
if (elapsed >= targetPollIntervalMicros) {
m_lastPollTime = now - (elapsed - targetPollIntervalMicros);
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
processAccelSample(xyz, timeDelta);
},
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
processGyroSample(xyz, timeDelta);
}
);
optimistic_yield(100);
if (!m_fusion.isUpdated()) {
return;
}
hadData = true;
m_fusion.clearUpdated();
}
// read fifo updating fusion
uint32_t now = micros();
constexpr uint32_t targetPollIntervalMicros = 6000;
uint32_t elapsed = now - m_lastPollTime;
if (elapsed >= targetPollIntervalMicros) {
m_lastPollTime = now - (elapsed - targetPollIntervalMicros);
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { processAccelSample(xyz, timeDelta); },
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) { processGyroSample(xyz, timeDelta); }
);
optimistic_yield(100);
if (!m_fusion.isUpdated()) return;
hadData = true;
m_fusion.clearUpdated();
}


// send new fusion values when time is up
now = micros();
Expand Down Expand Up @@ -280,15 +275,16 @@ class SoftFusionSensor : public Sensor {
m_Logger.info("Please recalibrate");
}

bool initResult = false;
bool initResult = false;

if constexpr (HasMotionlessCalib) {
typename imu::MotionlessCalibrationData calibData;
std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData));
initResult = m_sensor.initialize(calibData);
} else {
initResult = m_sensor.initialize();
}
if constexpr(HasMotionlessCalib) {
typename imu::MotionlessCalibrationData calibData;
std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData));
initResult = m_sensor.initialize(calibData);
}
else {
initResult = m_sensor.initialize();
}

if (!initResult) {
m_Logger.error("Sensor failed to initialize!");
Expand Down Expand Up @@ -396,21 +392,21 @@ class SoftFusionSensor : public Sensor {
const auto targetCalib = millis() + 1000 * GyroCalibSeconds;
uint32_t sampleCount = 0;

while (millis() < targetCalib) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
m_sensor.bulkRead(
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[&sumXYZ,
&sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
sumXYZ[0] += xyz[0];
sumXYZ[1] += xyz[1];
sumXYZ[2] += xyz[2];
++sampleCount;
}
);
}
while (millis() < targetCalib)
{
#ifdef ESP8266
ESP.wdtFeed();
#endif
m_sensor.bulkRead(
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[&sumXYZ, &sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
sumXYZ[0] += xyz[0];
sumXYZ[1] += xyz[1];
sumXYZ[2] += xyz[2];
++sampleCount;
}
);
}

ledManager.off();
m_calibration.G_off[0] = ((double)sumXYZ[0]) / sampleCount;
Expand Down Expand Up @@ -452,32 +448,22 @@ class SoftFusionSensor : public Sensor {
uint16_t numCurrentPositionSamples = 0;
bool waitForMotion = true;

auto accelCalibrationChunk
= std::make_unique<float[]>(numSamplesPerPosition * 3);
ledManager.pattern(100, 100, 6);
ledManager.on();
m_Logger.info("Gathering accelerometer data...");
m_Logger.info(
"Waiting for position %i, you can leave the device as is...",
numPositionsRecorded + 1
);
bool samplesGathered = false;
while (!samplesGathered) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
const sensor_real_t scaledData[]
= {static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[0])
),
static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[1])
),
static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[2])
)};
auto accelCalibrationChunk = std::make_unique<float[]>(numSamplesPerPosition * 3);
ledManager.pattern(100, 100, 6);
ledManager.on();
m_Logger.info("Gathering accelerometer data...");
m_Logger.info("Waiting for position %i, you can leave the device as is...", numPositionsRecorded + 1);
bool samplesGathered = false;
while (!samplesGathered) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
const sensor_real_t scaledData[] = {
static_cast<sensor_real_t>(AScale * static_cast<sensor_real_t>(xyz[0])),
static_cast<sensor_real_t>(AScale * static_cast<sensor_real_t>(xyz[1])),
static_cast<sensor_real_t>(AScale * static_cast<sensor_real_t>(xyz[2]))};

calibrationRestDetection.updateAcc(imu::AccTs, scaledData);
if (waitForMotion) {
Expand Down Expand Up @@ -518,16 +504,17 @@ class SoftFusionSensor : public Sensor {
numCurrentPositionSamples = 0;
}

if (numPositionsRecorded >= expectedPositions) {
samplesGathered = true;
}
},
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}
);
}
ledManager.off();
m_Logger.debug("Calculating accelerometer calibration data...");
accelCalibrationChunk.reset();
if (numPositionsRecorded >= expectedPositions)
{
samplesGathered = true;
}
},
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}
);
}
ledManager.off();
m_Logger.debug("Calculating accelerometer calibration data...");
accelCalibrationChunk.reset();

float A_BAinv[4][3];
magneto->current_calibration(A_BAinv);
Expand Down Expand Up @@ -562,20 +549,16 @@ class SoftFusionSensor : public Sensor {
uint32_t accelSamples = 0;
uint32_t gyroSamples = 0;

const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds;
m_Logger.debug("Counting samples now...");
uint32_t currentTime;
while ((currentTime = millis()) < calibTarget) {
m_sensor.bulkRead(
[&accelSamples](
const RawSensorT xyz[3],
const sensor_real_t timeDelta
) { accelSamples++; },
[&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
gyroSamples++;
}
);
}
const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds;
m_Logger.debug("Counting samples now...");
uint32_t currentTime;
while ((currentTime = millis()) < calibTarget)
{
m_sensor.bulkRead(
[&accelSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { accelSamples++; },
[&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) { gyroSamples++; }
);
}

const auto millisFromStart
= currentTime - (calibTarget - 1000 * SampleRateCalibSeconds);
Expand Down

0 comments on commit e2e1880

Please sign in to comment.