diff --git a/src/Parafoil/Configs/FlightModeManagerConfig.h b/src/Parafoil/Configs/FlightModeManagerConfig.h
index 983059aa10eda062a458714cba8b8cd4c4a3269b..71a62678bbcfac90886c04f663539d0cea980dc9 100644
--- a/src/Parafoil/Configs/FlightModeManagerConfig.h
+++ b/src/Parafoil/Configs/FlightModeManagerConfig.h
@@ -36,7 +36,6 @@ namespace FlightModeManager
/* linter-off */ using namespace Boardcore::Units::Time;
constexpr auto LOGGING_DELAY = 5_s;
-constexpr auto CONTROL_DELAY = 2_s;
} // namespace FlightModeManager
} // namespace Config
diff --git a/src/Parafoil/Configs/WESConfig.h b/src/Parafoil/Configs/WESConfig.h
index 4f6521100da10291c9e906b04aa8573af900c410..58a9208e1cce3d4cbf699707979aeb994910b930 100644
--- a/src/Parafoil/Configs/WESConfig.h
+++ b/src/Parafoil/Configs/WESConfig.h
@@ -39,7 +39,8 @@ namespace WES
/* linter-off */ using namespace Boardcore::Units::Time;
-constexpr auto CALIBRATE = false;
+constexpr auto CALIBRATE = false; // FIXME: Wind Estimation implementation is
+ // not finished yet
constexpr auto CALIBRATION_TIMEOUT = 5_s; // time needed for the first loop
constexpr auto ROTATION_PERIOD = 10_s;
diff --git a/src/Parafoil/Configs/WingConfig.h b/src/Parafoil/Configs/WingConfig.h
index 2b918f5dfa74c62366d8d71abf73b4b8b4c13da9..a73ecb968fffa18de90170c95109fb06c826d0b1 100644
--- a/src/Parafoil/Configs/WingConfig.h
+++ b/src/Parafoil/Configs/WingConfig.h
@@ -28,6 +28,8 @@
#include <units/Time.h>
#include <utils/Constants.h>
+#include <array>
+
namespace Parafoil
{
namespace Config
@@ -135,6 +137,26 @@ constexpr auto TARGET_ALTITUDE_THRESHOLD = 50_m;
} // namespace Guidance
+namespace Deployment
+{
+/* linter off */ using namespace Boardcore::Units::Time;
+
+constexpr auto PUMP_DELAY = 2_s;
+
+struct Pump
+{
+ Second flareTime;
+ Second resetTime;
+};
+
+constexpr std::array<Pump, 3> PUMPS = {
+ Pump{.flareTime = 2_s, .resetTime = 1_s},
+ Pump{.flareTime = 2_s, .resetTime = 1_s},
+ Pump{.flareTime = 1_s, .resetTime = 0.5_s},
+};
+
+} // namespace Deployment
+
namespace LandingFlare
{
/* linter off */ using namespace Boardcore::Units::Frequency;
diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
index 24eb3dffe58f19a8e48b197ac992c19621cd5fb5..73d5c105c1cf2d0f66d5ad8ee28485afdcae29a7 100644
--- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -438,17 +438,13 @@ State FlightModeManager::Flying(const Event& event)
State FlightModeManager::FlyingWingDescent(const Event& event)
{
- static uint16_t controlDelayId;
-
switch (event)
{
case EV_ENTRY:
{
updateState(FlightModeManagerState::FLYING_WING_DESCENT);
// Send the event to the WingController
- controlDelayId = EventBroker::getInstance().postDelayed(
- FLIGHT_WING_DESCENT, TOPIC_FLIGHT,
- Millisecond{config::CONTROL_DELAY}.value());
+ EventBroker::getInstance().post(FLIGHT_WING_DESCENT, TOPIC_FLIGHT);
getModule<FlightStatsRecorder>()->dropDetected(
TimestampTimer::getTimestamp());
@@ -458,7 +454,6 @@ State FlightModeManager::FlyingWingDescent(const Event& event)
case EV_EXIT:
{
- EventBroker::getInstance().removeDelayed(controlDelayId);
return HANDLED;
}
diff --git a/src/Parafoil/StateMachines/WingController/WingController.cpp b/src/Parafoil/StateMachines/WingController/WingController.cpp
index fd5defb10bc1ced7ee7fe29ad78d6acf86d3944f..5b37e4c72b395c76c98c885de582e6b5e748128b 100644
--- a/src/Parafoil/StateMachines/WingController/WingController.cpp
+++ b/src/Parafoil/StateMachines/WingController/WingController.cpp
@@ -127,8 +127,12 @@ State WingController::Flying(const Event& event)
State WingController::FlyingDeployment(const Boardcore::Event& event)
{
+ static uint16_t flareTimeoutEventId;
+ static uint16_t resetTimeoutEventId;
static uint16_t calibrationTimeoutEventId;
+ static uint16_t pumpCount = Wing::Deployment::PUMPS.size();
+
switch (event)
{
case EV_ENTRY:
@@ -140,9 +144,16 @@ State WingController::FlyingDeployment(const Boardcore::Event& event)
getModule<FlightStatsRecorder>()->deploymentDetected(
TimestampTimer::getTimestamp(), altitude);
- flareWing();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_SERVO_ACTUATION_DETECTED, TOPIC_DPL, 2000);
+ if (pumpCount > 0) // If there is at least one pump specified
+ {
+ flareTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_FLARE_START, TOPIC_DPL,
+ Millisecond{Wing::Deployment::PUMP_DELAY}.value());
+ }
+ else
+ {
+ EventBroker::getInstance().post(DPL_DONE, TOPIC_DPL);
+ }
if (Config::Wing::DynamicTarget::ENABLED)
initDynamicTarget(
@@ -153,6 +164,8 @@ State WingController::FlyingDeployment(const Boardcore::Event& event)
}
case EV_EXIT:
{
+ EventBroker::getInstance().removeDelayed(flareTimeoutEventId);
+ EventBroker::getInstance().removeDelayed(resetTimeoutEventId);
EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId);
return HANDLED;
}
@@ -160,42 +173,52 @@ State WingController::FlyingDeployment(const Boardcore::Event& event)
{
return tranSuper(&WingController::Flying);
}
- case DPL_SERVO_ACTUATION_DETECTED:
+ case DPL_FLARE_START:
{
- resetWing();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_WIGGLE, TOPIC_DPL, 1000);
+ pumpCount--;
+ if (pumpCount == 0)
+ {
+ EventBroker::getInstance().post(DPL_DONE, TOPIC_DPL);
+ return HANDLED;
+ }
+ auto pump = Wing::Deployment::PUMPS.at(pumpCount);
+
+ flareWing();
+ resetTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_FLARE_STOP, TOPIC_DPL, Millisecond{pump.flareTime}.value());
return HANDLED;
}
- case DPL_WIGGLE:
+ case DPL_FLARE_STOP:
{
- flareWing();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_NC_OPEN, TOPIC_DPL, 2000);
+ auto pump = Wing::Deployment::PUMPS.at(pumpCount);
+ resetWing();
+ flareTimeoutEventId = EventBroker::getInstance().postDelayed(
+ DPL_FLARE_START, TOPIC_DPL,
+ Millisecond{pump.resetTime}.value());
return HANDLED;
}
- case DPL_NC_OPEN:
+ case DPL_DONE:
{
- resetWing();
if (WES::CALIBRATE)
{
calibrationTimeoutEventId =
EventBroker::getInstance().postDelayed(
DPL_WES_CAL_DONE, TOPIC_DPL,
Millisecond{WES::CALIBRATION_TIMEOUT}.value());
- getModule<WindEstimation>()->startAlgorithm();
-
+ getModule<WindEstimation>()->startCalibration();
getModule<Actuators>()->startTwirl();
+ return HANDLED;
}
return transition(&WingController::FlyingControlledDescent);
}
case DPL_WES_CAL_DONE:
{
+ getModule<WindEstimation>()->stopCalibration();
getModule<Actuators>()->stopTwirl();
-
+ EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId);
return transition(&WingController::FlyingControlledDescent);
}
default:
diff --git a/src/common/Events.h b/src/common/Events.h
index 25357195449530a8bf996e3993907ad6b7ba3ee1..8ecb7b1af9c446df7f8a5a1d1ca05457361ae78f 100644
--- a/src/common/Events.h
+++ b/src/common/Events.h
@@ -63,12 +63,8 @@ enum Events : uint8_t
ARP_FIX_ROCKET,
DPL_CUT_DROGUE,
DPL_CUT_TIMEOUT,
- DPL_NC_OPEN,
- DPL_NC_RESET,
DPL_FLARE_START,
DPL_FLARE_STOP,
- DPL_SERVO_ACTUATION_DETECTED,
- DPL_WIGGLE,
DPL_WES_CAL_DONE,
DPL_DONE,
CAN_FORCE_INIT,
@@ -217,8 +213,6 @@ inline std::string getEventString(uint8_t event)
{ARP_FIX_ROCKET, "ARP_FIX_ROCKET"},
{DPL_CUT_DROGUE, "DPL_CUT_DROGUE"},
{DPL_CUT_TIMEOUT, "DPL_CUT_TIMEOUT"},
- {DPL_NC_OPEN, "DPL_NC_OPEN"},
- {DPL_NC_RESET, "DPL_NC_RESET"},
{DPL_FLARE_START, "DPL_FLARE_START"},
{DPL_FLARE_STOP, "DPL_FLARE_STOP"},
{DPL_DONE, "DPL_DONE"},
@@ -334,6 +328,7 @@ inline std::string getEventString(uint8_t event)
{TARS_REFINING_DONE, "TARS_REFINING_DONE"},
{ALTITUDE_TRIGGER_ALTITUDE_REACHED,
"ALTITUDE_TRIGGER_ALTITUDE_REACHED"},
+ {FLIGHT_LANDING_FLARE_STOP, "FLIGHT_LANDING_FLARE_STOP"},
{WING_ALGORITHM_ENDED, "WING_ALGORITHM_ENDED"},
{LAST_EVENT, "LAST_EVENT"},
};