diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp index f0542e62af41233747567654b41f4e583ba168e7..bac52cd90ad60961244ceb831c84bf2478f6569a 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();