From 434855b58774d8aa533a59046cb9b6d5cf089410 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Mon, 5 May 2025 18:38:48 +0200
Subject: [PATCH] [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 | 18 ++++++++++++++++--
src/shared/algorithms/Follower/Follower.h | 19 +++++++++++++++++--
2 files changed, 33 insertions(+), 4 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index bc5b45f8e..9f48adbc3 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -115,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)
@@ -162,8 +174,10 @@ 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 =
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 850a63f90..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.
@@ -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