From 2830cbcee166d0a9d9e1a9f4affa7289cb3cb51f 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:42:19 +0200 Subject: [PATCH] [ARP] SMA setting gains for feedback and no feedback - SMA: Now the SMA has configurations to change between feedback and no feedback the yaw and pitch gain --- skyward-boardcore | 2 +- .../Automated/Config/SMAConfig.h | 13 +++++++++++- src/Groundstation/Automated/SMA/SMA.cpp | 20 +++++++++++++++---- 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/skyward-boardcore b/skyward-boardcore index f457ea1f3..434855b58 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit f457ea1f3a4afeb76944b7dd518a5c01fd675b83 +Subproject commit 434855b58774d8aa533a59046cb9b6d5cf089410 diff --git a/src/Groundstation/Automated/Config/SMAConfig.h b/src/Groundstation/Automated/Config/SMAConfig.h index e22f0d193..acd709f01 100644 --- a/src/Groundstation/Automated/Config/SMAConfig.h +++ b/src/Groundstation/Automated/Config/SMAConfig.h @@ -1,5 +1,5 @@ /* Copyright (c) 2024 Skyward Experimental Rocketry - * Author: Federico Lolli + * Author: Federico Lolli, Nicolò Caruso * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -34,5 +34,16 @@ namespace SMAConfig /// @brief Period of the propagator algorithm [ms]. constexpr milliseconds UPDATE_PERIOD = 100ms; // 10 Hz +// No feedback gains for the Follower +static constexpr float YAW_GAIN_NF = + 1.0; ///< Yaw gain for the no feedback states +static constexpr float PITCH_GAIN_NF = + 1.0; ///< Pitch gain for the no feedback states + +// Feedback gains for the Follower +static constexpr float YAW_GAIN_F = 0.1; ///< Yaw gain for the feedback states +static constexpr float PITCH_GAIN_F = + 1.0; ///< Pitch gain for the feedback states + } // namespace SMAConfig } // namespace Antennas diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp index 9fa53f0d1..a5064828e 100644 --- a/src/Groundstation/Automated/SMA/SMA.cpp +++ b/src/Groundstation/Automated/SMA/SMA.cpp @@ -328,13 +328,19 @@ State SMA::state_feedback(const Event& event) case EV_ENTRY: { logStatus(SMAState::FEEDBACK); - getModule<Leds>()->setOn(LedColor::YELLOW); + getModule<Leds>()->setOn(LedColor::RED); + + // Set the gains for the no feedback phase + if (!follower.setMaxGain(SMAConfig::YAW_GAIN_F, + SMAConfig::PITCH_GAIN_F)) + LOG_ERR(logger, "Follower gain set failed!\n"); + return HANDLED; } case EV_EXIT: { getModule<Actuators>()->disarm(); - getModule<Leds>()->setOff(LedColor::YELLOW); + getModule<Leds>()->setOff(LedColor::RED); return HANDLED; } case EV_EMPTY: @@ -364,13 +370,19 @@ State SMA::state_no_feedback(const Event& event) case EV_ENTRY: { logStatus(SMAState::NO_FEEDBACK); - getModule<Leds>()->setOn(LedColor::YELLOW); + getModule<Leds>()->setOn(LedColor::RED); + + // Set the gains for the no feedback phase + if (!follower.setMaxGain(SMAConfig::YAW_GAIN_NF, + SMAConfig::PITCH_GAIN_NF)) + LOG_ERR(logger, "Follower gain set failed!\n"); + return HANDLED; } case EV_EXIT: { getModule<Actuators>()->disarm(); - getModule<Leds>()->setOff(LedColor::YELLOW); + getModule<Leds>()->setOff(LedColor::RED); return HANDLED; } case EV_EMPTY: -- GitLab