diff --git a/src/boards/Parafoil/Actuators/Actuators.cpp b/src/boards/Parafoil/Actuators/Actuators.cpp
deleted file mode 100644
index 4f317e60d641387835d48dcbb7630b572950ea57..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Actuators/Actuators.cpp
+++ /dev/null
@@ -1,221 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli, Angelo Prete
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * 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 "Actuators.h"
-
-#include <Parafoil/BoardScheduler.h>
-#include <Parafoil/Configs/ActuatorsConfigs.h>
-#include <Parafoil/Configs/WingConfig.h>
-#include <interfaces-impl/hwmapping.h>
-
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-using namespace miosix;
-using namespace Boardcore;
-using namespace Parafoil::ActuatorsConfigs;
-
-namespace Parafoil
-{
-
-Actuators::Actuators()
-{
- leftServo = new Servo(SERVO_1_TIMER, SERVO_1_PWM_CH, LEFT_SERVO_MIN_PULSE,
- LEFT_SERVO_MAX_PULSE);
- rightServo = new Servo(SERVO_2_TIMER, SERVO_2_PWM_CH, RIGHT_SERVO_MIN_PULSE,
- RIGHT_SERVO_MAX_PULSE);
-}
-
-bool Actuators::start()
-{
- // Servos
- enableServo(PARAFOIL_LEFT_SERVO);
- setServo(PARAFOIL_LEFT_SERVO, 0);
- enableServo(PARAFOIL_RIGHT_SERVO);
- setServo(PARAFOIL_RIGHT_SERVO, 0);
-
- return true;
-}
-
-bool Actuators::setServo(ServosList servoId, float percentage)
-{
- if (percentage > WingConfig::MAX_SERVO_APERTURE)
- {
- percentage = WingConfig::MAX_SERVO_APERTURE;
- }
-
- switch (servoId)
- {
- case PARAFOIL_LEFT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
- leftServo->setPosition(percentage);
- Logger::getInstance().log(leftServo->getState());
- break;
- }
- case PARAFOIL_RIGHT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
- rightServo->setPosition(percentage);
- Logger::getInstance().log(rightServo->getState());
- break;
- }
- default:
- {
- return false;
- }
- }
-
- return true;
-}
-
-bool Actuators::setServoAngle(ServosList servoId, float angle)
-{
- switch (servoId)
- {
- case PARAFOIL_LEFT_SERVO:
- {
- return Actuators::setServo(servoId, angle / LEFT_SERVO_ROTATION);
- }
- case PARAFOIL_RIGHT_SERVO:
- {
- return Actuators::setServo(servoId, angle / RIGHT_SERVO_ROTATION);
- }
- default:
- {
- return false;
- }
- }
-}
-
-bool Actuators::wiggleServo(ServosList servoId)
-{
-
- if (!setServo(servoId, 1))
- {
- return false;
- }
- Thread::sleep(1000);
- return setServo(servoId, 0);
-}
-
-bool Actuators::enableServo(ServosList servoId)
-{
- switch (servoId)
- {
- case PARAFOIL_LEFT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
- leftServo->enable();
- break;
- }
- case PARAFOIL_RIGHT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
- rightServo->enable();
- break;
- }
- default:
- return false;
- }
-
- return true;
-}
-
-bool Actuators::disableServo(ServosList servoId)
-{
- switch (servoId)
- {
- case PARAFOIL_LEFT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
- leftServo->disable();
- break;
- }
- case PARAFOIL_RIGHT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
- rightServo->disable();
- break;
- }
- default:
- return false;
- }
-
- return true;
-}
-
-float Actuators::getServoPosition(ServosList servoId)
-{
-
- switch (servoId)
- {
- case PARAFOIL_LEFT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
- return leftServo->getPosition();
- }
- case PARAFOIL_RIGHT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
- return rightServo->getPosition();
- }
- default:
- return 0;
- }
-
- return 0;
-}
-
-float Actuators::getServoAngle(ServosList servoId)
-{
- switch (servoId)
- {
- case PARAFOIL_LEFT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> ll(leftServoMutex);
- return leftServo->getPosition() * LEFT_SERVO_ROTATION;
- }
- case PARAFOIL_RIGHT_SERVO:
- {
- miosix::Lock<miosix::FastMutex> lr(rightServoMutex);
- return rightServo->getPosition() * RIGHT_SERVO_ROTATION;
- }
- default:
- return 0;
- }
-
- return 0;
-}
-
-void Actuators::startTwirl()
-{
- setServo(PARAFOIL_LEFT_SERVO, SERVO_TWIRL_RADIUS);
- setServo(PARAFOIL_RIGHT_SERVO, 0);
-}
-
-void Actuators::stopTwirl()
-{
- setServo(PARAFOIL_LEFT_SERVO, 0);
- setServo(PARAFOIL_RIGHT_SERVO, 0);
-}
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Actuators/Actuators.h b/src/boards/Parafoil/Actuators/Actuators.h
deleted file mode 100644
index afbb371fe66eafe2f24c5eb6a43289850abca61f..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Actuators/Actuators.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, Federico Lolli, Angelo Prete
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * 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 <actuators/Servo/Servo.h>
-#include <common/Mavlink.h>
-#include <scheduler/TaskScheduler.h>
-
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-namespace Parafoil
-{
-
-struct Actuators : public Boardcore::Module
-{
- explicit Actuators();
-
- [[nodiscard]] bool start();
-
- /**
- * @brief Moves the specified servo to the given position.
- *
- * @param servoId Servo to move.
- * @param percentage Angle to set [0-1].
- * @return True if the the angle was set.
- */
- bool setServo(ServosList servoId, float percentage);
-
- /**
- * @brief Moves the specified servo to the given position.
- *
- * @param servoId Servo to move.
- * @param angle Angle to set [degree].
- * @return True if the the angle was set.
- */
- bool setServoAngle(ServosList servoId, float angle);
-
- /**
- * @brief Wiggles the servo for few seconds.
- *
- * @param servoId Servo to move.
- * @return true
- * @return false
- */
- bool wiggleServo(ServosList servoId);
-
- /**
- * @brief Enables the specified servo.
- *
- * @param servoId Servo to enable.
- * @return True if the servo was enabled.
- * @return False if the servoId is invalid.
- */
- bool enableServo(ServosList servoId);
-
- /**
- * @brief Disables the specified servo.
- *
- * @param servoId Servo to disable.
- * @return True if the servo was disabled.
- * @return False if the servoId is invalid.
- */
- bool disableServo(ServosList servoId);
-
- /**
- * @brief Returns the current position of the specified servo.
- *
- * @param servoId Servo to read.
- * @return float current Servo position in range [0-1], 0 if the servoId is
- * invalid.
- */
- float getServoPosition(ServosList servoId);
-
- /**
- * @brief Returns the current angle of the specified servo.
- *
- * @param servoId Servo to read.
- * @return float current Servo angle in range [0-180], 0 if the servoId is
- * invalid.
- */
- float getServoAngle(ServosList servoId);
-
- /**
- * @brief Starts twirl (one servo is set to 0 and the other one is not).
- */
- void startTwirl();
-
- /**
- * @brief Stops twirling (both servos are set to 0).
- */
- void stopTwirl();
-
-private:
- Boardcore::Servo* leftServo = nullptr;
- Boardcore::Servo* rightServo = nullptr;
-
- // mutexes
- miosix::FastMutex leftServoMutex;
- miosix::FastMutex rightServoMutex;
-};
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Configs/WingConfig.h b/src/boards/Parafoil/Configs/WingConfig.h
deleted file mode 100644
index 1ea29f7ea76e24bd4e70a672ad04f7d7469e6db4..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Configs/WingConfig.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli, Angelo Prete
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * 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 <utils/Constants.h>
-
-#include <ostream>
-
-namespace Parafoil
-{
-
-namespace WingConfig
-{
-// Algorithm configuration
-
-#if defined(CLOSED_LOOP)
-constexpr int SELECTED_ALGORITHM = 0;
-#elif EARLY_MANEUVER
-constexpr int SELECTED_ALGORITHM = 1;
-#elif SEQUENCE
-constexpr int SELECTED_ALGORITHM = 2;
-#elif ROTATION
-constexpr int SELECTED_ALGORITHM = 3;
-#else
-constexpr int SELECTED_ALGORITHM = 0;
-#endif
-
-#if defined(JESOLO)
-constexpr float DEFAULT_TARGET_LAT = 45.565835;
-constexpr float DEFAULT_TARGET_LON = 12.577307;
-#else // Milan
-constexpr float DEFAULT_TARGET_LAT = 45.5013853;
-constexpr float DEFAULT_TARGET_LON = 9.1544219;
-#endif
-
-constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms]
-
-constexpr int WING_UPDATE_PERIOD = 1000; // [ms]
-
-constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = Boardcore::Constants::PI;
-constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
-
-constexpr int GUIDANCE_CONFIDENCE = 15;
-constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD = 250; //[m]
-constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD = 150; //[m]
-constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50; //[m]
-
-// TODO check this parameter preflight
-constexpr float KP = 1.3537;
-constexpr float KI = 0.0111;
-
-constexpr float ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE = 300; // [meters]
-constexpr int ALTITUDE_TRIGGER_CONFIDENCE = 10; // [number of sample]
-constexpr int ALTITUDE_TRIGGER_PERIOD = 100; //[ms]
-
-constexpr float MAX_SERVO_APERTURE = 0.8; //[%]
-
-struct WingConfigStruct
-{
- float lat = DEFAULT_TARGET_LAT;
- float lon = DEFAULT_TARGET_LON;
- float maxServoAperture = MAX_SERVO_APERTURE;
- int wingUpdatePeriod = WING_UPDATE_PERIOD;
- int altitudeTriggerUpdatePeriod = ALTITUDE_TRIGGER_PERIOD;
- int altitudeTriggerConfidence = ALTITUDE_TRIGGER_CONFIDENCE;
- int altitudeTriggerDeploymentAltitude =
- ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE;
- int selectedAlgorithm = SELECTED_ALGORITHM;
-
- static std::string header()
- {
- return "DEFAULT_TARGET_LAT,DEFAULT_TARGET_LON,MAX_SERVO_APERTURE,WING_"
- "UPDATE_PERIOD,ALTITUDE_TRIGGER_PERIOD,ALTITUDE_TRIGGER_"
- "CONFIDENCE,ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE,SELECTED_"
- "ALGORITHM\n";
- }
-
- void print(std::ostream& os) const
- {
- os << lat << "," << lon << "," << maxServoAperture << ","
- << wingUpdatePeriod << "," << altitudeTriggerUpdatePeriod << ","
- << altitudeTriggerConfidence << ","
- << altitudeTriggerDeploymentAltitude << "," << selectedAlgorithm
- << "\n";
- }
-};
-
-} // namespace WingConfig
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
deleted file mode 100644
index f0542e62af41233747567654b41f4e583ba168e7..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp
+++ /dev/null
@@ -1,510 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli, Radu Raul, Angelo Prete
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * 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/Actuators/Actuators.h>
-#include <Parafoil/BoardScheduler.h>
-#include <Parafoil/Configs/ActuatorsConfigs.h>
-#include <Parafoil/Configs/WESConfig.h>
-#include <Parafoil/Configs/WingConfig.h>
-#include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h>
-#include <Parafoil/StateMachines/NASController/NASController.h>
-#include <Parafoil/WindEstimationScheme/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 <diagnostic/PrintLogger.h>
-#include <drivers/timer/TimestampTimer.h>
-#include <events/EventBroker.h>
-
-using namespace Boardcore;
-using namespace Parafoil::WingConfig;
-using namespace Parafoil::WESConfig;
-using namespace Parafoil::ActuatorsConfigs;
-using namespace Common;
-using namespace miosix;
-
-namespace Parafoil
-{
-
-WingController::WingController(TaskScheduler* sched)
- : HSM(&WingController::state_idle), running(false), selectedAlgorithm(0),
- scheduler(sched)
-{
-
- EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
- EventBroker::getInstance().subscribe(this, TOPIC_DPL);
- EventBroker::getInstance().subscribe(this, TOPIC_WING);
- this->targetPositionGEO = {DEFAULT_TARGET_LAT, DEFAULT_TARGET_LON};
-}
-
-bool WingController::start()
-{
- return scheduler->addTask(std::bind(&WingController::update, this),
- WING_UPDATE_PERIOD) &&
- addAlgorithms() && HSM::start();
-}
-
-WingController::~WingController()
-{
- EventBroker::getInstance().unsubscribe(this);
-}
-
-WingControllerStatus WingController::getStatus()
-{
- miosix::Lock<miosix::FastMutex> s(statusMutex);
- return status;
-}
-
-State WingController::state_idle(const Boardcore::Event& event)
-{
- switch (event)
- {
- case EV_ENTRY:
- {
- logStatus(WingControllerState::IDLE);
- return HANDLED;
- }
- case FLIGHT_WING_DESCENT:
- {
- return transition(&WingController::state_flying);
- }
- case EV_EMPTY:
- {
- return tranSuper(&WingController::state_top);
- }
- default:
- {
- return UNHANDLED;
- }
- }
-}
-
-State WingController::state_flying(const Event& event)
-{
-
- switch (event)
- {
- case EV_ENTRY:
- {
- return HANDLED;
- }
- case EV_EXIT:
- {
- return HANDLED;
- }
- case EV_EMPTY:
- {
- return tranSuper(&WingController::state_top);
- }
- case EV_INIT:
- {
- return transition(&WingController::state_calibration);
- }
- case FLIGHT_LANDING_DETECTED:
- {
- return transition(&WingController::state_on_ground);
- }
- default:
- {
- return UNHANDLED;
- }
- }
-}
-
-State WingController::state_calibration(const Boardcore::Event& event)
-{
- ModuleManager& modules = ModuleManager::getInstance();
- static uint16_t calibrationTimeoutEventId;
-
- switch (event)
- {
- case EV_ENTRY: // starts twirling and calibration wes
- {
- logStatus(WingControllerState::CALIBRATION);
-
- modules.get<Actuators>()->startTwirl();
- calibrationTimeoutEventId = EventBroker::getInstance().postDelayed(
- DPL_WES_CAL_DONE, TOPIC_DPL, WES_CALIBRATION_TIMEOUT);
-
- modules.get<WindEstimation>()
- ->startWindEstimationSchemeCalibration();
-
- return HANDLED;
- }
- case EV_EXIT:
- {
- EventBroker::getInstance().removeDelayed(calibrationTimeoutEventId);
- return HANDLED;
- }
- case EV_EMPTY:
- {
- return tranSuper(&WingController::state_flying);
- }
- case DPL_WES_CAL_DONE:
- {
- modules.get<Actuators>()->stopTwirl();
-
- return transition(&WingController::state_controlled_descent);
- }
- default:
- {
- return UNHANDLED;
- }
- }
-}
-State WingController::state_controlled_descent(const Boardcore::Event& event)
-{
- switch (event)
- {
- case EV_ENTRY: // start automatic algorithm
- {
- logStatus(WingControllerState::ALGORITHM_CONTROLLED);
- setEarlyManeuverPoints(
- convertTargetPositionToNED(targetPositionGEO),
- {ModuleManager::getInstance()
- .get<NASController>()
- ->getNasState()
- .n,
- ModuleManager::getInstance()
- .get<NASController>()
- ->getNasState()
- .e});
-
- startAlgorithm();
- return HANDLED;
- }
- case EV_EMPTY:
- {
- return tranSuper(&WingController::state_flying);
- }
- case WING_ALGORITHM_ENDED:
- {
- return transition(&WingController::state_on_ground);
- }
- case EV_EXIT:
- {
- return HANDLED;
- }
- default:
- {
- return UNHANDLED;
- }
- }
-}
-
-State WingController::state_on_ground(const Boardcore::Event& event)
-{
- switch (event)
- {
- case EV_ENTRY:
- {
- logStatus(WingControllerState::ON_GROUND);
-
- ModuleManager::getInstance()
- .get<WindEstimation>()
- ->stopWindEstimationScheme();
- ModuleManager::getInstance()
- .get<WindEstimation>()
- ->stopWindEstimationSchemeCalibration();
- stopAlgorithm();
-
- // disable servos
- ModuleManager::getInstance().get<Actuators>()->disableServo(
- PARAFOIL_LEFT_SERVO);
- ModuleManager::getInstance().get<Actuators>()->disableServo(
- PARAFOIL_RIGHT_SERVO);
-
- return HANDLED;
- }
- case EV_EXIT:
- {
- return HANDLED;
- }
- case EV_EMPTY:
- {
- return tranSuper(&WingController::state_top);
- }
- default:
- {
- return UNHANDLED;
- }
- }
-}
-
-bool WingController::addAlgorithms()
-{
- WingAlgorithm* algorithm;
- WingAlgorithmData step;
-
- bool result = false;
-
- // Algorithm 0
- algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
- PARAFOIL_RIGHT_SERVO, clGuidance);
- result = algorithm->init();
- algorithms.push_back(algorithm);
- // Algorithm 1
- algorithm = new AutomaticWingAlgorithm(KP, KI, PARAFOIL_LEFT_SERVO,
- PARAFOIL_RIGHT_SERVO, emGuidance);
- result &= algorithm->init();
- algorithms.push_back(algorithm);
-
- // Algorithm 2
- algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
- step.servo1Angle = 0;
- step.servo2Angle = 120;
- step.timestamp = 0;
- algorithm->addStep(step);
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- step.timestamp += 1000 * WingConfig::WING_STRAIGHT_FLIGHT_TIMEOUT;
- algorithm->addStep(step);
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- step.timestamp += WingConfig::WING_STRAIGHT_FLIGHT_TIMEOUT;
- algorithm->addStep(step);
- result &= algorithm->init();
- // Add the algorithm to the vector
- algorithms.push_back(algorithm);
-
- // Algorithm 3 (rotation)
- algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO);
- step.timestamp = 0;
- step.servo1Angle = LEFT_SERVO_ROTATION / 2;
- step.servo2Angle = 0;
- algorithm->addStep(step);
- step.timestamp += WES_ROTATION_PERIOD; // us
- step.servo1Angle = 0;
- step.servo2Angle = RIGHT_SERVO_ROTATION / 2;
- algorithm->addStep(step);
- step.timestamp += WES_ROTATION_PERIOD; // us
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
- step.timestamp += WES_ROTATION_PERIOD; // us
- step.servo1Angle = LEFT_SERVO_ROTATION;
- step.servo2Angle = RIGHT_SERVO_ROTATION;
- algorithm->addStep(step);
- step.timestamp += WES_ROTATION_PERIOD; // us
- step.servo1Angle = 0;
- step.servo2Angle = RIGHT_SERVO_ROTATION;
- algorithm->addStep(step);
- step.timestamp += WES_ROTATION_PERIOD; // us
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
- step.timestamp += WES_ROTATION_PERIOD; // us
- step.servo1Angle = 0;
- step.servo2Angle = 0;
- algorithm->addStep(step);
- result &= algorithm->init();
- algorithms.push_back(algorithm);
-
- selectAlgorithm(SELECTED_ALGORITHM);
-
- return result;
-}
-
-bool WingController::selectAlgorithm(unsigned int index)
-{
- bool success = false;
- // We change the selected algorithm only if we are in IDLE
- if (getStatus().state == WingControllerState::IDLE)
- {
- stopAlgorithm();
- if (index < algorithms.size())
- {
- LOG_INFO(logger, "Algorithm {:1} selected", index);
- selectedAlgorithm = index;
- success = true;
- }
- else
- {
- // Select the 0 algorithm
- selectedAlgorithm = 0;
- success = false;
- }
- }
- return success;
-}
-
-void WingController::startAlgorithm()
-{
- // If the selected algorithm is valid --> also the
- // algorithms array is not empty i start the whole thing
- if (selectedAlgorithm < algorithms.size())
- {
- running = true;
-
- // Begin the selected algorithm
- algorithms[selectedAlgorithm]->begin();
-
- LOG_INFO(logger, "Wing algorithm started");
- }
-}
-
-void WingController::stopAlgorithm()
-{
- if (running)
- {
- // Set running to false
- running = false;
- // Stop the algorithm if selected
- if (selectedAlgorithm < algorithms.size())
- {
- algorithms[selectedAlgorithm]->end();
- reset();
- }
- }
-}
-
-void WingController::update()
-{
- if (running)
- {
- algorithms[selectedAlgorithm]->step();
- }
-}
-
-void WingController::flare()
-{
- // Set the servo position to flare (pull the two ropes as skydiving people
- // do)
-
- ModuleManager::getInstance().get<Actuators>()->setServo(PARAFOIL_LEFT_SERVO,
- 1);
- ModuleManager::getInstance().get<Actuators>()->setServo(
- PARAFOIL_RIGHT_SERVO, 1);
-}
-
-void WingController::reset()
-{
- // Set the servo position to reset
- ModuleManager::getInstance().get<Actuators>()->setServo(PARAFOIL_LEFT_SERVO,
- 0);
- ModuleManager::getInstance().get<Actuators>()->setServo(
- PARAFOIL_RIGHT_SERVO, 0);
-}
-
-void WingController::setTargetPosition(Eigen::Vector2f targetGEO)
-{
- if (ModuleManager::getInstance().get<NASController>()->getStatus().state ==
- NASControllerState::ACTIVE)
- {
- this->targetPositionGEO = targetGEO;
- setEarlyManeuverPoints(
- convertTargetPositionToNED(targetPositionGEO),
- {ModuleManager::getInstance().get<NASController>()->getNasState().n,
- ModuleManager::getInstance()
- .get<NASController>()
- ->getNasState()
- .e});
- }
-}
-
-void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED,
- Eigen::Vector2f currentPosNED)
-{
-
- Eigen::Vector2f targetOffsetNED = targetNED - currentPosNED;
-
- Eigen::Vector2f norm_point = targetOffsetNED / targetOffsetNED.norm();
-
- float psi0 = atan2(norm_point[1], norm_point[0]);
-
- float distFromCenterline = 20; // the distance that the M1 and M2 points
- // must have from the center line
- // TODO add parameter
-
- // 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;
-
- Eigen::Vector2f emcPosition =
- targetOffsetNED * 1.2 +
- currentPosNED; // EMC is calculated as target * 1.2
-
- Eigen::Vector2f m1Position =
- Eigen::Vector2f(cos(m1Angle), sin(m1Angle)) * maneuverPointsMagnitude +
- currentPosNED;
-
- Eigen::Vector2f m2Position =
- Eigen::Vector2f(cos(m2Angle), sin(m2Angle)) * maneuverPointsMagnitude +
- currentPosNED;
-
- emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position);
-
- clGuidance.setPoints(targetNED);
-
- WingTargetPositionData data;
-
- data.receivedLat = targetPositionGEO[0];
- data.receivedLon = targetPositionGEO[1];
-
- data.targetN = targetNED[0];
- data.targetE = targetNED[1];
-
- data.emcN = emcPosition[0];
- data.emcE = emcPosition[1];
-
- data.m1N = m1Position[0];
- data.m1E = m1Position[1];
-
- data.m2N = m2Position[0];
- data.m2E = m2Position[1];
-
- // Log the received position
- Logger::getInstance().log(data);
-}
-
-void WingController::logStatus(WingControllerState state)
-{
- miosix::Lock<miosix::FastMutex> s(statusMutex);
- status.timestamp = TimestampTimer::getTimestamp();
- status.state = state;
-
- Logger::getInstance().log(status);
-}
-
-Eigen::Vector2f WingController::convertTargetPositionToNED(
- Eigen::Vector2f targetGEO)
-{
- return Aeroutils::geodetic2NED(targetGEO, {ModuleManager::getInstance()
- .get<NASController>()
- ->getReferenceValues()
- .refLatitude,
- ModuleManager::getInstance()
- .get<NASController>()
- ->getReferenceValues()
- .refLongitude});
-}
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.h b/src/boards/Parafoil/StateMachines/WingController/WingController.h
deleted file mode 100644
index aa2f17f593c86225b960ca2aea0b1795bf734ff5..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/StateMachines/WingController/WingController.h
+++ /dev/null
@@ -1,189 +0,0 @@
-/* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro, Federico Mandelli, Angelo Prete
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * 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/ClosedLoopGuidanceAlgorithm.h>
-#include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
-#include <Parafoil/Wing/WingAlgorithm.h>
-#include <events/HSM.h>
-
-#include <Eigen/Core>
-#include <atomic>
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-#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 WingController : public Boardcore::HSM<WingController>,
- public Boardcore::Module
-
-{
-
-public:
- Boardcore::State state_idle(const Boardcore::Event& event);
- Boardcore::State state_flying(const Boardcore::Event& event);
- Boardcore::State state_calibration(const Boardcore::Event& event);
- Boardcore::State state_controlled_descent(const Boardcore::Event& event);
- Boardcore::State state_on_ground(const Boardcore::Event& event);
-
- /**
- * @brief Destroy the Wing Controller object.
- */
- ~WingController();
-
- /**
- * @brief Method to set the target position.
- */
- void setTargetPosition(Eigen::Vector2f target);
-
- /**
- * @brief Selects the algorithm if present.
- *
- * @param index The algorithms vector index
- *
- * @return true if the selection was successful
- */
- bool selectAlgorithm(unsigned int index);
-
- WingControllerStatus getStatus();
-
- /**
- * @brief Construct a new Wing Controller object
- */
- explicit WingController(Boardcore::TaskScheduler* sched);
-
- bool start() override;
-
-private:
- /**
- * @brief Method to add the algorithm in the list
- *
- * @param algorithm The algorithm with
- * all already done (e.g. steps already registered)
- */
- bool addAlgorithms();
-
- /**
- * @brief target position getter
- */
- Eigen::Vector2f convertTargetPositionToNED(Eigen::Vector2f targetGEO);
-
- /**
- * @brief set points needed by the Guidance
- */
- void setEarlyManeuverPoints(Eigen::Vector2f targetNED,
- Eigen::Vector2f currentPosNED);
-
- void logStatus(WingControllerState state);
-
- WingControllerStatus status;
-
- miosix::FastMutex statusMutex;
-
- /**
- * @brief Target position
- */
- Eigen::Vector2f targetPositionGEO;
-
- /**
- * @brief List of loaded algorithms (from SD or not)
- */
- std::vector<WingAlgorithm*> algorithms;
-
- /**
- * @brief PrintLogger
- */
- Boardcore::PrintLogger logger =
- Boardcore::Logging::getLogger("ParafoilTest");
-
- /**
- * @brief Internal running state
- */
- std::atomic<bool> running;
- /**
- * @brief This attribute is modified by the mavlink radio section.
- * The user using the Ground Station can select the pre-enumerated algorithm
- * to execute
- */
- std::atomic<size_t> selectedAlgorithm;
-
- /**
- * @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;
-
- /**
- * @brief starts the selected algorithm
- */
- void startAlgorithm();
-
- /**
- * @brief Sets the internal state to stop and
- * stops the selected algorithm
- */
- void stopAlgorithm();
-
- /**
- * @brief Stops any on going algorithm and flares the wing
- */
- void flare();
-
- /**
- * @brief Resets the servos in their initial position
- */
- void reset();
-
- void update();
-
- Boardcore::TaskScheduler* scheduler = nullptr;
-};
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/StateMachines/WingController/WingControllerData.h b/src/boards/Parafoil/StateMachines/WingController/WingControllerData.h
deleted file mode 100644
index 698b2ea0996f9dddb9b2aece06d02ca04eaad3b8..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/StateMachines/WingController/WingControllerData.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/* Copyright (c) 2022 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 <stdint.h>
-
-#include <iostream>
-#include <string>
-
-namespace Parafoil
-{
-
-enum class WingControllerState : uint8_t
-{
- UNINIT = 0,
- IDLE,
- CALIBRATION,
- ALGORITHM_CONTROLLED,
- ON_GROUND,
- END
-};
-
-struct WingControllerStatus
-{
- long long timestamp = 0;
- WingControllerState state = WingControllerState::UNINIT;
-
- static std::string header() { return "timestamp,state\n"; }
-
- void print(std::ostream& os) const
- {
- os << timestamp << "," << (int)state << "\n";
- }
-};
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
deleted file mode 100644
index 963b91d1dbdf65ed5c1e9081587f2b83595b2f6d..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
+++ /dev/null
@@ -1,181 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro, 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.
- */
-
-#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 <Parafoil/WindEstimationScheme/WindEstimation.h>
-#include <drivers/timer/TimestampTimer.h>
-#include <math.h>
-#include <utils/AeroUtils/AeroUtils.h>
-#include <utils/Constants.h>
-
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-using namespace Boardcore;
-using namespace Eigen;
-using namespace Parafoil::WingConfig;
-
-namespace Parafoil
-{
-AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki,
- ServosList servo1,
- ServosList servo2,
- GuidanceAlgorithm& guidance)
- : WingAlgorithm(servo1, servo2), guidance(guidance)
-{
- controller = new PIController(Kp, Ki, WING_UPDATE_PERIOD / 1000.0f,
- PI_CONTROLLER_SATURATION_MIN_LIMIT,
- PI_CONTROLLER_SATURATION_MAX_LIMIT);
-}
-
-AutomaticWingAlgorithm::~AutomaticWingAlgorithm() { delete (controller); }
-
-void AutomaticWingAlgorithm::step()
-{
- ModuleManager& modules = ModuleManager::getInstance();
-
- if (modules.get<Sensors>()->getUbxGpsLastSample().fix != 0)
- {
- // The PI calculated result
- float result = algorithmStep(
- modules.get<NASController>()->getNasState(),
- modules.get<WindEstimation>()->getWindEstimationScheme());
-
- // Actuate the result
- // To see how to interpret the PI output
- // https://www.geogebra.org/calculator/xrhwarpz
- // to system is
- /* N ^
- |
- |
- ----> E
- */
- if (result > 0)
- {
- // Activate the servo2 and reset servo1
- modules.get<Actuators>()->setServoAngle(servo1, 0);
- modules.get<Actuators>()->setServoAngle(servo2, result);
- }
- else
- {
- // Activate the servo1 and reset servo2
- modules.get<Actuators>()->setServoAngle(servo1, result * -1);
- modules.get<Actuators>()->setServoAngle(servo2, 0);
- }
-
- // Log the servo positions
- {
- miosix::Lock<FastMutex> l(mutex);
-
- data.timestamp = TimestampTimer::getTimestamp();
- data.servo1Angle =
- modules.get<Actuators>()->getServoPosition(servo1);
- data.servo2Angle =
- modules.get<Actuators>()->getServoPosition(servo2);
- SDlogger->log(data);
- }
- }
- else
- {
- // If we loose fix we set both servo at 0
- modules.get<Actuators>()->setServoAngle(servo1, 0);
- modules.get<Actuators>()->setServoAngle(servo2, 0);
- }
-}
-
-float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
-{
- float result;
- // For some algorithms the third component is needed!
- Vector3f currentPosition(state.n, state.e, state.d);
-
- Vector2f heading; // used for logging purposes
-
- float targetAngle = guidance.calculateTargetAngle(currentPosition, heading);
-
- Vector2f relativeVelocity(state.vn, state.ve);
- // Remove WES for this test
-
- // Compute the angle of the current velocity
- float velocityAngle;
-
- // All angle are computed as angle from the north direction
-
- velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
-
- // Compute the angle difference
- float error = angleDiff(targetAngle, velocityAngle);
-
- // Call the PI with the just calculated error. The result is in RADIANS,
- // if positive we activate one servo, if negative the other
- result = controller->update(error);
-
- // Convert the result from radians back to degrees
- result = result * (180.f / Constants::PI);
-
- // Flip the servo orientation
- // result *= -1;
- // TODO check if this is needed
-
- // 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;
-}
-
-float AutomaticWingAlgorithm::angleDiff(float a, float b)
-{
- float diff = a - b;
-
- // 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 diff;
-}
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.h b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.h
deleted file mode 100644
index 8b2f7561ac859428191e0bf46634271b10934e49..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/* 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/Guidance/GuidanceAlgorithm.h>
-#include <Parafoil/Wing/WingAlgorithm.h>
-#include <algorithms/NAS/NASState.h>
-#include <algorithms/PIController.h>
-#include <algorithms/ReferenceValues.h>
-
-#include <Eigen/Core>
-
-namespace Parafoil
-{
-class AutomaticWingAlgorithm : public WingAlgorithm
-{
-
-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);
-
- /**
- * @brief Destroy the Automatic Wing Algorithm object and the PI
- */
- ~AutomaticWingAlgorithm();
-
-protected:
- // Guidance algorithm that sets the yaw.
- GuidanceAlgorithm& guidance;
-
- // PI controller tuned on the Kp and Ki passed through constructor
- Boardcore::PIController* controller;
-
- /**
- * @brief Actual algorithm implementation, all parameters should be in NED
- *
- * @param state NAS current state
- * @param targetNED Target North & East
- * @param windNED Wind velocity North & East
- */
- float algorithmStep(Boardcore::NASState state, Eigen::Vector2f windNED);
-
- /**
- * @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)
- */
- float angleDiff(float a, float b);
-
- // Logging structure
- WingAlgorithmData data;
-
- /**
- * @brief Mutex
- */
- miosix::FastMutex mutex;
-};
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Wing/FileWingAlgorithm.cpp b/src/boards/Parafoil/Wing/FileWingAlgorithm.cpp
deleted file mode 100644
index 9d784f51cdbca120fde1ebd334861f000852cfff..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/FileWingAlgorithm.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/* 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.
- */
-
-#include <Parafoil/Wing/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/boards/Parafoil/Wing/FileWingAlgorithm.h b/src/boards/Parafoil/Wing/FileWingAlgorithm.h
deleted file mode 100644
index 5b2d61f792906695735ab30ba778ce8db22f28cc..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/FileWingAlgorithm.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/* 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/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp b/src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
deleted file mode 100644
index d0416a404c16d946f590c2be27241e1b5449e3f1..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-/* 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.
- */
-
-#include <Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
-#include <math.h>
-
-namespace Parafoil
-{
-
-float ClosedLoopGuidanceAlgorithm::calculateTargetAngle(
- const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
-{
- heading[0] = targetNED[0] - currentPositionNED[0];
- heading[1] = targetNED[1] - currentPositionNED[1];
- return atan2(heading[1], heading[0]);
-}
-
-void ClosedLoopGuidanceAlgorithm::setPoints(Eigen::Vector2f targetNED)
-{
- this->targetNED = targetNED;
-}
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h b/src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h
deleted file mode 100644
index bed2a087434b8318004e38f64d8c0a71036af006..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/* 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[in] target NED position of the target in [m]
- * @param[out] heading Saves the heading vector for logging purposes
- *
- * @returns the yaw angle of the parafoil in [rad]
- */
- float calculateTargetAngle(const Eigen::Vector3f& currentPositionNED,
- Eigen::Vector2f& heading) override;
-
-public:
- void setPoints(Eigen::Vector2f targetNED);
-};
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/boards/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
deleted file mode 100644
index c1f7701fa4766f46685d3b2b01b06121bb31e79b..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp
+++ /dev/null
@@ -1,150 +0,0 @@
-/* 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.
- */
-
-#include <Parafoil/Configs/WingConfig.h>
-#include <Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
-
-#include <Eigen/Core>
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-using namespace Parafoil::WingConfig;
-
-namespace Parafoil
-{
-
-EarlyManeuversGuidanceAlgorithm::EarlyManeuversGuidanceAlgorithm()
- : activeTarget(Target::EMC), targetAltitudeConfidence(0),
- m2AltitudeConfidence(0), m1AltitudeConfidence(0),
- emcAltitudeConfidence(0){};
-
-EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){};
-
-float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
- const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading)
-{
- using namespace Boardcore;
-
- float altitude = abs(currentPositionNED[2]);
-
- computeActiveTarget(altitude);
-
- 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 atan2(heading[1], heading[0]);
-}
-
-void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float 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
- }
-
- switch (activeTarget)
- {
- case Target::EMC:
- if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
- {
- activeTarget = Target::FINAL;
- emcAltitudeConfidence = 0;
- }
- else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
- {
- activeTarget = Target::M2;
- emcAltitudeConfidence = 0;
- }
- else if (m1AltitudeConfidence >= GUIDANCE_CONFIDENCE)
- {
- activeTarget = Target::M1;
- emcAltitudeConfidence = 0;
- }
- break;
- case Target::M1:
- if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
- {
- activeTarget = Target::FINAL;
- m1AltitudeConfidence = 0;
- }
- else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
- {
- activeTarget = Target::M2;
- m1AltitudeConfidence = 0;
- }
- break;
- case Target::M2:
- if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
- {
- activeTarget = Target::FINAL;
- m2AltitudeConfidence = 0;
- }
- break;
- case Target::FINAL:
- break;
- }
-}
-
-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;
-}
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h b/src/boards/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
deleted file mode 100644
index 2dc823c8b2c7f63de62a03b30ffda41041e39fa2..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/* 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
- *
- * requires: WingControllers
- */
-class EarlyManeuversGuidanceAlgorithm : public GuidanceAlgorithm
-{
-
-public:
- /**
- * @brief Constructor for the EM Algorithm
- *
- */
- 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
- * m
- * @param[in] targetPositionNED NED position of the target in m
- * @param[out] heading Saves the heading vector for logging purposes
- *
- * @returns the yaw angle of the parafoil in rad
- *
- */
- float calculateTargetAngle(const Eigen::Vector3f& currentPositionNED,
- Eigen::Vector2f& heading) override;
-
- /**
- * @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);
-
-private:
- /** @brief Updates the class target
- *
- */
- void computeActiveTarget(float altitude);
-
- /**
- * @brief Enumerates all the possible targets of the EM algorithm
- */
- enum class Target
- {
- EMC = 0,
- M1,
- M2,
- FINAL
- };
-
- // Point we are currently poinying to
- Target activeTarget;
-
- // Eigen::Vector2f targetNED; // NED
-
- Eigen::Vector2f EMC; // NED
-
- Eigen::Vector2f M1; // NED
-
- Eigen::Vector2f M2; // NED
-
- unsigned int targetAltitudeConfidence;
- unsigned int m2AltitudeConfidence;
- unsigned int m1AltitudeConfidence;
- unsigned int emcAltitudeConfidence;
-};
-
-} // namespace Parafoil
diff --git a/src/boards/Parafoil/Wing/Guidance/GuidanceAlgorithm.h b/src/boards/Parafoil/Wing/Guidance/GuidanceAlgorithm.h
deleted file mode 100644
index bd63b528432fe060dc2b2023d7777c57493da071..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/Guidance/GuidanceAlgorithm.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/* 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 <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[in] position the current NED position of the parafoil in [m]
- * @param[in] target NED position of the target in [m]
- * @param[out] heading The current heading, it is used for logging purposes
- *
- * @returns the yaw angle of the parafoil in [rad]
- */
- virtual float 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/boards/Parafoil/Wing/WingAlgorithm.cpp b/src/boards/Parafoil/Wing/WingAlgorithm.cpp
deleted file mode 100644
index c43d0302682e467a3f522f929d1958fb9f0c94cc..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/WingAlgorithm.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-/* 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.
- */
-
-#include <Parafoil/Configs/WingConfig.h>
-#include <Parafoil/Wing/WingAlgorithm.h>
-#include <common/Events.h>
-#include <drivers/timer/TimestampTimer.h>
-#include <events/EventBroker.h>
-
-#include <utils/ModuleManager/ModuleManager.hpp>
-
-using namespace Boardcore;
-using namespace Parafoil::WingConfig;
-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()
-{
- ModuleManager& modules = ModuleManager::getInstance();
- 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
- modules.get<Actuators>()->setServoAngle(servo1,
- steps[stepIndex].servo1Angle);
- modules.get<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/boards/Parafoil/Wing/WingAlgorithm.h b/src/boards/Parafoil/Wing/WingAlgorithm.h
deleted file mode 100644
index 33c1b7b3d90a39787cb06d16afa69557ae1a6dd3..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/WingAlgorithm.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/* 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 <miosix.h>
-namespace Parafoil
-{
-class WingAlgorithm : public Boardcore::Algorithm
-{
-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/boards/Parafoil/Wing/WingAlgorithmData.h b/src/boards/Parafoil/Wing/WingAlgorithmData.h
deleted file mode 100644
index 40e982483a1c5e9c430358b67b1b7d671e893efa..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/WingAlgorithmData.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/* 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 <sensors/SensorData.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; // First timestamp is 0 (in microseconds)
- float servo1Angle; // degrees
- float servo2Angle; // degrees
- float targetAngle; // radians (automatic only)
- float velocityAngle; // radians
- float targetX; // NED (only automatic algorithm)
- float targetY; // NED (only automatic algorithm)
- float error;
- float pidOutput;
-
- 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/boards/Parafoil/Wing/WingTargetPositionData.h b/src/boards/Parafoil/Wing/WingTargetPositionData.h
deleted file mode 100644
index 32aca881f24ec94f018bc62b14650a3b2e2cb23c..0000000000000000000000000000000000000000
--- a/src/boards/Parafoil/Wing/WingTargetPositionData.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Alberto Nidasio, 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 <ostream>
-#include <string>
-
-namespace Parafoil
-{
-
-struct WingTargetPositionData
-{
- float receivedLat;
- float receivedLon;
- float targetN;
- float targetE;
- float emcN;
- float emcE;
- float m1N;
- float m1E;
- float m2N;
- float m2E;
-
- static std::string header()
- {
- return "receivedLat, receivedLon, "
- "targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n";
- }
-
- void print(std::ostream& os) const
- {
- os << receivedLat << "," << receivedLon << "," << targetN << ","
- << targetE << "," << emcN << "," << emcE << "," << m1N << "," << m1E
- << "," << m2N << "," << m2E << "\n";
- }
-};
-
-} // namespace Parafoil
diff --git a/src/entrypoints/Parafoil/parafoil-entry.cpp b/src/entrypoints/Parafoil/parafoil-entry.cpp
index e515c772b66e8ab74d919bfeeba3f861a2b7b196..2d6f07de2d13326e4032da8d783b62e215e991f9 100644
--- a/src/entrypoints/Parafoil/parafoil-entry.cpp
+++ b/src/entrypoints/Parafoil/parafoil-entry.cpp
@@ -30,7 +30,6 @@
#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/TMRepository/TMRepository.h>
#include <Parafoil/WindEstimationScheme/WindEstimation.h>
#include <common/Events.h>
@@ -76,13 +75,9 @@ int main()
Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
AltitudeTrigger* altTrigger =
new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
- WingController* wingController =
- new WingController(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
WindEstimation* windEstimation =
new WindEstimation(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
- Actuators* actuators = new Actuators();
-
// Components without a scheduler
TMRepository* tmRepo = new TMRepository();
FlightModeManager* fmm = new FlightModeManager();
@@ -131,24 +126,12 @@ int main()
LOG_ERR(logger, "Error inserting the TMRepository module");
}
- if (!modules.insert<Actuators>(actuators))
- {
- initResult = false;
- LOG_ERR(logger, "Error inserting the Actuators module");
- }
-
if (!modules.insert<AltitudeTrigger>(altTrigger))
{
initResult = false;
LOG_ERR(logger, "Error inserting the Altitude Trigger module");
}
- if (!modules.insert<WingController>(wingController))
- {
- initResult = false;
- LOG_ERR(logger, "Error inserting the WingController module");
- }
-
if (!modules.insert<WindEstimation>(windEstimation))
{
initResult = false;
@@ -202,24 +185,12 @@ int main()
LOG_ERR(logger, "Error starting the Radio module");
}
- if (!modules.get<Actuators>()->start())
- {
- initResult = false;
- LOG_ERR(logger, "Error starting the Actuators module");
- }
-
if (!modules.get<AltitudeTrigger>()->start())
{
initResult = false;
LOG_ERR(logger, "Error starting the AltitudeTrigger module");
}
- if (!modules.get<WingController>()->start())
- {
- initResult = false;
- LOG_ERR(logger, "Error starting the WingController module");
- }
-
if (!modules.get<WindEstimation>()->start())
{
initResult = false;
@@ -259,13 +230,6 @@ int main()
LOG_ERR(logger, "Failed to initialize");
}
- modules.get<WingController>()->selectAlgorithm(
- WingConfig::SELECTED_ALGORITHM);
-
- // Log configs
- WingConfig::WingConfigStruct f;
- Logger::getInstance().log(f);
-
// Periodic statistics
while (true)
{