diff --git a/src/boards/Payload/BoardScheduler.h b/src/boards/Payload/BoardScheduler.h index d10a4de252cfe467eccbda2f4b2b4669d78946e9..1340b8992aca633a0ebf11e35bb3c1aef7baa57a 100644 --- a/src/boards/Payload/BoardScheduler.h +++ b/src/boards/Payload/BoardScheduler.h @@ -74,6 +74,11 @@ public: return Priority::MEDIUM; } + static Priority::PriorityLevel wingControllerPriority() + { + return Priority::MEDIUM; + } + /** * @brief Starts all the schedulers */ diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h index 3a28c460901e39e72683bd9bca9c1014b9e95db8..91c0b50b2237a569aa3d1070a7ebfe05a0d24baf 100644 --- a/src/boards/Payload/Configs/WingConfig.h +++ b/src/boards/Payload/Configs/WingConfig.h @@ -25,6 +25,8 @@ #include <units/Frequency.h> #include <utils/Constants.h> +#include <chrono> + namespace Payload { namespace Config @@ -32,45 +34,57 @@ 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 +/* linter off */ using namespace std::chrono_literals; +/* linter off */ using namespace Boardcore::Units::Frequency; +/** + * @brief The available algorithms for the wing controller. + */ +enum class AlgorithmId : size_t +{ + EARLY_MANEUVER = 0, + CLOSED_LOOP, + SEQUENCE, ///< A predefined sequence of maneuvers + ROTATION, ///< A sequence of maneuvers to rotate the wing + LAST, ///< Used to count the number of algorithms +}; + +namespace Default +{ +// TODO: Verify the default target coordinates for all sites #if defined(EUROC) -constexpr float DEFAULT_TARGET_LAT = 39.389733; -constexpr float DEFAULT_TARGET_LON = -8.288992; +constexpr auto TARGET_LAT = 39.389733f; +constexpr auto TARGET_LON = -8.288992f; #elif defined(ROCCARASO) -constexpr float DEFAULT_TARGET_LAT = 41.8089005; -constexpr float DEFAULT_TARGET_LON = 14.0546716; +constexpr auto TARGET_LAT = 41.809216; +constexpr auto TARGET_LON = 14.055310; #else // Milan -constexpr float DEFAULT_TARGET_LAT = 45.5010679; -constexpr float DEFAULT_TARGET_LON = 9.1563769; +constexpr auto TARGET_LAT = 45.5010679f; +constexpr auto TARGET_LON = 9.1563769f; #endif -constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms] +constexpr auto ALGORITHM = AlgorithmId::EARLY_MANEUVER; +} // namespace Default -constexpr int WING_UPDATE_PERIOD = 1000; // [ms] +constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15s; +constexpr auto UPDATE_RATE = 1_hz; -constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = Boardcore::Constants::PI; -constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -Boardcore::Constants::PI; +namespace PI +{ +constexpr auto SATURATION_MIN_LIMIT = -Boardcore::Constants::PI; +constexpr auto SATURATION_MAX_LIMIT = Boardcore::Constants::PI; -constexpr int GUIDANCE_CONFIDENCE = 15; -constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD = 250; //[m] -constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD = 150; //[m] -constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50; //[m] +constexpr auto KP = 0.9f; +constexpr auto KI = 0.05f; +} // namespace PI -// TODO check this parameter preflight -constexpr float KP = 0.9; -constexpr float KI = 0.05; +namespace Guidance +{ +constexpr auto CONFIDENCE = 15; // [samples] +constexpr auto M1_ALTITUDE_THRESHOLD = 250; // [m] +constexpr auto M2_ALTITUDE_THRESHOLD = 150; // [m] +constexpr auto TARGET_ALTITUDE_THRESHOLD = 50; // [m] +} // namespace Guidance constexpr float TWIRL_RADIUS = 0.5; // [%] diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp index de80e13cd1d443ad51f98e5163c1c1c69b10e7e3..13d02817f39e78d06e9dbb718adb09688bad3064 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.cpp +++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2024 Skyward Experimental Rocketry - * Authors: Federico Mandelli, Angelo Prete + * 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 @@ -51,37 +51,15 @@ namespace Payload { WingController::WingController() - : HSM(&WingController::state_idle), running(false), selectedAlgorithm(0) + : HSM(&WingController::Idle, miosix::STACK_DEFAULT_FOR_PTHREAD, + BoardScheduler::wingControllerPriority()) { 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 - addAlgorithms(); -} - -void WingController::inject(DependencyInjector& injector) -{ - for (auto& algorithm : algorithms) - { - algorithm->inject(injector); - } - Super::inject(injector); -} - -bool WingController::start() -{ - auto& scheduler = getModule<BoardScheduler>()->wingController(); - bool success = true; - - success &= std::all_of(algorithms.begin(), algorithms.end(), - [](auto& algorithm) { return algorithm->init(); }); - - return success && - scheduler.addTask([this] { update(); }, WING_UPDATE_PERIOD) && - HSM::start(); + loadAlgorithms(); } WingController::~WingController() @@ -89,21 +67,16 @@ WingController::~WingController() EventBroker::getInstance().unsubscribe(this); } -WingControllerStatus WingController::getStatus() -{ - miosix::Lock<miosix::FastMutex> s(statusMutex); - return status; -} - -State WingController::state_idle(const Boardcore::Event& event) +State WingController::Idle(const Boardcore::Event& event) { switch (event) { case EV_ENTRY: { - logStatus(WingControllerState::IDLE); + updateState(WingControllerState::IDLE); return HANDLED; } + case FLIGHT_WING_DESCENT: { auto nasState = getModule<NASController>()->getNasState(); @@ -113,12 +86,14 @@ State WingController::state_idle(const Boardcore::Event& event) getModule<FlightStatsRecorder>()->deploymentDetected( TimestampTimer::getTimestamp(), altitude); - return transition(&WingController::state_flying); + return transition(&WingController::Flying); } + case EV_EMPTY: { return tranSuper(&WingController::state_top); } + default: { return UNHANDLED; @@ -126,31 +101,35 @@ State WingController::state_idle(const Boardcore::Event& event) } } -State WingController::state_flying(const Event& event) +State WingController::Flying(const Event& event) { - switch (event) { case EV_ENTRY: { return HANDLED; } + case EV_EXIT: { return HANDLED; } + case EV_EMPTY: { return tranSuper(&WingController::state_top); } + case EV_INIT: { - return transition(&WingController::state_calibration); + return transition(&WingController::FlyingCalibration); } + case FLIGHT_LANDING_DETECTED: { - return transition(&WingController::state_on_ground); + return transition(&WingController::OnGround); } + default: { return UNHANDLED; @@ -158,7 +137,7 @@ State WingController::state_flying(const Event& event) } } -State WingController::state_calibration(const Boardcore::Event& event) +State WingController::FlyingCalibration(const Boardcore::Event& event) { static uint16_t calibrationTimeoutEventId; @@ -166,89 +145,97 @@ State WingController::state_calibration(const Boardcore::Event& event) { case EV_ENTRY: // starts twirling and calibration wes { - logStatus(WingControllerState::CALIBRATION); + updateState(WingControllerState::FLYING_CALIBRATION); - flare(); + flareWing(); 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); + return tranSuper(&WingController::Flying); } + case DPL_SERVO_ACTUATION_DETECTED: { - reset(); + resetWing(); calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( DPL_WIGGLE, TOPIC_DPL, 1000); return HANDLED; } + case DPL_WIGGLE: { - flare(); + flareWing(); calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( DPL_NC_OPEN, TOPIC_DPL, 2000); return HANDLED; } + case DPL_NC_OPEN: { - reset(); + resetWing(); calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( DPL_WES_CAL_DONE, TOPIC_DPL, milliseconds{Config::WES::ROTATION_PERIOD}.count()); getModule<WindEstimation>()->startWindEstimationSchemeCalibration(); - twirl(); + twirlWing(); return HANDLED; } + case DPL_WES_CAL_DONE: { - reset(); + resetWing(); - return transition(&WingController::state_controlled_descent); + return transition(&WingController::FlyingControlledDescent); } + default: { return UNHANDLED; } } } -State WingController::state_controlled_descent(const Boardcore::Event& event) +State WingController::FlyingControlledDescent(const Boardcore::Event& event) { switch (event) { - case EV_ENTRY: // start automatic algorithm + case EV_ENTRY: { - logStatus(WingControllerState::ALGORITHM_CONTROLLED); - setEarlyManeuverPoints( - convertTargetPositionToNED(targetPositionGEO), - {getModule<NASController>()->getNasState().n, - getModule<NASController>()->getNasState().e}); + updateState(WingControllerState::FLYING_CONTROLLED_DESCENT); + startAlgorithm(); return HANDLED; } + case EV_EMPTY: { - return tranSuper(&WingController::state_flying); + return tranSuper(&WingController::Flying); } + case WING_ALGORITHM_ENDED: { - return transition(&WingController::state_on_ground); + return transition(&WingController::OnGround); } + case EV_EXIT: { return HANDLED; } + default: { return UNHANDLED; @@ -256,13 +243,13 @@ State WingController::state_controlled_descent(const Boardcore::Event& event) } } -State WingController::state_on_ground(const Boardcore::Event& event) +State WingController::OnGround(const Boardcore::Event& event) { switch (event) { case EV_ENTRY: { - logStatus(WingControllerState::ON_GROUND); + updateState(WingControllerState::ON_GROUND); getModule<WindEstimation>()->stopWindEstimationScheme(); getModule<WindEstimation>()->stopWindEstimationSchemeCalibration(); @@ -274,14 +261,17 @@ State WingController::state_on_ground(const Boardcore::Event& event) return HANDLED; } + case EV_EXIT: { return HANDLED; } + case EV_EMPTY: { return tranSuper(&WingController::state_top); } + default: { return UNHANDLED; @@ -289,184 +279,227 @@ State WingController::state_on_ground(const Boardcore::Event& event) } } -bool WingController::addAlgorithms() +void WingController::inject(DependencyInjector& injector) { - 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); - 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); - 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(SELECTED_ALGORITHM); - - return result; + for (auto& algorithm : algorithms) + { + algorithm->inject(injector); + } + Super::inject(injector); } -bool WingController::selectAlgorithm(unsigned int index) +bool WingController::start() { - bool success = false; - // We change the selected algorithm only if we are in IDLE - if (getStatus().state == WingControllerState::IDLE) + auto& scheduler = getModule<BoardScheduler>()->wingController(); + + bool algoStarted = + std::all_of(algorithms.begin(), algorithms.end(), + [](auto& algorithm) { return algorithm->init(); }); + + if (!algoStarted) { - stopAlgorithm(); - if (index < algorithms.size()) - { - LOG_INFO(logger, "Algorithm {:1} selected", index); - selectedAlgorithm = index; - success = true; - } - else - { - // Select the 0 algorithm - selectedAlgorithm = 0; - success = false; - } + LOG_ERR(logger, "Failed to initialize wing algorithms"); + return false; + } + + auto updateTask = scheduler.addTask([this] { update(); }, UPDATE_RATE); + + if (updateTask == 0) + { + LOG_ERR(logger, "Failed to add wing controller update task"); + return false; + } + + if (!HSM::start()) + { + LOG_ERR(logger, "Failed to start WingController HSM active object"); + return false; } - return success; + + started = true; + return true; } -void WingController::startAlgorithm() +bool WingController::isStarted() { return started; } + +WingControllerState WingController::getState() { return state; } + +bool WingController::setTargetPosition(Eigen::Vector2f targetPositionGEO) { - // If the selected algorithm is valid --> also the - // algorithms array is not empty i start the whole thing - if (selectedAlgorithm < algorithms.size()) + // Allow changing the target position in the IDLE state only + if (state != WingControllerState::IDLE) { - running = true; + return false; + } - // Begin the selected algorithm - algorithms[selectedAlgorithm]->begin(); + this->targetPositionGEO = targetPositionGEO; - LOG_INFO(logger, "Wing algorithm started"); - } + auto data = WingTargetPositionData{ + .targetLat = targetPositionGEO[0], .targetLon = targetPositionGEO[1], + // TODO: populate early maneuver points + }; + Logger::getInstance().log(data); + + return true; } -void WingController::stopAlgorithm() +bool WingController::selectAlgorithm(uint8_t index) { - if (running) + // Allow changing the algorithm in the IDLE state only + if (state != WingControllerState::IDLE) { - // Set running to false - running = false; - // Stop the algorithm if selected - if (selectedAlgorithm < algorithms.size()) + return false; + } + + switch (index) + { + case static_cast<uint8_t>(AlgorithmId::EARLY_MANEUVER): + case static_cast<uint8_t>(AlgorithmId::CLOSED_LOOP): + case static_cast<uint8_t>(AlgorithmId::SEQUENCE): + case static_cast<uint8_t>(AlgorithmId::ROTATION): + { + selectedAlgorithm = static_cast<AlgorithmId>(index); + + auto data = WingControllerAlgorithmData{ + .timestamp = TimestampTimer::getTimestamp(), + .algorithm = index}; + Logger::getInstance().log(data); + + return true; + } + + default: { - algorithms[selectedAlgorithm]->end(); - reset(); + return false; } } } -void WingController::update() +void WingController::loadAlgorithms() { - if (running) + // Early Maneuver Guidance Automatic Algorithm + algorithms[static_cast<size_t>(AlgorithmId::EARLY_MANEUVER)] = + std::make_unique<AutomaticWingAlgorithm>( + PI::KP, PI::KI, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, + emGuidance); + + // Closed Loop Guidance Automatic Algorithm + algorithms[static_cast<size_t>(AlgorithmId::CLOSED_LOOP)] = + std::make_unique<AutomaticWingAlgorithm>( + PI::KP, PI::KI, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, + clGuidance); + + // Sequence { - algorithms[selectedAlgorithm]->step(); + auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO, + PARAFOIL_RIGHT_SERVO); + WingAlgorithmData step; + + step.timestamp = 0; + step.servo1Angle = 0; + step.servo2Angle = 120; + algorithm->addStep(step); + + step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count(); + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + + step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count(); + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + + algorithms[static_cast<size_t>(AlgorithmId::SEQUENCE)] = + std::move(algorithm); } -} -void WingController::flare() -{ - // Set the servo position to flare (pull the two ropes as skydiving people - // do) - getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f); - getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f); + // Rotation + { + auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO, + PARAFOIL_RIGHT_SERVO); + WingAlgorithmData step; + + 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); + + algorithms[static_cast<size_t>(AlgorithmId::ROTATION)] = + std::move(algorithm); + } } -void WingController::twirl() +WingAlgorithm& WingController::getCurrentAlgorithm() { - // Set the servo position to twirl - getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, TWIRL_RADIUS); - getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f); + auto index = static_cast<size_t>(selectedAlgorithm.load()); + return *algorithms[index].get(); } -void WingController::reset() +void WingController::startAlgorithm() { - // Set the servo position to reset - getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f); - getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f); + running = true; + updateEarlyManeuverPoints(); + + getCurrentAlgorithm().begin(); } -void WingController::setTargetPosition(Eigen::Vector2f targetGEO) +void WingController::stopAlgorithm() { - if (getModule<NASController>()->getState() == NASControllerState::ACTIVE) + if (running) { - this->targetPositionGEO = targetGEO; - setEarlyManeuverPoints(convertTargetPositionToNED(targetPositionGEO), - {getModule<NASController>()->getNasState().n, - getModule<NASController>()->getNasState().e}); + running = false; + + getCurrentAlgorithm().end(); + resetWing(); } } -void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED, - Eigen::Vector2f currentPosNED) +void WingController::updateEarlyManeuverPoints() { + using namespace Eigen; - Eigen::Vector2f targetOffsetNED = targetNED - currentPosNED; + auto nas = getModule<NASController>(); + auto nasState = nas->getNasState(); + auto nasRef = nas->getReferenceValues(); - Eigen::Vector2f norm_point = targetOffsetNED / targetOffsetNED.norm(); + Vector2f currentPositionNED = {nasState.n, nasState.e}; + Vector2f targetNED = Aeroutils::geodetic2NED( + targetPositionGEO, {nasRef.refLatitude, nasRef.refLongitude}); - float psi0 = atan2(norm_point[1], norm_point[0]); + Vector2f targetOffsetNED = targetNED - currentPositionNED; + Vector2f normPoint = targetOffsetNED / targetOffsetNED.norm(); + float psi0 = atan2(normPoint[1], normPoint[0]); float distFromCenterline = 20; // the distance that the M1 and M2 points // must have from the center line @@ -480,59 +513,71 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED, float m2Angle = psi0 + psiMan; float m1Angle = psi0 - psiMan; - Eigen::Vector2f emcPosition = - targetOffsetNED * 1.2 + - currentPosNED; // EMC is calculated as target * 1.2 + // EMC is calculated as target * 1.2 + Vector2f emcPosition = targetOffsetNED * 1.2 + currentPositionNED; - Eigen::Vector2f m1Position = - Eigen::Vector2f(cos(m1Angle), sin(m1Angle)) * maneuverPointsMagnitude + - currentPosNED; + Vector2f m1Position = + Vector2f{cos(m1Angle), sin(m1Angle)} * maneuverPointsMagnitude + + currentPositionNED; - Eigen::Vector2f m2Position = - Eigen::Vector2f(cos(m2Angle), sin(m2Angle)) * maneuverPointsMagnitude + - currentPosNED; + Vector2f m2Position = + Vector2f{cos(m2Angle), sin(m2Angle)} * maneuverPointsMagnitude + + currentPositionNED; emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position); - clGuidance.setPoints(targetNED); - WingTargetPositionData data; - - data.receivedLat = targetPositionGEO[0]; - data.receivedLon = targetPositionGEO[1]; - - data.targetN = targetNED[0]; - data.targetE = targetNED[1]; - - data.emcN = emcPosition[0]; - data.emcE = emcPosition[1]; - - data.m1N = m1Position[0]; - data.m1E = m1Position[1]; + // Log the updated points + auto data = WingTargetPositionData{ + .targetLat = targetPositionGEO[0], + .targetLon = targetPositionGEO[1], + .targetN = targetNED[0], + .targetE = targetNED[1], + .emcN = emcPosition[0], + .emcE = emcPosition[1], + .m1N = m1Position[0], + .m1E = m1Position[1], + .m2N = m2Position[0], + .m2E = m2Position[1], + }; + Logger::getInstance().log(data); +} - data.m2N = m2Position[0]; - data.m2E = m2Position[1]; +void WingController::update() +{ + if (running) + { + getCurrentAlgorithm().step(); + } +} - // Log the received position - Logger::getInstance().log(data); +void WingController::flareWing() +{ + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f); } -void WingController::logStatus(WingControllerState state) +void WingController::twirlWing() { - miosix::Lock<miosix::FastMutex> s(statusMutex); - status.timestamp = TimestampTimer::getTimestamp(); - status.state = state; + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, TWIRL_RADIUS); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f); +} - Logger::getInstance().log(status); +void WingController::resetWing() +{ + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f); } -Eigen::Vector2f WingController::convertTargetPositionToNED( - Eigen::Vector2f targetGEO) +void WingController::updateState(WingControllerState newState) { - return Aeroutils::geodetic2NED( - targetGEO, - {getModule<NASController>()->getReferenceValues().refLatitude, - getModule<NASController>()->getReferenceValues().refLongitude}); + state = newState; + + auto status = WingControllerStatus{ + .timestamp = TimestampTimer::getTimestamp(), + .state = newState, + }; + Logger::getInstance().log(status); } } // namespace Payload diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h index 2ff217581faafea179985931b2d07ee2ab6cb2c6..66874623bff274daff39d7381e80922d22bda4ff 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.h +++ b/src/boards/Payload/StateMachines/WingController/WingController.h @@ -1,5 +1,5 @@ /* Copyright (c) 2024 Skyward Experimental Rocketry - * Authors: Federico Mandelli, Angelo Prete + * 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 @@ -22,6 +22,7 @@ #pragma once +#include <Payload/Configs/WingConfig.h> #include <Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h> #include <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h> #include <Payload/Wing/WingAlgorithm.h> @@ -83,40 +84,41 @@ public: */ ~WingController(); - Boardcore::State state_idle(const Boardcore::Event& event); - Boardcore::State state_flying(const Boardcore::Event& event); - Boardcore::State state_calibration(const Boardcore::Event& event); - Boardcore::State state_controlled_descent(const Boardcore::Event& event); - Boardcore::State state_on_ground(const Boardcore::Event& event); + // HSM states + Boardcore::State Idle(const Boardcore::Event& event); + Boardcore::State Flying(const Boardcore::Event& event); + Boardcore::State FlyingCalibration(const Boardcore::Event& event); + Boardcore::State FlyingControlledDescent(const Boardcore::Event& event); + Boardcore::State OnGround(const Boardcore::Event& event); /** - * @brief Method to set the target position. + * @brief Override the inject method to inject dependencies into the + * algorithms, which are insantiated later than top-level modules. */ - void setTargetPosition(Eigen::Vector2f target); + void inject(Boardcore::DependencyInjector& injector) override; - /** - * @brief Selects the algorithm if present. - * - * @param index The algorithms vector index - * - * @return true if the selection was successful - */ - bool selectAlgorithm(unsigned int index); + bool start() override; - WingControllerStatus getStatus(); + bool isStarted(); - bool start() override; + WingControllerState getState(); /** - * @brief Override the inject method to inject dependencies into the - * algorithms, which are insantiated later than top-level modules. + * @brief Sets the target position. + * @note The provided position must be using geodetic coordinates. */ - void inject(Boardcore::DependencyInjector& injector) override; + bool setTargetPosition(Eigen::Vector2f targetPositionGEO); /** - * @brief This method is a forward method from early maneuvers guidance - * algorithm to calculate the yaw angle of the parafoil knowing the current - * position and the target position. + * @brief Changes the selected algorithm. + * @return Whether the provided index selected a valid algorithm. + */ + bool selectAlgorithm(uint8_t index); + + /** + * @brief This is a forward method to the early maneuvers guidance algorithm + * to calculate the yaw angle of the parafoil knowing the current position + * and the target position. * * @param[in] currentPositionNED the current NED position of the parafoil in * m @@ -124,7 +126,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) @@ -134,58 +135,75 @@ public: private: /** - * @brief Instantiates the algorithms and adds them to the algorithms - * vector. + * @brief Loads all algorithms. * * @note Algorithms should be instantiated before dependency injection so * that they can be injected with dependencies when the WingController is * injected. */ - bool addAlgorithms(); + void loadAlgorithms(); /** - * @brief target position getter + * @brief Returns the currently selected algorithm. */ - Eigen::Vector2f convertTargetPositionToNED(Eigen::Vector2f targetGEO); + WingAlgorithm& getCurrentAlgorithm(); /** - * @brief set points needed by the Guidance + * @brief Starts the currently selected algorithm. If the algorithm is + * already running, it resets the algorithm. */ - void setEarlyManeuverPoints(Eigen::Vector2f targetNED, - Eigen::Vector2f currentPosNED); - - void logStatus(WingControllerState state); - - WingControllerStatus status; + void startAlgorithm(); - miosix::FastMutex statusMutex; + /** + * @brief Stops the currently selected algorithm. + */ + void stopAlgorithm(); /** - * @brief Target position + * @brief Update early maneuver guidance points (EMC, M1, M2) based on the + * current position and the target position. */ - Eigen::Vector2f targetPositionGEO; + void updateEarlyManeuverPoints(); /** - * @brief List of loaded algorithms (from SD or not) + * @brief Periodic update method that steps the currently selected + * algorithm. */ - std::vector<WingAlgorithm*> algorithms; + void update(); /** - * @brief PrintLogger + * @brief Flare the wing. + * Pulls the two ropes as skydiving people do. */ - Boardcore::PrintLogger logger = - Boardcore::Logging::getLogger("WingController"); + void flareWing(); /** - * @brief Internal running state + * @brief Twirl the wing to the left. */ - std::atomic<bool> running; + void twirlWing(); + /** - * @brief This attribute is modified by the mavlink radio section. - * The user using the Ground Station can select the pre-enumerated algorithm - * to execute + * @brief Reset the wing to the initial position. */ - std::atomic<size_t> selectedAlgorithm; + void resetWing(); + + void updateState(WingControllerState newState); + + Eigen::Vector2f targetPositionGEO{Config::Wing::Default::TARGET_LAT, + Config::Wing::Default::TARGET_LON}; + + std::atomic<bool> targetPositionDirty{ + false}; ///< Whether the target position was changed and EM points need + ///< to be updated + + std::atomic<Config::Wing::AlgorithmId> selectedAlgorithm{ + Config::Wing::Default::ALGORITHM}; + + std::atomic<WingControllerState> state{WingControllerState::UNINIT}; + + std::array<std::unique_ptr<WingAlgorithm>, + static_cast<size_t>(Config::Wing::AlgorithmId::LAST)> + algorithms; ///< The available algorithms /** * @brief Instance of the Early Maneuver Guidance Algorithm used by @@ -199,30 +217,11 @@ private: */ ClosedLoopGuidanceAlgorithm clGuidance; - /** - * @brief starts the selected algorithm - */ - void startAlgorithm(); - - /** - * @brief Sets the internal state to stop and - * stops the selected algorithm - */ - void stopAlgorithm(); - - /** - * @brief Stops any on going algorithm and flares the wing - */ - void flare(); - - void twirl(); + std::atomic<bool> started{false}; + std::atomic<bool> running{false}; ///< Whether the algorithm is running - /** - * @brief Resets the servos in their initial position - */ - void reset(); - - void update(); + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("WingController"); }; } // namespace Payload diff --git a/src/boards/Payload/StateMachines/WingController/WingControllerData.h b/src/boards/Payload/StateMachines/WingController/WingControllerData.h index 82e444561b10465ed3c179b058b9198c5de7cf9a..0d175ea3530314d974a21c8a563781e7790fffa2 100644 --- a/src/boards/Payload/StateMachines/WingController/WingControllerData.h +++ b/src/boards/Payload/StateMachines/WingController/WingControllerData.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry - * Author: Federico Mandelli +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 @@ -22,8 +22,7 @@ #pragma once -#include <stdint.h> - +#include <cstdint> #include <iostream> #include <string> @@ -34,15 +33,15 @@ enum class WingControllerState : uint8_t { UNINIT = 0, IDLE, - CALIBRATION, - ALGORITHM_CONTROLLED, + FLYING_CALIBRATION, + FLYING_CONTROLLED_DESCENT, ON_GROUND, END }; struct WingControllerStatus { - long long timestamp = 0; + uint64_t timestamp = 0; WingControllerState state = WingControllerState::UNINIT; static std::string header() { return "timestamp,state\n"; } @@ -53,4 +52,17 @@ struct WingControllerStatus } }; +struct WingControllerAlgorithmData +{ + uint64_t timestamp = 0; + uint8_t algorithm = 0; + + static std::string header() { return "timestamp,algorithm\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)algorithm << "\n"; + } +}; + } // namespace Payload diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp index 8639f619ae60d2ae6282256284f4ce8d488956b3..278894a11ef4a050f97b6890b01ed023beb36339 100644 --- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp +++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp @@ -44,12 +44,13 @@ AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki, GuidanceAlgorithm& guidance) : Super(servo1, servo2), guidance(guidance) { - controller = new PIController(Kp, Ki, WING_UPDATE_PERIOD / 1000.0f, - PI_CONTROLLER_SATURATION_MIN_LIMIT, - PI_CONTROLLER_SATURATION_MAX_LIMIT); -} + // PIController needs the sample period in floating point seconds + auto samplePeriod = 1.0f / Hertz{UPDATE_RATE}.value(); -AutomaticWingAlgorithm::~AutomaticWingAlgorithm() { delete (controller); } + controller = std::make_unique<PIController>(Kp, Ki, samplePeriod, + PI::SATURATION_MIN_LIMIT, + PI::SATURATION_MAX_LIMIT); +} void AutomaticWingAlgorithm::step() { @@ -72,14 +73,14 @@ void AutomaticWingAlgorithm::step() if (result > 0) { // Activate the servo2 and reset servo1 - getModule<Actuators>()->setServoAngle(servo1, 0); + getModule<Actuators>()->setServoAngle(servo1, 0.0f); getModule<Actuators>()->setServoAngle(servo2, result); } else { // Activate the servo1 and reset servo2 - getModule<Actuators>()->setServoAngle(servo1, result * -1); - getModule<Actuators>()->setServoAngle(servo2, 0); + getModule<Actuators>()->setServoAngle(servo1, result * -1.0f); + getModule<Actuators>()->setServoAngle(servo2, 0.0f); } // Log the servo positions @@ -95,8 +96,8 @@ void AutomaticWingAlgorithm::step() else { // If we loose fix we set both servo at 0 - getModule<Actuators>()->setServoAngle(servo1, 0); - getModule<Actuators>()->setServoAngle(servo2, 0); + getModule<Actuators>()->setServoAngle(servo1, 0.0f); + getModule<Actuators>()->setServoAngle(servo2, 0.0f); } } diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.h b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h index 1b9b9fc23d6e27f178d099ffaaf13a898c45578d..7d3eb87c73634bae18b78a4a541928a21ab1c5af 100644 --- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.h +++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h @@ -56,17 +56,12 @@ public: AutomaticWingAlgorithm(float Kp, float Ki, ServosList servo1, ServosList servo2, GuidanceAlgorithm& guidance); - /** - * @brief Destroy the Automatic Wing Algorithm object and the PI - */ - ~AutomaticWingAlgorithm(); - protected: // Guidance algorithm that sets the yaw. GuidanceAlgorithm& guidance; // PI controller tuned on the Kp and Ki passed through constructor - Boardcore::PIController* controller; + std::unique_ptr<Boardcore::PIController> controller; /** * @brief Actual algorithm implementation, all parameters should be in NED diff --git a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp index d1d3e3a25667e7e2038c746a14b5bb339c2545d0..698f5f6cb04493188ee52a8b65b7ba5df712e028 100644 --- a/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp +++ b/src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp @@ -70,18 +70,18 @@ float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle( void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude) { if (altitude <= - GUIDANCE_TARGET_ALTITUDE_THRESHOLD) // Altitude is low, head directly - // to target + Guidance::TARGET_ALTITUDE_THRESHOLD) // Altitude is low, head directly + // to target { targetAltitudeConfidence++; } - else if (altitude <= GUIDANCE_M2_ALTITUDE_THRESHOLD) // Altitude is almost - // okay, go to M2 + else if (altitude <= Guidance::M2_ALTITUDE_THRESHOLD) // Altitude is almost + // okay, go to M2 { m2AltitudeConfidence++; } else if (altitude <= - GUIDANCE_M1_ALTITUDE_THRESHOLD) // Altitude is high, go to M1 + Guidance::M1_ALTITUDE_THRESHOLD) // Altitude is high, go to M1 { m1AltitudeConfidence++; } @@ -93,41 +93,44 @@ void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude) switch (activeTarget) { case Target::EMC: - if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE) + if (targetAltitudeConfidence >= Guidance::CONFIDENCE) { activeTarget = Target::FINAL; emcAltitudeConfidence = 0; } - else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE) + else if (m2AltitudeConfidence >= Guidance::CONFIDENCE) { activeTarget = Target::M2; emcAltitudeConfidence = 0; } - else if (m1AltitudeConfidence >= GUIDANCE_CONFIDENCE) + else if (m1AltitudeConfidence >= Guidance::CONFIDENCE) { activeTarget = Target::M1; emcAltitudeConfidence = 0; } break; + case Target::M1: - if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE) + if (targetAltitudeConfidence >= Guidance::CONFIDENCE) { activeTarget = Target::FINAL; m1AltitudeConfidence = 0; } - else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE) + else if (m2AltitudeConfidence >= Guidance::CONFIDENCE) { activeTarget = Target::M2; m1AltitudeConfidence = 0; } break; + case Target::M2: - if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE) + if (targetAltitudeConfidence >= Guidance::CONFIDENCE) { activeTarget = Target::FINAL; m2AltitudeConfidence = 0; } break; + case Target::FINAL: break; } diff --git a/src/boards/Payload/Wing/WingAlgorithmData.h b/src/boards/Payload/Wing/WingAlgorithmData.h index 38e20628c332a5965e0a50e62381d6df61fc4ab8..6bf746d62e4ea2270c3c4d629ab54f22cec77258 100644 --- a/src/boards/Payload/Wing/WingAlgorithmData.h +++ b/src/boards/Payload/Wing/WingAlgorithmData.h @@ -31,15 +31,15 @@ namespace Payload */ struct WingAlgorithmData { - uint64_t timestamp; // First timestamp is 0 (in microseconds) - float servo1Angle; // degrees - float servo2Angle; // degrees - float targetAngle; // radians (automatic only) - float velocityAngle; // radians - float targetX; // NED (only automatic algorithm) - float targetY; // NED (only automatic algorithm) - float error; - float pidOutput; + uint64_t timestamp = 0; // First timestamp is 0 (in microseconds) + float servo1Angle = 0; // degrees + float servo2Angle = 0; // degrees + float targetAngle = 0; // radians (automatic only) + float velocityAngle = 0; // radians + float targetX = 0; // NED (only automatic algorithm) + float targetY = 0; // NED (only automatic algorithm) + float error = 0; + float pidOutput = 0; static std::string header() { diff --git a/src/boards/Payload/Wing/WingTargetPositionData.h b/src/boards/Payload/Wing/WingTargetPositionData.h index cb27979246a74c72cc61a22c67b13b6b2c89c634..1f875b5fa979be3726c58ca67dd92b7bd6c4c463 100644 --- a/src/boards/Payload/Wing/WingTargetPositionData.h +++ b/src/boards/Payload/Wing/WingTargetPositionData.h @@ -30,28 +30,28 @@ namespace Payload struct WingTargetPositionData { - float receivedLat; - float receivedLon; - float targetN; - float targetE; - float emcN; - float emcE; - float m1N; - float m1E; - float m2N; - float m2E; + float targetLat = 0; + float targetLon = 0; + float targetN = 0; + float targetE = 0; + float emcN = 0; + float emcE = 0; + float m1N = 0; + float m1E = 0; + float m2N = 0; + float m2E = 0; static std::string header() { - return "receivedLat, receivedLon, " - "targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n"; + return "targetLat, " + "targetLon,targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n"; } void print(std::ostream& os) const { - os << receivedLat << "," << receivedLon << "," << targetN << "," - << targetE << "," << emcN << "," << emcE << "," << m1N << "," << m1E - << "," << m2N << "," << m2E << "\n"; + os << targetLat << "," << targetLon << "," << targetN << "," << targetE + << "," << emcN << "," << emcE << "," << m1N << "," << m1E << "," + << m2N << "," << m2E << "\n"; } }; diff --git a/src/scripts/logdecoder/Payload/logdecoder.cpp b/src/scripts/logdecoder/Payload/logdecoder.cpp index 3a120781dd3919dca7baf5cc0b685b19dc2bdee4..1919157dd9cca085391ade4962bb3324dd84b721 100644 --- a/src/scripts/logdecoder/Payload/logdecoder.cpp +++ b/src/scripts/logdecoder/Payload/logdecoder.cpp @@ -68,6 +68,7 @@ void registerTypes(Deserializer& ds) ds.registerType<DynamicPressureData>(); ds.registerType<SensorsCalibrationParameter>(); ds.registerType<PinChangeData>(); + ds.registerType<WingControllerAlgorithmData>(); ds.registerType<WingAlgorithmData>(); ds.registerType<WingTargetPositionData>(); ds.registerType<WindLogging>();