diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h index 64d7423c7d957bd0548a46cf642a7a01128ecd95..85c4c1d5e90aad595e70c38d97006e49c21e1358 100644 --- a/src/shared/logger/LogTypes.h +++ b/src/shared/logger/LogTypes.h @@ -80,6 +80,7 @@ #include <sensors/analog/pressure/nxp/MPXH6400AData.h> #include <sensors/analog/pressure/nxp/MPXHZ6130AData.h> #include <sensors/logAnglesData.h> +#include <test/AnnoyingScalar.h> #include <fstream> #include <iostream> @@ -184,6 +185,7 @@ void registerTypes(Deserializer& ds) ds.registerType<FollowerState>(); ds.registerType<LogAntennasCoordinates>(); ds.registerType<LogRocketCoordinates>(); + ds.registerType<LogAngles>(); } } // namespace LogTypes diff --git a/src/shared/sensors/logAnglesData.h b/src/shared/sensors/logAnglesData.h index 5632f109e2aeb60338bfadcd94f6119c1e44f824..40247bf96431a314997887dad43332c0b9ce15e4 100644 --- a/src/shared/sensors/logAnglesData.h +++ b/src/shared/sensors/logAnglesData.h @@ -1,5 +1,7 @@ #include <iostream> +#include <cmath> +namespace Boardcore{ struct LogAngles { float roll, pitch, yaw; @@ -24,3 +26,4 @@ struct LogAngles os << roll << "," << pitch << "," << yaw << "\n"; } }; +} \ No newline at end of file diff --git a/src/tests/sensors/logAnglesData.h b/src/tests/sensors/logAnglesData.h index 90667e0b7276f7d1c14d7f729eac6420df5741b2..40247bf96431a314997887dad43332c0b9ce15e4 100644 --- a/src/tests/sensors/logAnglesData.h +++ b/src/tests/sensors/logAnglesData.h @@ -1,4 +1,5 @@ #include <iostream> +#include <cmath> namespace Boardcore{ struct LogAngles diff --git a/src/tests/sensors/test-vn300_gyro_to_angles.cpp b/src/tests/sensors/test-vn300_gyro_to_angles.cpp index b0622115023f1c2fb737c5952ae0431e57c96c31..f8f2e96a2efe7545bab7350fc194ba95baa310db 100644 --- a/src/tests/sensors/test-vn300_gyro_to_angles.cpp +++ b/src/tests/sensors/test-vn300_gyro_to_angles.cpp @@ -21,7 +21,7 @@ */ #include <drivers/timer/TimestampTimer.h> -#include <logAnglesData.h> +#include <sensors/logAnglesData.h> #include <logger/Logger.h> #include <sensors/Vectornav/VN300/VN300.h> @@ -50,11 +50,13 @@ void computeEulerAngles(const Vector3d& gyro, double dt, Vector3d& angles) cos(ass(1)); // rotation matrix angular_rate --> body_rate Vector3d angular_rate = G.transpose() * body_rate; - + printf("angles -->%f,%f,%f\n",angles(0),angles(1),angles(2)); + printf("angular_rate -->%f,%f,%f\n",angular_rate(0),angular_rate(1),angular_rate(2)); + printf("dt: %f", (dt / 1000000)); Vector3d new_angles = - angles + angular_rate * (dt / 10000000); // integration + angles + angular_rate * (dt / 1000000); // integration angles = new_angles; - + printf("new_angles -->%f,%f,%f\n",angles(0),angles(1),angles(2)); // rad --> deg }