From 14fba81e14fc9930c72fa287ac7e515764065263 Mon Sep 17 00:00:00 2001
From: Federico Mandelli <federicomandelli@skywarder.eu>
Date: Wed, 22 May 2024 11:57:58 +0200
Subject: [PATCH] [WingController] Added flare in flight sequence (last minute
fix, to be improved)
---
.../WingController/WingController.cpp | 35 ++++++++++++++++---
1 file changed, 30 insertions(+), 5 deletions(-)
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
index f0542e62a..bac52cd90 100644
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
@@ -145,12 +145,9 @@ State WingController::state_calibration(const Boardcore::Event& event)
{
logStatus(WingControllerState::CALIBRATION);
- modules.get<Actuators>()->startTwirl();
+ flare();
calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_WES_CAL_DONE, TOPIC_DPL, WES_CALIBRATION_TIMEOUT);
-
- modules.get<WindEstimation>()
- ->startWindEstimationSchemeCalibration();
+ DPL_SERVO_ACTUATION_DETECTED, TOPIC_DPL, 2000);
return HANDLED;
}
@@ -163,6 +160,34 @@ State WingController::state_calibration(const Boardcore::Event& event)
{
return tranSuper(&WingController::state_flying);
}
+ case DPL_SERVO_ACTUATION_DETECTED:
+ {
+ reset();
+ calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_WIGGLE, TOPIC_DPL, 1000);
+
+ return HANDLED;
+ }
+ case DPL_WIGGLE:
+ {
+ flare();
+ calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_NC_OPEN, TOPIC_DPL, 2000);
+
+ return HANDLED;
+ }
+ case DPL_NC_OPEN:
+ {
+ reset();
+ calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_WES_CAL_DONE, TOPIC_DPL, WES_CALIBRATION_TIMEOUT);
+ modules.get<WindEstimation>()
+ ->startWindEstimationSchemeCalibration();
+
+ modules.get<Actuators>()->startTwirl();
+
+ return HANDLED;
+ }
case DPL_WES_CAL_DONE:
{
modules.get<Actuators>()->stopTwirl();
--
GitLab