diff --git a/skyward-boardcore b/skyward-boardcore index f457ea1f3a4afeb76944b7dd518a5c01fd675b83..434855b58774d8aa533a59046cb9b6d5cf089410 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 e22f0d193040846badd9b80a333e262c0ec8b063..acd709f0152f178ea7170c2930b9b0d356c471a8 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 9fa53f0d1ee386c213f7e9533684193e296d69ad..a5064828ea23e321c35e8d36e6bb26420af8d19c 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: