diff --git a/CMakeLists.txt b/CMakeLists.txt
index 11d68fa236ff1d7089ebc53b684a179763e35b1b..ce92b66665d75340ae5d18e57f4844b853ab66ae 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -151,13 +151,18 @@ sbs_target(parafoil-t-approach-milano stm32f429zi_death_stack_v2)
add_executable(parafoil-t-approach-dynamic src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
target_include_directories(parafoil-t-approach-dynamic PRIVATE ${OBSW_INCLUDE_DIRS})
- target_compile_definitions(parafoil-t-approach-dynamic PRIVATE
+target_compile_definitions(parafoil-t-approach-dynamic PRIVATE
ALGORITHM_EARLY_MANEUVER DYNAMIC_TARGET
DYNAMIC_TARGET_LATITUDE_OFFSET=35
DYNAMIC_TARGET_LONGITUDE_OFFSET=-156
)
sbs_target(parafoil-t-approach-dynamic stm32f429zi_death_stack_v2)
+add_executable(parafoil-ground-test src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
+target_include_directories(parafoil-ground-test PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-ground-test PRIVATE ALGORITHM_PROGRESSIVE_ROTATION MILANO DISABLE_LANDING_FLARE)
+sbs_target(parafoil-ground-test stm32f429zi_death_stack_v2)
+
add_executable(mockup-main src/MockupMain/mockup-entry.cpp ${MOCKUP_MAIN_COMPUTER})
target_include_directories(mockup-main PRIVATE ${OBSW_INCLUDE_DIRS})
sbs_target(mockup-main stm32f429zi_death_stack_v2)
\ No newline at end of file
diff --git a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
index c029ea89be576748073192edd7ae04bdd71834e3..64153468bf1c09d84c55a37c0f98ea4ee2940ec6 100644
--- a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
+++ b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp
@@ -31,17 +31,20 @@
using namespace Boardcore;
using namespace Boardcore::Units::Length;
using namespace Common;
-namespace config = Parafoil::Config;
namespace Parafoil
{
+AltitudeTrigger::AltitudeTrigger(AltitudeTriggerConfig config) : config(config)
+{
+ this->thresholdAltitude = config.threshold.value();
+}
+
bool AltitudeTrigger::start()
{
auto& scheduler = getModule<BoardScheduler>()->altitudeTrigger();
- auto task = scheduler.addTask([this] { update(); },
- config::AltitudeTrigger::UPDATE_RATE);
+ auto task = scheduler.addTask([this] { update(); }, config.updateRate);
if (task == 0)
return false;
@@ -67,7 +70,7 @@ bool AltitudeTrigger::isEnabled() { return running; }
void AltitudeTrigger::setDeploymentAltitude(Meter altitude)
{
- targetAltitude = altitude.value();
+ thresholdAltitude = altitude.value();
}
void AltitudeTrigger::update()
@@ -79,12 +82,12 @@ void AltitudeTrigger::update()
auto nasState = getModule<NASController>()->getNasState();
float altitude = -nasState.d;
- if (altitude < targetAltitude)
+ if (altitude < thresholdAltitude)
confidence++;
else
confidence = 0;
- if (confidence >= config::AltitudeTrigger::CONFIDENCE)
+ if (confidence >= config.confidence)
{
confidence = 0;
EventBroker::getInstance().post(ALTITUDE_TRIGGER_ALTITUDE_REACHED,
diff --git a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
index 7531f60a6e4e993210589a8c33787e0914fd1e67..7024795542b2de8068820ee7af628f8b59f78b5d 100644
--- a/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
+++ b/src/Parafoil/AltitudeTrigger/AltitudeTrigger.h
@@ -28,6 +28,8 @@
#include <atomic>
#include <utils/ModuleManager/ModuleManager.hpp>
+#include "AltitudeTriggerConfig.h"
+
namespace Parafoil
{
class BoardScheduler;
@@ -37,6 +39,11 @@ class AltitudeTrigger
: public Boardcore::InjectableWithDeps<BoardScheduler, NASController>
{
public:
+ /**
+ * @brief Constructor
+ */
+ AltitudeTrigger(AltitudeTriggerConfig config);
+
/**
* @brief Adds the update() task to the task scheduler.
*/
@@ -68,6 +75,8 @@ public:
void setDeploymentAltitude(Boardcore::Units::Length::Meter altitude);
private:
+ AltitudeTriggerConfig config;
+
// Update method that posts a FLIGHT_WING_ALT_PASSED when the correct
// altitude is reached
void update();
@@ -79,9 +88,7 @@ private:
// altitude
int confidence = 0;
- std::atomic<float> targetAltitude{Boardcore::Units::Length::Meter{
- Config::AltitudeTrigger::DEPLOYMENT_ALTITUDE}
- .value()};
+ std::atomic<float> thresholdAltitude{0};
};
} // namespace Parafoil
diff --git a/src/Parafoil/AltitudeTrigger/AltitudeTriggerConfig.h b/src/Parafoil/AltitudeTrigger/AltitudeTriggerConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..b0a9bff7afd5e04d9766e1feae7aa6ed2661b121
--- /dev/null
+++ b/src/Parafoil/AltitudeTrigger/AltitudeTriggerConfig.h
@@ -0,0 +1,41 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <units/Frequency.h>
+#include <units/Length.h>
+
+namespace Parafoil
+{
+
+/* linter-off */ using namespace Boardcore::Units::Length;
+/* linter-off */ using namespace Boardcore::Units::Frequency;
+
+struct AltitudeTriggerConfig
+{
+ Meter threshold; ///< [m] Altitude threshold
+ int confidence; ///< Number of samples to reach the threshold
+ Hertz updateRate; ///< [Hz] Update rate of the trigger
+};
+
+} // namespace Parafoil
diff --git a/src/Parafoil/Configs/WingConfig.h b/src/Parafoil/Configs/WingConfig.h
index d8eeea8b96b814ed33385cf7976eba1e974c6ee4..89e440403e8c762a2895e6b1c6ea63f62678d5ab 100644
--- a/src/Parafoil/Configs/WingConfig.h
+++ b/src/Parafoil/Configs/WingConfig.h
@@ -135,17 +135,24 @@ constexpr auto TARGET_ALTITUDE_THRESHOLD = 50_m;
} // namespace Guidance
-} // namespace Wing
-
-namespace AltitudeTrigger
+namespace LandingFlare
{
/* linter off */ using namespace Boardcore::Units::Frequency;
/* linter off */ using namespace Boardcore::Units::Length;
-constexpr auto DEPLOYMENT_ALTITUDE = 300_m;
-constexpr auto CONFIDENCE = 10; // [samples]
-constexpr auto UPDATE_RATE = 10_hz;
-} // namespace AltitudeTrigger
+#ifdef DISABLE_LANDING_FLARE
+constexpr auto ENABLED = false;
+#else
+constexpr auto ENABLED = true;
+#endif
+
+constexpr auto FLARE_ALTITUDE = 15_m;
+constexpr auto CONFIDENCE = 10; // [samples]
+constexpr auto UPDATE_RATE = 10_hz;
+constexpr auto FLARE_DURATION = 5_s;
+} // namespace LandingFlare
+
+} // namespace Wing
} // namespace Config
} // namespace Parafoil
diff --git a/src/Parafoil/StateMachines/WingController/WingController.cpp b/src/Parafoil/StateMachines/WingController/WingController.cpp
index 33543e55b5be1a24fad6ab57f6788db8c7a9796b..b63eebcb5723c3534a29763b1ba4597c6c30a794 100644
--- a/src/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/Parafoil/StateMachines/WingController/WingController.cpp
@@ -205,6 +205,8 @@ State WingController::FlyingDeployment(const Boardcore::Event& event)
State WingController::FlyingControlledDescent(const Boardcore::Event& event)
{
+ static uint16_t flareTimeoutEventId;
+
switch (event)
{
case EV_ENTRY: // start automatic algorithms
@@ -222,10 +224,31 @@ State WingController::FlyingControlledDescent(const Boardcore::Event& event)
{
return transition(&WingController::OnGround);
}
+ case ALTITUDE_TRIGGER_ALTITUDE_REACHED:
+ {
+ pauseAlgorithm();
+ flareWing();
+
+ flareTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_FLARE_STOP, TOPIC_FLIGHT,
+ Millisecond{Config::Wing::LandingFlare::FLARE_DURATION}
+ .value());
+
+ return HANDLED;
+ }
+ case DPL_FLARE_STOP:
+ {
+ resetWing();
+ resumeAlgorithm();
+
+ return HANDLED;
+ }
case EV_EXIT:
{
stopAlgorithm();
+ EventBroker::getInstance().removeDelayed(flareTimeoutEventId);
+
getModule<WindEstimation>()->stopAlgorithm();
getModule<WindEstimation>()->stopCalibration();
@@ -567,6 +590,9 @@ void WingController::stopAlgorithm()
}
}
+void WingController::pauseAlgorithm() { running = false; }
+void WingController::resumeAlgorithm() { running = true; }
+
void WingController::updateEarlyManeuverPoints()
{
using namespace Eigen;
diff --git a/src/Parafoil/StateMachines/WingController/WingController.h b/src/Parafoil/StateMachines/WingController/WingController.h
index 0b277170f63cc6dfab0bd893039d3fa1e42cf05a..4edeb0245ae5257a814fb271573e17d8ebf17328 100644
--- a/src/Parafoil/StateMachines/WingController/WingController.h
+++ b/src/Parafoil/StateMachines/WingController/WingController.h
@@ -203,6 +203,16 @@ private:
*/
void stopAlgorithm();
+ /**
+ * @brief Pauses the currently selected algorithm.
+ */
+ void pauseAlgorithm();
+
+ /**
+ * @brief Resumes the currently selected algorithm.
+ */
+ void resumeAlgorithm();
+
/**
* @brief Update early maneuver guidance points (EMC, M1, M2) based on the
* current position and the target position.
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index 918ea77d2e0b7a097f1052e3685d4302f14d983b..034e23310f3bbf648bc1a39b1d8c93cb25a49b77 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -121,9 +121,17 @@ int main()
auto radio = new Radio();
initResult &= depman.insert(radio);
+ // Landing flare activation
+ namespace LandingFlareConfig = Config::Wing::LandingFlare;
+ AltitudeTriggerConfig landingFlareConfig = {
+ .threshold = LandingFlareConfig::FLARE_ALTITUDE,
+ .confidence = LandingFlareConfig::CONFIDENCE,
+ .updateRate = LandingFlareConfig::UPDATE_RATE,
+ };
+ auto landingFlare = new AltitudeTrigger(landingFlareConfig);
+ initResult &= depman.insert(landingFlare);
+
// Flight algorithms
- auto altitudeTrigger = new AltitudeTrigger();
- initResult &= depman.insert(altitudeTrigger);
auto wingController = new WingController();
initResult &= depman.insert(wingController);
auto windEstimation = new WindEstimation();
@@ -152,7 +160,7 @@ int main()
START_MODULE(radio);
START_MODULE(nasController);
START_MODULE(flightModeManager);
- START_MODULE(altitudeTrigger);
+ START_MODULE(landingFlare);
START_MODULE(windEstimation);
START_MODULE(wingController);
START_MODULE(actuators);