From 6f3ff7b354292a80437ab662d33f21571ba8647e Mon Sep 17 00:00:00 2001
From: Federico Mandelli <federico.mandelli@skywarder.eu>
Date: Thu, 5 Dec 2024 22:24:50 +0100
Subject: [PATCH] [WingController] Added progressive rotation algorithm
---
CMakeLists.txt | 7 +++++++
src/boards/Parafoil/Configs/WingConfig.h | 4 ++++
.../WingController/WingController.cpp | 16 ++++++++++++++++
3 files changed, 27 insertions(+)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b2bdb2ce1..8cd13fe76 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,6 +33,13 @@ project(OnBoardSoftware)
# Flight entrypoints #
#-----------------------------------------------------------------------------#
+
+
+add_executable(parafoil-progressive-rotation src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
+target_include_directories(parafoil-progressive-rotation PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-progressive-rotation PRIVATE PROGRESSIVE_ROTATION MILANO)
+sbs_target(parafoil-progressive-rotation stm32f429zi_skyward_death_stack_x)
+
add_executable(parafoil-guided-milano src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
target_include_directories(parafoil-guided-milano PRIVATE ${OBSW_INCLUDE_DIRS})
target_compile_definitions(parafoil-guided-milano PRIVATE CLOSED_LOOP MILANO)
diff --git a/src/boards/Parafoil/Configs/WingConfig.h b/src/boards/Parafoil/Configs/WingConfig.h
index 3d3a31e6c..c97771451 100644
--- a/src/boards/Parafoil/Configs/WingConfig.h
+++ b/src/boards/Parafoil/Configs/WingConfig.h
@@ -41,6 +41,8 @@ constexpr int SELECTED_ALGORITHM = 1;
constexpr int SELECTED_ALGORITHM = 2;
#elif ROTATION
constexpr int SELECTED_ALGORITHM = 3;
+#elif PROGRESSIVE_ROTATION
+constexpr int SELECTED_ALGORITHM = 4;
#else
constexpr int SELECTED_ALGORITHM = 0;
#endif
@@ -54,6 +56,8 @@ constexpr float DEFAULT_TARGET_LON = 9.1544219;
#endif
constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms]
+constexpr int PARAFOIL_COMMAND_PERIOD = 5 * 1000; // [ms]
+constexpr int PARAFOIL_WING_INCREMENT = 10; // [°]
constexpr int WING_UPDATE_PERIOD = 1000; // [ms]
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
index bac52cd90..d7ca32b11 100644
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
@@ -348,6 +348,22 @@ bool WingController::addAlgorithms()
result &= algorithm->init();
algorithms.push_back(algorithm);
+ // Algorithm 4 (Progressive rotation)
+ algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
+
+ step.timestamp = 0;
+ for (int i = 0; i < 80; i += PARAFOIL_WING_INCREMENT)
+ {
+ step.timestamp += PARAFOIL_COMMAND_PERIOD * 1000; // us
+ step.servo1Angle = i;
+ step.servo2Angle = 0;
+ algorithm->addStep(step);
+ step.timestamp += PARAFOIL_COMMAND_PERIOD * 1000; // us
+ step.servo1Angle = 0;
+ step.servo2Angle = i;
+ algorithm->addStep(step);
+ }
+
selectAlgorithm(SELECTED_ALGORITHM);
return result;
--
GitLab