diff --git a/CMakeLists.txt b/CMakeLists.txt index b2bdb2ce11c30384f96f9031dc89284554c953a6..8cd13fe76134d0529eb58ef6515547823d8e5beb 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 3d3a31e6c51c89d2eb495466590b0588d6542c19..c97771451a3223693159029bf2826f04f26bdfdc 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 bac52cd90ad60961244ceb831c84bf2478f6569a..d7ca32b11c2f062a308cbe831c88efb461d34d76 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;