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