From 974b48ccc560fde1258049f7f76851770c0c5a8d Mon Sep 17 00:00:00 2001
From: Davide Basso <davide.basso@skywarder.eu>
Date: Tue, 6 Feb 2024 11:18:38 +0100
Subject: [PATCH] [Units] Implement units for AccelerationData
---
src/shared/algorithms/NAS/NAS.cpp | 8 +-
src/shared/sensors/BMX160/BMX160.cpp | 18 ++--
src/shared/sensors/BMX160/BMX160Data.h | 11 +--
.../sensors/BMX160/BMX160WithCorrection.cpp | 4 +-
.../sensors/BMX160/BMX160WithCorrectionData.h | 6 +-
.../sensors/H3LIS331DL/H3LIS331DLData.h | 14 ++-
src/shared/sensors/LIS331HH/LIS331HH.cpp | 8 +-
src/shared/sensors/LIS331HH/LIS331HHData.h | 5 +-
src/shared/sensors/LIS3DSH/LIS3DSH.h | 41 +++++----
src/shared/sensors/LIS3DSH/LIS3DSHData.h | 18 ++--
src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp | 88 +++++++++++--------
src/shared/sensors/LSM6DSRX/LSM6DSRXData.h | 10 ++-
src/shared/sensors/MPU9250/MPU9250.cpp | 14 +--
src/shared/sensors/MPU9250/MPU9250Data.h | 12 +--
src/shared/sensors/SensorData.h | 24 +++--
src/shared/sensors/VN100/VN100.cpp | 10 ++-
src/shared/sensors/VN100/VN100Data.h | 14 +--
.../SensorDataExtra/SensorDataExtra.cpp | 12 +--
src/shared/units/Time.h | 5 --
19 files changed, 187 insertions(+), 135 deletions(-)
diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index 099e557b7..0500008ba 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -123,8 +123,9 @@ void NAS::predictAcc(const Vector3f& acceleration)
void NAS::predictAcc(const AccelerometerData& acceleration)
{
- predictAcc(Vector3f{acceleration.accelerationX, acceleration.accelerationY,
- acceleration.accelerationZ});
+ predictAcc(Vector3f{acceleration.accelerationX.value(),
+ acceleration.accelerationY.value(),
+ acceleration.accelerationZ.value()});
}
void NAS::predictGyro(const Vector3f& angularSpeed)
@@ -295,7 +296,8 @@ void NAS::correctAcc(const Vector3f& acc)
void NAS::correctAcc(const AccelerometerData& acc)
{
- Vector3f accV = {acc.accelerationX, acc.accelerationY, acc.accelerationZ};
+ Vector3f accV = {acc.accelerationX.value(), acc.accelerationY.value(),
+ acc.accelerationZ.value()};
correctAcc(accV.normalized());
}
diff --git a/src/shared/sensors/BMX160/BMX160.cpp b/src/shared/sensors/BMX160/BMX160.cpp
index 41aea4459..2fdd343e6 100644
--- a/src/shared/sensors/BMX160/BMX160.cpp
+++ b/src/shared/sensors/BMX160/BMX160.cpp
@@ -34,7 +34,7 @@ BMX160::BMX160(SPIBusInterface& bus, miosix::GpioPin cs, BMX160Config config)
spiSlave.config.clockDivider = SPI::ClockDivider::DIV_32;
oldMag.magneticFieldTimestamp = 0.0f;
oldGyr.angularSpeedTimestamp = 0.0f;
- oldAcc.accelerationTimestamp = 0.0f;
+ oldAcc.accelerationTimestamp = Microsecond(0);
}
BMX160::BMX160(SPIBusInterface& bus, miosix::GpioPin cs, BMX160Config config,
@@ -43,7 +43,7 @@ BMX160::BMX160(SPIBusInterface& bus, miosix::GpioPin cs, BMX160Config config,
{
oldMag.magneticFieldTimestamp = 0.0f;
oldGyr.angularSpeedTimestamp = 0.0f;
- oldAcc.accelerationTimestamp = 0.0f;
+ oldAcc.accelerationTimestamp = Microsecond(0);
}
bool BMX160::init()
@@ -608,9 +608,11 @@ AccelerometerData BMX160::buildAccData(BMX160Defs::AccRaw data,
{
using namespace Constants;
- return AccelerometerData{timestamp, data.x * accSensibility * g,
- data.y * accSensibility * g,
- data.z * accSensibility * g};
+ return AccelerometerData{
+ Microsecond(timestamp),
+ MeterPerSecondSquared(data.x * accSensibility * g),
+ MeterPerSecondSquared(data.y * accSensibility * g),
+ MeterPerSecondSquared(data.z * accSensibility * g)};
}
GyroscopeData BMX160::buildGyrData(BMX160Defs::GyrRaw data, uint64_t timestamp)
@@ -734,8 +736,8 @@ void BMX160::readFifo(bool headerless)
oldMag.magneticFieldTimestamp -= interruptTimestampDelta;
if (oldGyr.angularSpeedTimestamp != 0)
oldGyr.angularSpeedTimestamp -= interruptTimestampDelta;
- if (oldAcc.accelerationTimestamp != 0)
- oldAcc.accelerationTimestamp -= interruptTimestampDelta;
+ if (oldAcc.accelerationTimestamp.value() != 0)
+ oldAcc.accelerationTimestamp -= Microsecond(interruptTimestampDelta);
// Calculate time offset
uint64_t timeOffset = std::min({
@@ -864,7 +866,7 @@ void BMX160::readFifo(bool headerless)
for (int i = 0; i < lastFifoLevel; i++)
{
lastFifo[i].accelerationTimestamp +=
- lastInterruptTimestamp - watermarkTimestamp;
+ Microsecond(lastInterruptTimestamp - watermarkTimestamp);
lastFifo[i].angularSpeedTimestamp +=
lastInterruptTimestamp - watermarkTimestamp;
lastFifo[i].magneticFieldTimestamp +=
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index bc2c33160..be594c562 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -32,8 +32,9 @@ struct BMX160Data : public AccelerometerData,
public MagnetometerData
{
BMX160Data()
- : AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0},
- MagnetometerData{0, 0.0, 0.0, 0.0}
+ : AccelerometerData{Microsecond(0), MeterPerSecondSquared(0),
+ MeterPerSecondSquared(0), MeterPerSecondSquared(0)},
+ GyroscopeData{0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0, 0.0}
{
}
@@ -52,9 +53,9 @@ struct BMX160Data : public AccelerometerData,
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << ","
- << angularSpeedTimestamp << "," << angularSpeedX << ","
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "," << angularSpeedTimestamp << "," << angularSpeedX << ","
<< angularSpeedY << "," << angularSpeedZ << ","
<< magneticFieldTimestamp << "," << magneticFieldX << ","
<< magneticFieldY << "," << magneticFieldZ << "\n";
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.cpp b/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
index ba68500bb..ba32d9897 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
+++ b/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
@@ -115,9 +115,9 @@ BMX160WithCorrectionData BMX160WithCorrection::sampleImpl()
result.accelerationTimestamp = fifoElement.accelerationTimestamp;
static_cast<AccelerometerData&>(result) << avgAccel;
- result.magneticFieldTimestamp = fifoElement.accelerationTimestamp;
+ result.magneticFieldTimestamp = fifoElement.accelerationTimestamp.value();
static_cast<MagnetometerData&>(result) << avgMag;
- result.angularSpeedTimestamp = fifoElement.accelerationTimestamp;
+ result.angularSpeedTimestamp = fifoElement.accelerationTimestamp.value();
static_cast<GyroscopeData&>(result) << (avgGyro - gyroscopeBias);
if (calibrating)
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrectionData.h b/src/shared/sensors/BMX160/BMX160WithCorrectionData.h
index c3e22c094..ee95eddd2 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrectionData.h
+++ b/src/shared/sensors/BMX160/BMX160WithCorrectionData.h
@@ -76,9 +76,9 @@ struct BMX160WithCorrectionData : public BMX160Data
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << ","
- << angularSpeedTimestamp << "," << angularSpeedX << ","
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "," << angularSpeedTimestamp << "," << angularSpeedX << ","
<< angularSpeedY << "," << angularSpeedZ << ","
<< magneticFieldTimestamp << "," << magneticFieldX << ","
<< magneticFieldY << "," << magneticFieldZ << "\n";
diff --git a/src/shared/sensors/H3LIS331DL/H3LIS331DLData.h b/src/shared/sensors/H3LIS331DL/H3LIS331DLData.h
index 054ef30c4..090649e47 100644
--- a/src/shared/sensors/H3LIS331DL/H3LIS331DLData.h
+++ b/src/shared/sensors/H3LIS331DL/H3LIS331DLData.h
@@ -30,10 +30,15 @@ namespace Boardcore
struct H3LIS331DLData : public AccelerometerData
{
- H3LIS331DLData() : AccelerometerData(0, 0, 0, 0){};
+ H3LIS331DLData()
+ : AccelerometerData(Microsecond(0), MeterPerSecondSquared(0),
+ MeterPerSecondSquared(0),
+ MeterPerSecondSquared(0)){};
H3LIS331DLData(uint64_t ts, float aX, float aY, float aZ)
- : AccelerometerData(ts, aX, aY, aZ){};
+ : AccelerometerData(Microsecond(0), MeterPerSecondSquared(aX),
+ MeterPerSecondSquared(aY),
+ MeterPerSecondSquared(aZ)){};
explicit H3LIS331DLData(AccelerometerData acc) : AccelerometerData(acc){};
@@ -44,8 +49,9 @@ struct H3LIS331DLData : public AccelerometerData
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << "\n";
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "\n";
}
};
diff --git a/src/shared/sensors/LIS331HH/LIS331HH.cpp b/src/shared/sensors/LIS331HH/LIS331HH.cpp
index 1f2d612f5..2af538bc7 100644
--- a/src/shared/sensors/LIS331HH/LIS331HH.cpp
+++ b/src/shared/sensors/LIS331HH/LIS331HH.cpp
@@ -75,18 +75,18 @@ LIS331HHData LIS331HH::sampleImpl()
int16_t val;
LIS331HHData data;
- data.accelerationTimestamp = TimestampTimer::getTimestamp();
+ data.accelerationTimestamp = Microsecond(TimestampTimer::getTimestamp());
SPITransaction spi(slave);
spi.readRegisters(0x40 | OUT_X_L, reinterpret_cast<uint8_t*>(&val), 2);
- data.accelerationX = sensitivity * val;
+ data.accelerationX = MeterPerSecondSquared(sensitivity * val);
spi.readRegisters(0x40 | OUT_Y_L, reinterpret_cast<uint8_t*>(&val), 2);
- data.accelerationY = sensitivity * val;
+ data.accelerationY = MeterPerSecondSquared(sensitivity * val);
spi.readRegisters(0x40 | OUT_Z_L, reinterpret_cast<uint8_t*>(&val), 2);
- data.accelerationZ = sensitivity * val;
+ data.accelerationZ = MeterPerSecondSquared(sensitivity * val);
return data;
}
diff --git a/src/shared/sensors/LIS331HH/LIS331HHData.h b/src/shared/sensors/LIS331HH/LIS331HHData.h
index 72a572141..ebf85ca3c 100644
--- a/src/shared/sensors/LIS331HH/LIS331HHData.h
+++ b/src/shared/sensors/LIS331HH/LIS331HHData.h
@@ -39,8 +39,9 @@ struct LIS331HHData : public AccelerometerData
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << "\n";
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "\n";
}
};
diff --git a/src/shared/sensors/LIS3DSH/LIS3DSH.h b/src/shared/sensors/LIS3DSH/LIS3DSH.h
index d41962195..9ae1c7674 100644
--- a/src/shared/sensors/LIS3DSH/LIS3DSH.h
+++ b/src/shared/sensors/LIS3DSH/LIS3DSH.h
@@ -187,9 +187,9 @@ public:
for (uint8_t i = 0; i < numSamples; i++)
{
AccelerometerData accelData = readAccelData();
- X_ST[i] = accelData.accelerationX;
- Y_ST[i] = accelData.accelerationY;
- Z_ST[i] = accelData.accelerationZ;
+ X_ST[i] = accelData.accelerationX.value();
+ Y_ST[i] = accelData.accelerationY.value();
+ Z_ST[i] = accelData.accelerationZ.value();
miosix::Thread::sleep(10);
}
// reset the self-test bits
@@ -206,9 +206,9 @@ public:
for (uint8_t i = 0; i < numSamples; i++)
{
AccelerometerData accelData = readAccelData();
- X_NO_ST[i] = accelData.accelerationX;
- Y_NO_ST[i] = accelData.accelerationY;
- Z_NO_ST[i] = accelData.accelerationZ;
+ X_NO_ST[i] = accelData.accelerationX.value();
+ Y_NO_ST[i] = accelData.accelerationY.value();
+ Z_NO_ST[i] = accelData.accelerationZ.value();
miosix::Thread::sleep(10);
}
// compute averages vectors:
@@ -365,25 +365,28 @@ private:
{ // bit 7 of status set to 1 (some data overwritten)
accelData.accelerationTimestamp =
- TimestampTimer::getTimestamp();
+ Microsecond(TimestampTimer::getTimestamp());
// read acceleration on X
- int8_t accel_L = spi.readRegister(OUT_X_L);
- int8_t accel_H = spi.readRegister(OUT_X_H);
- accelData.accelerationX =
- static_cast<float>(combine(accel_H, accel_L)) * sensitivity;
+ int8_t accel_L = spi.readRegister(OUT_X_L);
+ int8_t accel_H = spi.readRegister(OUT_X_H);
+ accelData.accelerationX = MeterPerSecondSquared(
+ static_cast<float>(combine(accel_H, accel_L)) *
+ sensitivity);
// read acceleration on Y
- accel_L = spi.readRegister(OUT_Y_L);
- accel_H = spi.readRegister(OUT_Y_H);
- accelData.accelerationY =
- static_cast<float>(combine(accel_H, accel_L)) * sensitivity;
+ accel_L = spi.readRegister(OUT_Y_L);
+ accel_H = spi.readRegister(OUT_Y_H);
+ accelData.accelerationY = MeterPerSecondSquared(
+ static_cast<float>(combine(accel_H, accel_L)) *
+ sensitivity);
// read acceleration on Z
- accel_L = spi.readRegister(OUT_Z_L);
- accel_H = spi.readRegister(OUT_Z_H);
- accelData.accelerationZ =
- static_cast<float>(combine(accel_H, accel_L)) * sensitivity;
+ accel_L = spi.readRegister(OUT_Z_L);
+ accel_H = spi.readRegister(OUT_Z_H);
+ accelData.accelerationZ = MeterPerSecondSquared(
+ static_cast<float>(combine(accel_H, accel_L)) *
+ sensitivity);
lastError = SensorErrors::NO_ERRORS;
}
diff --git a/src/shared/sensors/LIS3DSH/LIS3DSHData.h b/src/shared/sensors/LIS3DSH/LIS3DSHData.h
index 4a1b4f54b..e42b9b88b 100644
--- a/src/shared/sensors/LIS3DSH/LIS3DSHData.h
+++ b/src/shared/sensors/LIS3DSH/LIS3DSHData.h
@@ -26,15 +26,21 @@
namespace Boardcore
{
+using namespace Units::Acceleration;
+using namespace Units::Time;
struct LIS3DSHData : public AccelerometerData, public TemperatureData
{
- LIS3DSHData() : AccelerometerData{0, 0.0, 0.0, 0.0}, TemperatureData{0, 0.0}
+ LIS3DSHData()
+ : AccelerometerData{Microsecond(0), MeterPerSecondSquared(0),
+ MeterPerSecondSquared(0), MeterPerSecondSquared(0)},
+ TemperatureData{0, 0.0}
{
}
- LIS3DSHData(uint64_t t, float x, float y, float z, float temp)
- : AccelerometerData{t, x, y, z}, TemperatureData{t, temp}
+ LIS3DSHData(Microsecond t, MeterPerSecondSquared x, MeterPerSecondSquared y,
+ MeterPerSecondSquared z, float temp)
+ : AccelerometerData{t, x, y, z}, TemperatureData{t.value(), temp}
{
}
@@ -55,9 +61,9 @@ struct LIS3DSHData : public AccelerometerData, public TemperatureData
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << ","
- << temperatureTimestamp << "," << temperature << "\n";
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "," << temperatureTimestamp << "," << temperature << "\n";
}
};
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
index 3294db578..e98c87af4 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
@@ -329,12 +329,15 @@ bool LSM6DSRX::selfTestAcc()
}
// read data
- averageNormal.accelerationX += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A, 0.061));
- averageNormal.accelerationY += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A, 0.061));
- averageNormal.accelerationZ += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A, 0.061));
+ averageNormal.accelerationX += MeterPerSecondSquared(
+ static_cast<float>(getAxisData(LSM6DSRXDefs::REG_OUTX_L_A,
+ LSM6DSRXDefs::REG_OUTX_H_A, 0.061)));
+ averageNormal.accelerationY += MeterPerSecondSquared(
+ static_cast<float>(getAxisData(LSM6DSRXDefs::REG_OUTY_L_A,
+ LSM6DSRXDefs::REG_OUTY_H_A, 0.061)));
+ averageNormal.accelerationZ += MeterPerSecondSquared(
+ static_cast<float>(getAxisData(LSM6DSRXDefs::REG_OUTZ_L_A,
+ LSM6DSRXDefs::REG_OUTZ_H_A, 0.061)));
}
averageNormal.accelerationX /= SIZE_DATA;
averageNormal.accelerationY /= SIZE_DATA;
@@ -375,28 +378,37 @@ bool LSM6DSRX::selfTestAcc()
}
// read data
- averageSF.accelerationX += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A, 0.061));
- averageSF.accelerationY += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A, 0.061));
- averageSF.accelerationZ += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A, 0.061));
+ averageSF.accelerationX += MeterPerSecondSquared(
+ static_cast<float>(getAxisData(LSM6DSRXDefs::REG_OUTX_L_A,
+ LSM6DSRXDefs::REG_OUTX_H_A, 0.061)));
+ averageSF.accelerationY += MeterPerSecondSquared(
+ static_cast<float>(getAxisData(LSM6DSRXDefs::REG_OUTY_L_A,
+ LSM6DSRXDefs::REG_OUTY_H_A, 0.061)));
+ averageSF.accelerationZ += MeterPerSecondSquared(
+ static_cast<float>(getAxisData(LSM6DSRXDefs::REG_OUTZ_L_A,
+ LSM6DSRXDefs::REG_OUTZ_H_A, 0.061)));
}
averageSF.accelerationX /= SIZE_DATA;
averageSF.accelerationY /= SIZE_DATA;
averageSF.accelerationZ /= SIZE_DATA;
if (LSM6DSRXDefs::ACC_SELF_TEST_MIN <=
- std::abs(averageSF.accelerationX - averageNormal.accelerationX) &&
- std::abs(averageSF.accelerationX - averageNormal.accelerationX) <=
+ std::abs(averageSF.accelerationX.value() -
+ averageNormal.accelerationX.value()) &&
+ std::abs(averageSF.accelerationX.value() -
+ averageNormal.accelerationX.value()) <=
LSM6DSRXDefs::ACC_SELF_TEST_MAX &&
LSM6DSRXDefs::ACC_SELF_TEST_MIN <=
- std::abs(averageSF.accelerationY - averageNormal.accelerationY) &&
- std::abs(averageSF.accelerationY - averageNormal.accelerationY) <=
+ std::abs(averageSF.accelerationY.value() -
+ averageNormal.accelerationY.value()) &&
+ std::abs(averageSF.accelerationY.value() -
+ averageNormal.accelerationY.value()) <=
LSM6DSRXDefs::ACC_SELF_TEST_MAX &&
LSM6DSRXDefs::ACC_SELF_TEST_MIN <=
- std::abs(averageSF.accelerationZ - averageNormal.accelerationZ) &&
- std::abs(averageSF.accelerationZ - averageNormal.accelerationZ) <=
+ std::abs(averageSF.accelerationZ.value() -
+ averageNormal.accelerationZ.value()) &&
+ std::abs(averageSF.accelerationZ.value() -
+ averageNormal.accelerationZ.value()) <=
LSM6DSRXDefs::ACC_SELF_TEST_MAX)
{
returnValue = true;
@@ -612,14 +624,17 @@ uint16_t LSM6DSRX::combineHighLowBitsUnsigned(uint8_t low, uint8_t high)
void LSM6DSRX::getAccelerometerData(LSM6DSRXData& data)
{
- data.accelerationTimestamp = TimestampTimer::getTimestamp();
-
- data.accelerationX = getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A, sensitivityAcc);
- data.accelerationY = getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A, sensitivityAcc);
- data.accelerationZ = getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A, sensitivityAcc);
+ data.accelerationTimestamp = Microsecond(TimestampTimer::getTimestamp());
+
+ data.accelerationX = MeterPerSecondSquared(
+ getAxisData(LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A,
+ sensitivityAcc));
+ data.accelerationY = MeterPerSecondSquared(
+ getAxisData(LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A,
+ sensitivityAcc));
+ data.accelerationZ = MeterPerSecondSquared(
+ getAxisData(LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A,
+ sensitivityAcc));
}
void LSM6DSRX::getGyroscopeData(LSM6DSRXData& data)
@@ -839,14 +854,17 @@ void LSM6DSRX::readFromFifo()
case 0x02:
// Accelerometer data
timestamps[timeslotTag].data.accelerationX =
- static_cast<float>(combineHighLowBits(xl, xh)) *
- sensitivityAcc;
+ MeterPerSecondSquared(
+ static_cast<float>(combineHighLowBits(xl, xh)) *
+ sensitivityAcc);
timestamps[timeslotTag].data.accelerationY =
- static_cast<float>(combineHighLowBits(yl, yh)) *
- sensitivityAcc;
+ MeterPerSecondSquared(
+ static_cast<float>(combineHighLowBits(yl, yh)) *
+ sensitivityAcc);
timestamps[timeslotTag].data.accelerationZ =
- static_cast<float>(combineHighLowBits(zl, zh)) *
- sensitivityAcc;
+ MeterPerSecondSquared(
+ static_cast<float>(combineHighLowBits(zl, zh)) *
+ sensitivityAcc);
timestamps[timeslotTag].accPresent = true;
@@ -867,9 +885,9 @@ void LSM6DSRX::readFromFifo()
// Set new data
timestamps[timeslotTag].data.accelerationTimestamp =
- convertTimestamp(static_cast<uint64_t>(t));
+ Microsecond(convertTimestamp(static_cast<uint64_t>(t)));
timestamps[timeslotTag].data.angularSpeedTimestamp =
- timestamps[timeslotTag].data.accelerationTimestamp;
+ timestamps[timeslotTag].data.accelerationTimestamp.value();
timestamps[timeslotTag].accPresent = false;
timestamps[timeslotTag].gyrPresent = false;
@@ -894,7 +912,7 @@ void LSM6DSRX::pushIntoFifo(LSM6DSRXDefs::FifoTimeslotData& timeslot,
// check if data can be pushed
if ((fifoIdx > 0 && timeslot.data.accelerationTimestamp ==
lastFifo[fifoIdx - 1].accelerationTimestamp) ||
- timeslot.data.accelerationTimestamp == 0)
+ timeslot.data.accelerationTimestamp.value() == 0)
{
// the new sample has the same timestamp of the previous one or
// timestamp is 0 --> discarded
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h b/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
index 0cd5083fb..6135ef24c 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
@@ -30,7 +30,9 @@ namespace Boardcore
struct LSM6DSRXData : public AccelerometerData, public GyroscopeData
{
LSM6DSRXData()
- : AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0}
+ : AccelerometerData{Microsecond(0), MeterPerSecondSquared(0),
+ MeterPerSecondSquared(0), MeterPerSecondSquared(0)},
+ GyroscopeData{0, 0.0, 0.0, 0.0}
{
}
@@ -43,9 +45,9 @@ struct LSM6DSRXData : public AccelerometerData, public GyroscopeData
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << ","
- << angularSpeedTimestamp << "," << angularSpeedX << ","
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "," << angularSpeedTimestamp << "," << angularSpeedX << ","
<< angularSpeedY << "," << angularSpeedZ << "\n";
}
};
diff --git a/src/shared/sensors/MPU9250/MPU9250.cpp b/src/shared/sensors/MPU9250/MPU9250.cpp
index 512bbbea3..ce54e4bb3 100644
--- a/src/shared/sensors/MPU9250/MPU9250.cpp
+++ b/src/shared/sensors/MPU9250/MPU9250.cpp
@@ -112,18 +112,18 @@ MPU9250Data MPU9250::sampleImpl()
// Save timestamps
uint64_t timestamp = TimestampTimer::getTimestamp();
- data.accelerationTimestamp = timestamp;
+ data.accelerationTimestamp = Microsecond(timestamp);
data.temperatureTimestamp = timestamp;
data.angularSpeedTimestamp = timestamp;
data.magneticFieldTimestamp = timestamp;
// Save data
- data.accelerationX =
- normalizeAcceleration(swapBytes16(rawData.bits.accelX));
- data.accelerationY =
- normalizeAcceleration(swapBytes16(rawData.bits.accelY));
- data.accelerationZ =
- normalizeAcceleration(swapBytes16(rawData.bits.accelZ));
+ data.accelerationX = MeterPerSecondSquared(
+ normalizeAcceleration(swapBytes16(rawData.bits.accelX)));
+ data.accelerationY = MeterPerSecondSquared(
+ normalizeAcceleration(swapBytes16(rawData.bits.accelY)));
+ data.accelerationZ = MeterPerSecondSquared(
+ normalizeAcceleration(swapBytes16(rawData.bits.accelZ)));
data.temperature = normalizeTemperature(swapBytes16(rawData.bits.temp));
data.angularSpeedX = normalizeGyroscope(swapBytes16(rawData.bits.gyroX));
data.angularSpeedY = normalizeGyroscope(swapBytes16(rawData.bits.gyroY));
diff --git a/src/shared/sensors/MPU9250/MPU9250Data.h b/src/shared/sensors/MPU9250/MPU9250Data.h
index 45e314b33..b113cb61f 100644
--- a/src/shared/sensors/MPU9250/MPU9250Data.h
+++ b/src/shared/sensors/MPU9250/MPU9250Data.h
@@ -33,8 +33,10 @@ struct MPU9250Data : public AccelerometerData,
public TemperatureData
{
MPU9250Data()
- : AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0},
- MagnetometerData{0, 0.0, 0.0, 0.0}, TemperatureData{0, 0.0}
+ : AccelerometerData{Microsecond(0), MeterPerSecondSquared(0),
+ MeterPerSecondSquared(0), MeterPerSecondSquared(0)},
+ GyroscopeData{0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0, 0.0},
+ TemperatureData{0, 0.0}
{
}
@@ -54,9 +56,9 @@ struct MPU9250Data : public AccelerometerData,
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << ","
- << angularSpeedTimestamp << "," << angularSpeedX << ","
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "," << angularSpeedTimestamp << "," << angularSpeedX << ","
<< angularSpeedY << "," << angularSpeedZ << ","
<< magneticFieldTimestamp << "," << magneticFieldX << ","
<< magneticFieldY << "," << magneticFieldZ << ","
diff --git a/src/shared/sensors/SensorData.h b/src/shared/sensors/SensorData.h
index 28c752cde..e45b97726 100644
--- a/src/shared/sensors/SensorData.h
+++ b/src/shared/sensors/SensorData.h
@@ -22,11 +22,16 @@
#pragma once
+#include <units/Acceleration.h>
+#include <units/Time.h>
+
#include <Eigen/Core>
#include <ostream>
namespace Boardcore
{
+using namespace Units::Acceleration;
+using namespace Units::Time;
/**
* @brief Generic error codes that a sensor can generate.
@@ -108,14 +113,15 @@ struct HumidityData
*/
struct AccelerometerData
{
- uint64_t accelerationTimestamp = 0;
- float accelerationX = 0;
- float accelerationY = 0;
- float accelerationZ = 0;
+ Microsecond accelerationTimestamp = Microsecond(0);
+ MeterPerSecondSquared accelerationX = MeterPerSecondSquared(0);
+ MeterPerSecondSquared accelerationY = MeterPerSecondSquared(0);
+ MeterPerSecondSquared accelerationZ = MeterPerSecondSquared(0);
AccelerometerData() {}
- AccelerometerData(uint64_t timestamp, float x, float y, float z)
+ AccelerometerData(Microsecond timestamp, MeterPerSecondSquared x,
+ MeterPerSecondSquared y, MeterPerSecondSquared z)
: accelerationTimestamp(timestamp), accelerationX(x), accelerationY(y),
accelerationZ(z)
{
@@ -135,13 +141,15 @@ struct AccelerometerData
void print(std::ostream& os) const
{
- os << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << "\n";
+ os << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "\n";
}
operator Eigen::Vector3f() const
{
- return {accelerationX, accelerationY, accelerationZ};
+ return {accelerationX.value(), accelerationY.value(),
+ accelerationZ.value()};
}
};
diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100.cpp
index 7a808633d..dac1f18f7 100644
--- a/src/shared/sensors/VN100/VN100.cpp
+++ b/src/shared/sensors/VN100/VN100.cpp
@@ -523,10 +523,12 @@ AccelerometerData VN100::sampleAccelerometer()
}
// Parse the data
- data.accelerationTimestamp = TimestampTimer::getTimestamp();
- data.accelerationX = strtod(recvString + indexStart + 1, &nextNumber);
- data.accelerationY = strtod(nextNumber + 1, &nextNumber);
- data.accelerationZ = strtod(nextNumber + 1, NULL);
+ data.accelerationTimestamp = Microsecond(TimestampTimer::getTimestamp());
+ data.accelerationX =
+ MeterPerSecondSquared(strtod(recvString + indexStart + 1, &nextNumber));
+ data.accelerationY =
+ MeterPerSecondSquared(strtod(nextNumber + 1, &nextNumber));
+ data.accelerationZ = MeterPerSecondSquared(strtod(nextNumber + 1, NULL));
return data;
}
diff --git a/src/shared/sensors/VN100/VN100Data.h b/src/shared/sensors/VN100/VN100Data.h
index 0b1f19042..00f9b973b 100644
--- a/src/shared/sensors/VN100/VN100Data.h
+++ b/src/shared/sensors/VN100/VN100Data.h
@@ -23,6 +23,8 @@
#pragma once
#include <sensors/SensorData.h>
+#include <units/Acceleration.h>
+#include <units/Time.h>
namespace Boardcore
{
@@ -45,8 +47,10 @@ struct VN100Data : public QuaternionData,
VN100Data()
: QuaternionData{0, 0.0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0,
0.0},
- AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0},
- TemperatureData{0, 0.0}, PressureData{0, 0.0}
+ AccelerometerData{Microsecond(0), MeterPerSecondSquared(0),
+ MeterPerSecondSquared(0), MeterPerSecondSquared(0)},
+ GyroscopeData{0, 0.0, 0.0, 0.0}, TemperatureData{0, 0.0},
+ PressureData{0, 0.0}
{
}
@@ -82,9 +86,9 @@ struct VN100Data : public QuaternionData,
<< "," << quaternionZ << "," << quaternionW << ","
<< magneticFieldTimestamp << "," << magneticFieldX << ","
<< magneticFieldY << "," << magneticFieldZ << ","
- << accelerationTimestamp << "," << accelerationX << ","
- << accelerationY << "," << accelerationZ << ","
- << angularSpeedTimestamp << "," << angularSpeedX << ","
+ << accelerationTimestamp.value() << "," << accelerationX.value()
+ << "," << accelerationY.value() << "," << accelerationZ.value()
+ << "," << angularSpeedTimestamp << "," << angularSpeedX << ","
<< angularSpeedY << "," << angularSpeedZ << ","
<< temperatureTimestamp << "," << temperature << ","
<< pressureTimestamp << "," << pressure << "\n";
diff --git a/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp b/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp
index faaf8caea..1157e26f7 100644
--- a/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp
+++ b/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp
@@ -29,16 +29,16 @@ namespace Boardcore
void operator<<(AccelerometerData& lhs, const Vector3f& rhs)
{
- lhs.accelerationX = rhs[0];
- lhs.accelerationY = rhs[1];
- lhs.accelerationZ = rhs[2];
+ lhs.accelerationX = MeterPerSecondSquared(rhs[0]);
+ lhs.accelerationY = MeterPerSecondSquared(rhs[1]);
+ lhs.accelerationZ = MeterPerSecondSquared(rhs[2]);
}
void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs)
{
- lhs[0] = rhs.accelerationX;
- lhs[1] = rhs.accelerationY;
- lhs[2] = rhs.accelerationZ;
+ lhs[0] = rhs.accelerationX.value();
+ lhs[1] = rhs.accelerationY.value();
+ lhs[2] = rhs.accelerationZ.value();
}
void operator<<(GyroscopeData& lhs, const Vector3f& rhs)
diff --git a/src/shared/units/Time.h b/src/shared/units/Time.h
index 155fef4d9..482938be0 100644
--- a/src/shared/units/Time.h
+++ b/src/shared/units/Time.h
@@ -43,11 +43,6 @@ ToTime time_cast(FromTime const &from)
return ToTime(from);
}
-std::chrono::duration<float> to_chrono(Time<> const &from)
-{
- return std::chrono::duration<float>(from.value());
-}
-
using Nanosecond = Time<std::ratio<1, 1000000000>>; // Time in nanoseconds
using Microsecond = Time<std::ratio<1, 1000000>>; // Time in microseconds
using Millisecond = Time<std::ratio<1, 1000>>; // Time in milliseconds
--
GitLab