diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index 8824118432eafa8af0d58db2fcd558d88c8aad15..86be338a7f5efe0fa082afdc8b52f172cf17e4c1 100644
--- a/src/boards/Payload/Configs/WingConfig.h
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -85,7 +85,7 @@ constexpr auto KI = 0.05f;
namespace Guidance
{
-constexpr auto CONFIDENCE = 15; // [samples]
+constexpr auto CONFIDENCE = 10; // [samples]
constexpr auto M1_ALTITUDE_THRESHOLD = 250; // [m]
constexpr auto M2_ALTITUDE_THRESHOLD = 150; // [m]
constexpr auto TARGET_ALTITUDE_THRESHOLD = 50; // [m]
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp
index 9c3396ca3399072fdeef7784a2df307cab7589d7..466011afa89aed1c38d0ed4562068fb186040dd8 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.cpp
+++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp
@@ -324,6 +324,27 @@ bool WingController::start()
return false;
}
+ auto activeTargetTask = scheduler.addTask(
+ [this]
+ {
+ // Do not update the active target if the wing is not flying
+ if (!running)
+ {
+ return;
+ }
+
+ auto nasState = getModule<NASController>()->getNasState();
+ float altitude = -nasState.d;
+ emGuidance.updateActiveTarget(altitude);
+ },
+ 10_hz);
+
+ if (activeTargetTask == 0)
+ {
+ LOG_ERR(logger, "Failed to add early maneuver active target task");
+ return false;
+ }
+
if (!HSM::start())
{
LOG_ERR(logger, "Failed to start WingController HSM active object");
@@ -482,8 +503,8 @@ WingAlgorithm& WingController::getCurrentAlgorithm()
void WingController::startAlgorithm()
{
- running = true;
updateEarlyManeuverPoints();
+ running = true;
getCurrentAlgorithm().begin();
}
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
index aed38bf29b34e77d6dae11bf27fb2bf65bbbcf7c..e8ac40537e6c7bfd28d50bc5cd6a684d962133b6 100644
--- a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
@@ -22,9 +22,13 @@
#include <Payload/Configs/WingConfig.h>
#include <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
+#include <Payload/Wing/WingTargetPositionData.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <logger/Logger.h>
#include <Eigen/Core>
+using namespace Boardcore;
using namespace Payload::Config::Wing;
namespace Payload
@@ -40,10 +44,6 @@ EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){};
float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
{
- float altitude = abs(currentPositionNED[2]);
-
- computeActiveTarget(altitude);
-
switch (activeTarget)
{
case Target::EMC:
@@ -67,7 +67,7 @@ float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
return atan2(heading[1], heading[0]);
}
-void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
+void EarlyManeuversGuidanceAlgorithm::updateActiveTarget(float altitude)
{
if (altitude <=
Guidance::TARGET_ALTITUDE_THRESHOLD) // Altitude is low, head directly
@@ -90,22 +90,25 @@ void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
emcAltitudeConfidence++; // Altitude is too high, head to the emc
}
- switch (activeTarget)
+ auto currentTarget = activeTarget.load();
+ auto newTarget = currentTarget;
+
+ switch (currentTarget)
{
case Target::EMC:
if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
{
- activeTarget = Target::FINAL;
+ newTarget = Target::FINAL;
emcAltitudeConfidence = 0;
}
else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
{
- activeTarget = Target::M2;
+ newTarget = Target::M2;
emcAltitudeConfidence = 0;
}
else if (m1AltitudeConfidence >= Guidance::CONFIDENCE)
{
- activeTarget = Target::M1;
+ newTarget = Target::M1;
emcAltitudeConfidence = 0;
}
break;
@@ -113,12 +116,12 @@ void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
case Target::M1:
if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
{
- activeTarget = Target::FINAL;
+ newTarget = Target::FINAL;
m1AltitudeConfidence = 0;
}
else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
{
- activeTarget = Target::M2;
+ newTarget = Target::M2;
m1AltitudeConfidence = 0;
}
break;
@@ -126,7 +129,7 @@ void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
case Target::M2:
if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
{
- activeTarget = Target::FINAL;
+ newTarget = Target::FINAL;
m2AltitudeConfidence = 0;
}
break;
@@ -134,6 +137,19 @@ void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
case Target::FINAL:
break;
}
+
+ if (newTarget != currentTarget)
+ {
+ activeTarget = newTarget;
+
+ // Log the active target change
+ auto data = EarlyManeuversActiveTargetData{
+ .timestamp = TimestampTimer::getTimestamp(),
+ .target = static_cast<uint32_t>(newTarget),
+ .altitude = altitude,
+ };
+ Logger::getInstance().log(data);
+ }
}
void EarlyManeuversGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED,
diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
index 58ef53d3f319147a2cacb65faba1f069808a51ab..7a91a241c3d328702e5a11ff02f04aecf26861cd 100644
--- a/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
+++ b/src/boards/Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
@@ -51,12 +51,18 @@ struct EarlyManeuversPoints
*/
class EarlyManeuversGuidanceAlgorithm : public GuidanceAlgorithm
{
-
public:
/**
- * @brief Constructor for the EM Algorithm
- *
+ * @brief Enumerates all the possible targets of the EM algorithm
*/
+ enum class Target : uint32_t
+ {
+ EMC = 0,
+ M1,
+ M2,
+ FINAL
+ };
+
EarlyManeuversGuidanceAlgorithm();
virtual ~EarlyManeuversGuidanceAlgorithm();
@@ -71,7 +77,6 @@ public:
* @param[out] heading Saves the heading vector for logging purposes
*
* @returns the yaw angle of the parafoil in rad
- *
*/
float calculateTargetAngle(const Eigen::Vector3f& currentPositionNED,
Eigen::Vector2f& heading) override;
@@ -82,7 +87,6 @@ public:
* @param[in] EMC NED
* @param[in] M1 NED
* @param[in] M2 NED
- *
*/
void setPoints(Eigen::Vector2f targetNED, Eigen::Vector2f EMC,
Eigen::Vector2f M1, Eigen::Vector2f M2);
@@ -98,38 +102,24 @@ public:
*/
Eigen::Vector2f getActiveTarget();
-private:
- /** @brief Updates the class target
- *
- */
- void computeActiveTarget(float altitude);
-
/**
- * @brief Enumerates all the possible targets of the EM algorithm
+ * @brief Updates the active target based on the current altitude.
*/
- enum class Target
- {
- EMC = 0,
- M1,
- M2,
- FINAL
- };
+ void updateActiveTarget(float altitude);
+private:
// Point we are currently poinying to
std::atomic<Target> activeTarget;
- // Eigen::Vector2f targetNED; // NED
-
+ // Eigen::Vector2f targetNED; // NED, defined in the base class
Eigen::Vector2f EMC; // NED
+ Eigen::Vector2f M1; // NED
+ Eigen::Vector2f M2; // NED
- Eigen::Vector2f M1; // NED
-
- Eigen::Vector2f M2; // NED
-
- unsigned int targetAltitudeConfidence;
- unsigned int m2AltitudeConfidence;
- unsigned int m1AltitudeConfidence;
- unsigned int emcAltitudeConfidence;
+ uint32_t targetAltitudeConfidence;
+ uint32_t m2AltitudeConfidence;
+ uint32_t m1AltitudeConfidence;
+ uint32_t emcAltitudeConfidence;
};
} // namespace Payload
diff --git a/src/boards/Payload/Wing/WingTargetPositionData.h b/src/boards/Payload/Wing/WingTargetPositionData.h
index 1f875b5fa979be3726c58ca67dd92b7bd6c4c463..fdd2f4718272423d652b2f54894a6658cc236c3f 100644
--- a/src/boards/Payload/Wing/WingTargetPositionData.h
+++ b/src/boards/Payload/Wing/WingTargetPositionData.h
@@ -55,4 +55,18 @@ struct WingTargetPositionData
}
};
+struct EarlyManeuversActiveTargetData
+{
+ uint64_t timestamp = 0;
+ uint32_t target = 0; ///< Active target enumeration
+ float altitude = 0; ///< Altitude when the target was changed
+
+ static std::string header() { return "timestamp,target,altitude\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << target << "," << altitude << "\n";
+ }
+};
+
} // namespace Payload
diff --git a/src/scripts/logdecoder/Payload/logdecoder.cpp b/src/scripts/logdecoder/Payload/logdecoder.cpp
index 2be72b49c2af8b7c8e13b2a8352e7d66543773ea..84e301a5960c351830e5153523df127b02bb7e09 100644
--- a/src/scripts/logdecoder/Payload/logdecoder.cpp
+++ b/src/scripts/logdecoder/Payload/logdecoder.cpp
@@ -71,7 +71,7 @@ void registerTypes(Deserializer& ds)
ds.registerType<WingControllerAlgorithmData>();
ds.registerType<WingAlgorithmData>();
ds.registerType<WingTargetPositionData>();
- ds.registerType<WindLogging>();
+ ds.registerType<EarlyManeuversActiveTargetData>();
}
// cppcheck-suppress passedByValue