diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 6c9e71d281e415f2a7d637c283c303e12d7561c1..bc5b45f8ed2fba457d47079d8f7f2667619edd65 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -50,9 +50,8 @@ float minimizeRotation(float angle)
return angle;
}
-Follower::Follower(std::chrono::milliseconds updatePeriod)
- : updatePeriod(static_cast<float>(updatePeriod.count()) / 1000),
- targetAngles({0, 0, 0})
+Follower::Follower(std::chrono::milliseconds updPeriod)
+ : updatePeriod{updPeriod}, targetAngles({0, 0, 0})
{
}
@@ -168,9 +167,13 @@ void Follower::step()
// Calculate angular velocity for moving the antennas toward position
float horizontalSpeed =
- std::abs((diffAngles.yaw * 1000) / (360 * updatePeriod));
+ std::abs((diffAngles.yaw) /
+ (360 * (static_cast<float>(updatePeriod.count()) / 1000)));
+ TRACE("[Follower] horizontalSpeed is: %f\n", horizontalSpeed);
float verticalSpeed =
- std::abs((diffAngles.pitch * 1000) / (360 * updatePeriod));
+ std::abs((diffAngles.pitch) /
+ (360 * (static_cast<float>(updatePeriod.count()) / 1000)));
+ TRACE("[Follower] Vertical speed is: %f\n", horizontalSpeed);
// Update the state of the follower
FollowerState newState;
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 3a6fcda47b1208489d65e8c3429fb69f4a1c828b..850a63f90e8e1793d8c889d93aa07cf1c0813616 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -144,8 +144,8 @@ private:
*/
Eigen::Vector3f getRocketNASOrigin();
- // actuation update period [s]
- float updatePeriod;
+ // actuation update period [ms]
+ std::chrono::milliseconds updatePeriod;
// Initialization flag
std::atomic<bool> isInit{false};
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index 85c4c1d5e90aad595e70c38d97006e49c21e1358..7a48471f693d211e116ce5e7531cc615404d4469 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -80,7 +80,6 @@
#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>
diff --git a/src/shared/sensors/logAnglesData.h b/src/shared/sensors/logAnglesData.h
index 40247bf96431a314997887dad43332c0b9ce15e4..9935ce0367229e48783addd39e85911b29fdebad 100644
--- a/src/shared/sensors/logAnglesData.h
+++ b/src/shared/sensors/logAnglesData.h
@@ -1,9 +1,11 @@
-#include <iostream>
#include <cmath>
+#include <iostream>
-namespace Boardcore{
+namespace Boardcore
+{
struct LogAngles
{
+ uint64_t timestamp;
float roll, pitch, yaw;
LogAngles() : roll{0}, pitch{0}, yaw(0) {}
@@ -19,11 +21,14 @@ struct LogAngles
this->yaw = z * (180 / M_PI);
}
- static std::string header() { return "Roll,Pitch,Yaw,quaternionZ\n"; }
+ static std::string header()
+ {
+ return "Timestamp,Roll,Pitch,Yaw,quaternionZ\n";
+ }
void print(std::ostream& os) const
{
- os << roll << "," << pitch << "," << yaw << "\n";
+ os << timestamp << "," << roll << "," << pitch << "," << yaw << "\n";
}
};
-}
\ No newline at end of file
+} // namespace Boardcore
diff --git a/src/tests/sensors/logAnglesData.h b/src/tests/sensors/logAnglesData.h
deleted file mode 100644
index 40247bf96431a314997887dad43332c0b9ce15e4..0000000000000000000000000000000000000000
--- a/src/tests/sensors/logAnglesData.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#include <iostream>
-#include <cmath>
-
-namespace Boardcore{
-struct LogAngles
-{
- float roll, pitch, yaw;
-
- LogAngles() : roll{0}, pitch{0}, yaw(0) {}
- LogAngles(float Roll, float Pitch, float Yaw)
- : roll{Roll}, pitch{Pitch}, yaw(Yaw)
- {
- }
-
- void getAngles(float x, float y, float z)
- {
- this->roll = x * (180 / M_PI);
- this->pitch = y * (180 / M_PI);
- this->yaw = z * (180 / M_PI);
- }
-
- static std::string header() { return "Roll,Pitch,Yaw,quaternionZ\n"; }
-
- void print(std::ostream& os) const
- {
- os << roll << "," << pitch << "," << yaw << "\n";
- }
-};
-}
\ No newline at end of file
diff --git a/src/tests/sensors/test-vn300_gyro_to_angles.cpp b/src/tests/sensors/test-vn300_gyro_to_angles.cpp
index f8f2e96a2efe7545bab7350fc194ba95baa310db..a783af6aff5e9f5d7693dc02a08123574488288f 100644
--- a/src/tests/sensors/test-vn300_gyro_to_angles.cpp
+++ b/src/tests/sensors/test-vn300_gyro_to_angles.cpp
@@ -21,9 +21,9 @@
*/
#include <drivers/timer/TimestampTimer.h>
-#include <sensors/logAnglesData.h>
#include <logger/Logger.h>
#include <sensors/Vectornav/VN300/VN300.h>
+#include <sensors/logAnglesData.h>
#include <Eigen/Dense>
#include <cmath>
@@ -50,13 +50,14 @@ 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("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 / 1000000); // integration
angles = new_angles;
- printf("new_angles -->%f,%f,%f\n",angles(0),angles(1),angles(2));
+ printf("new_angles -->%f,%f,%f\n", angles(0), angles(1), angles(2));
// rad --> deg
}
@@ -91,6 +92,10 @@ int main()
Logger::getInstance().start();
+ printf("[GYRO] LOG NR %d \n", Logger::getInstance().getStats().logNumber);
+
+ Thread::sleep(100);
+
uint64_t t0 = TimestampTimer::getTimestamp();
Vector3d angles = {0, 0, 0};
@@ -110,8 +115,9 @@ int main()
Vector3d gyro = {sample.angularSpeedX, sample.angularSpeedY,
sample.angularSpeedZ};
- computeEulerAngles(gyro, dt, angles);
+ computeEulerAngles(gyro, t1 - t0, angles);
+ angles_log.timestamp = t1;
angles_log.getAngles(angles(0), angles(1), angles(2));
printf("Angles: %f, %f, %f\n", angles_log.pitch, angles_log.roll,
angles_log.yaw);
@@ -119,6 +125,8 @@ int main()
t0 = TimestampTimer::getTimestamp();
}
+ printf("[GYRO] LOG NR %d \n",
+ Logger::getInstance().getStats().logNumber);
Thread::sleep(20);
}