From a3a0b7672f1c62081e2e45a18ab86a86a6270e0c 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
[ARP] Follower now accept gain changes
- Follower: Now the gain can be changed externally from the SMA.
This is done to change them in case of feedback and no-feedback states.
Also, they are limited to a maximum of 1.0
---
src/shared/algorithms/Follower/Follower.cpp | 31 ++++++++++++++++-----
src/shared/algorithms/Follower/Follower.h | 23 ++++++++++++---
2 files changed, 43 insertions(+), 11 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 6c9e71d28..9f48adbc3 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})
{
}
@@ -116,6 +115,18 @@ AntennaAngles Follower::getTargetAngles()
return targetAngles;
}
+bool Follower::setMaxGain(float yawGainNew, float pitchGainNew)
+{
+ // In case of negative or over the limit values, do not set the gains
+ if (yawGainNew < 0 || yawGainNew > YAW_GAIN_LIMIT || pitchGainNew < 0 ||
+ pitchGainNew > PITCH_GAIN_LIMIT)
+ return false;
+
+ yawGain = yawGainNew;
+ pitchGain = pitchGainNew;
+ return true;
+}
+
bool Follower::init()
{
if (isInit)
@@ -163,14 +174,20 @@ void Follower::step()
}
// Rotate in the shortest direction
- diffAngles.yaw = YAW_GAIN * minimizeRotation(diffAngles.yaw);
- diffAngles.pitch = PITCH_GAIN * minimizeRotation(diffAngles.pitch);
+ diffAngles.yaw =
+ std::min(yawGain, YAW_GAIN_LIMIT) * minimizeRotation(diffAngles.yaw);
+ diffAngles.pitch = std::min(pitchGain, PITCH_GAIN_LIMIT) *
+ minimizeRotation(diffAngles.pitch);
// 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..b161cf02e 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -38,8 +38,10 @@
namespace Boardcore
{
-static constexpr float YAW_GAIN = 0.1;
-static constexpr float PITCH_GAIN = 1.0;
+static constexpr float YAW_GAIN_LIMIT =
+ 1.0; ///< Max limit for the Yaw gain, cannot be set more
+static constexpr float PITCH_GAIN_LIMIT =
+ 1.0; ///< Max limit for the pirch gain cannot be set more
/**
* @brief Follower class to output the yaw ad pitch necessary to track from the
@@ -89,6 +91,16 @@ public:
*/
void setLastAntennaAttitude(const VN300Data& attitudeData);
+ /**
+ * @brief Set the maximum gain for the yaw and pitch
+ *
+ * @param yawGainNew the gain for the yaw
+ * @param pitchGainNew the gain for the pitch
+ * @return true if set correctly
+ * @return false if negative or over the maximum limits
+ */
+ bool setMaxGain(float yawGainNew, float pitchGainNew);
+
/**
* @brief Synchronized getter for the State of the follower algorithm.
* @returns The state of the follower algorithm.
@@ -144,8 +156,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};
@@ -181,6 +193,9 @@ private:
// General mutex for the follower
miosix::FastMutex followerMutex;
+
+ float yawGain = YAW_GAIN_LIMIT; ///< Gain on the yaw
+ float pitchGain = PITCH_GAIN_LIMIT; ///< Gain on the pitch
};
} // namespace Boardcore
--
GitLab