diff --git a/old_examples/shared/sensors/LSM6DS3H/LSM6DS3HData.h b/old_examples/shared/sensors/LSM6DS3H/LSM6DS3HData.h index d321f0c15e26ed102496c2da8548b1a87d47b323..128707df272bcdda304895018fa35f71c28b5cea 100644 --- a/old_examples/shared/sensors/LSM6DS3H/LSM6DS3HData.h +++ b/old_examples/shared/sensors/LSM6DS3H/LSM6DS3HData.h @@ -35,8 +35,8 @@ struct LSM6DS3HData static std::string header() { - return "timestamp,acc_x,acc_y,acc_z,angularVelocityX,angularVelocityY," - "angularVelocityZ\n"; + return "timestamp,acc_x,acc_y,acc_z,angularSpeedX,angularSpeedY," + "angularSpeedZ\n"; } void print(std::ostream& os) const diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp index 07f5f0b323369418582b7a5a49374f04195fa5f3..40e57dc55447ef5165bfebdb252643d6c366125d 100644 --- a/src/entrypoints/bmx160-calibration-entry.cpp +++ b/src/entrypoints/bmx160-calibration-entry.cpp @@ -417,9 +417,8 @@ void calibrateGyroscope() auto data = fifo.at(i); Logger::getInstance().log(data); - calibrationModel.feed({data.angularVelocityX, - data.angularVelocityY, - data.angularVelocityZ}); + calibrationModel.feed({data.angularSpeedX, data.angularSpeedY, + data.angularSpeedZ}); count++; } }, diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp index 5f745b7951ff63c6383fb20ce8268d97f04f2455..d84b39d311d8fa80e2892d47f71b07cf983a5c22 100644 --- a/src/shared/algorithms/NAS/NAS.cpp +++ b/src/shared/algorithms/NAS/NAS.cpp @@ -127,12 +127,12 @@ void NAS::predictAcc(const AccelerometerData& acceleration) acceleration.accelerationZ}); } -void NAS::predictGyro(const Vector3f& angularVelocity) +void NAS::predictGyro(const Vector3f& angularSpeed) { Vector3f bias = x.block<3, 1>(IDX_BIAS, 0); Vector4f q = x.block<4, 1>(IDX_QUAT, 0); - Vector3f omega = angularVelocity - bias; + Vector3f omega = angularSpeed - bias; // clang-format off Matrix4f omega_mat{ { 0.0f, omega(2), -omega(1), omega(0)}, @@ -156,11 +156,10 @@ void NAS::predictGyro(const Vector3f& angularVelocity) P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq; } -void NAS::predictGyro(const GyroscopeData& angularVelocity) +void NAS::predictGyro(const GyroscopeData& angularSpeed) { - predictGyro(Vector3f{angularVelocity.angularVelocityX, - angularVelocity.angularVelocityY, - angularVelocity.angularVelocityZ}); + predictGyro(Vector3f{angularSpeed.angularSpeedX, angularSpeed.angularSpeedY, + angularSpeed.angularSpeedZ}); } void NAS::correctBaro(const float pressure) diff --git a/src/shared/algorithms/NAS/NAS.h b/src/shared/algorithms/NAS/NAS.h index a5f7d0d1a3a9e324970a1730732a5c02e0c56c71..a200c215b76adc4f4c65b969900bf097f4b5f973 100644 --- a/src/shared/algorithms/NAS/NAS.h +++ b/src/shared/algorithms/NAS/NAS.h @@ -69,16 +69,16 @@ public: /** * @brief Prediction with gyroscope data. * - * @param angularVelocity Vector with angular velocity data [x y z][rad/s]. + * @param angularSpeed Vector with angular velocity data [x y z][rad/s]. */ - void predictGyro(const Eigen::Vector3f& angularVelocity); + void predictGyro(const Eigen::Vector3f& angularSpeed); /** * @brief Prediction with gyroscope data. * - * @param angularVelocity Gyroscope data [rad/s]. + * @param angularSpeed Gyroscope data [rad/s]. */ - void predictGyro(const GyroscopeData& angularVelocity); + void predictGyro(const GyroscopeData& angularSpeed); /** * @brief Correction with barometer data. diff --git a/src/shared/sensors/BMX160/BMX160.cpp b/src/shared/sensors/BMX160/BMX160.cpp index 2a40cdcafc9723ba44fa0e09455f5e2b9c3b0a54..41aea4459273f7d6200de8d8084f39011e9d48b9 100644 --- a/src/shared/sensors/BMX160/BMX160.cpp +++ b/src/shared/sensors/BMX160/BMX160.cpp @@ -31,19 +31,19 @@ namespace Boardcore BMX160::BMX160(SPIBusInterface& bus, miosix::GpioPin cs, BMX160Config config) : BMX160(bus, cs, config, SPIBusConfig{}) { - spiSlave.config.clockDivider = SPI::ClockDivider::DIV_32; - oldMag.magneticFieldTimestamp = 0.0f; - oldGyr.angularVelocityTimestamp = 0.0f; - oldAcc.accelerationTimestamp = 0.0f; + spiSlave.config.clockDivider = SPI::ClockDivider::DIV_32; + oldMag.magneticFieldTimestamp = 0.0f; + oldGyr.angularSpeedTimestamp = 0.0f; + oldAcc.accelerationTimestamp = 0.0f; } BMX160::BMX160(SPIBusInterface& bus, miosix::GpioPin cs, BMX160Config config, SPIBusConfig bus_config) : spiSlave(bus, cs, bus_config), config(config) { - oldMag.magneticFieldTimestamp = 0.0f; - oldGyr.angularVelocityTimestamp = 0.0f; - oldAcc.accelerationTimestamp = 0.0f; + oldMag.magneticFieldTimestamp = 0.0f; + oldGyr.angularSpeedTimestamp = 0.0f; + oldAcc.accelerationTimestamp = 0.0f; } bool BMX160::init() @@ -732,8 +732,8 @@ void BMX160::readFifo(bool headerless) // current frame. if (oldMag.magneticFieldTimestamp != 0) oldMag.magneticFieldTimestamp -= interruptTimestampDelta; - if (oldGyr.angularVelocityTimestamp != 0) - oldGyr.angularVelocityTimestamp -= interruptTimestampDelta; + if (oldGyr.angularSpeedTimestamp != 0) + oldGyr.angularSpeedTimestamp -= interruptTimestampDelta; if (oldAcc.accelerationTimestamp != 0) oldAcc.accelerationTimestamp -= interruptTimestampDelta; @@ -865,7 +865,7 @@ void BMX160::readFifo(bool headerless) { lastFifo[i].accelerationTimestamp += lastInterruptTimestamp - watermarkTimestamp; - lastFifo[i].angularVelocityTimestamp += + lastFifo[i].angularSpeedTimestamp += lastInterruptTimestamp - watermarkTimestamp; lastFifo[i].magneticFieldTimestamp += lastInterruptTimestamp - watermarkTimestamp; diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h index 5347ad81cdd5cbbded71cbdea25b7033c350f333..bc2c3316050a8b4c4f075f7da79e4358dd04f648 100644 --- a/src/shared/sensors/BMX160/BMX160Data.h +++ b/src/shared/sensors/BMX160/BMX160Data.h @@ -45,8 +45,8 @@ struct BMX160Data : public AccelerometerData, static std::string header() { return "accelerationTimestamp,accelerationX,accelerationY," - "accelerationZ,angularVelocityTimestamp,angularVelocityX," - "angularVelocityY,angularVelocityZ,magneticFieldTimestamp," + "accelerationZ,angularSpeedTimestamp,angularSpeedX," + "angularSpeedY,angularSpeedZ,magneticFieldTimestamp," "magneticFieldX,magneticFieldY,magneticFieldZ\n"; } @@ -54,8 +54,8 @@ struct BMX160Data : public AccelerometerData, { os << accelerationTimestamp << "," << accelerationX << "," << accelerationY << "," << accelerationZ << "," - << angularVelocityTimestamp << "," << angularVelocityX << "," - << angularVelocityY << "," << angularVelocityZ << "," + << 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 5246aa3b21ac990ce8419d66de9bb648deb53791..ba68500bb4214faab3e5f48e8323bc92310a629b 100644 --- a/src/shared/sensors/BMX160/BMX160WithCorrection.cpp +++ b/src/shared/sensors/BMX160/BMX160WithCorrection.cpp @@ -117,7 +117,7 @@ BMX160WithCorrectionData BMX160WithCorrection::sampleImpl() static_cast<AccelerometerData&>(result) << avgAccel; result.magneticFieldTimestamp = fifoElement.accelerationTimestamp; static_cast<MagnetometerData&>(result) << avgMag; - result.angularVelocityTimestamp = fifoElement.accelerationTimestamp; + result.angularSpeedTimestamp = fifoElement.accelerationTimestamp; 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 65a737f5a3bc2b7bae7be63b808cf279318666eb..c3e22c0948bd80a888934fd8462e375da69b1806 100644 --- a/src/shared/sensors/BMX160/BMX160WithCorrectionData.h +++ b/src/shared/sensors/BMX160/BMX160WithCorrectionData.h @@ -52,9 +52,9 @@ struct BMX160WithCorrectionData : public BMX160Data BMX160WithCorrectionData& operator=(GyroscopeData gyr) { - angularVelocityX = gyr.angularVelocityX; - angularVelocityY = gyr.angularVelocityY; - angularVelocityZ = gyr.angularVelocityZ; + angularSpeedX = gyr.angularSpeedX; + angularSpeedY = gyr.angularSpeedY; + angularSpeedZ = gyr.angularSpeedZ; return *this; } @@ -69,21 +69,17 @@ struct BMX160WithCorrectionData : public BMX160Data static std::string header() { return "accelerationTimestamp,accelerationX,accelerationY," - "accelerationZ,gyro_" - "timestamp," - "angularVelocityX," - "angularVelocityY," - "angularVelocityZ,magneticFieldTimestamp,magneticFieldX," - "magneticFieldY," - "magneticFieldZ\n"; + "accelerationZ,angularSpeedTimestamp,angularSpeedX," + "angularSpeedY,angularSpeedZ,magneticFieldTimestamp," + "magneticFieldX,magneticFieldY,magneticFieldZ\n"; } void print(std::ostream& os) const { os << accelerationTimestamp << "," << accelerationX << "," << accelerationY << "," << accelerationZ << "," - << angularVelocityTimestamp << "," << angularVelocityX << "," - << angularVelocityY << "," << angularVelocityZ << "," + << angularSpeedTimestamp << "," << angularSpeedX << "," + << angularSpeedY << "," << angularSpeedZ << "," << magneticFieldTimestamp << "," << magneticFieldX << "," << magneticFieldY << "," << magneticFieldZ << "\n"; } diff --git a/src/shared/sensors/L3GD20/L3GD20Data.h b/src/shared/sensors/L3GD20/L3GD20Data.h index fa8e08955a40624372553f3d614b3cb3ec408353..e46833302b5e8c5c4898cc579f7b51e6f693fc9b 100644 --- a/src/shared/sensors/L3GD20/L3GD20Data.h +++ b/src/shared/sensors/L3GD20/L3GD20Data.h @@ -41,14 +41,14 @@ struct L3GD20Data : public GyroscopeData static std::string header() { - return "angularVelocityTimestamp,angularVelocityX,angularVelocityY," - "angularVelocityZ\n"; + return "angularSpeedTimestamp,angularSpeedX,angularSpeedY," + "angularSpeedZ\n"; } void print(std::ostream& os) const { - os << angularVelocityTimestamp << "," << angularVelocityX << "," - << angularVelocityY << "," << angularVelocityZ << "\n"; + os << angularSpeedTimestamp << "," << angularSpeedX << "," + << angularSpeedY << "," << angularSpeedZ << "\n"; } }; diff --git a/src/shared/sensors/MPU9250/MPU9250.cpp b/src/shared/sensors/MPU9250/MPU9250.cpp index 23c98cd7c85fd11d1fe8eb5507c8a13a076ec4ba..512bbbea3926c7e7b91e9a04d7a85c08da2c2ed8 100644 --- a/src/shared/sensors/MPU9250/MPU9250.cpp +++ b/src/shared/sensors/MPU9250/MPU9250.cpp @@ -111,11 +111,11 @@ MPU9250Data MPU9250::sampleImpl() } // Save timestamps - uint64_t timestamp = TimestampTimer::getTimestamp(); - data.accelerationTimestamp = timestamp; - data.temperatureTimestamp = timestamp; - data.angularVelocityTimestamp = timestamp; - data.magneticFieldTimestamp = timestamp; + uint64_t timestamp = TimestampTimer::getTimestamp(); + data.accelerationTimestamp = timestamp; + data.temperatureTimestamp = timestamp; + data.angularSpeedTimestamp = timestamp; + data.magneticFieldTimestamp = timestamp; // Save data data.accelerationX = @@ -124,10 +124,10 @@ MPU9250Data MPU9250::sampleImpl() normalizeAcceleration(swapBytes16(rawData.bits.accelY)); data.accelerationZ = normalizeAcceleration(swapBytes16(rawData.bits.accelZ)); - data.temperature = normalizeTemperature(swapBytes16(rawData.bits.temp)); - data.angularVelocityX = normalizeGyroscope(swapBytes16(rawData.bits.gyroX)); - data.angularVelocityY = normalizeGyroscope(swapBytes16(rawData.bits.gyroY)); - data.angularVelocityZ = normalizeGyroscope(swapBytes16(rawData.bits.gyroZ)); + data.temperature = normalizeTemperature(swapBytes16(rawData.bits.temp)); + data.angularSpeedX = normalizeGyroscope(swapBytes16(rawData.bits.gyroX)); + data.angularSpeedY = normalizeGyroscope(swapBytes16(rawData.bits.gyroY)); + data.angularSpeedZ = normalizeGyroscope(swapBytes16(rawData.bits.gyroZ)); data.magneticFieldX = normalizeMagnetometer(rawData.bits.magX, magSensAdjCoeff[0]); data.magneticFieldY = diff --git a/src/shared/sensors/MPU9250/MPU9250Data.h b/src/shared/sensors/MPU9250/MPU9250Data.h index 2f7837b93064d844c94204071a29f815f58487ae..45e314b335db47ce7698a299102d6ffbd4f746f2 100644 --- a/src/shared/sensors/MPU9250/MPU9250Data.h +++ b/src/shared/sensors/MPU9250/MPU9250Data.h @@ -43,8 +43,8 @@ struct MPU9250Data : public AccelerometerData, return "accelerationTimestamp,accelerationX,accelerationY," "accelerationZ,gyro_" "timestamp," - "angularVelocityX," - "angularVelocityY,angularVelocityZ,magneticFieldTimestamp," + "angularSpeedX," + "angularSpeedY,angularSpeedZ,magneticFieldTimestamp," "magneticFieldX," "magneticFieldY," "magneticFieldZ," @@ -56,8 +56,8 @@ struct MPU9250Data : public AccelerometerData, { os << accelerationTimestamp << "," << accelerationX << "," << accelerationY << "," << accelerationZ << "," - << angularVelocityTimestamp << "," << angularVelocityX << "," - << angularVelocityY << "," << angularVelocityZ << "," + << angularSpeedTimestamp << "," << angularSpeedX << "," + << angularSpeedY << "," << angularSpeedZ << "," << magneticFieldTimestamp << "," << magneticFieldX << "," << magneticFieldY << "," << magneticFieldZ << "," << temperatureTimestamp << "," << temperature << "\n"; diff --git a/src/shared/sensors/SensorData.h b/src/shared/sensors/SensorData.h index 8c79d8be71661cdc5ed5dd6d1bf469b3c1fdef3e..002ca80a728277c8a20c6b5375e029b68a71357a 100644 --- a/src/shared/sensors/SensorData.h +++ b/src/shared/sensors/SensorData.h @@ -150,41 +150,40 @@ struct AccelerometerData */ struct GyroscopeData { - uint64_t angularVelocityTimestamp = 0; - float angularVelocityX = 0; - float angularVelocityY = 0; - float angularVelocityZ = 0; + uint64_t angularSpeedTimestamp = 0; + float angularSpeedX = 0; + float angularSpeedY = 0; + float angularSpeedZ = 0; GyroscopeData() {} GyroscopeData(uint64_t timestamp, float x, float y, float z) - : angularVelocityTimestamp(timestamp), angularVelocityX(x), - angularVelocityY(y), angularVelocityZ(z) + : angularSpeedTimestamp(timestamp), angularSpeedX(x), angularSpeedY(y), + angularSpeedZ(z) { } GyroscopeData(const GyroscopeData& data) = default; explicit GyroscopeData(const Eigen::Vector3f& vel) - : angularVelocityX(vel(0)), angularVelocityY(vel(1)), - angularVelocityZ(vel(2)) + : angularSpeedX(vel(0)), angularSpeedY(vel(1)), angularSpeedZ(vel(2)) { } static std::string header() { - return "timestamp,angularVelocityX,angularVelocityY,angularVelocityZ\n"; + return "timestamp,angularSpeedX,angularSpeedY,angularSpeedZ\n"; } void print(std::ostream& os) const { - os << angularVelocityTimestamp << "," << angularVelocityX << "," - << angularVelocityY << "," << angularVelocityZ << "\n"; + os << angularSpeedTimestamp << "," << angularSpeedX << "," + << angularSpeedY << "," << angularSpeedZ << "\n"; } operator Eigen::Vector3f() const { - return {angularVelocityX, angularVelocityY, angularVelocityZ}; + return {angularSpeedX, angularSpeedY, angularSpeedZ}; } }; diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100.cpp index 5ee4019dc91cd208212c2462c7ca782e8eef7204..826d51f855f7289affd4eb67b784aed5b4fb99c8 100644 --- a/src/shared/sensors/VN100/VN100.cpp +++ b/src/shared/sensors/VN100/VN100.cpp @@ -578,10 +578,10 @@ GyroscopeData VN100::sampleGyroscope() } // Parse the data - data.angularVelocityTimestamp = TimestampTimer::getTimestamp(); - data.angularVelocityX = strtod(recvString + indexStart + 1, &nextNumber); - data.angularVelocityY = strtod(nextNumber + 1, &nextNumber); - data.angularVelocityZ = strtod(nextNumber + 1, NULL); + data.angularSpeedTimestamp = TimestampTimer::getTimestamp(); + data.angularSpeedX = strtod(recvString + indexStart + 1, &nextNumber); + data.angularSpeedY = strtod(nextNumber + 1, &nextNumber); + data.angularSpeedZ = strtod(nextNumber + 1, NULL); return data; } diff --git a/src/shared/sensors/VN100/VN100Data.h b/src/shared/sensors/VN100/VN100Data.h index 210a3b82d77b5788afeb00a808a345c7dc1f6fad..319a7621b28c87a718256ab9ff99cda43d143eda 100644 --- a/src/shared/sensors/VN100/VN100Data.h +++ b/src/shared/sensors/VN100/VN100Data.h @@ -83,9 +83,9 @@ struct VN100Data : public QuaternionData, return "quatTimestamp,quatX,quatY,quatZ,quatW,magneticFieldTimestamp," "magneticFieldX,magneticFieldY,magneticFieldZ," "accelerationTimestamp,accelerationX,accelerationY," - "accelerationZ,gyro_timestamp,angularVelocityX,angularVelocityY," - "angularVelocityZ,temperatureTimestamp,temperature," - "pressureTimestamp,pressure\n"; + "accelerationZ,angularSpeedTimestamp,angularSpeedX," + "angularSpeedY,angularSpeedZ,temperatureTimestamp," + "temperature,pressureTimestamp,pressure\n"; } void print(std::ostream& os) const @@ -95,8 +95,8 @@ struct VN100Data : public QuaternionData, << magneticFieldX << "," << magneticFieldY << "," << magneticFieldZ << "," << accelerationTimestamp << "," << accelerationX << "," << accelerationY << "," << accelerationZ << "," - << angularVelocityTimestamp << "," << angularVelocityX << "," - << angularVelocityY << "," << angularVelocityZ << "," + << 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 ed69028b1d522410bb381d021002bcd39bc0739c..faaf8caea076a9f861b50c2e57880595c9d379a1 100644 --- a/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp +++ b/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp @@ -43,16 +43,16 @@ void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs) void operator<<(GyroscopeData& lhs, const Vector3f& rhs) { - lhs.angularVelocityX = rhs[0]; - lhs.angularVelocityY = rhs[1]; - lhs.angularVelocityZ = rhs[2]; + lhs.angularSpeedX = rhs[0]; + lhs.angularSpeedY = rhs[1]; + lhs.angularSpeedZ = rhs[2]; } void operator<<(Eigen::Vector3f& lhs, const GyroscopeData& rhs) { - lhs[0] = rhs.angularVelocityX; - lhs[1] = rhs.angularVelocityY; - lhs[2] = rhs.angularVelocityZ; + lhs[0] = rhs.angularSpeedX; + lhs[1] = rhs.angularSpeedY; + lhs[2] = rhs.angularSpeedZ; } void operator<<(MagnetometerData& lhs, const Vector3f& rhs) diff --git a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp index 2fc066cbeff7d240bd7ce46a20f5658d57696783..3af6f6222da312a1521abc54ccce120e1e205236 100644 --- a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp +++ b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp @@ -111,8 +111,8 @@ void imuStep() auto data = imu->getLastSample(); Vector3f acceleration(data.accelerationX, data.accelerationY, data.accelerationZ); - Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY, - data.angularVelocityZ); + Vector3f angularSpeed(data.angularSpeedX, data.angularSpeedY, + data.angularSpeedZ); Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ); @@ -120,7 +120,7 @@ void imuStep() { Vector3f bias(0.0218462708018154, 0.0281755574886535, 0.0264119470499244); - angularVelocity -= bias; + angularSpeed -= bias; Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423); magneticField -= offset; magneticField = {magneticField[1], magneticField[0], -magneticField[2]}; @@ -130,7 +130,7 @@ void imuStep() magneticField.normalize(); // Predict step - nas->predictGyro(angularVelocity); + nas->predictGyro(angularSpeed); // Correct step nas->correctMag(magneticField); diff --git a/src/tests/algorithms/NAS/test-attitude-stack.cpp b/src/tests/algorithms/NAS/test-attitude-stack.cpp index 58fa874f554f9ba3e32a71e42de1f377067fd665..00d0145f7737a20a708c2a65fb5d725ce292d214 100644 --- a/src/tests/algorithms/NAS/test-attitude-stack.cpp +++ b/src/tests/algorithms/NAS/test-attitude-stack.cpp @@ -132,16 +132,16 @@ void imuStep() auto data = imu->getLastSample(); Vector3f acceleration(data.accelerationX, data.accelerationY, data.accelerationZ); - Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY, - data.angularVelocityZ); + Vector3f angularSpeed(data.angularSpeedX, data.angularSpeedY, + data.angularSpeedZ); Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ); // Calibration { Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451}; - angularVelocity = angularVelocity - offset; - angularVelocity = angularVelocity / 180 * Constants::PI / 10; + angularSpeed = angularSpeed - offset; + angularSpeed = angularSpeed / 180 * Constants::PI / 10; Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269}; Matrix3f A{{0.688760050772712, 0, 0}, {0, 0.637715211784480, 0}, @@ -153,13 +153,13 @@ void imuStep() magneticField.normalize(); // Predict step - nas->predictGyro(angularVelocity); + nas->predictGyro(angularSpeed); // Correct step nas->correctMag(magneticField); nas->correctAcc(acceleration); - // std::cout << acceleration.transpose() << angularVelocity.transpose() + // std::cout << acceleration.transpose() << angularSpeed.transpose() // << magneticField.transpose() << std::endl; auto nasState = nas->getState(); diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp index be943b057a3b42f05f941ffdc49fd69de9379a9b..7af7e41c3861b95483cd136018fe2977216046e1 100644 --- a/src/tests/algorithms/NAS/test-nas-parafoil.cpp +++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp @@ -129,8 +129,8 @@ void step() Vector3f acceleration(imuData.accelerationX, imuData.accelerationY, imuData.accelerationZ); - Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY, - imuData.angularVelocityZ); + Vector3f angularSpeed(imuData.angularSpeedX, imuData.angularSpeedY, + imuData.angularSpeedZ); Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY, imuData.magneticFieldZ); @@ -146,14 +146,14 @@ void step() Vector3f biasAcc(-0.1255, 0.2053, -0.2073); acceleration -= biasAcc; Vector3f bias(-0.0291, 0.0149, 0.0202); - angularVelocity -= bias; + angularSpeed -= bias; Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423); magneticField -= offset; magneticField = {magneticField[1], magneticField[0], -magneticField[2]}; } // Predict step - nas->predictGyro(angularVelocity); + nas->predictGyro(angularSpeed); if (gpsPos[0] < 1e3 && gpsPos[0] > -1e3 && gpsPos[1] < 1e3 && gpsPos[1] > -1e3) nas->predictAcc(acceleration); diff --git a/src/tests/algorithms/NAS/test-nas-with-triad.cpp b/src/tests/algorithms/NAS/test-nas-with-triad.cpp index a940738f664fb4c26d0de48e89f5349d30a4691d..fb5c1c20fd2f5c984e14d16fc3c749fbe0b2c426 100644 --- a/src/tests/algorithms/NAS/test-nas-with-triad.cpp +++ b/src/tests/algorithms/NAS/test-nas-with-triad.cpp @@ -128,8 +128,8 @@ void bmxCallback() auto data = bmx160->getLastSample(); Vector3f acceleration(data.accelerationX, data.accelerationY, data.accelerationZ); - Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY, - data.angularVelocityZ); + Vector3f angularSpeed(data.angularSpeedX, data.angularSpeedY, + data.angularSpeedZ); Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ); @@ -137,8 +137,8 @@ void bmxCallback() Vector3f acc_b(0.3763 + 0.094, -0.1445 - 0.0229, -0.1010 + 0.0150); acceleration -= acc_b; Vector3f gyro_b{-1.63512255486542, 3.46523431469979, -3.08516033954451}; - angularVelocity = angularVelocity - gyro_b; - angularVelocity = angularVelocity / 180 * Constants::PI / 10; + angularSpeed = angularSpeed - gyro_b; + angularSpeed = angularSpeed / 180 * Constants::PI / 10; Vector3f mag_b{21.0730033648425, -24.3997259703105, -2.32621524742862}; Matrix3f A{{0.659926504672263, 0, 0}, {0, 0.662442130094073, 0}, @@ -182,11 +182,11 @@ void bmxCallback() // Predict step { // nas->predictAcc(acceleration); - nas->predictGyro(angularVelocity); + nas->predictGyro(angularSpeed); - data.angularVelocityX = angularVelocity[0]; - data.angularVelocityY = angularVelocity[1]; - data.angularVelocityZ = angularVelocity[2]; + data.angularSpeedX = angularSpeed[0]; + data.angularSpeedY = angularSpeed[1]; + data.angularSpeedZ = angularSpeed[2]; } // Correct step @@ -201,10 +201,10 @@ void bmxCallback() auto nasState = nas->getState(); - nasState.timestamp = TimestampTimer::getTimestamp(); - data.accelerationTimestamp = nasState.timestamp; - data.magneticFieldTimestamp = nasState.timestamp; - data.angularVelocityTimestamp = nasState.timestamp; + nasState.timestamp = TimestampTimer::getTimestamp(); + data.accelerationTimestamp = nasState.timestamp; + data.magneticFieldTimestamp = nasState.timestamp; + data.angularSpeedTimestamp = nasState.timestamp; // Logger::getInstance().log(nasState); // Logger::getInstance().log(data); diff --git a/src/tests/algorithms/NAS/test-nas.cpp b/src/tests/algorithms/NAS/test-nas.cpp index 306026834aa99703c6165479b18da30af64d8e9a..c62f60ffcca4cee4d86c86551254a606174f8321 100644 --- a/src/tests/algorithms/NAS/test-nas.cpp +++ b/src/tests/algorithms/NAS/test-nas.cpp @@ -151,14 +151,14 @@ void step() Vector3f acceleration(imuData.accelerationX, imuData.accelerationY, imuData.accelerationZ); - Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY, - imuData.angularVelocityZ); + Vector3f angularSpeed(imuData.angularSpeedX, imuData.angularSpeedY, + imuData.angularSpeedZ); Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY, imuData.magneticFieldZ); // Calibration { - angularVelocity -= Vector3f{-0.00863, 0.00337, 0.01284}; + angularSpeed -= Vector3f{-0.00863, 0.00337, 0.01284}; Matrix3f A{{0.66306, 0, 0}, {0, 0.66940, 0}, {0, 0, 2.25299}}; Vector3f b{40.70668, -17.83740, -13.52012}; @@ -187,7 +187,7 @@ void step() } // Predict step - nas->predictGyro(angularVelocity); + nas->predictGyro(angularSpeed); nas->predictAcc(acceleration); // Correct step diff --git a/src/tests/sensors/test-bmx160-with-correction.cpp b/src/tests/sensors/test-bmx160-with-correction.cpp index 015df9635abbe10ed6ea6ceb976dc02c0175952b..db14f2c9679bb72a0dad6953101cfc8d2b80984f 100644 --- a/src/tests/sensors/test-bmx160-with-correction.cpp +++ b/src/tests/sensors/test-bmx160-with-correction.cpp @@ -302,8 +302,8 @@ SixParametersCorrector calibrateGyroscope(SixParametersCorrector corr) auto data = fifo.at(i); Logger::getInstance().log(data); - Vector3f gyro = {data.angularVelocityX, data.angularVelocityY, - data.angularVelocityZ}; + Vector3f gyro = {data.angularSpeedX, data.angularSpeedY, + data.angularSpeedZ}; avgGyro = (avgGyro * count + gyro) / (count + 1); count++; diff --git a/src/tests/sensors/test-bmx160.cpp b/src/tests/sensors/test-bmx160.cpp index 36e04b7046e9a0b9384fc4a3d36cfb10cf5d9fd1..27f3f332fcc8b599cb5ae542a89c644f5ee6845d 100644 --- a/src/tests/sensors/test-bmx160.cpp +++ b/src/tests/sensors/test-bmx160.cpp @@ -114,9 +114,8 @@ int main() data.magneticFieldZ); printf("Gyr [%.4f s]:\t%.2f\t%.2f\t%.2f\n", - data.angularVelocityTimestamp / 1000000.0f, - data.angularVelocityX, data.angularVelocityY, - data.angularVelocityZ); + data.angularSpeedTimestamp / 1000000.0f, data.angularSpeedX, + data.angularSpeedY, data.angularSpeedZ); printf("Acc [%.4f s]:\t%.2f\t%.2f\t%.2f\n", data.accelerationTimestamp / 1000000.0f, data.accelerationX, diff --git a/src/tests/sensors/test-l3gd20-fifo.cpp b/src/tests/sensors/test-l3gd20-fifo.cpp index 24e2c4d3a723adef7581539f2e6fe8e2ae4160b9..faa927dbb0f1cbd1b7791821286eb5f06fa0a2e0 100644 --- a/src/tests/sensors/test-l3gd20-fifo.cpp +++ b/src/tests/sensors/test-l3gd20-fifo.cpp @@ -219,13 +219,13 @@ int main() // clang-format off printf("%d,%llu,%llu,%llu,%llu,%f,%f,%f,%.2f\n", data[i].fifoNum, - data[i].gyro.angularVelocityTimestamp, + data[i].gyro.angularSpeedTimestamp, data[i].wtmDelta, data[i].update, - (data[i].gyro.angularVelocityTimestamp - data[i - 1].gyro.angularVelocityTimestamp), - data[i].gyro.angularVelocityX, - data[i].gyro.angularVelocityY, - data[i].gyro.angularVelocityZ, + (data[i].gyro.angularSpeedTimestamp - data[i - 1].gyro.angularSpeedTimestamp), + data[i].gyro.angularSpeedX, + data[i].gyro.angularSpeedY, + data[i].gyro.angularSpeedZ, data[i].cpu); // clang-format on } diff --git a/src/tests/sensors/test-l3gd20.cpp b/src/tests/sensors/test-l3gd20.cpp index 33d4134e2bafa831286fdcc00b63d9005d17afd7..1ce300bf7a435490c3afc4ecb11e284d40d2bbf3 100644 --- a/src/tests/sensors/test-l3gd20.cpp +++ b/src/tests/sensors/test-l3gd20.cpp @@ -147,7 +147,7 @@ int main() L3GD20Data d = gyro->getLastSample(); // Store the sample in the array, togheter with other useful data - data[dataCounter++] = {d.angularVelocityTimestamp, sampleDelta, d, + data[dataCounter++] = {d.angularSpeedTimestamp, sampleDelta, d, CpuMeter::getCpuStats().mean}; // Wait until SAMPLE_PERIOD milliseconds from the start of this @@ -166,9 +166,9 @@ int main() TimerUtils::toIntMicroSeconds( TimestampTimer::timestampTimer.getTimer(), data[i].sampleDelta), (data[i].timestamp - data[i - 1].timestamp), - data[i].data.angularVelocityX, - data[i].data.angularVelocityY, - data[i].data.angularVelocityZ, + data[i].data.angularSpeedX, + data[i].data.angularSpeedY, + data[i].data.angularSpeedZ, data[i].cpu); // clang-format on } diff --git a/src/tests/sensors/test-mpu9250.cpp b/src/tests/sensors/test-mpu9250.cpp index 03e798fa2ac28a1d7c23b334f8c715b67d2db0ae..4a334572ed929b3033c281706db7b99a6649cc3d 100644 --- a/src/tests/sensors/test-mpu9250.cpp +++ b/src/tests/sensors/test-mpu9250.cpp @@ -72,9 +72,8 @@ int main() MPU9250Data data = mpu9250.getLastSample(); printf("%lld,%f,%f,%f;", data.accelerationTimestamp, data.accelerationX, data.accelerationY, data.accelerationZ); - printf("%lld,%f,%f,%f;", data.angularVelocityTimestamp, - data.angularVelocityX, data.angularVelocityY, - data.angularVelocityZ); + printf("%lld,%f,%f,%f;", data.angularSpeedTimestamp, data.angularSpeedX, + data.angularSpeedY, data.angularSpeedZ); printf("%lld,%f,%f,%f\n", data.magneticFieldTimestamp, data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ); diff --git a/src/tests/sensors/test-vn100.cpp b/src/tests/sensors/test-vn100.cpp index df1d1870ec448660c8fe809f3cf3fb41783c7be3..991f26650bedc095a014b686ab81a92e488b47d8 100644 --- a/src/tests/sensors/test-vn100.cpp +++ b/src/tests/sensors/test-vn100.cpp @@ -61,8 +61,8 @@ int main() printf("acc: %" PRIu64 ", %.3f, %.3f, %.3f\n", sample.accelerationTimestamp, sample.accelerationX, sample.accelerationY, sample.accelerationZ); - printf("ang: %.3f, %.3f, %.3f\n", sample.angularVelocityX, - sample.angularVelocityY, sample.angularVelocityZ); + printf("ang: %.3f, %.3f, %.3f\n", sample.angularSpeedX, + sample.angularSpeedY, sample.angularSpeedZ); sensor.sampleRaw(); sampleRaw = sensor.getLastRawSample(); diff --git a/src/tests/test-sensormanager.cpp b/src/tests/test-sensormanager.cpp index 011941abbc470c0586eecd68afa4d1e7ff4f70c3..644d70ae62febde949dffdc7def8a2cccb544e0a 100644 --- a/src/tests/test-sensormanager.cpp +++ b/src/tests/test-sensormanager.cpp @@ -105,8 +105,8 @@ struct MySensorDataFIFO : public AccelerometerData, public GyroscopeData MySensorDataFIFO(AccelerometerData acc, GyroscopeData gyro) : AccelerometerData{TimestampTimer::getTimestamp(), acc.accelerationX, acc.accelerationY, acc.accelerationZ}, - GyroscopeData{TimestampTimer::getTimestamp(), gyro.angularVelocityX, - gyro.angularVelocityY, gyro.angularVelocityZ} + GyroscopeData{TimestampTimer::getTimestamp(), gyro.angularSpeedX, + gyro.angularSpeedY, gyro.angularSpeedZ} { } }; @@ -131,9 +131,8 @@ public: TRACE("Accel : %llu %f %f %f \n", acc.accelerationTimestamp, acc.accelerationX, acc.accelerationY, acc.accelerationZ); - TRACE("Gyro : %llu %f %f %f \n", gyro.angularVelocityTimestamp, - gyro.angularVelocityX, gyro.angularVelocityY, - gyro.angularVelocityZ); + TRACE("Gyro : %llu %f %f %f \n", gyro.angularSpeedTimestamp, + gyro.angularSpeedX, gyro.angularSpeedY, gyro.angularSpeedZ); Thread::sleep(5); } @@ -269,9 +268,8 @@ int main() TRACE("AccelProxy : %llu %f %f %f \n", data.accelerationTimestamp, data.accelerationX, data.accelerationY, data.accelerationZ); - TRACE("GyroProxy : %llu %f %f %f \n", data.angularVelocityTimestamp, - data.angularVelocityX, data.angularVelocityY, - data.angularVelocityZ); + TRACE("GyroProxy : %llu %f %f %f \n", data.angularSpeedTimestamp, + data.angularSpeedX, data.angularSpeedY, data.angularSpeedZ); Thread::sleep(1000); }