From d9ac0a2c8f03dee8a332260aa3f4dc9c61ea935c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Tue, 10 Sep 2024 12:32:56 +0200
Subject: [PATCH] [Payload][Wing] Finalize available algorithms, remove unused
ones
---
src/boards/Payload/Configs/WingConfig.h | 3 +-
.../WingController/WingController.cpp | 41 +++----------------
2 files changed, 6 insertions(+), 38 deletions(-)
diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index 33b3f342d..882411843 100644
--- a/src/boards/Payload/Configs/WingConfig.h
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -44,8 +44,7 @@ enum class AlgorithmId : size_t
{
EARLY_MANEUVER = 0,
CLOSED_LOOP,
- SEQUENCE, ///< A predefined sequence of maneuvers
- ROTATION, ///< A sequence of maneuvers to rotate the wing
+ ROTATION, ///< A predefined sequence of rotation maneuvers
LAST, ///< Used to count the number of algorithms
};
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index edb384022..9c3396ca3 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -390,7 +390,6 @@ bool WingController::selectAlgorithm(uint8_t index)
{
case static_cast<uint8_t>(AlgorithmId::EARLY_MANEUVER):
case static_cast<uint8_t>(AlgorithmId::CLOSED_LOOP):
- case static_cast<uint8_t>(AlgorithmId::SEQUENCE):
case static_cast<uint8_t>(AlgorithmId::ROTATION):
{
selectedAlgorithm = static_cast<AlgorithmId>(index);
@@ -434,32 +433,7 @@ void WingController::loadAlgorithms()
PI::KP, PI::KI, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO,
clGuidance);
- // Sequence
- {
- auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO,
- PARAFOIL_RIGHT_SERVO);
- WingAlgorithmData step;
-
- step.timestamp = 0;
- step.servo1Angle = 0;
- step.servo2Angle = 120;
- algorithm->addStep(step);
-
- step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count();
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
-
- step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count();
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
-
- algorithms[static_cast<size_t>(AlgorithmId::SEQUENCE)] =
- std::move(algorithm);
- }
-
- // Rotation
+ // Rotation Sequence
{
auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO,
PARAFOIL_RIGHT_SERVO);
@@ -481,21 +455,16 @@ void WingController::loadAlgorithms()
algorithm->addStep(step);
step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = LeftServo::ROTATION;
- step.servo2Angle = RightServo::ROTATION;
- algorithm->addStep(step);
-
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
- step.servo1Angle = 0;
- step.servo2Angle = RightServo::ROTATION;
+ step.servo1Angle = LeftServo::ROTATION / 2;
+ step.servo2Angle = 0;
algorithm->addStep(step);
step.timestamp += microseconds{ROTATION_PERIOD}.count();
step.servo1Angle = 0;
- step.servo2Angle = 0;
+ step.servo2Angle = RightServo::ROTATION / 2;
algorithm->addStep(step);
- step.timestamp += microseconds{ROTATION_PERIOD}.count();
+ step.timestamp += microseconds{2min}.count();
step.servo1Angle = 0;
step.servo2Angle = 0;
algorithm->addStep(step);
--
GitLab