diff --git a/.vscode/settings.json b/.vscode/settings.json index f4302917253850581626c8aebaa0f2f98370e74b..9f619c4c25cfc6271094ec17fa8889e1437a618a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -148,6 +148,7 @@ "Duca", "Eigen", "EXTI", + "funv", "Gpio", "GPIOF", "GPIOG", @@ -155,6 +156,7 @@ "JESOLO", "KALM", "kalman", + "ldlt", "Leds", "Lolli", "magcal", diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 6aa1c5f3cb9c85bb9f6ae98a04b336d73c76b218..d840a11fc0ab6e0f4f62d6d084b71595f20b47ac 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -120,7 +120,17 @@ set (LYRA_GS ) set(PARAFOIL_COMPUTER + src/Parafoil/Actuators/Actuators.cpp + src/Parafoil/AltitudeTrigger/AltitudeTrigger.cpp + src/Parafoil/PinHandler/PinHandler.cpp src/Parafoil/Sensors/Sensors.cpp - # src/Parafoil/StateMachines/NASController/NASController.cpp src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp + src/Parafoil/StateMachines/NASController/NASController.cpp + src/Parafoil/StateMachines/WingController/WingController.cpp + src/Parafoil/WindEstimation/WindEstimation.cpp + src/Parafoil/Wing/AutomaticWingAlgorithm.cpp + src/Parafoil/Wing/FileWingAlgorithm.cpp + src/Parafoil/Wing/WingAlgorithm.cpp + src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp + src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp ) \ No newline at end of file diff --git a/src/Parafoil/Actuators/Actuators.cpp b/src/Parafoil/Actuators/Actuators.cpp index 02324701b39434240c40d66b4184080468742e67..b0cd1f8f8732878e347bed63e520f109cc74fc0d 100644 --- a/src/Parafoil/Actuators/Actuators.cpp +++ b/src/Parafoil/Actuators/Actuators.cpp @@ -26,7 +26,6 @@ #include <Parafoil/Configs/ActuatorsConfig.h> #include <interfaces-impl/hwmapping.h> -using namespace std::chrono; using namespace miosix; using namespace Boardcore; using namespace Boardcore::Units::Angle; @@ -52,10 +51,6 @@ Actuators::Actuators() bool Actuators::start() { - using namespace std::chrono; - - auto& scheduler = getModule<BoardScheduler>()->actuators(); - leftServo.servo->enable(); rightServo.servo->enable(); diff --git a/src/Parafoil/Actuators/Actuators.h b/src/Parafoil/Actuators/Actuators.h index 673b6bb81de5fa9d89d68b5734835d6c55a04e39..7554f2c585d3f427d245965b749883f32ae7dd81 100644 --- a/src/Parafoil/Actuators/Actuators.h +++ b/src/Parafoil/Actuators/Actuators.h @@ -42,8 +42,9 @@ public: struct ServoActuator { std::unique_ptr<Boardcore::Servo> servo; - Units::Angle::Degree fullRangeAngle = - Units::Angle::Degree(0); ///< The full range of the servo [degrees] + Boardcore::Units::Angle::Degree fullRangeAngle = + Boardcore::Units::Angle::Degree( + 0); ///< The full range of the servo [degrees] miosix::FastMutex mutex; }; @@ -69,7 +70,8 @@ public: * @param angle Angle to set [degree]. * @return True if the the angle was set. */ - bool setServoAngle(ServosList servoId, Units::Angle::Degree angle); + bool setServoAngle(ServosList servoId, + Boardcore::Units::Angle::Degree angle); /** * @brief Wiggles the servo for few seconds. @@ -114,7 +116,7 @@ public: * @return float current Servo angle in range [0-180], (-1) if the servoId * is invalid. */ - Units::Angle::Degree getServoAngle(ServosList servoId); + Boardcore::Units::Angle::Degree getServoAngle(ServosList servoId); /** * @brief Starts twirl (one servo is set to 0 and the other one is not). diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h index 4e8d51eefd7d589cb62c3554ef21a91b72e04c01..53a885511600d374961ecaf88bf8cfce72e9ff3a 100644 --- a/src/Parafoil/BoardScheduler.h +++ b/src/Parafoil/BoardScheduler.h @@ -56,6 +56,8 @@ public: Boardcore::TaskScheduler& sensors() { return high; } Boardcore::TaskScheduler& pinHandler() { return high; } Boardcore::TaskScheduler& altitudeTrigger() { return medium; } + Boardcore::TaskScheduler& wingController() { return medium; } + Boardcore::TaskScheduler& windEstimation() { return low; } Boardcore::TaskScheduler& actuators() { return low; } static Priority::PriorityLevel flightModeManagerPriority() diff --git a/src/Parafoil/Configs/FlightModeManagerConfig.h b/src/Parafoil/Configs/FlightModeManagerConfig.h index 2aed44f469f3af11af476dcdd1ea02c0a462378c..e616a5c8a9eeb148bcc100bbe00ac93eeb80acfd 100644 --- a/src/Parafoil/Configs/FlightModeManagerConfig.h +++ b/src/Parafoil/Configs/FlightModeManagerConfig.h @@ -22,6 +22,8 @@ #pragma once +#include <units/Time.h> + #include <chrono> namespace Parafoil @@ -31,10 +33,10 @@ namespace Config namespace FlightModeManager { -/* linter-off */ using namespace std::chrono_literals; +/* linter-off */ using namespace Boardcore::Units::Time; -constexpr auto LOGGING_DELAY = 5s; -constexpr auto CONTROL_DELAY = 5s; +constexpr auto LOGGING_DELAY = 5_s; +constexpr auto CONTROL_DELAY = 5_s; } // namespace FlightModeManager } // namespace Config diff --git a/src/Parafoil/Configs/NASConfig.h b/src/Parafoil/Configs/NASConfig.h index 6b0ee310d9c593c692d9ee02d17e13cb6648a957..6a9e041dcbbd471d71a8659ae36e9029b6a59d8f 100644 --- a/src/Parafoil/Configs/NASConfig.h +++ b/src/Parafoil/Configs/NASConfig.h @@ -24,7 +24,9 @@ #include <algorithms/NAS/NASConfig.h> #include <common/ReferenceConfig.h> +#include <units/Acceleration.h> #include <units/Frequency.h> +#include <units/Time.h> namespace Parafoil { @@ -33,16 +35,18 @@ namespace Config namespace NAS { +/* linter off */ using namespace Boardcore::Units::Time; /* linter off */ using namespace Boardcore::Units::Frequency; +/* linter off */ using namespace Boardcore::Units::Acceleration; -constexpr Hertz UPDATE_RATE = 50_hz; -constexpr float UPDATE_RATE_SECONDS = 0.02; // [s] +constexpr auto UPDATE_RATE = 50_hz; +constexpr auto UPDATE_RATE_SECONDS = 0.02_s; -constexpr int CALIBRATION_SAMPLES_COUNT = 20; -constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms] +constexpr int CALIBRATION_SAMPLES_COUNT = 20; +constexpr auto CALIBRATION_SLEEP_TIME = 100_ms; static const Boardcore::NASConfig CONFIG = { - .T = UPDATE_RATE_SECONDS, + .T = UPDATE_RATE_SECONDS.value(), .SIGMA_BETA = 0.0001, .SIGMA_W = 0.0019, .SIGMA_ACC = 0.202, @@ -61,17 +65,14 @@ static const Boardcore::NASConfig CONFIG = { .SATS_NUM = 6.0, .NED_MAG = Common::ReferenceConfig::nedMag}; -// TODO: make sure this is correct! - // Only use one out of every 50 samples (1 Hz) constexpr int MAGNETOMETER_DECIMATE = 50; // Maximum allowed acceleration to correct with GPS -constexpr float DISABLE_GPS_ACCELERATION = 34.0f; // [m/s^2]. +constexpr auto DISABLE_GPS_ACCELERATION_THRESHOLD = 34.0_mps2; -// How much confidence (in m/s^2) to apply to the accelerometer to check if it -// is 1g -constexpr float ACCELERATION_1G_CONFIDENCE = 0.5; +// How much confidence to apply to the accelerometer to check if it is 1g +constexpr auto ACCELERATION_1G_CONFIDENCE = 0.5_mps2; // How many samples will determine that we are in fact measuring gravity // acceleration constexpr int ACCELERATION_1G_SAMPLES = 20; diff --git a/src/Parafoil/Configs/WESConfig.h b/src/Parafoil/Configs/WESConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..4f6521100da10291c9e906b04aa8573af900c410 --- /dev/null +++ b/src/Parafoil/Configs/WESConfig.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Mandelli, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <miosix.h> +#include <units/Time.h> + +namespace Parafoil +{ + +namespace Config +{ + +/** + * Configuration for the Wing Estimation Scheme (WES) algorithm. + */ +namespace WES +{ + +/* linter-off */ using namespace Boardcore::Units::Time; + +constexpr auto CALIBRATE = false; + +constexpr auto CALIBRATION_TIMEOUT = 5_s; // time needed for the first loop +constexpr auto ROTATION_PERIOD = 10_s; +constexpr auto CALIBRATION_SAMPLE_NUMBER = + 20; // number of samples to take in the first loop +constexpr auto CALIBRATION_UPDATE_PERIOD = + CALIBRATION_TIMEOUT / CALIBRATION_SAMPLE_NUMBER; +constexpr auto PREDICTION_UPDATE_PERIOD = 100_ms; // update period of WES[ms] + +} // namespace WES + +} // namespace Config + +} // namespace Parafoil diff --git a/src/Parafoil/Configs/WingConfig.h b/src/Parafoil/Configs/WingConfig.h index 68315dc5c836ea5fbb04069ab551eb630f8f2d5a..f1fde1c1aa8a4062f6f53552ac522473809d6869 100644 --- a/src/Parafoil/Configs/WingConfig.h +++ b/src/Parafoil/Configs/WingConfig.h @@ -25,10 +25,9 @@ #include <units/Angle.h> #include <units/Frequency.h> #include <units/Length.h> +#include <units/Time.h> #include <utils/Constants.h> -#include <chrono> - namespace Parafoil { namespace Config @@ -36,15 +35,17 @@ namespace Config namespace Wing { -/* linter off */ using namespace std::chrono_literals; /* linter off */ using namespace Boardcore::Units::Frequency; /* linter off */ using namespace Boardcore::Units::Length; +/* linter off */ using namespace Boardcore::Units::Time; /* linter off */ using namespace Boardcore::Units::Angle; -constexpr auto UPDATE_RATE = 1_hz; -constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15s; -constexpr auto COMMAND_PERIOD = 6s; -constexpr auto WING_DECREMENT = 30_deg; +constexpr auto UPDATE_RATE = 1_hz; +constexpr auto TARGET_UPDATE_RATE = 10_hz; +constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15_s; +constexpr auto PROGRESSIVE_ROTATION_TIMEOUT = 5_s; +constexpr auto COMMAND_PERIOD = 6_s; +constexpr auto WING_DECREMENT = 30_deg; /** * @brief The available algorithms for the wing controller. @@ -56,6 +57,7 @@ enum class AlgorithmId : size_t SEQUENCE, ROTATION, PROGRESSIVE_ROTATION, + LAST ///< Used to count the number of algorithms }; namespace Default diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp index 6e6048716f00ad61f50e0aa9c859c64cf31b24a2..94c15de3a31690dddb8bd37545efd3da453bd1b7 100644 --- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp +++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -31,7 +31,7 @@ using namespace Boardcore; using namespace Common; -using namespace std::chrono; +using namespace Boardcore::Units::Time; namespace config = Parafoil::Config::FlightModeManager; namespace Parafoil @@ -448,7 +448,7 @@ State FlightModeManager::FlyingWingDescent(const Event& event) // Send the event to the WingController controlDelayId = EventBroker::getInstance().postDelayed( FLIGHT_WING_DESCENT, TOPIC_FLIGHT, - milliseconds{config::CONTROL_DELAY}.count()); + Millisecond{config::CONTROL_DELAY}.value()); return HANDLED; } @@ -492,7 +492,7 @@ State FlightModeManager::Landed(const Event& event) TOPIC_FLIGHT); EventBroker::getInstance().postDelayed( FMM_STOP_LOGGING, TOPIC_FMM, - milliseconds{config::LOGGING_DELAY}.count()); + Millisecond{config::LOGGING_DELAY}.value()); return HANDLED; } @@ -543,4 +543,4 @@ void FlightModeManager::updateState(FlightModeManagerState newState) Logger::getInstance().log(status); } -} // namespace Parafoil \ No newline at end of file +} // namespace Parafoil diff --git a/src/Parafoil/StateMachines/NASController/NASController.cpp b/src/Parafoil/StateMachines/NASController/NASController.cpp index c4cf7f5c9b17a0c726a06207862abb91b537b705..3317629072692bf12b5f11f539934044f53eed75 100644 --- a/src/Parafoil/StateMachines/NASController/NASController.cpp +++ b/src/Parafoil/StateMachines/NASController/NASController.cpp @@ -31,6 +31,8 @@ #include <utils/SkyQuaternion/SkyQuaternion.h> using namespace Boardcore; +using namespace Boardcore::Units::Time; +using namespace Boardcore::Units::Acceleration; using namespace Eigen; using namespace Common; namespace config = Parafoil::Config::NAS; @@ -220,7 +222,7 @@ void NASController::calibrate() baroSum += baro.pressure; - Thread::sleep(config::CALIBRATION_SLEEP_TIME); + Thread::sleep(Millisecond{config::CALIBRATION_SLEEP_TIME}.value()); } Vector3f meanAcc = accSum / config::CALIBRATION_SAMPLES_COUNT; @@ -239,7 +241,7 @@ void NASController::calibrate() ReferenceValues reference = nas.getReferenceValues(); reference.refPressure = meanBaro; reference.refAltitude = Aeroutils::relAltitude( - reference.refPressure, reference.mslPressure, reference.mslTemperature); + reference.refPressure, reference.mslPressure, reference.mslTemperature); // Also update the reference with the GPS if we have fix UBXGPSData gps = sensors->getUBXGPSLastSample(); @@ -261,9 +263,7 @@ void NASController::update() { // Update the NAS state only if the FSM is active if (state != NASControllerState::ACTIVE) - { return; - } auto* sensors = getModule<Sensors>(); @@ -272,8 +272,8 @@ void NASController::update() auto baro = sensors->getLPS22DFLastSample(); // Calculate acceleration - Vector3f acc = static_cast<AccelerometerData>(imu); - float accLength = acc.norm(); + Vector3f acc = static_cast<AccelerometerData>(imu); + auto accLength = MeterPerSecondSquared{acc.norm()}; miosix::Lock<miosix::FastMutex> l(nasMutex); @@ -282,36 +282,28 @@ void NASController::update() nas.predictAcc(imu); if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 && - accLength < Config::NAS::DISABLE_GPS_ACCELERATION) + accLength < Config::NAS::DISABLE_GPS_ACCELERATION_THRESHOLD) { nas.correctGPS(gps); } if (lastBaroTimestamp < baro.pressureTimestamp) - { nas.correctBaro(baro.pressure); - } // Correct with accelerometer if the acceleration is in specs if (lastAccTimestamp < imu.accelerationTimestamp && acc1g) - { nas.correctAcc(imu); - } // Check if the accelerometer is measuring 1g - if (accLength < - (Constants::g + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) && - accLength > - (Constants::g - Config::NAS::ACCELERATION_1G_CONFIDENCE / 2)) + if (accLength < (MeterPerSecondSquared{G{1}} + + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) && + accLength > (MeterPerSecondSquared{G{1}} - + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2)) { if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES) - { acc1gSamplesCount++; - } else - { acc1g = true; - } } else { @@ -326,7 +318,7 @@ void NASController::update() lastBaroTimestamp = baro.pressureTimestamp; auto state = nas.getState(); - auto ref = nas.getReferenceValues(); + // auto ref = nas.getReferenceValues(); Logger::getInstance().log(state); } diff --git a/src/Parafoil/StateMachines/WingController/WingController.cpp b/src/Parafoil/StateMachines/WingController/WingController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..90ab7e46e88bf061afda0b42e8763c70bb91d377 --- /dev/null +++ b/src/Parafoil/StateMachines/WingController/WingController.cpp @@ -0,0 +1,619 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#include "WingController.h" + +#include <Parafoil/BoardScheduler.h> +#include <Parafoil/Configs/ActuatorsConfig.h> +#include <Parafoil/Configs/WESConfig.h> +#include <Parafoil/Configs/WingConfig.h> +#include <Parafoil/StateMachines/NASController/NASController.h> +#include <Parafoil/WindEstimation/WindEstimation.h> +#include <Parafoil/Wing/AutomaticWingAlgorithm.h> +#include <Parafoil/Wing/FileWingAlgorithm.h> +#include <Parafoil/Wing/WingAlgorithm.h> +#include <Parafoil/Wing/WingAlgorithmData.h> +#include <Parafoil/Wing/WingTargetPositionData.h> +#include <common/Events.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <units/Length.h> +#include <units/Time.h> + +using namespace Boardcore; +using namespace Boardcore::Units::Time; +using namespace Boardcore::Units::Length; +using namespace Common; +using namespace Parafoil::Config; + +namespace Parafoil +{ +WingController::WingController() + : 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); + + loadAlgorithms(); +} + +WingController::~WingController() +{ + EventBroker::getInstance().unsubscribe(this); +} + +State WingController::Idle(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(WingControllerState::IDLE); + return HANDLED; + } + + case FLIGHT_WING_DESCENT: + { + return transition(&WingController::Flying); + } + + case EV_EMPTY: + { + return tranSuper(&WingController::state_top); + } + + default: + { + return UNHANDLED; + } + } +} + +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::FlyingCalibration); + } + case FLIGHT_LANDING_DETECTED: + { + return transition(&WingController::OnGround); + } + default: + { + return UNHANDLED; + } + } +} + +State WingController::FlyingCalibration(const Boardcore::Event& event) +{ + static uint16_t calibrationTimeoutEventId; + + switch (event) + { + case EV_ENTRY: // starts twirling and calibration wes + { + updateState(WingControllerState::FLYING_CALIBRATION); + + 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::Flying); + } + case DPL_SERVO_ACTUATION_DETECTED: + { + resetWing(); + calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( + DPL_WIGGLE, TOPIC_DPL, 1000); + + return HANDLED; + } + case DPL_WIGGLE: + { + flareWing(); + calibrationTimeoutEventId = EventBroker::getInstance().postDelayed( + DPL_NC_OPEN, TOPIC_DPL, 2000); + + return HANDLED; + } + case DPL_NC_OPEN: + { + resetWing(); + if (WES::CALIBRATE) + { + calibrationTimeoutEventId = + EventBroker::getInstance().postDelayed( + DPL_WES_CAL_DONE, TOPIC_DPL, + Millisecond{WES::CALIBRATION_TIMEOUT}.value()); + getModule<WindEstimation>()->startAlgorithm(); + + getModule<Actuators>()->startTwirl(); + } + + return transition(&WingController::FlyingControlledDescent); + } + case DPL_WES_CAL_DONE: + { + getModule<Actuators>()->stopTwirl(); + + return transition(&WingController::FlyingControlledDescent); + } + default: + { + return UNHANDLED; + } + } +} + +State WingController::FlyingControlledDescent(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: // start automatic algorithms + { + updateState(WingControllerState::FLYING_CONTROLLED_DESCENT); + + startAlgorithm(); + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&WingController::Flying); + } + case WING_ALGORITHM_ENDED: + { + return transition(&WingController::OnGround); + } + case EV_EXIT: + { + stopAlgorithm(); + + getModule<WindEstimation>()->stopAlgorithm(); + getModule<WindEstimation>()->stopCalibration(); + + return HANDLED; + } + default: + { + return UNHANDLED; + } + } +} + +State WingController::OnGround(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(WingControllerState::ON_GROUND); + + // disable servos + getModule<Actuators>()->disableServo(PARAFOIL_LEFT_SERVO); + getModule<Actuators>()->disableServo(PARAFOIL_RIGHT_SERVO); + + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&WingController::state_top); + } + default: + { + return UNHANDLED; + } + } +} + +void WingController::inject(DependencyInjector& injector) +{ + for (auto& algorithm : algorithms) + algorithm->inject(injector); + Super::inject(injector); +} + +bool WingController::start() +{ + auto& scheduler = getModule<BoardScheduler>()->wingController(); + + bool algoStarted = std::all_of( + algorithms.begin(), algorithms.end(), + [](const std::unique_ptr<Parafoil::WingAlgorithm>& algorithm) + { return algorithm->init(); }); + + if (!algoStarted) + { + LOG_ERR(logger, "Failed to initialize wing algorithms"); + return false; + } + + auto updateTask = + scheduler.addTask([this] { update(); }, Wing::UPDATE_RATE); + + if (updateTask == 0) + { + LOG_ERR(logger, "Failed to start wing controller update task"); + 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(Meter{altitude}); + }, + Wing::TARGET_UPDATE_RATE); + + 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"); + return false; + } + + started = true; + return true; +} + +bool WingController::isStarted() { return started; } + +WingControllerState WingController::getState() { return state; } + +Eigen::Vector2f WingController::getTargetCoordinates() +{ + return targetPositionGEO.load(); +} + +bool WingController::setTargetCoordinates(float latitude, float longitude) +{ + // Allow changing the target position in the IDLE state only + if (state != WingControllerState::IDLE) + return false; + + targetPositionGEO = Coordinates{latitude, longitude}; + + // Log early maneuver points to highlight any discrepancies if any + auto earlyManeuverPoints = getEarlyManeuverPoints(); + + auto data = WingTargetPositionData{ + .targetLat = latitude, + .targetLon = longitude, + .targetN = earlyManeuverPoints.targetN, + .targetE = earlyManeuverPoints.targetE, + .emcN = earlyManeuverPoints.emcN, + .emcE = earlyManeuverPoints.emcE, + .m1N = earlyManeuverPoints.m1N, + .m1E = earlyManeuverPoints.m1E, + .m2N = earlyManeuverPoints.m2N, + .m2E = earlyManeuverPoints.m2E, + }; + Logger::getInstance().log(data); + + return true; +} + +bool WingController::selectAlgorithm(Wing::AlgorithmId id) +{ + // Allow changing the algorithm in the IDLE state only + if (state != WingControllerState::IDLE) + return false; + + switch (id) + { + case Wing::AlgorithmId::EARLY_MANEUVER: + case Wing::AlgorithmId::CLOSED_LOOP: + case Wing::AlgorithmId::ROTATION: + { + selectedAlgorithm = id; + + auto data = WingControllerAlgorithmData{ + .timestamp = TimestampTimer::getTimestamp(), + .algorithm = static_cast<uint8_t>(id), + }; + Logger::getInstance().log(data); + + return true; + } + + default: + { + return false; + } + } +} + +EarlyManeuversPoints WingController::getEarlyManeuverPoints() +{ + return emGuidance.getPoints(); +} + +Eigen::Vector2f WingController::getActiveTarget() +{ + return emGuidance.getActiveTarget(); +} + +void WingController::loadAlgorithms() +{ + using namespace Wing; + using namespace Actuators; + + // 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); + + // 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); + + // Sequence + { + auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO, + PARAFOIL_RIGHT_SERVO); + WingAlgorithmData step; + + step.timestamp = 0; + step.servo1Angle = 0_deg; + step.servo2Angle = 120_deg; + algorithm->addStep(step); + + step.timestamp += Microsecond{STRAIGHT_FLIGHT_TIMEOUT}.value(); + step.servo1Angle = 0_deg; + step.servo2Angle = 0_deg; + algorithm->addStep(step); + + step.timestamp += Microsecond{STRAIGHT_FLIGHT_TIMEOUT}.value(); + step.servo1Angle = 0_deg; + step.servo2Angle = 0_deg; + algorithm->addStep(step); + + algorithms[static_cast<size_t>(AlgorithmId::SEQUENCE)] = + std::move(algorithm); + } + + // 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_deg; + algorithm->addStep(step); + + step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value(); + step.servo1Angle = 0_deg; + step.servo2Angle = RightServo::ROTATION / 2; + algorithm->addStep(step); + + step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value(); + step.servo1Angle = 0_deg; + step.servo2Angle = 0_deg; + algorithm->addStep(step); + + step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value(); + step.servo1Angle = LeftServo::ROTATION; + step.servo2Angle = RightServo::ROTATION; + algorithm->addStep(step); + + step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value(); + step.servo1Angle = 0_deg; + step.servo2Angle = RightServo::ROTATION / 2; + algorithm->addStep(step); + + step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value(); + step.servo1Angle = 0_deg; + step.servo2Angle = 0_deg; + algorithm->addStep(step); + + step.timestamp += Microsecond{WES::ROTATION_PERIOD}.value(); + step.servo1Angle = 0_deg; + step.servo2Angle = 0_deg; + algorithm->addStep(step); + + algorithms[static_cast<size_t>(AlgorithmId::ROTATION)] = + std::move(algorithm); + } + + // Progressive rotation + { + auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO, + PARAFOIL_RIGHT_SERVO); + WingAlgorithmData step; + + step.timestamp = Microsecond(PROGRESSIVE_ROTATION_TIMEOUT).value(); + + for (auto angle = 150_deg; angle >= 0_deg; angle -= WING_DECREMENT) + { + step.servo1Angle = angle; + step.servo2Angle = 0_deg; + algorithm->addStep(step); + step.timestamp += Microsecond(COMMAND_PERIOD).value(); + + step.servo1Angle = 0_deg; + step.servo2Angle = angle; + algorithm->addStep(step); + step.timestamp += Microsecond(COMMAND_PERIOD).value(); + } + + algorithms[static_cast<size_t>(AlgorithmId::PROGRESSIVE_ROTATION)] = + std::move(algorithm); + } +} + +WingAlgorithm& WingController::getCurrentAlgorithm() +{ + auto index = static_cast<size_t>(selectedAlgorithm.load()); + return *algorithms[index].get(); +} + +void WingController::startAlgorithm() +{ + updateEarlyManeuverPoints(); + running = true; + + getCurrentAlgorithm().begin(); +} + +void WingController::stopAlgorithm() +{ + if (running) + { + running = false; + + getCurrentAlgorithm().end(); + } +} + +void WingController::updateEarlyManeuverPoints() +{ + using namespace Eigen; + + auto nas = getModule<NASController>(); + auto nasState = nas->getNasState(); + auto nasRef = nas->getReferenceValues(); + auto targetGEO = targetPositionGEO.load(); + + Vector2f currentPositionNED = {nasState.n, nasState.e}; + Vector2f targetNED = Aeroutils::geodetic2NED( + targetGEO, {nasRef.refLatitude, nasRef.refLongitude}); + + Vector2f targetOffsetNED = targetNED - currentPositionNED; + Vector2f normPoint = targetOffsetNED / targetOffsetNED.norm(); + float psi0 = atan2(normPoint.y(), normPoint.x()); + + float distFromCenterline = 20; // the distance that the M1 and M2 + // points must have from the center line + + // Calculate the angle between the lines <NED Origin, target> and <NED + // Origin, M1> This angle is the same for M2 since is symmetric to M1 + // relatively to the center line + float psiMan = atan2(distFromCenterline, targetOffsetNED.norm()); + + float maneuverPointsMagnitude = distFromCenterline / sin(psiMan); + float m2Angle = psi0 + psiMan; + float m1Angle = psi0 - psiMan; + + // EMC is calculated as target * 1.2 + Vector2f emcPosition = targetOffsetNED * 1.2 + currentPositionNED; + + Vector2f m1Position = + Vector2f{cos(m1Angle), sin(m1Angle)} * maneuverPointsMagnitude + + currentPositionNED; + + Vector2f m2Position = + Vector2f{cos(m2Angle), sin(m2Angle)} * maneuverPointsMagnitude + + currentPositionNED; + + emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position); + clGuidance.setPoints(targetNED); + + // Log the updated points + auto data = WingTargetPositionData{ + .targetLat = targetGEO.latitude, + .targetLon = targetGEO.longitude, + .targetN = targetNED.x(), + .targetE = targetNED.y(), + .emcN = emcPosition.x(), + .emcE = emcPosition.y(), + .m1N = m1Position.x(), + .m1E = m1Position.y(), + .m2N = m2Position.x(), + .m2E = m2Position.y(), + }; + Logger::getInstance().log(data); +} + +void WingController::update() +{ + if (running) + getCurrentAlgorithm().step(); +} + +void WingController::flareWing() +{ + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 1.0f); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 1.0f); +} + +void WingController::resetWing() +{ + getModule<Actuators>()->setServoPosition(PARAFOIL_LEFT_SERVO, 0.0f); + getModule<Actuators>()->setServoPosition(PARAFOIL_RIGHT_SERVO, 0.0f); +} + +void WingController::updateState(WingControllerState newState) +{ + state = newState; + + auto status = WingControllerStatus{ + .timestamp = TimestampTimer::getTimestamp(), + .state = newState, + }; + Logger::getInstance().log(status); +} + +} // namespace Parafoil diff --git a/src/Parafoil/StateMachines/WingController/WingController.h b/src/Parafoil/StateMachines/WingController/WingController.h new file mode 100644 index 0000000000000000000000000000000000000000..a38d95ece554b46e72d0acd7a15a377f90173ef2 --- /dev/null +++ b/src/Parafoil/StateMachines/WingController/WingController.h @@ -0,0 +1,267 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Parafoil/Configs/WingConfig.h> +#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h> +#include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h> +#include <Parafoil/Wing/WingAlgorithm.h> +#include <diagnostic/PrintLogger.h> +#include <events/HSM.h> +#include <utils/DependencyManager/DependencyManager.h> + +#include <Eigen/Core> +#include <atomic> + +#include "WingControllerData.h" + +/** + * @brief This class allows the user to select the wing algorithm + * that has to be used during the tests. It also registers his + * dedicated function in the task scheduler in order to be + * executed every fixed period and to update the two servos position + * depending on the selected algorithm. + * + * Use case example: + * controller = new WingController(scheduler); + * + * controller.addAlgorithm("filename"); + * OR + * controller.addAlgorithm(algorithm); + * + * controller.selectAlgorithm(index); + * + * controller.start(); + * controller.stop(); //If you want to abort the execution + * controller.start(); //If you want to start again from the beginning + */ + +namespace Parafoil +{ +class BoardScheduler; +class Actuators; +class NASController; +class FlightStatsRecorder; +class WindEstimation; + +/** + * State machine that manages the wings of the Parafoil. + * + * HSM Schema: + * + * Idle + * + * Flying + * ├── FlyingCalibration + * ├── FlyingDeployment + * └── FlyingControlledDescent + * + * OnGround + */ +class WingController + : public Boardcore::HSM<WingController>, + public Boardcore::InjectableWithDeps<BoardScheduler, Actuators, + NASController, FlightStatsRecorder, + WindEstimation> +{ +public: + /** + * @brief Initializes the wing controller. + * + * Sets the initial FSM state to idle, subscribes to DPL and flight events + * and instantiates the wing algorithms. + */ + WingController(); + + /** + * @brief Destroy the Wing Controller object. + */ + ~WingController(); + + // 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 FlyingDeployment(const Boardcore::Event& event); + Boardcore::State FlyingControlledDescent(const Boardcore::Event& event); + Boardcore::State OnGround(const Boardcore::Event& event); + + /** + * @brief Override the inject method to inject dependencies into the + * algorithms, which are instantiated later than top-level modules. + */ + void inject(Boardcore::DependencyInjector& injector) override; + + bool start() override; + + bool isStarted(); + + WingControllerState getState(); + + /** + * @brief Returns the target coordinates. + * @return The GEO coordinates of the active target. + */ + Eigen::Vector2f getTargetCoordinates(); + + /** + * @brief Sets the target coordinates. + */ + bool setTargetCoordinates(float latitude, float longitude); + + /** + * @brief Returns the selected algorithm. + */ + uint8_t getSelectedAlgorithm(); + + /** + * @brief Changes the selected algorithm. + * @return Whether the provided index selected a valid algorithm. + */ + bool selectAlgorithm(Config::Wing::AlgorithmId id); + + /** + * @brief Returns the currently set early maneuver points. + */ + EarlyManeuversPoints getEarlyManeuverPoints(); + + /** + * @brief Returns the early maneuver active target. + */ + Eigen::Vector2f getActiveTarget(); + + /** + * @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 + * @param[out] heading Saves the heading vector for logging purposes + * + * @returns the yaw angle of the parafoil in rad + */ + Boardcore::Units::Angle::Radian calculateTargetAngle( + const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading) + { + return emGuidance.calculateTargetAngle(currentPositionNED, heading); + } + +private: + /** + * @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. + */ + void loadAlgorithms(); + + /** + * @brief Returns the currently selected algorithm. + */ + WingAlgorithm& getCurrentAlgorithm(); + + /** + * @brief Starts the currently selected algorithm. If the algorithm is + * already running, it resets the algorithm. + */ + void startAlgorithm(); + + /** + * @brief Stops the currently selected algorithm. + */ + void stopAlgorithm(); + + /** + * @brief Update early maneuver guidance points (EMC, M1, M2) based on the + * current position and the target position. + */ + void updateEarlyManeuverPoints(); + + /** + * @brief Periodic update method that steps the currently selected + * algorithm. + */ + void update(); + + /** + * @brief Flare the wing. + * Pulls the two ropes as skydiving people do. + */ + void flareWing(); + + /** + * @brief Twirl the wing to the left. + */ + void twirlWing(); + + /** + * @brief Reset the wing to the initial position. + */ + void resetWing(); + + void updateState(WingControllerState newState); + + struct Coordinates + { + float latitude; + float longitude; + + operator Eigen::Vector2f() const { return {latitude, longitude}; } + }; + + std::atomic<Coordinates> targetPositionGEO{Coordinates{ + .latitude = Config::Wing::Default::TARGET_LAT, + .longitude = Config::Wing::Default::TARGET_LON, + }}; + + std::atomic<Config::Wing::AlgorithmId> selectedAlgorithm{ + Config::Wing::Default::ALGORITHM}; + + std::atomic<WingControllerState> state{WingControllerState::IDLE}; + + 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 + * AutomaticWingAlgorithm + */ + EarlyManeuversGuidanceAlgorithm emGuidance; + + /** + * @brief Instance of the Closed Loop Guidance Algorithm used by + * AutomaticWingAlgorithm + */ + ClosedLoopGuidanceAlgorithm clGuidance; + + std::atomic<bool> started{false}; + std::atomic<bool> running{false}; ///< Whether the algorithm is running + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("WingController"); +}; + +} // namespace Parafoil diff --git a/src/Parafoil/StateMachines/WingController/WingControllerData.h b/src/Parafoil/StateMachines/WingController/WingControllerData.h new file mode 100644 index 0000000000000000000000000000000000000000..1b239e58463db25f9d5692b5cbc326cbe493c8f0 --- /dev/null +++ b/src/Parafoil/StateMachines/WingController/WingControllerData.h @@ -0,0 +1,67 @@ +/* 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 + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <cstdint> +#include <iostream> +#include <string> + +namespace Parafoil +{ + +enum class WingControllerState : uint8_t +{ + IDLE = 0, + FLYING_CALIBRATION, + FLYING_DEPLOYMENT, + FLYING_CONTROLLED_DESCENT, + ON_GROUND, +}; + +struct WingControllerStatus +{ + uint64_t timestamp = 0; + WingControllerState state = WingControllerState::IDLE; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +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 Parafoil diff --git a/src/Parafoil/WindEstimation/WindEstimation.cpp b/src/Parafoil/WindEstimation/WindEstimation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3fec873993b0dd8a8f3db23ca6d87515607cbaf6 --- /dev/null +++ b/src/Parafoil/WindEstimation/WindEstimation.cpp @@ -0,0 +1,271 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Mandelli, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "WindEstimation.h" + +#include <Parafoil/BoardScheduler.h> +#include <Parafoil/Configs/WESConfig.h> +#include <Parafoil/Sensors/Sensors.h> + +using namespace Boardcore; +using namespace Parafoil::Config; +using namespace Units::Speed; + +namespace Parafoil +{ +WindEstimation::WindEstimation() { funv << 1.0f, 0.0f, 0.0f, 1.0f; } + +WindEstimation::~WindEstimation() +{ + stopAlgorithm(); + stopCalibration(); +} + +bool WindEstimation::start() +{ + auto& scheduler = getModule<BoardScheduler>()->windEstimation(); + + auto calibrationTask = scheduler.addTask([this] { updateCalibration(); }, + WES::CALIBRATION_UPDATE_PERIOD); + + if (calibrationTask == 0) + { + LOG_ERR(logger, "Failed to start wind estimation calibration task"); + return false; + } + + auto algorithmTask = scheduler.addTask([this] { updateAlgorithm(); }, + WES::PREDICTION_UPDATE_PERIOD); + + if (algorithmTask == 0) + { + LOG_ERR(logger, "Failed to start wind estimation algorithm task"); + return false; + } + + started = true; + return true; +} + +bool WindEstimation::isStarted() { return started; } + +void WindEstimation::startCalibration() +{ + if (!calibrating) + { + calibrating = true; + nSampleCalibration = 0; + + velocity.vn = 0_mps; + velocity.ve = 0_mps; + + speedSquared = 0; + + LOG_INFO(logger, "Wind estimation calibration started"); + } + else + { + LOG_WARN(logger, "Wind estimation calibration already started"); + } +} + +void WindEstimation::stopCalibration() +{ + if (calibrating) + { + LOG_INFO(logger, "Wind estimation calibration stopped"); + + if (!running) + { + miosix::Lock<FastMutex> l(mutex); + wind = calibratingWind; + } + else + { + LOG_WARN(logger, + "Wind estimation algorithm is running, calibration wind " + "will not be applied"); + } + + logStatus(); + calibrating = false; + } + else + { + LOG_WARN(logger, "Wind estimation calibration already stopped"); + } +} + +void WindEstimation::updateCalibration() +{ + if (calibrating) + { + auto gps = getModule<Sensors>()->getUBXGPSLastSample(); + + if (gps.fix != 0) + { + if (nSampleCalibration < WES::CALIBRATION_SAMPLE_NUMBER) + { + auto gpsVelocity = + GeoVelocity{MeterPerSecond{gps.velocityNorth}, + MeterPerSecond{gps.velocityEast}}; + + calibrationMatrix(nSampleCalibration, 0) = + gpsVelocity.vn.value(); + calibrationMatrix(nSampleCalibration, 1) = + gpsVelocity.ve.value(); + calibrationV2(nSampleCalibration) = gpsVelocity.normSquared(); + + velocity.vn += gpsVelocity.vn; + velocity.ve += gpsVelocity.ve; + speedSquared += gpsVelocity.normSquared(); + + nSampleCalibration++; + } + else + { + velocity.vn /= nSampleCalibration; + velocity.ve /= nSampleCalibration; + speedSquared /= nSampleCalibration; + + // Update calibration matrix + for (int i = 0; i < nSampleCalibration; i++) + { + calibrationMatrix(i, 0) = + calibrationMatrix(i, 0) - velocity.vn.value(); + calibrationMatrix(i, 1) = + calibrationMatrix(i, 1) - velocity.ve.value(); + calibrationV2(i) = 0.5f * (calibrationV2(i) - speedSquared); + } + + Eigen::MatrixXf calibrationMatrixT = + calibrationMatrix.transpose(); + auto calibration = + (calibrationMatrixT * calibrationMatrix) + .ldlt() + .solve(calibrationMatrixT * calibrationV2); + calibratingWind.vn = MeterPerSecond(calibration(0)); + calibratingWind.ve = MeterPerSecond(calibration(1)); + + stopCalibration(); + startAlgorithm(); + } + } + } +} + +void WindEstimation::startAlgorithm() +{ + if (!running) + { + running = true; + nSampleAlgorithm = nSampleCalibration; + + LOG_INFO(logger, "Wind estimation algorithm started"); + } + else + { + LOG_WARN(logger, "Wind estimation algorithm already started"); + } +} + +void WindEstimation::stopAlgorithm() +{ + if (running) + { + running = false; + + LOG_INFO(logger, "Wind estimation algorithm stopped"); + } + else + { + LOG_WARN(logger, "Wind estimation algorithm already stopped"); + } +} + +void WindEstimation::updateAlgorithm() +{ + if (running) + { + auto gps = getModule<Sensors>()->getUBXGPSLastSample(); + if (gps.fix != 0) + { + Eigen::Vector2f phi; + Eigen::Matrix<float, 1, 2> phiT; + Eigen::Vector2f temp; + + float y; + + auto gpsVelocity = GeoVelocity{MeterPerSecond{gps.velocityNorth}, + MeterPerSecond{gps.velocityEast}}; + + // update avg + nSampleAlgorithm++; + velocity.vn = (velocity.vn * nSampleAlgorithm + gpsVelocity.vn) / + (nSampleAlgorithm + 1); + velocity.ve = (velocity.ve * nSampleAlgorithm + gpsVelocity.ve) / + (nSampleAlgorithm + 1); + speedSquared = (speedSquared * nSampleAlgorithm + + (gpsVelocity.normSquared())) / + (nSampleAlgorithm + 1); + phi(0) = gpsVelocity.vn.value() - velocity.vn.value(); + phi(1) = gpsVelocity.ve.value() - velocity.ve.value(); + y = 0.5f * ((gpsVelocity.normSquared()) - speedSquared); + + phiT = phi.transpose(); + funv = + (funv - (funv * phi * phiT * funv) / (1 + (phiT * funv * phi))); + temp = (0.5 * (funv + funv.transpose()) * phi) * + (y - phiT * getPrediction().asVector()); + + { + miosix::Lock<FastMutex> l(mutex); + wind.vn = MeterPerSecond(wind.vn.value() + temp(0)); + wind.ve = MeterPerSecond(wind.ve.value() + temp(1)); + } + + logStatus(); + } + } +} + +GeoVelocity WindEstimation::getPrediction() +{ + miosix::Lock<FastMutex> l(mutex); + return wind; +} + +void WindEstimation::logStatus() +{ + auto timestamp = TimestampTimer::getTimestamp(); + + WindEstimationData data{ + .timestamp = timestamp, + .velocityNorth = wind.vn, + .velocityEast = wind.ve, + .calibration = calibrating, + }; + + Logger::getInstance().log(data); +} + +} // namespace Parafoil diff --git a/src/Parafoil/WindEstimation/WindEstimation.h b/src/Parafoil/WindEstimation/WindEstimation.h new file mode 100644 index 0000000000000000000000000000000000000000..a9277c71f6eafc25c54bf53bee7396903cc586ee --- /dev/null +++ b/src/Parafoil/WindEstimation/WindEstimation.h @@ -0,0 +1,134 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Mandelli, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Parafoil/BoardScheduler.h> +#include <Parafoil/Configs/WESConfig.h> +#include <Parafoil/Sensors/Sensors.h> +#include <diagnostic/PrintLogger.h> +#include <logger/Logger.h> + +#include <Eigen/Core> +#include <atomic> +#include <utils/ModuleManager/ModuleManager.hpp> + +#include "WindEstimationData.h" + +namespace Parafoil +{ +class BoardScheduler; +class Sensors; + +/** + * @brief This class implements the Wind Estimation Scheme(WES) prediction + * algorithm, the first part is the initial setup, and then the continuos + * algorithms runs; + */ +class WindEstimation + : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors> +{ +public: + WindEstimation(); + + ~WindEstimation(); + + /** + * @brief Starts the wind estimation module, subscribing the calibration and + * algorithm update tasks to the scheduler. + * @return true if the module started correctly, false otherwise. + */ + bool start(); + + bool isStarted(); + + /** + * @brief Starts the calibration of the wind estimation. + */ + void startCalibration(); + + /** + * @brief Stops the calibration of the wind estimation. Overrides the + * current wind estimation with the calibrated one. + */ + void stopCalibration(); + + /** + * @brief Starts the algorithm of the wind estimation. + */ + void startAlgorithm(); + + /** + * @brief Stops the algorithm of the wind estimation. + */ + void stopAlgorithm(); + + /** + * @brief Returns the current wind estimation prediction. + */ + GeoVelocity getPrediction(); + +private: + /** + * @brief Updates the calibration matrix with the new calibration values. + * Automatically stops the calibration when the calibration matrix is full + * and starts the algorithm. + */ + void updateCalibration(); + + /** + * @brief Updates the wind matrix with the updated wind prediction values. + */ + void updateAlgorithm(); + + /** + * @brief Logs the current prediction. + */ + void logStatus(); + + GeoVelocity calibratingWind; ///< Wind currently being calibrated + GeoVelocity wind; ///< Wind after calibration + + // Calibration & Algorithm variables + uint8_t nSampleCalibration = 0; + uint8_t nSampleAlgorithm = 0; + + Eigen::Matrix<float, Config::WES::CALIBRATION_SAMPLE_NUMBER, 2> + calibrationMatrix; + Eigen::Vector<float, Config::WES::CALIBRATION_SAMPLE_NUMBER> calibrationV2; + Eigen::Matrix2f funv; + + GeoVelocity velocity; ///< Wind velocity (North, East) + float speedSquared{0}; ///< Wind speed squared + + miosix::FastMutex mutex; + + // Status variables + std::atomic<bool> started{false}; + std::atomic<bool> running{false}; ///< Whether the algorithm is running + std::atomic<bool> calibrating{ + false}; ///< Whether the algorithm is being calibrated + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("WindEstimation"); +}; +} // namespace Parafoil diff --git a/src/Parafoil/WindEstimation/WindEstimationData.h b/src/Parafoil/WindEstimation/WindEstimationData.h new file mode 100644 index 0000000000000000000000000000000000000000..60e9668095b11b24d5cf3883a85eb4ea40c76e92 --- /dev/null +++ b/src/Parafoil/WindEstimation/WindEstimationData.h @@ -0,0 +1,73 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <units/Speed.h> + +#include <Eigen/Core> +#include <iostream> +#include <string> + +namespace Parafoil +{ + +/** + * Wind Estimation Scheme data. + */ +struct WindEstimationData +{ + uint64_t timestamp = 0; + Boardcore::Units::Speed::MeterPerSecond velocityNorth; + Boardcore::Units::Speed::MeterPerSecond velocityEast; + bool calibration = false; ///< True if the wind estimation is in + ///< calibration mode, false otherwise + + static std::string header() + { + return "timestamp,velocityNorth,velocityEast,calibration\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << velocityNorth << "," << velocityEast << "," + << calibration << "\n "; + } +}; + +struct GeoVelocity +{ + Boardcore::Units::Speed::MeterPerSecond vn{0}; // North velocity + Boardcore::Units::Speed::MeterPerSecond ve{0}; // East velocity + + /** + * @brief Calculate the squared norm of the velocity vector. + */ + float normSquared() const + { + return vn.value() * vn.value() + ve.value() * ve.value(); + } + + Eigen::Vector2f asVector() const { return {vn.value(), ve.value()}; } +}; + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3c750418f2be5a0290e11b73ac3231ea6de2c7a4 --- /dev/null +++ b/src/Parafoil/Wing/AutomaticWingAlgorithm.cpp @@ -0,0 +1,162 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Niccolò Betto, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "AutomaticWingAlgorithm.h" + +#include <Parafoil/Configs/WingConfig.h> +#include <Parafoil/Sensors/Sensors.h> +#include <Parafoil/StateMachines/NASController/NASController.h> +#include <Parafoil/StateMachines/WingController/WingController.h> +#include <drivers/timer/TimestampTimer.h> +#include <math.h> +#include <utils/AeroUtils/AeroUtils.h> +#include <utils/Constants.h> + +using namespace Boardcore; +using namespace Eigen; +using namespace Parafoil::Config::Wing; +using namespace Units::Angle; + +namespace Parafoil +{ +AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki, + ServosList servo1, + ServosList servo2, + GuidanceAlgorithm& guidance) + : Super(servo1, servo2), guidance(guidance) +{ + // PIController needs the sample period in floating point seconds + auto samplePeriod = 1.0f / UPDATE_RATE.value(); + + controller = std::make_unique<PIController>(Kp, Ki, samplePeriod, + PI::SATURATION_MIN_LIMIT, + PI::SATURATION_MAX_LIMIT); +} + +void AutomaticWingAlgorithm::step() +{ + if (getModule<Sensors>()->getUBXGPSLastSample().fix == 3) + { + // The PI calculated result + auto result = algorithmStep(getModule<NASController>()->getNasState()); + + // Actuate the result + // To see how to interpret the PI output + // https://www.geogebra.org/calculator/xrhwarpz + // + // Reference system + // N ^ + // | + // | + // ----> E + if (result > 0_deg) + { + // Activate the servo2 and reset servo1 + getModule<Actuators>()->setServoAngle(servo1, 0_deg); + getModule<Actuators>()->setServoAngle(servo2, result); + } + else + { + // Activate the servo1 and reset servo2 + getModule<Actuators>()->setServoAngle(servo1, -result); + getModule<Actuators>()->setServoAngle(servo2, 0_deg); + } + + // Log the servo positions + { + miosix::Lock<FastMutex> l(mutex); + + data.timestamp = TimestampTimer::getTimestamp(); + data.servo1Angle = getModule<Actuators>()->getServoAngle(servo1); + data.servo2Angle = getModule<Actuators>()->getServoAngle(servo2); + SDlogger->log(data); + } + } + else + { + // If we loose fix we set both servo at 0 + getModule<Actuators>()->setServoAngle(servo1, 0_deg); + getModule<Actuators>()->setServoAngle(servo2, 0_deg); + } +} + +Degree AutomaticWingAlgorithm::algorithmStep(const NASState& state) +{ + // For some algorithms the third component is needed! + Vector3f currentPosition(state.n, state.e, state.d); + + Vector2f heading; // used for logging purposes + + auto targetAngle = guidance.calculateTargetAngle(currentPosition, heading); + + Vector2f relativeVelocity(state.vn, state.ve); + + // Compute the angle of the current velocity + // All angle are computed as angle from the north direction + auto velocityAngle = + Radian(atan2(relativeVelocity[1], relativeVelocity[0])); + + // Compute the angle difference + auto error = angleDiff(targetAngle, velocityAngle); + + // Call the PI with the just calculated error. The result is in RADIANS, + // if positive we activate one servo, if negative the other + // We also need to convert the result from radians back to degrees + auto result = Degree(Radian(controller->update(error.value()))); + + // Logs the outputs + { + miosix::Lock<FastMutex> l(mutex); + data.targetX = heading[0]; + data.targetY = heading[1]; + data.targetAngle = targetAngle; + data.velocityAngle = velocityAngle; + data.error = error; + data.pidOutput = result; + } + + return result; +} + +Radian AutomaticWingAlgorithm::angleDiff(Radian a, Radian b) +{ + auto diff = (a - b).value(); + + // Angle difference + if (diff < -Constants::PI || Constants::PI < diff) + { + diff += Constants::PI; + bool positiveInput = diff > 0; + + diff = diff - floor(diff / (2 * Constants::PI)) * (2 * Constants::PI); + + // diff = fmod(diff, 2 * Constants::PI); + if (diff == 0 && positiveInput) + diff = 2 * Constants::PI; + + diff -= Constants::PI; + } + + return Radian{diff}; +} + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/AutomaticWingAlgorithm.h b/src/Parafoil/Wing/AutomaticWingAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..8074a44670352a508aab439ec51392e74e2eb856 --- /dev/null +++ b/src/Parafoil/Wing/AutomaticWingAlgorithm.h @@ -0,0 +1,101 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Matteo Pignataro, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#pragma once + +#include <Parafoil/Wing/Guidance/GuidanceAlgorithm.h> +#include <Parafoil/Wing/WingAlgorithm.h> +#include <algorithms/NAS/NASState.h> +#include <algorithms/PIController.h> +#include <algorithms/ReferenceValues.h> +#include <units/Angle.h> +#include <utils/DependencyManager/DependencyManager.h> + +#include <Eigen/Core> + +namespace Parafoil +{ + +class Sensors; +class NASController; +class Actuators; + +class AutomaticWingAlgorithm : public Boardcore::InjectableWithDeps< + Boardcore::InjectableBase<WingAlgorithm>, + Sensors, NASController, Actuators> +{ +public: + /** + * @brief Construct a new Automatic Wing Algorithm object + * + * @param Kp Proportional value for PI controller + * @param Ki Integral value for PI controller + * @param servo1 The first servo + * @param servo2 The second servo + * @param guidance The algorithm used to compute the target yaw and the + * heading + */ + AutomaticWingAlgorithm(float Kp, float Ki, ServosList servo1, + ServosList servo2, GuidanceAlgorithm& guidance); + +protected: + // Guidance algorithm that sets the yaw. + GuidanceAlgorithm& guidance; + + // PI controller tuned on the Kp and Ki passed through constructor + std::unique_ptr<Boardcore::PIController> controller; + + /** + * @brief Actual algorithm implementation, all parameters should be in NED + * + * @return The angle to set to the servo, positive is right and negative is + * left + */ + Boardcore::Units::Angle::Degree algorithmStep( + const Boardcore::NASState& state); + + /** + * @brief This method implements the automatic algorithm that will steer the + * parafoil according to its position and velocity. IN THIS METHOD THE + * GUIDANCE IS TRANSLATED + */ + void step() override; + + /** + * @brief Computes the difference between two angles + * + * @param a The first angle + * @param b The second angle + * + * @returns angle(a) - angle(b) + */ + Boardcore::Units::Angle::Radian angleDiff( + Boardcore::Units::Angle::Radian a, Boardcore::Units::Angle::Radian b); + + // Logging structure + WingAlgorithmData data; + + /** + * @brief Mutex + */ + miosix::FastMutex mutex; +}; +} // namespace Parafoil diff --git a/src/Parafoil/Wing/FileWingAlgorithm.cpp b/src/Parafoil/Wing/FileWingAlgorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b30c7bb20da1d2aedccef538f437885ff9483cd7 --- /dev/null +++ b/src/Parafoil/Wing/FileWingAlgorithm.cpp @@ -0,0 +1,67 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "FileWingAlgorithm.h" + +#include <drivers/timer/TimestampTimer.h> + +using namespace Boardcore; + +namespace Parafoil +{ +std::istream& operator>>(std::istream& input, WingAlgorithmData& data) +{ + input >> data.timestamp; + input.ignore(1, ','); + input >> data.servo1Angle; + input.ignore(1, ','); + input >> data.servo2Angle; + return input; +} + +FileWingAlgorithm::FileWingAlgorithm(ServosList servo1, ServosList servo2, + const char* filename) + : WingAlgorithm(servo1, servo2), parser(filename) +{ + setServo(servo1, servo2); +} + +bool FileWingAlgorithm::init() +{ + // Returns a std::vector which contains + // all the csv parsed with the data structure in mind + steps = parser.collect(); + + // Return if the size collected is greater than 0 + fileValid = steps.size() > 0; + + // Communicate it via serial + if (fileValid) + LOG_INFO(logger, "File valid"); + + // Close the file + parser.close(); + + return fileValid; +} + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/FileWingAlgorithm.h b/src/Parafoil/Wing/FileWingAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..5b2d61f792906695735ab30ba778ce8db22f28cc --- /dev/null +++ b/src/Parafoil/Wing/FileWingAlgorithm.h @@ -0,0 +1,91 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once +#include <Parafoil/Wing/WingAlgorithm.h> +#include <utils/CSVReader/CSVReader.h> + +/** + * @brief This class abstracts the concept of wing timestamp based + * algorithm. These algorithms are stored in files (formatted in csv). + * We use a CSV parser to properly parse the procedure and every step + * we check if it is time to advance and in case actuate the step with + * the two servos. + * + * Brief use case: + * + * Actuators::getInstance().enableServo(PARAFOIL_LEFT_SERVO); + * Actuators::getInstance().enableServo(PARAFOIL_RIGHT_SERVO); + * + * WingAlgorithm algorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, "Optional + * File") algorithm.init(); + * + * //In case of a non valid file/empty string you can add the steps + * algorithm.addStep(WingAlgorithmData{timestamp, angle1, angle2}); + * + * algorithm.begin(); + * + * algorithm.update()... + * + * //End of algorithm + * + * algorithm.begin(); + * + * algorithm.update()... + */ + +namespace Parafoil +{ + +class FileWingAlgorithm : public WingAlgorithm +{ +public: + /** + * @brief Construct a new Wing Algorithm object + * + * @param servo1 The first servo + * @param servo2 The second servo + * @param filename The csv file where all the operations are stored + */ + FileWingAlgorithm(ServosList servo1, ServosList servo2, + const char* filename); + + /** + * @brief This method parses the file and stores it into a std::vector + * + * @return true If the initialization finished correctly + * @return false If something went wrong + */ + bool init() override; + +protected: + /** + * @brief CSV format file parser + */ + Boardcore::CSVParser<WingAlgorithmData> parser; + + /** + * @brief Indicates whether the current file of the algorithm is readable + */ + bool fileValid = false; +}; +} // namespace Parafoil diff --git a/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9686dc8180947288a89d9f7d8c232fb5e1062c98 --- /dev/null +++ b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp @@ -0,0 +1,44 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Radu Raul, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h> +#include <math.h> + +using namespace Boardcore::Units::Angle; + +namespace Parafoil +{ + +Radian ClosedLoopGuidanceAlgorithm::calculateTargetAngle( + const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading) +{ + heading[0] = targetNED[0] - currentPositionNED[0]; + heading[1] = targetNED[1] - currentPositionNED[1]; + return Radian{atan2(heading[1], heading[0])}; +} + +void ClosedLoopGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED) +{ + this->targetNED = targetNED; +} + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..50a053edfb3b6866e312670eaf42a2aa9e3acd86 --- /dev/null +++ b/src/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h @@ -0,0 +1,57 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Radu Raul + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Parafoil/Wing/Guidance/GuidanceAlgorithm.h> + +#include <Eigen/Core> + +namespace Parafoil +{ + +/** + * This class is the implementation of the Simple Closed Loop guidance. + * It calculates the yaw between the current position and the target position by + * calculating the difference between the two vectors and the angle formed by + * the diff vector + */ +class ClosedLoopGuidanceAlgorithm : public GuidanceAlgorithm +{ + /** + * @brief This method calculates the yaw angle of the parafoil knowing + * the current position and the target position. + * + * @param[in] position the current NED position of the parafoil in [m] + * @param[out] heading Saves the heading vector for logging purposes + * + * @returns the yaw angle of the parafoil in [rad] + */ + Boardcore::Units::Angle::Radian calculateTargetAngle( + const Eigen::Vector3f& currentPositionNED, + Eigen::Vector2f& heading) override; + +public: + void setPoints(Eigen::Vector2f targetNED); +}; + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..85011723cbdeabaffa89a8c05c7f243c28ee98a5 --- /dev/null +++ b/src/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp @@ -0,0 +1,198 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Radu Raul, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Parafoil/Configs/WingConfig.h> +#include <Parafoil/Wing/WingTargetPositionData.h> +#include <drivers/timer/TimestampTimer.h> +#include <logger/Logger.h> + +#include <Eigen/Core> + +#include "EarlyManeuversGuidanceAlgorithm.h" + +using namespace Boardcore; +using namespace Parafoil::Config::Wing; + +namespace Parafoil +{ + +EarlyManeuversGuidanceAlgorithm::EarlyManeuversGuidanceAlgorithm() + : activeTarget(Target::EMC), targetAltitudeConfidence(0), + m2AltitudeConfidence(0), m1AltitudeConfidence(0), + emcAltitudeConfidence(0){}; + +EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){}; + +Radian EarlyManeuversGuidanceAlgorithm::calculateTargetAngle( + const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading) +{ + switch (activeTarget) + { + case Target::EMC: + heading[0] = EMC[0] - currentPositionNED[0]; + heading[1] = EMC[1] - currentPositionNED[1]; + break; + case Target::M1: + heading[0] = M1[0] - currentPositionNED[0]; + heading[1] = M1[1] - currentPositionNED[1]; + break; + case Target::M2: + heading[0] = M2[0] - currentPositionNED[0]; + heading[1] = M2[1] - currentPositionNED[1]; + break; + case Target::FINAL: + heading[0] = targetNED[0] - currentPositionNED[0]; + heading[1] = targetNED[1] - currentPositionNED[1]; + break; + } + + return Radian{static_cast<float>(atan2(heading[1], heading[0]))}; +} + +void EarlyManeuversGuidanceAlgorithm::updateActiveTarget(Meter altitude) +{ + if (altitude <= + 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 + { + m2AltitudeConfidence++; + } + else if (altitude <= + Guidance::M1_ALTITUDE_THRESHOLD) // Altitude is high, go to M1 + { + m1AltitudeConfidence++; + } + else + { + emcAltitudeConfidence++; // Altitude is too high, head to the emc + } + + auto currentTarget = activeTarget.load(); + auto newTarget = currentTarget; + + switch (currentTarget) + { + case Target::EMC: + if (targetAltitudeConfidence >= Guidance::CONFIDENCE) + { + newTarget = Target::FINAL; + emcAltitudeConfidence = 0; + } + else if (m2AltitudeConfidence >= Guidance::CONFIDENCE) + { + newTarget = Target::M2; + emcAltitudeConfidence = 0; + } + else if (m1AltitudeConfidence >= Guidance::CONFIDENCE) + { + newTarget = Target::M1; + emcAltitudeConfidence = 0; + } + break; + + case Target::M1: + if (targetAltitudeConfidence >= Guidance::CONFIDENCE) + { + newTarget = Target::FINAL; + m1AltitudeConfidence = 0; + } + else if (m2AltitudeConfidence >= Guidance::CONFIDENCE) + { + newTarget = Target::M2; + m1AltitudeConfidence = 0; + } + break; + + case Target::M2: + if (targetAltitudeConfidence >= Guidance::CONFIDENCE) + { + newTarget = Target::FINAL; + m2AltitudeConfidence = 0; + } + break; + + 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, + Eigen::Vector2f EMC, + Eigen::Vector2f M1, + Eigen::Vector2f M2) +{ + this->targetNED = targetNED; + this->EMC = EMC; + this->M1 = M1; + this->M2 = M2; +} + +EarlyManeuversPoints EarlyManeuversGuidanceAlgorithm::getPoints() +{ + return EarlyManeuversPoints{ + .targetN = targetNED[0], + .targetE = targetNED[1], + .emcN = EMC[0], + .emcE = EMC[1], + .m1N = M1[0], + .m1E = M1[1], + .m2N = M2[0], + .m2E = M2[1], + }; +} + +Eigen::Vector2f EarlyManeuversGuidanceAlgorithm::getActiveTarget() +{ + switch (activeTarget) + { + case Target::EMC: + return EMC; + case Target::M1: + return M1; + case Target::M2: + return M2; + case Target::FINAL: + return targetNED; + default: + return {0, 0}; + } +} + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..b1f341203571974327b5116de32c97cdc67223a7 --- /dev/null +++ b/src/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h @@ -0,0 +1,126 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Radu Raul, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Parafoil/Wing/Guidance/GuidanceAlgorithm.h> +#include <units/Length.h> + +#include <Eigen/Core> + +namespace Parafoil +{ + +struct EarlyManeuversPoints +{ + float targetN; + float targetE; + float emcN; + float emcE; + float m1N; + float m1E; + float m2N; + float m2E; +}; + +/** + * This class is the implementation of the Simple Closed Loop guidance. + * It calculates the yaw between the current position and the target position by + * calculating the difference between the two vectors and the angle formed by + * the diff vector + * + * requires: WingControllers + */ +class EarlyManeuversGuidanceAlgorithm : public GuidanceAlgorithm +{ +public: + /** + * @brief Enumerates all the possible targets of the EM algorithm + */ + enum class Target : uint32_t + { + EMC = 0, + M1, + M2, + FINAL + }; + + EarlyManeuversGuidanceAlgorithm(); + + virtual ~EarlyManeuversGuidanceAlgorithm(); + + /** + * @brief This method calculates 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 + * meters + * @param[out] heading Saves the heading vector for logging purposes + * + * @returns the yaw angle of the parafoil in rad + */ + Boardcore::Units::Angle::Radian calculateTargetAngle( + const Eigen::Vector3f& currentPositionNED, + Eigen::Vector2f& heading) override; + + /** + * @brief Set Early Maneuvers points + * + * @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); + + /** + * @brief Get Early Maneuvers points. + */ + EarlyManeuversPoints getPoints(); + + /** + * @brief Get the active target. + * @return The NED coordinates of the active target. + */ + Eigen::Vector2f getActiveTarget(); + + /** + * @brief Updates the active target based on the current altitude. + */ + void updateActiveTarget(Boardcore::Units::Length::Meter altitude); + +private: + // Point we are currently poinying to + std::atomic<Target> activeTarget; + + // Eigen::Vector2f targetNED; // NED, defined in the base class + Eigen::Vector2f EMC; // NED + Eigen::Vector2f M1; // NED + Eigen::Vector2f M2; // NED + + uint32_t targetAltitudeConfidence; + uint32_t m2AltitudeConfidence; + uint32_t m1AltitudeConfidence; + uint32_t emcAltitudeConfidence; +}; + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/Guidance/GuidanceAlgorithm.h b/src/Parafoil/Wing/Guidance/GuidanceAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..3f296b33c8b94a8b7aa31bd59cc46b2afb1e458a --- /dev/null +++ b/src/Parafoil/Wing/Guidance/GuidanceAlgorithm.h @@ -0,0 +1,62 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Radu Raul, Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <units/Angle.h> + +#include <Eigen/Core> + +namespace Parafoil +{ + +/** + * This class acts as an interface for a generic guidance algorithm that is used + * by the Automatic Wing Algorithm. + */ +class GuidanceAlgorithm +{ +public: + /** + * @brief This method must calculate the yaw angle of the parafoil knowing + * the current position and the target position without changing the vectors + * passed as arguments. + * + * @note the args are const references to reduce access time by avoiding + * copying objects that will be read-only. + * + * @param position the current NED position of the parafoil in [m] + * @param heading The current heading, it is used for logging purposes + * + * @returns the yaw angle of the parafoil in [rad] + */ + virtual Boardcore::Units::Angle::Radian calculateTargetAngle( + const Eigen::Vector3f& currentPositionNED, + Eigen::Vector2f& heading) = 0; + + Eigen::Vector2f getTargetNED() { return targetNED; } + +protected: + Eigen::Vector2f targetNED{0, 0}; // NED +}; + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/WingAlgorithm.cpp b/src/Parafoil/Wing/WingAlgorithm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9e73a21e7d68e63572ca0702fcfc073e3c2ab86a --- /dev/null +++ b/src/Parafoil/Wing/WingAlgorithm.cpp @@ -0,0 +1,121 @@ +/* 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 + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Parafoil/Configs/WingConfig.h> +#include <Parafoil/Wing/WingAlgorithm.h> +#include <common/Events.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> + +using namespace Boardcore; +using namespace Parafoil::Config::Wing; +using namespace Common; + +namespace Parafoil +{ +WingAlgorithm::WingAlgorithm(ServosList servo1, ServosList servo2) +{ + this->servo1 = servo1; + this->servo2 = servo2; + stepIndex = 0; + // Create the vector for algorithm data + steps = std::vector<WingAlgorithmData>(); +} + +bool WingAlgorithm::init() +{ + return true; // In this case the init is always true +} + +void WingAlgorithm::setServo(ServosList servo1, ServosList servo2) +{ + this->servo1 = servo1; + this->servo2 = servo2; +} + +void WingAlgorithm::addStep(WingAlgorithmData step) { steps.push_back(step); } + +void WingAlgorithm::begin() +{ + running = true; + shouldReset = true; + + // Set the reference timestamp + timeStart = TimestampTimer::getTimestamp(); +} + +void WingAlgorithm::end() +{ + running = false; + + // Set the reference timestamp to 0 + timeStart = 0; +} + +void WingAlgorithm::step() +{ + uint64_t currentTimestamp = TimestampTimer::getTimestamp(); + + if (shouldReset) + { + // If the algorithm has been stopped + // i want to start from the beginning + stepIndex = 0; + shouldReset = false; + } + + if (stepIndex >= steps.size()) + { + LOG_INFO(logger, "Algorithm end {:d} >= {:d}", stepIndex, steps.size()); + // End the procedure so it won't be executed + // Set the index to 0 in case of another future execution + stepIndex = 0; + // Terminate here + EventBroker::getInstance().post(WING_ALGORITHM_ENDED, TOPIC_WING); + return; + } + + if (currentTimestamp - timeStart >= steps[stepIndex].timestamp) + { + // I need to execute the current step + getModule<Actuators>()->setServoAngle(servo1, + steps[stepIndex].servo1Angle); + getModule<Actuators>()->setServoAngle(servo2, + steps[stepIndex].servo2Angle); + + // Log the data setting the timestamp to the absolute one + WingAlgorithmData data; + data.timestamp = TimestampTimer::getTimestamp(); + data.servo1Angle = steps[stepIndex].servo1Angle; + data.servo2Angle = steps[stepIndex].servo2Angle; + + // After copy i can log the actual step + SDlogger->log(data); + + // finally increment the stepIndex + stepIndex++; + + LOG_INFO(logger, "Step"); + } +} + +} // namespace Parafoil diff --git a/src/Parafoil/Wing/WingAlgorithm.h b/src/Parafoil/Wing/WingAlgorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..ff84671bb4204b2843df7c1a2bfd607d8f25c333 --- /dev/null +++ b/src/Parafoil/Wing/WingAlgorithm.h @@ -0,0 +1,109 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Authors: Matteo Pignataro, 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 + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#pragma once + +#include <Parafoil/Actuators/Actuators.h> +#include <Parafoil/Wing/WingAlgorithmData.h> +#include <algorithms/Algorithm.h> +#include <diagnostic/PrintLogger.h> +#include <logger/Logger.h> +#include <utils/DependencyManager/DependencyManager.h> + +namespace Parafoil +{ +class WingAlgorithm : public Boardcore::Algorithm, + public Boardcore::InjectableWithDeps<Actuators> +{ +public: + /** + * @brief Construct a new Wing Algorithm object + * + * @param servo1 The first servo + * @param servo2 The second servo + */ + WingAlgorithm(ServosList servo1, ServosList servo2); + + /** + * @brief Method to initialize the algorithm + * + * @return true If the init process goes well + * @return false If the init process doesn't go well + */ + bool init() override; + + /** + * @brief Set the Servos object + * + * @param servo1 The first algorithm servo + * @param servo2 The second algorithm servo + */ + void setServo(ServosList servo1, ServosList servo2); + + /** + * @brief Add a step to the algorithm sequence + * + * @param step The step to add + */ + void addStep(WingAlgorithmData step); + + /** + * @brief This sets the reference timestamp for the algorithm + */ + void begin(); + + /** + * @brief This method disables the algorithm + */ + void end(); + /** + * @brief This method implements the algorithm step based on the current + * timestamp + */ + void step() override; + +protected: + // The actuators + ServosList servo1, servo2; + + // Reference timestamp for relative algorithm timestamps + uint64_t timeStart; + + // Procedure array to memorize all the steps needed to perform the algorithm + std::vector<WingAlgorithmData> steps; + + // PrintLogger + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("WingAlgorithm"); + + // SD logger + Boardcore::Logger* SDlogger = &Boardcore::Logger::getInstance(); + + // This boolean is used to understand when to reset + // the index where the algorithm has stopped. + // In case of end call, we want to be able to perform + // another time this algorithm starting from 0 + std::atomic<bool> shouldReset; + + // Variable to remember what step that has to be done + unsigned int stepIndex; +}; +} // namespace Parafoil diff --git a/src/Parafoil/Wing/WingAlgorithmData.h b/src/Parafoil/Wing/WingAlgorithmData.h new file mode 100644 index 0000000000000000000000000000000000000000..17146089442ebe3382cd36c797ca0cce0337c01e --- /dev/null +++ b/src/Parafoil/Wing/WingAlgorithmData.h @@ -0,0 +1,64 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#pragma once +#include <sensors/SensorData.h> +#include <units/Angle.h> + +namespace Parafoil +{ + +/** + * This class represents the algorithm data structure that needs to be logged + * into the onboard SD card. It has the timestamp(absolute) and the servo + * position set by the selected algorithm + */ +struct WingAlgorithmData +{ + uint64_t timestamp = 0; // First timestamp is 0 (in microseconds) + Boardcore::Units::Angle::Degree servo1Angle{0}; // Angle of the first servo + Boardcore::Units::Angle::Degree servo2Angle{ + 0}; // Angle of the second servo + Boardcore::Units::Angle::Radian targetAngle{ + 0}; // Angle tracked by the algorithm + Boardcore::Units::Angle::Radian velocityAngle{ + 0}; // Angle of the velocity vector + float targetX = 0; // NED (only automatic algorithm) + float targetY = 0; // NED (only automatic algorithm) + Boardcore::Units::Angle::Radian error{ + 0}; // Error between target and velocity angle + Boardcore::Units::Angle::Degree pidOutput{ + 0}; // Output of the PID controller + + static std::string header() + { + return "WingAlgorithmTimestamp,servo1Angle,servo2Angle,targetAngle," + "velocityAngle,targetX,targetY,error,pidOutput\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << servo1Angle << "," << servo2Angle << "," + << targetAngle << "," << velocityAngle << "," << targetX << "," + << targetY << "," << error << "," << pidOutput << "\n"; + } +}; +} // namespace Parafoil diff --git a/src/Parafoil/Wing/WingTargetPositionData.h b/src/Parafoil/Wing/WingTargetPositionData.h new file mode 100644 index 0000000000000000000000000000000000000000..baac961b9985b05b0a083cc1ecfe29efa847fcae --- /dev/null +++ b/src/Parafoil/Wing/WingTargetPositionData.h @@ -0,0 +1,75 @@ +/* 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 + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <units/Length.h> + +#include <ostream> +#include <string> + +namespace Parafoil +{ + +struct WingTargetPositionData +{ + 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 "targetLat, " + "targetLon,targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n"; + } + + void print(std::ostream& os) const + { + os << targetLat << "," << targetLon << "," << targetN << "," << targetE + << "," << emcN << "," << emcE << "," << m1N << "," << m1E << "," + << m2N << "," << m2E << "\n"; + } +}; + +struct EarlyManeuversActiveTargetData +{ + uint64_t timestamp = 0; + uint32_t target = 0; ///< Active target enumeration + Boardcore::Units::Length::Meter 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 Parafoil diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp index 971f6cf58802a55afe3b2f8a5a50f79d35b9130a..857e93de8c958512a84481581837be503a1a1574 100644 --- a/src/Parafoil/parafoil-entry.cpp +++ b/src/Parafoil/parafoil-entry.cpp @@ -28,6 +28,8 @@ #include <Parafoil/Sensors/Sensors.h> #include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h> #include <Parafoil/StateMachines/NASController/NASController.h> +#include <Parafoil/StateMachines/WingController/WingController.h> +#include <Parafoil/WindEstimation/WindEstimation.h> #include <common/Events.h> #include <common/Topics.h> #include <diagnostic/PrintLogger.h> @@ -116,7 +118,10 @@ int main() // Flight algorithms auto altitudeTrigger = new AltitudeTrigger(); initResult &= depman.insert(altitudeTrigger); - // TODO: Wing Algorithm + auto wingController = new WingController(); + initResult &= depman.insert(wingController); + auto windEstimation = new WindEstimation(); + initResult &= depman.insert(windEstimation); // Actuators auto actuators = new Actuators(); @@ -130,11 +135,13 @@ int main() } START_MODULE(flightModeManager); + START_MODULE(pinHandler); // START_MODULE(radio) { miosix::led2On(); } START_MODULE(nasController); START_MODULE(altitudeTrigger); - // START_MODULE(wingController); + START_MODULE(windEstimation); + START_MODULE(wingController); START_MODULE(actuators); START_MODULE(scheduler); diff --git a/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp index a30a15872a6c1548c76fbe2dfa55d8f191d59b9e..1eb821bf5c95f0aba7cf37c417d52f8452a92687 100644 --- a/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp +++ b/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp @@ -37,9 +37,9 @@ namespace Payload EarlyManeuversGuidanceAlgorithm::EarlyManeuversGuidanceAlgorithm() : activeTarget(Target::EMC), targetAltitudeConfidence(0), m2AltitudeConfidence(0), m1AltitudeConfidence(0), - emcAltitudeConfidence(0) {}; + emcAltitudeConfidence(0){}; -EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm() {}; +EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){}; float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle( const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading) @@ -64,7 +64,7 @@ float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle( break; } - return atan2(heading[1], heading[0]); + return static_cast<float>(atan2(heading[1], heading[0])); } void EarlyManeuversGuidanceAlgorithm::updateActiveTarget(float altitude) diff --git a/src/common/Events.h b/src/common/Events.h index fc524997e62255a4e295a0af299ca491f8c49618..54b2b1cda1fa558489548f78430d6a08e50a0f0b 100644 --- a/src/common/Events.h +++ b/src/common/Events.h @@ -67,6 +67,9 @@ enum Events : uint8_t DPL_NC_RESET, DPL_FLARE_START, DPL_FLARE_STOP, + DPL_SERVO_ACTUATION_DETECTED, + DPL_WIGGLE, + DPL_WES_CAL_DONE, DPL_DONE, CAN_FORCE_INIT, CAN_ARM,