From 3365c6fbac14f63e7aa16420d4aa3fc5362a014f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Tue, 10 Sep 2024 14:07:03 +0200
Subject: [PATCH] [Payload][EarlyManeuver] Update active target at a higher
frequency than the algorithm frequency
Because of the wing algorithms running at 1 Hz, the wing traveled 75 meters vertically before updating the active target, given a falling rate of 5m/s and the confidence to switch targets requiring 15 consecutive samples.
A periodic task running at 10 Hz is introduced to update the active target.
Also log active target changes.
---
src/boards/Payload/Configs/WingConfig.h | 2 +-
.../WingController/WingController.cpp | 23 ++++++++-
.../EarlyManeuverGuidanceAlgorithm.cpp | 40 +++++++++++-----
.../EarlyManeuversGuidanceAlgorithm.h | 48 ++++++++-----------
.../Payload/Wing/WingTargetPositionData.h | 14 ++++++
src/scripts/logdecoder/Payload/logdecoder.cpp | 2 +-
6 files changed, 85 insertions(+), 44 deletions(-)
diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index 882411843..86be338a7 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 9c3396ca3..466011afa 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 aed38bf29..e8ac40537 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 58ef53d3f..7a91a241c 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 1f875b5fa..fdd2f4718 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 2be72b49c..84e301a59 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
--
GitLab