diff --git a/src/shared/sensors/SensorData.h b/src/shared/sensors/SensorData.h index 01dcac4b1d842c757f8f5ed35641d530f193d643..d3bbc8519c0deaf5df1095534d9282f6d2deff27 100644 --- a/src/shared/sensors/SensorData.h +++ b/src/shared/sensors/SensorData.h @@ -145,6 +145,50 @@ struct AccelerometerData } }; +/** + * @brief Structure to handle quaternion data. + */ +struct QuaternionData +{ + uint64_t quaternionTimestamp = 0; + float quaternionX = 0; + float quaternionY = 0; + float quaternionZ = 0; + float quaternionW = 0; + + QuaternionData() {} + + QuaternionData(uint64_t timestamp, float x, float y, float z, float w) + : quaternionTimestamp(timestamp), quaternionX(x), quaternionY(y), + quaternionZ(z), quaternionW(w) + { + } + + QuaternionData(const QuaternionData& data) = default; + + explicit QuaternionData(const Eigen::Vector4f& quat) + : quaternionX(quat(0)), quaternionY(quat(1)), quaternionZ(quat(2)), + quaternionW(quat(3)) + { + } + + static std::string header() + { + return "timestamp,quaternionX,quaternionY,quaternionZ,quaterionW\n"; + } + + void print(std::ostream& os) const + { + os << quaternionTimestamp << "," << quaternionX << "," << quaternionY + << "," << quaternionZ << "," << quaternionW << "\n"; + } + + operator Eigen::Vector4f() const + { + return {quaternionX, quaternionY, quaternionZ, quaternionW}; + } +}; + /** * @brief Structure to handle gyroscope data. */ diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100.cpp index e55f1937fe1d7f91037eab57213b679e46fbd501..f4fee6778509858a9a4c61b23ca8dfcd102180af 100644 --- a/src/shared/sensors/VN100/VN100.cpp +++ b/src/shared/sensors/VN100/VN100.cpp @@ -484,11 +484,11 @@ QuaternionData VN100::sampleQuaternion() } // Parse the data - data.quatTimestamp = TimestampTimer::getTimestamp(); - data.quatX = strtod(recvString + indexStart + 1, &nextNumber); - data.quatY = strtod(nextNumber + 1, &nextNumber); - data.quatZ = strtod(nextNumber + 1, &nextNumber); - data.quatW = strtod(nextNumber + 1, NULL); + data.quaternionTimestamp = TimestampTimer::getTimestamp(); + data.quaternionX = strtod(recvString + indexStart + 1, &nextNumber); + data.quaternionY = strtod(nextNumber + 1, &nextNumber); + data.quaternionZ = strtod(nextNumber + 1, &nextNumber); + data.quaternionW = strtod(nextNumber + 1, NULL); return data; } diff --git a/src/shared/sensors/VN100/VN100Data.h b/src/shared/sensors/VN100/VN100Data.h index 319a7621b28c87a718256ab9ff99cda43d143eda..0b1f190425d5cd0db4b451c2d69aae3d0af8ff60 100644 --- a/src/shared/sensors/VN100/VN100Data.h +++ b/src/shared/sensors/VN100/VN100Data.h @@ -27,18 +27,6 @@ namespace Boardcore { -/** - * @brief Structure to handle quaternion data - */ -struct QuaternionData -{ - uint64_t quatTimestamp; - float quatX; - float quatY; - float quatZ; - float quatW; -}; - /** * @brief data type class */ @@ -90,10 +78,11 @@ struct VN100Data : public QuaternionData, void print(std::ostream& os) const { - os << quatTimestamp << "," << quatX << "," << quatY << "," << quatZ - << "," << quatW << "," << magneticFieldTimestamp << "," - << magneticFieldX << "," << magneticFieldY << "," << magneticFieldZ - << "," << accelerationTimestamp << "," << accelerationX << "," + os << quaternionTimestamp << "," << quaternionX << "," << quaternionY + << "," << quaternionZ << "," << quaternionW << "," + << magneticFieldTimestamp << "," << magneticFieldX << "," + << magneticFieldY << "," << magneticFieldZ << "," + << accelerationTimestamp << "," << accelerationX << "," << accelerationY << "," << accelerationZ << "," << angularSpeedTimestamp << "," << angularSpeedX << "," << angularSpeedY << "," << angularSpeedZ << ","