diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index 91c0b50b2237a569aa3d1070a7ebfe05a0d24baf..7b530a01a8224798f919c74bd6cc1fe0e392f914 100644
--- a/src/boards/Payload/Configs/WingConfig.h
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -69,6 +69,13 @@ constexpr auto ALGORITHM = AlgorithmId::EARLY_MANEUVER;
constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15s;
constexpr auto UPDATE_RATE = 1_hz;
+constexpr auto CUTTERS_TIMEOUT = 1s;
+
+constexpr auto FLARE_WAIT = 5s; ///< Time to wait before the first flare
+constexpr auto FLARE_COUNT = 2; ///< Number of flares
+constexpr auto FLARE_DURATION = 2s; ///< Duration of the flare maneuver
+constexpr auto FLARE_INTERVAL = 1s; ///< Interval between two flares
+
namespace PI
{
constexpr auto SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
@@ -86,8 +93,6 @@ constexpr auto M2_ALTITUDE_THRESHOLD = 150; // [m]
constexpr auto TARGET_ALTITUDE_THRESHOLD = 50; // [m]
} // namespace Guidance
-constexpr float TWIRL_RADIUS = 0.5; // [%]
-
} // namespace Wing
namespace AltitudeTrigger
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index 3bd2144a6ffd0da38e3f8eeee8311b30bf396ca8..edb38402249d2bba47f14ede6c47f15c171376c3 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -79,13 +79,6 @@ State WingController::Idle(const Boardcore::Event& event)
case FLIGHT_WING_DESCENT:
{
- auto nasState = getModule<NASController>()->getNasState();
- float altitude = -nasState.d;
-
- getModule<Actuators>()->cuttersOn();
- getModule<FlightStatsRecorder>()->deploymentDetected(
- TimestampTimer::getTimestamp(), altitude);
-
return transition(&WingController::Flying);
}
@@ -112,6 +105,13 @@ State WingController::Flying(const Event& event)
case EV_EXIT:
{
+ if (cuttersOffEventId != 0)
+ {
+ EventBroker::getInstance().removeDelayed(cuttersOffEventId);
+ }
+
+ getModule<Actuators>()->cuttersOff();
+
return HANDLED;
}
@@ -122,7 +122,13 @@ State WingController::Flying(const Event& event)
case EV_INIT:
{
- return transition(&WingController::FlyingCalibration);
+ return transition(&WingController::FlyingDeployment);
+ }
+
+ case DPL_CUT_TIMEOUT:
+ {
+ getModule<Actuators>()->cuttersOff();
+ return HANDLED;
}
case FLIGHT_LANDING_DETECTED:
@@ -137,26 +143,38 @@ State WingController::Flying(const Event& event)
}
}
-State WingController::FlyingCalibration(const Boardcore::Event& event)
+State WingController::FlyingDeployment(const Boardcore::Event& event)
{
- static uint16_t calibrationTimeoutEventId;
-
switch (event)
{
- case EV_ENTRY: // starts twirling and calibration wes
+ case EV_ENTRY:
{
- updateState(WingControllerState::FLYING_CALIBRATION);
+ updateState(WingControllerState::FLYING_DEPLOYMENT);
- flareWing();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_SERVO_ACTUATION_DETECTED, TOPIC_DPL, 2000);
+ getModule<Actuators>()->cuttersOn();
+ cuttersOffEventId = EventBroker::getInstance().postDelayed(
+ DPL_CUT_TIMEOUT, TOPIC_DPL,
+ milliseconds{CUTTERS_TIMEOUT}.count());
+
+ auto nasState = getModule<NASController>()->getNasState();
+ float altitude = -nasState.d;
+ getModule<FlightStatsRecorder>()->deploymentDetected(
+ TimestampTimer::getTimestamp(), altitude);
+
+ flareEventId = EventBroker::getInstance().postDelayed(
+ DPL_FLARE_START, TOPIC_DPL, milliseconds{FLARE_WAIT}.count());
return HANDLED;
}
case EV_EXIT:
{
- EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId);
+ if (flareEventId != 0)
+ {
+ EventBroker::getInstance().removeDelayed(flareEventId);
+ }
+ resetWing();
+
return HANDLED;
}
@@ -165,41 +183,37 @@ State WingController::FlyingCalibration(const Boardcore::Event& event)
return tranSuper(&WingController::Flying);
}
- case DPL_SERVO_ACTUATION_DETECTED:
- {
- resetWing();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_WIGGLE, TOPIC_DPL, 1000);
-
- return HANDLED;
- }
-
- case DPL_WIGGLE:
+ case DPL_FLARE_START:
{
flareWing();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_NC_OPEN, TOPIC_DPL, 2000);
+ flareEventId = EventBroker::getInstance().postDelayed(
+ DPL_FLARE_STOP, TOPIC_DPL,
+ milliseconds{FLARE_DURATION}.count());
return HANDLED;
}
- case DPL_NC_OPEN:
+ case DPL_FLARE_STOP:
{
resetWing();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_WES_CAL_DONE, TOPIC_DPL,
- milliseconds{Config::WES::ROTATION_PERIOD}.count());
- getModule<WindEstimation>()->startWindEstimationSchemeCalibration();
-
- twirlWing();
+ flareCount--;
+
+ if (flareCount > 0)
+ {
+ flareEventId = EventBroker::getInstance().postDelayed(
+ DPL_FLARE_START, TOPIC_DPL,
+ milliseconds{FLARE_INTERVAL}.count());
+ }
+ else
+ {
+ EventBroker::getInstance().post(DPL_DONE, TOPIC_DPL);
+ }
return HANDLED;
}
- case DPL_WES_CAL_DONE:
+ case DPL_DONE:
{
- resetWing();
-
return transition(&WingController::FlyingControlledDescent);
}
@@ -254,8 +268,8 @@ State WingController::OnGround(const Boardcore::Event& event)
getModule<WindEstimation>()->stopWindEstimationScheme();
getModule<WindEstimation>()->stopWindEstimationSchemeCalibration();
stopAlgorithm();
+ resetWing();
- // disable servos
getModule<Actuators>()->disableServo(PARAFOIL_LEFT_SERVO);
getModule<Actuators>()->disableServo(PARAFOIL_RIGHT_SERVO);
@@ -512,7 +526,6 @@ void WingController::stopAlgorithm()
running = false;
getCurrentAlgorithm().end();
- resetWing();
}
}
@@ -589,12 +602,6 @@ void WingController::flareWing()
getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f);
}
-void WingController::twirlWing()
-{
- getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, TWIRL_RADIUS);
- getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f);
-}
-
void WingController::resetWing()
{
getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f);
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
index 4c3ada8ed3b7a4ed52eaf5ae5b2a4d4ff773fea0..4f0274654aa644dd7e553ba1442dde1bc6f73451 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.h
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -87,7 +87,7 @@ public:
// HSM states
Boardcore::State Idle(const Boardcore::Event& event);
Boardcore::State Flying(const Boardcore::Event& event);
- Boardcore::State FlyingCalibration(const Boardcore::Event& event);
+ Boardcore::State FlyingDeployment(const Boardcore::Event& event);
Boardcore::State FlyingControlledDescent(const Boardcore::Event& event);
Boardcore::State OnGround(const Boardcore::Event& event);
@@ -243,6 +243,11 @@ private:
*/
ClosedLoopGuidanceAlgorithm clGuidance;
+ uint16_t cuttersOffEventId = 0;
+ uint16_t flareEventId = 0;
+
+ uint32_t flareCount = Config::Wing::FLARE_COUNT;
+
std::atomic<bool> started{false};
std::atomic<bool> running{false}; ///< Whether the algorithm is running
diff --git a/src/boards/Payload/StateMachines/WingController/WingControllerData.h b/src/boards/Payload/StateMachines/WingController/WingControllerData.h
index 0d175ea3530314d974a21c8a563781e7790fffa2..fdfb81da287dfb4073c0da8500810361c607fc29 100644
--- a/src/boards/Payload/StateMachines/WingController/WingControllerData.h
+++ b/src/boards/Payload/StateMachines/WingController/WingControllerData.h
@@ -33,7 +33,7 @@ enum class WingControllerState : uint8_t
{
UNINIT = 0,
IDLE,
- FLYING_CALIBRATION,
+ FLYING_DEPLOYMENT,
FLYING_CONTROLLED_DESCENT,
ON_GROUND,
END
diff --git a/src/boards/common/Events.h b/src/boards/common/Events.h
index b274d2b579913a62ba7c0ba3fdc09f64bdfeca81..44fe47db9452370fd12d975f499dd830d15d2bc1 100644
--- a/src/boards/common/Events.h
+++ b/src/boards/common/Events.h
@@ -59,9 +59,9 @@ enum Events : uint8_t
DPL_CUT_TIMEOUT,
DPL_NC_OPEN,
DPL_NC_RESET,
- DPL_SERVO_ACTUATION_DETECTED,
- DPL_WIGGLE,
- DPL_WES_CAL_DONE,
+ DPL_FLARE_START,
+ DPL_FLARE_STOP,
+ DPL_DONE,
CAN_FORCE_INIT,
CAN_ARM,
CAN_ENTER_TEST_MODE,
@@ -177,9 +177,9 @@ inline std::string getEventString(uint8_t event)
{DPL_CUT_TIMEOUT, "DPL_CUT_TIMEOUT"},
{DPL_NC_OPEN, "DPL_NC_OPEN"},
{DPL_NC_RESET, "DPL_NC_RESET"},
- {DPL_SERVO_ACTUATION_DETECTED, "DPL_SERVO_ACTUATION_DETECTED"},
- {DPL_WIGGLE, "DPL_WIGGLE"},
- {DPL_WES_CAL_DONE, "DPL_WES_CAL_DONE"},
+ {DPL_FLARE_START, "DPL_FLARE_START"},
+ {DPL_FLARE_STOP, "DPL_FLARE_STOP"},
+ {DPL_DONE, "DPL_DONE"},
{CAN_FORCE_INIT, "CAN_FORCE_INIT"},
{CAN_ARM, "CAN_ARM"},
{CAN_ENTER_TEST_MODE, "CAN_ENTER_TEST_MODE"},