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