From 9e4f5cd7f7410ee42c0141172782b83968535dbf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 27 Mar 2025 11:59:49 +0100
Subject: [PATCH] [ARP] Fix follower stepper velocity
- Follower: the stepper velocity was saturating due to the fact that it was used the time as seconds while treated as in ms. This issue made saturate always the stepper velocities
---
src/shared/algorithms/Follower/Follower.cpp | 13 ++++++++-----
src/shared/algorithms/Follower/Follower.h | 4 ++--
2 files changed, 10 insertions(+), 7 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 6c9e71d28..bc5b45f8e 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 3a6fcda47..850a63f90 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};
--
GitLab