diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 6508dc5c7e23f0ff9b7fa50ac09146dabe2e0053..240c2798bd18756083de7e6b51864aea50e9fff2 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -92,6 +92,7 @@ set(PAYLOAD_COMPUTER src/boards/Payload/AltitudeTrigger/AltitudeTrigger.cpp src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp + src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp src/boards/Payload/Wing/FileWingAlgorithm.cpp src/boards/Payload/Wing/WingAlgorithm.cpp src/boards/Payload/WindEstimationScheme/WindEstimation.cpp diff --git a/src/boards/Payload/Configs/WESConfig.h b/src/boards/Payload/Configs/WESConfig.h index 36382803a9472568c8e9bfdf195847383cf561a5..da2357dc4f7a800a76030c32425cc8b8bdb52fb3 100644 --- a/src/boards/Payload/Configs/WESConfig.h +++ b/src/boards/Payload/Configs/WESConfig.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry +/* Copyright (c) 2024 Skyward Experimental Rocketry * Author: Federico Mandelli * * Permission is hereby granted, free of charge, to any person obtaining a copy @@ -22,24 +22,33 @@ #pragma once -#include <miosix.h> +#include <units/Frequency.h> + +#include <chrono> +#include <cstdint> namespace Payload { - -namespace WESConfig +namespace Config +{ +namespace WES { +/* linter off */ using namespace std::chrono_literals; +/* linter off */ using namespace Boardcore::Units::Frequency; -constexpr uint32_t WES_CALIBRATION_TIMEOUT = - 5 * 1000; // time needed for the first loop [ms] +constexpr auto ROTATION_PERIOD = 10s; -constexpr int WES_CALIBRATION_SAMPLE_NUMBER = - 20; // number to sample to take in the first loop -constexpr uint32_t WES_CALIBRATION_UPDATE_PERIOD = - WES_CALIBRATION_TIMEOUT / WES_CALIBRATION_SAMPLE_NUMBER; -constexpr uint32_t WES_PREDICTION_UPDATE_PERIOD = - 100; // update period of WES[ms] +namespace Calibration +{ +constexpr auto SAMPLE_RATE = 4_hz; +constexpr auto SAMPLE_COUNT = 20; ///< Samples to collect during calibration +} // namespace Calibration -} // namespace WESConfig +namespace Prediction +{ +constexpr auto UPDATE_RATE = 10_hz; +} +} // namespace WES +} // namespace Config } // namespace Payload diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h index ee07030cb31622a223cec7864f4e3c91198bc996..3a28c460901e39e72683bd9bca9c1014b9e95db8 100644 --- a/src/boards/Payload/Configs/WingConfig.h +++ b/src/boards/Payload/Configs/WingConfig.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2022-2024 Skyward Experimental Rocketry - * Authors: Federico Mandelli, Nicclò Betto +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -23,6 +23,7 @@ #pragma once #include <units/Frequency.h> +#include <utils/Constants.h> namespace Payload { @@ -30,24 +31,37 @@ namespace Config { namespace Wing { + +// Algorithm configuration +#if defined(CLOSED_LOOP) +constexpr int SELECTED_ALGORITHM = 0; +#elif EARLY_MANEUVER +constexpr int SELECTED_ALGORITHM = 1; +#elif SEQUENCE +constexpr int SELECTED_ALGORITHM = 2; +#elif ROTATION +constexpr int SELECTED_ALGORITHM = 3; +#else +constexpr int SELECTED_ALGORITHM = 0; +#endif + #if defined(EUROC) constexpr float DEFAULT_TARGET_LAT = 39.389733; constexpr float DEFAULT_TARGET_LON = -8.288992; #elif defined(ROCCARASO) -constexpr float DEFAULT_TARGET_LAT = 41.809216; -constexpr float DEFAULT_TARGET_LON = 14.055310; -#elif defined(EUROC) -constexpr float DEFAULT_TARGET_LAT = 39.383; -constexpr float DEFAULT_TARGET_LON = -8.27963; +constexpr float DEFAULT_TARGET_LAT = 41.8089005; +constexpr float DEFAULT_TARGET_LON = 14.0546716; #else // Milan -constexpr float DEFAULT_TARGET_LAT = 39.383; -constexpr float DEFAULT_TARGET_LON = -8.27963; +constexpr float DEFAULT_TARGET_LAT = 45.5010679; +constexpr float DEFAULT_TARGET_LON = 9.1563769; #endif +constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms] + constexpr int WING_UPDATE_PERIOD = 1000; // [ms] -constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = 2.09439; -constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -2.09439; +constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = Boardcore::Constants::PI; +constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -Boardcore::Constants::PI; constexpr int GUIDANCE_CONFIDENCE = 15; constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD = 250; //[m] @@ -55,15 +69,18 @@ constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD = 150; //[m] constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50; //[m] // TODO check this parameter preflight -constexpr float KP = 0.4; //[m] -constexpr float KI = 0.08; //[m] +constexpr float KP = 0.9; +constexpr float KI = 0.05; + +constexpr float TWIRL_RADIUS = 0.5; // [%] + } // namespace Wing namespace AltitudeTrigger { /* linter off */ using namespace Boardcore::Units::Frequency; -constexpr auto DEPLOYMENT_ALTITUDE = 400; // [meters] +constexpr auto DEPLOYMENT_ALTITUDE = 300; // [meters] constexpr auto CONFIDENCE = 10; // [samples] constexpr auto UPDATE_RATE = 10_hz; } // namespace AltitudeTrigger diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp index 62e64bcb31ad75fa55047db63022c4b1f9d2ddbb..de80e13cd1d443ad51f98e5163c1c1c69b10e7e3 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.cpp +++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Angelo Prete * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -23,6 +23,7 @@ #include <Payload/Actuators/Actuators.h> #include <Payload/BoardScheduler.h> +#include <Payload/Configs/ActuatorsConfig.h> #include <Payload/Configs/WESConfig.h> #include <Payload/Configs/WingConfig.h> #include <Payload/FlightStatsRecorder/FlightStatsRecorder.h> @@ -39,10 +40,12 @@ #include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> +using namespace std::chrono; using namespace Boardcore; -using namespace Payload::Config::Wing; -using namespace Payload::WESConfig; using namespace Common; +using namespace Payload::Config::Wing; +using namespace Payload::Config::WES; +using namespace Payload::Config::Actuators; namespace Payload { @@ -52,6 +55,7 @@ WingController::WingController() { EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); EventBroker::getInstance().subscribe(this, TOPIC_DPL); + EventBroker::getInstance().subscribe(this, TOPIC_WING); this->targetPositionGEO = {DEFAULT_TARGET_LAT, DEFAULT_TARGET_LON}; // Instantiate the algorithms @@ -156,33 +160,61 @@ State WingController::state_flying(const Event& event) State WingController::state_calibration(const Boardcore::Event& event) { + static uint16_t calibrationTimeoutEventId; + switch (event) { case EV_ENTRY: // starts twirling and calibration wes { logStatus(WingControllerState::CALIBRATION); - getModule<Actuators>()->setServoPosition( - ServosList::PARAFOIL_LEFT_SERVO, 1); - getModule<Actuators>()->setServoPosition( - ServosList::PARAFOIL_RIGHT_SERVO, 0); - EventBroker::getInstance().postDelayed(DPL_WES_CAL_DONE, TOPIC_DPL, - WES_CALIBRATION_TIMEOUT); - getModule<WindEstimation>()->startWindEstimationSchemeCalibration(); + + flare(); + calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( + DPL_SERVO_ACTUATION_DETECTED, TOPIC_DPL, 2000); + return HANDLED; } case EV_EXIT: { + EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId); return HANDLED; } case EV_EMPTY: { return tranSuper(&WingController::state_flying); } + case DPL_SERVO_ACTUATION_DETECTED: + { + reset(); + calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( + DPL_WIGGLE, TOPIC_DPL, 1000); + + return HANDLED; + } + case DPL_WIGGLE: + { + flare(); + calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( + DPL_NC_OPEN, TOPIC_DPL, 2000); + + return HANDLED; + } + case DPL_NC_OPEN: + { + reset(); + calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( + DPL_WES_CAL_DONE, TOPIC_DPL, + milliseconds{Config::WES::ROTATION_PERIOD}.count()); + getModule<WindEstimation>()->startWindEstimationSchemeCalibration(); + + twirl(); + + return HANDLED; + } case DPL_WES_CAL_DONE: { reset(); - // Turn off the cutters - getModule<Actuators>()->cuttersOff(); + return transition(&WingController::state_controlled_descent); } default: @@ -198,7 +230,6 @@ State WingController::state_controlled_descent(const Boardcore::Event& event) case EV_ENTRY: // start automatic algorithm { logStatus(WingControllerState::ALGORITHM_CONTROLLED); - selectAlgorithm(0); setEarlyManeuverPoints( convertTargetPositionToNED(targetPositionGEO), {getModule<NASController>()->getNasState().n, @@ -210,6 +241,10 @@ State WingController::state_controlled_descent(const Boardcore::Event& event) { return tranSuper(&WingController::state_flying); } + case WING_ALGORITHM_ENDED: + { + return transition(&WingController::state_on_ground); + } case EV_EXIT: { return HANDLED; @@ -232,6 +267,11 @@ State WingController::state_on_ground(const Boardcore::Event& event) getModule<WindEstimation>()->stopWindEstimationScheme(); getModule<WindEstimation>()->stopWindEstimationSchemeCalibration(); stopAlgorithm(); + + // disable servos + getModule<Actuators>()->disableServo(PARAFOIL_LEFT_SERVO); + getModule<Actuators>()->disableServo(PARAFOIL_RIGHT_SERVO); + return HANDLED; } case EV_EXIT: @@ -249,48 +289,79 @@ State WingController::state_on_ground(const Boardcore::Event& event) } } -void WingController::addAlgorithms() +bool WingController::addAlgorithms() { WingAlgorithm* algorithm; WingAlgorithmData step; + bool result = false; + + // Algorithm 0 + algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO, + PARAFOIL_RIGHT_SERVO, clGuidance); + result = algorithm->init(); + algorithms.push_back(algorithm); + // Algorithm 1 algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, emGuidance); - // Ensure that the servos are correct - algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO); + result &= algorithm->init(); + algorithms.push_back(algorithm); + // Algorithm 2 + algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO); + step.servo1Angle = 0; + step.servo2Angle = 120; + step.timestamp = 0; + algorithm->addStep(step); + step.servo1Angle = 0; + step.servo2Angle = 0; + step.timestamp += 1000 * WING_STRAIGHT_FLIGHT_TIMEOUT; + algorithm->addStep(step); + step.servo1Angle = 0; + step.servo2Angle = 0; + step.timestamp += WING_STRAIGHT_FLIGHT_TIMEOUT; + algorithm->addStep(step); + result &= algorithm->init(); // Add the algorithm to the vector algorithms.push_back(algorithm); + // Algorithm 3 (rotation) algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO); - - for (int i = 0; i < 3; i++) - { - step.servo1Angle = 120; - step.servo2Angle = 0; - step.timestamp = 0 + i * 40000000; - algorithm->addStep(step); - step.servo1Angle = 0; - step.servo2Angle = 0; - step.timestamp += 10000000 + i * 40000000; - algorithm->addStep(step); - step.servo1Angle = 0; - step.servo2Angle = 120; - step.timestamp += 10000000 + i * 40000000; - algorithm->addStep(step); - step.servo1Angle = 0; - step.servo2Angle = 0; - step.timestamp += 10000000 + i * 40000000; - algorithm->addStep(step); - } - - // Ensure that the servos are correct - algorithm->setServo(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO); - - // Add the algorithm to the vector + step.timestamp = 0; + step.servo1Angle = LeftServo::ROTATION / 2; + step.servo2Angle = 0; + algorithm->addStep(step); + step.timestamp += microseconds{ROTATION_PERIOD}.count(); + step.servo1Angle = 0; + step.servo2Angle = RightServo::ROTATION / 2; + algorithm->addStep(step); + step.timestamp += microseconds{ROTATION_PERIOD}.count(); + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + step.timestamp += microseconds{ROTATION_PERIOD}.count(); + step.servo1Angle = LeftServo::ROTATION; + step.servo2Angle = RightServo::ROTATION; + algorithm->addStep(step); + step.timestamp += microseconds{ROTATION_PERIOD}.count(); + step.servo1Angle = 0; + step.servo2Angle = RightServo::ROTATION; + algorithm->addStep(step); + step.timestamp += microseconds{ROTATION_PERIOD}.count(); + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + step.timestamp += microseconds{ROTATION_PERIOD}.count(); + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + + result &= algorithm->init(); algorithms.push_back(algorithm); - selectAlgorithm(0); + selectAlgorithm(SELECTED_ALGORITHM); + + return result; } bool WingController::selectAlgorithm(unsigned int index) @@ -358,20 +429,27 @@ void WingController::flare() { // Set the servo position to flare (pull the two ropes as skydiving people // do) - getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1); - getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1); + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f); +} + +void WingController::twirl() +{ + // Set the servo position to twirl + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, TWIRL_RADIUS); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f); } void WingController::reset() { // Set the servo position to reset - getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0); - getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0); + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f); } void WingController::setTargetPosition(Eigen::Vector2f targetGEO) { - if (getModule<NASController>()->getState() != NASControllerState::READY) + if (getModule<NASController>()->getState() == NASControllerState::ACTIVE) { this->targetPositionGEO = targetGEO; setEarlyManeuverPoints(convertTargetPositionToNED(targetPositionGEO), @@ -415,6 +493,9 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED, currentPosNED; emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position); + + clGuidance.setPoints(targetNED); + WingTargetPositionData data; data.receivedLat = targetPositionGEO[0]; diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h index 3bdda1a71f600a78a89c8642e984ff5a5a7c64c7..7fb6f482f7f7935349ca7f1ad97801d4be88c0d1 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.h +++ b/src/boards/Payload/StateMachines/WingController/WingController.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Matteo Pignataro, Federico Mandelli +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Angelo Prete * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -22,6 +22,7 @@ #pragma once +#include <Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h> #include <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h> #include <Payload/Wing/WingAlgorithm.h> #include <diagnostic/PrintLogger.h> @@ -121,7 +122,7 @@ private: * that they can be injected with dependencies when the WingController is * injected. */ - void addAlgorithms(); + bool addAlgorithms(); /** * @brief target position getter @@ -154,7 +155,7 @@ private: * @brief PrintLogger */ Boardcore::PrintLogger logger = - Boardcore::Logging::getLogger("PayloadTest"); + Boardcore::Logging::getLogger("WingController"); /** * @brief Internal running state @@ -173,6 +174,12 @@ private: */ EarlyManeuversGuidanceAlgorithm emGuidance; + /** + * @brief Instance of the Closed Loop Guidance Algorithm used by + * AutomaticWingAlgorithm + */ + ClosedLoopGuidanceAlgorithm clGuidance; + /** * @brief starts the selected algorithm */ @@ -189,6 +196,8 @@ private: */ void flare(); + void twirl(); + /** * @brief Resets the servos in their initial position */ diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp index 8bf0d32f2ca8b0e1f2ea0aa76f9ba98851146e38..4c5df6049aa68a1195f2377d97ae249d90ade402 100644 --- a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp +++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp @@ -1,4 +1,4 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry +/* Copyright (c) 2024 Skyward Experimental Rocketry * Author: Federico Mandelli * * Permission is hereby granted, free of charge, to any person obtaining a copy @@ -28,9 +28,9 @@ #include <common/Events.h> #include <events/EventBroker.h> -using namespace Payload::WESConfig; using namespace Boardcore; using namespace Common; +namespace config = Payload::Config::WES; namespace Payload { @@ -42,14 +42,16 @@ WindEstimation::WindEstimation() : running(false), calRunning(false) bool WindEstimation::start() { + using namespace std::chrono; + auto& scheduler = getModule<BoardScheduler>()->windEstimation(); scheduler.addTask([this] { windEstimationSchemeCalibration(); }, - WES_CALIBRATION_UPDATE_PERIOD); + config::Calibration::SAMPLE_RATE); // Register the WES task scheduler.addTask([this] { windEstimationScheme(); }, - WES_PREDICTION_UPDATE_PERIOD); + config::Prediction::UPDATE_RATE); return true; } @@ -68,8 +70,8 @@ void WindEstimation::startWindEstimationSchemeCalibration() { calRunning = true; nSampleCal = 0; - vx = 0; - vy = 0; + vn = 0; + ve = 0; v2 = 0; LOG_INFO(logger, "WindEstimationCalibration started"); } @@ -85,8 +87,9 @@ void WindEstimation::stopWindEstimationSchemeCalibration() miosix::Lock<FastMutex> l(mutex); wind = windCalibration; } - windLogger.vn = windCalibration[0]; - windLogger.ve = windCalibration[1]; + windLogger.cal = true; + windLogger.vn = windCalibration[0]; + windLogger.ve = windCalibration[1]; startWindEstimationScheme(); logStatus(); } @@ -99,7 +102,7 @@ void WindEstimation::windEstimationSchemeCalibration() auto gpsData = getModule<Sensors>()->getUBXGPSLastSample(); if (gpsData.fix != 0) { - if (nSampleCal < WES_CALIBRATION_SAMPLE_NUMBER) + if (nSampleCal < config::Calibration::SAMPLE_COUNT) { float gpsN = 0, gpsE = 0; @@ -109,20 +112,20 @@ void WindEstimation::windEstimationSchemeCalibration() calibrationMatrix(nSampleCal, 1) = gpsE; calibrationV2(nSampleCal) = gpsN * gpsN + gpsE * gpsE; - vx += gpsN; - vy += gpsE; + vn += gpsN; + ve += gpsE; v2 += gpsN * gpsN + gpsE * gpsE; nSampleCal++; } else { - vx = vx / nSampleCal; - vy = vy / nSampleCal; + vn = vn / nSampleCal; + ve = ve / nSampleCal; v2 = v2 / nSampleCal; for (int i = 0; i < nSampleCal; i++) { - calibrationMatrix(i, 0) = calibrationMatrix(i, 0) - vx; - calibrationMatrix(i, 1) = calibrationMatrix(i, 1) - vy; + calibrationMatrix(i, 0) = calibrationMatrix(i, 0) - vn; + calibrationMatrix(i, 1) = calibrationMatrix(i, 1) - ve; calibrationV2(i) = 0.5f * (calibrationV2(i) - v2); } Eigen::MatrixXf calibrationMatrixT = @@ -172,11 +175,11 @@ void WindEstimation::windEstimationScheme() gpsE = gpsData.velocityEast; // update avg nSample++; - vx = (vx * nSample + gpsN) / (nSample + 1); - vy = (vy * nSample + gpsE) / (nSample + 1); + vn = (vn * nSample + gpsN) / (nSample + 1); + ve = (ve * nSample + gpsE) / (nSample + 1); v2 = (v2 * nSample + (gpsN * gpsN + gpsE * gpsE)) / (nSample + 1); - phi(0) = gpsN - vx; - phi(1) = gpsE - vy; + phi(0) = gpsN - vn; + phi(1) = gpsE - ve; y = 0.5f * ((gpsN * gpsN + gpsE * gpsE) - v2); phiT = phi.transpose(); @@ -186,9 +189,10 @@ void WindEstimation::windEstimationScheme() (y - phiT * getWindEstimationScheme()); { miosix::Lock<FastMutex> l(mutex); - wind = wind + temp; - windLogger.vn = wind[0]; - windLogger.ve = wind[1]; + wind = wind + temp; + windLogger.vn = wind[0]; + windLogger.ve = wind[1]; + windLogger.cal = false; } logStatus(); } diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.h b/src/boards/Payload/WindEstimationScheme/WindEstimation.h index 8f76fd78cd0ca4f168e5a48b39446ee02b3e844f..b190beee531fb54178264490db57079bca9a81b1 100644 --- a/src/boards/Payload/WindEstimationScheme/WindEstimation.h +++ b/src/boards/Payload/WindEstimationScheme/WindEstimation.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry +/* Copyright (c) 2024 Skyward Experimental Rocketry * Author: Federico Mandelli * * Permission is hereby granted, free of charge, to any person obtaining a copy @@ -90,12 +90,11 @@ private: */ Eigen::Vector2f windCalibration{0.0f, 0.0f}; uint8_t nSampleCal = 0; - Eigen::Matrix<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER, 2> + Eigen::Matrix<float, Config::WES::Calibration::SAMPLE_COUNT, 2> calibrationMatrix; - Eigen::Vector<float, WESConfig::WES_CALIBRATION_SAMPLE_NUMBER> - calibrationV2; - float vx = 0; - float vy = 0; + Eigen::Vector<float, Config::WES::Calibration::SAMPLE_COUNT> calibrationV2; + float vn = 0; + float ve = 0; float v2 = 0; /** @@ -114,7 +113,7 @@ private: * @brief PrintLogger */ Boardcore::PrintLogger logger = - Boardcore::Logging::getLogger("PayloadTest"); + Boardcore::Logging::getLogger("WindEstimation"); /** * @brief Logging struct diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimationData.h b/src/boards/Payload/WindEstimationScheme/WindEstimationData.h index 49d674fc96df107474c7b001e301530e1879770e..3104692f7dd2e80f09209e4f2a183569c6db4fa9 100644 --- a/src/boards/Payload/WindEstimationScheme/WindEstimationData.h +++ b/src/boards/Payload/WindEstimationScheme/WindEstimationData.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry - * Author: Alberto Nidasio +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Mandelli * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -34,12 +34,13 @@ struct WindLogging { long long timestamp = 0; float vn = 0, ve = 0; + bool cal = 0; - static std::string header() { return "timestamp,vn,ve\n"; } + static std::string header() { return "timestamp,vn,ve,cal\n"; } void print(std::ostream& os) const { - os << timestamp << "," << vn << "," << ve << "\n "; + os << timestamp << "," << vn << "," << ve << "," << cal << "\n "; } }; diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp index 2a93c8167870e2accb8132011b5b4e1ee3eb76f7..8639f619ae60d2ae6282256284f4ce8d488956b3 100644 --- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp +++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro, Radu Raul +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Niccolò Betto * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -61,17 +61,25 @@ void AutomaticWingAlgorithm::step() getModule<WindEstimation>()->getWindEstimationScheme()); // Actuate the result + // To see how to interpret the PI output + // https://www.geogebra.org/calculator/xrhwarpz + // to system is + /* N ^ + | + | + ----> E + */ if (result > 0) { - // Activate the servo1 and reset servo2 - getModule<Actuators>()->setServoAngle(servo1, result); - getModule<Actuators>()->setServoAngle(servo2, 0); + // Activate the servo2 and reset servo1 + getModule<Actuators>()->setServoAngle(servo1, 0); + getModule<Actuators>()->setServoAngle(servo2, result); } else { - // Activate the servo2 and reset servo1 - getModule<Actuators>()->setServoAngle(servo1, 0); - getModule<Actuators>()->setServoAngle(servo2, result * -1); + // Activate the servo1 and reset servo2 + getModule<Actuators>()->setServoAngle(servo1, result * -1); + getModule<Actuators>()->setServoAngle(servo2, 0); } // Log the servo positions @@ -102,24 +110,15 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED) float targetAngle = guidance.calculateTargetAngle(currentPosition, heading); - Vector2f relativeVelocity(state.vn - windNED[0], state.ve - windNED[1]); + // WES is currently unused + Vector2f relativeVelocity(state.vn, state.ve); // Compute the angle of the current velocity float velocityAngle; - // In case of a 0 north velocity i force the angle to 90 - if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0) - { - velocityAngle = 0; - } - else if (relativeVelocity[1] == 0) - { - velocityAngle = (relativeVelocity[0] > 0 ? 1 : -1) * Constants::PI / 2; - } - else - { - velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]); - } + // All angle are computed as angle from the north direction + + velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]); // Compute the angle difference float error = angleDiff(targetAngle, velocityAngle); @@ -132,7 +131,8 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED) result = result * (180.f / Constants::PI); // Flip the servo orientation - result *= -1; + // result *= -1; + // TODO check if this is needed // Logs the outputs { diff --git a/src/boards/Payload/Wing/WingAlgorithm.cpp b/src/boards/Payload/Wing/WingAlgorithm.cpp index 2ec7936062f12461435b1f855ed2be4cdadda9e4..2195782c3889801060ea142eb17840466381ad4b 100644 --- a/src/boards/Payload/Wing/WingAlgorithm.cpp +++ b/src/boards/Payload/Wing/WingAlgorithm.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Matteo Pignataro, Federico Mandelli +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Mandelli * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -24,6 +24,7 @@ #include <Payload/Wing/WingAlgorithm.h> #include <common/Events.h> #include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> using namespace Boardcore; using namespace Payload::Config::Wing; @@ -89,7 +90,7 @@ void WingAlgorithm::step() // Set the index to 0 in case of another future execution stepIndex = 0; // Terminate here - // EventBroker::getInstance().post(ALGORITHM_ENDED, TOPIC_ALGOS); + EventBroker::getInstance().post(WING_ALGORITHM_ENDED, TOPIC_WING); return; } diff --git a/src/boards/Payload/Wing/WingTargetPositionData.h b/src/boards/Payload/Wing/WingTargetPositionData.h index a1cab4dbf456c366d1a301487aacfd1b11a0099c..cb27979246a74c72cc61a22c67b13b6b2c89c634 100644 --- a/src/boards/Payload/Wing/WingTargetPositionData.h +++ b/src/boards/Payload/Wing/WingTargetPositionData.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry - * Author: Alberto Nidasio, Radu Raul +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Mandelli * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/src/boards/common/Events.h b/src/boards/common/Events.h index 92b4fdfd4544d27ae684114e0c5439f5fb23c3c4..b274d2b579913a62ba7c0ba3fdc09f64bdfeca81 100644 --- a/src/boards/common/Events.h +++ b/src/boards/common/Events.h @@ -147,6 +147,7 @@ enum Events : uint8_t TARS_FILLING_DONE, TARS_REFINING_DONE, ALTITUDE_TRIGGER_ALTITUDE_REACHED, + WING_ALGORITHM_ENDED, LAST_EVENT }; @@ -265,6 +266,7 @@ inline std::string getEventString(uint8_t event) {TARS_REFINING_DONE, "TARS_REFINING_DONE"}, {ALTITUDE_TRIGGER_ALTITUDE_REACHED, "ALTITUDE_TRIGGER_ALTITUDE_REACHED"}, + {WING_ALGORITHM_ENDED, "WING_ALGORITHM_ENDED"}, {LAST_EVENT, "LAST_EVENT"}, }; diff --git a/src/boards/common/Topics.h b/src/boards/common/Topics.h index eaa0429e89f472c4efe71081ac28124aea898faf..8855f866f5776048489d3e7a36b63107658f5985 100644 --- a/src/boards/common/Topics.h +++ b/src/boards/common/Topics.h @@ -44,12 +44,13 @@ enum Topics : uint8_t TOPIC_MOTOR, TOPIC_TARS, TOPIC_ALT, + TOPIC_WING, }; const std::vector<uint8_t> TOPICS_LIST{ - TOPIC_ABK, TOPIC_ADA, TOPIC_MEA, TOPIC_DPL, TOPIC_CAN, - TOPIC_FLIGHT, TOPIC_FMM, TOPIC_FSR, TOPIC_NAS, TOPIC_TMTC, - TOPIC_MOTOR, TOPIC_TARS, TOPIC_ALT, + TOPIC_ABK, TOPIC_ADA, TOPIC_MEA, TOPIC_DPL, TOPIC_CAN, + TOPIC_FLIGHT, TOPIC_FMM, TOPIC_FSR, TOPIC_NAS, TOPIC_TMTC, + TOPIC_MOTOR, TOPIC_TARS, TOPIC_ALT, TOPIC_WING, }; } // namespace Common