diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index cf03a37d920bcb398dbb165b0019aa71f6a3e716..3df9705e18d942811aa4596eb2fe8ea6b372e698 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -97,7 +97,6 @@ set(PAYLOAD_COMPUTER src/boards/Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.cpp src/boards/Payload/Wing/FileWingAlgorithm.cpp src/boards/Payload/Wing/WingAlgorithm.cpp - src/boards/Payload/WindEstimationScheme/WindEstimation.cpp ) set(GROUNDSTATION_BASE diff --git a/src/boards/Payload/BoardScheduler.h b/src/boards/Payload/BoardScheduler.h index 1340b8992aca633a0ebf11e35bb3c1aef7baa57a..0e9a0e33022fdfdd12705013a12bf87dc761a61a 100644 --- a/src/boards/Payload/BoardScheduler.h +++ b/src/boards/Payload/BoardScheduler.h @@ -60,7 +60,6 @@ public: Boardcore::TaskScheduler& altitudeTrigger() { return medium; } Boardcore::TaskScheduler& wingController() { return medium; } Boardcore::TaskScheduler& verticalVelocityTrigger() { return medium; } - Boardcore::TaskScheduler& windEstimation() { return medium; } Boardcore::TaskScheduler& actuators() { return low; } Boardcore::TaskScheduler& flightStatsRecorder() { return low; } diff --git a/src/boards/Payload/HIL/HIL.cpp b/src/boards/Payload/HIL/HIL.cpp index 9375f2e3bb19834f5a77b31557f7c78bd152c772..9505d87623c57332154c7aac3c2548091e3a5052 100644 --- a/src/boards/Payload/HIL/HIL.cpp +++ b/src/boards/Payload/HIL/HIL.cpp @@ -30,7 +30,6 @@ #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> #include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/WindEstimationScheme/WindEstimation.h> #include <common/Events.h> #include <events/EventBroker.h> #include <hil/HIL.h> @@ -255,7 +254,6 @@ bool PayloadHIL::start() ActuatorData PayloadHIL::updateActuatorData() { auto nas = getModule<Payload::NASController>(); - auto wes = getModule<Payload::WindEstimation>(); auto fmm = getModule<Payload::FlightModeManager>(); auto wing = getModule<Payload::WingController>(); auto actuators = getModule<Payload::Actuators>(); @@ -267,8 +265,6 @@ ActuatorData PayloadHIL::updateActuatorData() actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO), static_cast<float>(miosix::gpios::mainDeploy::value())); - WESDataHIL wesDataHIL(wes->getWindEstimationScheme()); - auto deltaA = actuators->getServoPosition(ServosList::PARAFOIL_LEFT_SERVO) - actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO); deltaA /= 10; // Mapping to interval [-0.1, 0.1] @@ -282,7 +278,7 @@ ActuatorData PayloadHIL::updateActuatorData() // Returning the feedback for the simulator return ActuatorData( - nasStateHIL, actuatorsStateHIL, wesDataHIL, guidanceData, + nasStateHIL, actuatorsStateHIL, guidanceData, (fmm->testState(&Payload::FlightModeManager::FlyingAscending) ? 3 : 0)); }; } // namespace Payload \ No newline at end of file diff --git a/src/boards/Payload/HIL/HIL.h b/src/boards/Payload/HIL/HIL.h index 72bd8b5a27b81878fd9e08fff73fa73346d92cdf..1275b2985922ce2aaf9611106c74e389ab79b081 100644 --- a/src/boards/Payload/HIL/HIL.h +++ b/src/boards/Payload/HIL/HIL.h @@ -29,7 +29,6 @@ #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> #include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/WindEstimationScheme/WindEstimation.h> #include <common/Events.h> #include <hil/HIL.h> #include <utils/DependencyManager/DependencyManager.h> @@ -69,8 +68,7 @@ class PayloadHIL : public Boardcore::HIL<PayloadFlightPhases, SimulatorData, ActuatorData>, public Boardcore::InjectableWithDeps< Payload::Buses, Payload::Actuators, Payload::FlightModeManager, - Payload::WindEstimation, Payload::WingController, - Payload::NASController> + Payload::WingController, Payload::NASController> { public: diff --git a/src/boards/Payload/HIL/HILData.h b/src/boards/Payload/HIL/HILData.h index 71f0f04d9f8dbaa336c94817bc3b7eac6ea72587..fed7c3d1194cb1ec80caade5915eb77d0a2ca8f7 100644 --- a/src/boards/Payload/HIL/HILData.h +++ b/src/boards/Payload/HIL/HILData.h @@ -159,21 +159,6 @@ struct ActuatorsStateHIL } }; -struct WESDataHIL -{ - float windX; - float windY; - - explicit WESDataHIL(const Eigen::Vector2f& wind) - : windX(wind[0]), windY(wind[1]) - { - } - - WESDataHIL() : windX(0.0f), windY(0.0f) {} - - void print() { printf("wind: [%f,%f]\n", windX, windY); } -}; - struct GuidanceDataHIL { float psiRef; @@ -230,20 +215,15 @@ struct ActuatorData { NASStateHIL nasState; ActuatorsStateHIL actuatorsState; - WESDataHIL wesData; GuidanceDataHIL guidanceData; float signal; - ActuatorData() - : nasState(), actuatorsState(), wesData(), guidanceData(), signal(0) - { - } + ActuatorData() : nasState(), actuatorsState(), guidanceData(), signal(0) {} ActuatorData(const NASStateHIL& nasState, const ActuatorsStateHIL& actuatorsState, - const WESDataHIL& wesData, const GuidanceDataHIL& guidanceData, - float signal) - : nasState(nasState), actuatorsState(actuatorsState), wesData(wesData), + const GuidanceDataHIL& guidanceData, float signal) + : nasState(nasState), actuatorsState(actuatorsState), guidanceData(guidanceData), signal(signal) { } @@ -252,7 +232,6 @@ struct ActuatorData { nasState.print(); actuatorsState.print(); - wesData.print(); guidanceData.print(); } }; diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp index f9ab030c8dff454698f163b980be98568a6ad268..ed5ecc6ca741031b2e4a79bc22dc36bb1305f2c9 100644 --- a/src/boards/Payload/Radio/MessageHandler.cpp +++ b/src/boards/Payload/Radio/MessageHandler.cpp @@ -30,7 +30,6 @@ #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> #include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/WindEstimationScheme/WindEstimation.h> #include <common/Events.h> #include <common/Topics.h> #include <diagnostic/CpuMeter/CpuMeter.h> @@ -562,7 +561,6 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) auto* sensors = parent.getModule<Sensors>(); auto* nas = parent.getModule<NASController>(); - auto* wes = parent.getModule<WindEstimation>(); auto* fmm = parent.getModule<FlightModeManager>(); auto imu = sensors->getIMULastSample(); @@ -572,7 +570,6 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) auto pressDynamic = sensors->getDynamicPressureLastSample(); auto nasState = nas->getNasState(); auto ref = nas->getReferenceValues(); - auto wind = wes->getWindEstimationScheme(); float airspeedPitot = (pressDynamic.pressure > 0 @@ -623,8 +620,8 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) tm.nas_bias_x = nasState.bx; tm.nas_bias_y = nasState.by; tm.nas_bias_z = nasState.bz; - tm.wes_n = wind.x(); - tm.wes_e = wind.y(); + tm.wes_n = -1.0f; + tm.wes_e = -1.0f; tm.battery_voltage = sensors->getBatteryVoltage().batVoltage; tm.cam_battery_voltage = sensors->getCamBatteryVoltage().batVoltage; diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h index 6353e4d723b82053b9764cb72925fc41108f86a3..817e961f88f4351086d5334be52feff8deb124e6 100644 --- a/src/boards/Payload/Radio/Radio.h +++ b/src/boards/Payload/Radio/Radio.h @@ -47,13 +47,12 @@ class WingController; class AltitudeTrigger; class PinHandler; class CanHandler; -class WindEstimation; class FlightStatsRecorder; class Radio : public Boardcore::InjectableWithDeps< BoardScheduler, Sensors, Buses, FlightModeManager, Actuators, NASController, WingController, AltitudeTrigger, PinHandler, - CanHandler, WindEstimation, FlightStatsRecorder> + CanHandler, FlightStatsRecorder> { public: /** diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp index 466011afa89aed1c38d0ed4562068fb186040dd8..13205008544097e13bcc5fb6e7f5a6ebf44599ba 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.cpp +++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp @@ -29,7 +29,6 @@ #include <Payload/FlightStatsRecorder/FlightStatsRecorder.h> #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> -#include <Payload/WindEstimationScheme/WindEstimation.h> #include <Payload/Wing/AutomaticWingAlgorithm.h> #include <Payload/Wing/FileWingAlgorithm.h> #include <Payload/Wing/WingAlgorithm.h> @@ -265,8 +264,6 @@ State WingController::OnGround(const Boardcore::Event& event) { updateState(WingControllerState::ON_GROUND); - getModule<WindEstimation>()->stopWindEstimationScheme(); - getModule<WindEstimation>()->stopWindEstimationSchemeCalibration(); stopAlgorithm(); resetWing(); diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h index 4f0274654aa644dd7e553ba1442dde1bc6f73451..c6da60c22ae81caa672f99b5edf03473418e7b69 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.h +++ b/src/boards/Payload/StateMachines/WingController/WingController.h @@ -61,14 +61,12 @@ namespace Payload class BoardScheduler; class Actuators; class NASController; -class WindEstimation; class FlightStatsRecorder; class WingController : public Boardcore::HSM<WingController>, public Boardcore::InjectableWithDeps<BoardScheduler, Actuators, - NASController, WindEstimation, - FlightStatsRecorder> + NASController, FlightStatsRecorder> { public: /** diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp b/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp deleted file mode 100644 index 4c5df6049aa68a1195f2377d97ae249d90ade402..0000000000000000000000000000000000000000 --- a/src/boards/Payload/WindEstimationScheme/WindEstimation.cpp +++ /dev/null @@ -1,214 +0,0 @@ -/* 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 "WindEstimation.h" - -#include <Payload/BoardScheduler.h> -#include <Payload/Configs/WESConfig.h> -#include <Payload/Sensors/Sensors.h> -#include <common/Events.h> -#include <events/EventBroker.h> - -using namespace Boardcore; -using namespace Common; -namespace config = Payload::Config::WES; - -namespace Payload -{ - -WindEstimation::WindEstimation() : running(false), calRunning(false) -{ - funv << 1.0f, 0.0f, 0.0f, 1.0f; // cppcheck-suppress constStatement -} - -bool WindEstimation::start() -{ - using namespace std::chrono; - - auto& scheduler = getModule<BoardScheduler>()->windEstimation(); - - scheduler.addTask([this] { windEstimationSchemeCalibration(); }, - config::Calibration::SAMPLE_RATE); - - // Register the WES task - scheduler.addTask([this] { windEstimationScheme(); }, - config::Prediction::UPDATE_RATE); - - return true; -} - -WindEstimation::~WindEstimation() -{ - stopWindEstimationSchemeCalibration(); - stopWindEstimationScheme(); -} - -bool WindEstimation::getStatus() { return running; } - -void WindEstimation::startWindEstimationSchemeCalibration() -{ - if (!calRunning && !running) - { - calRunning = true; - nSampleCal = 0; - vn = 0; - ve = 0; - v2 = 0; - LOG_INFO(logger, "WindEstimationCalibration started"); - } -} - -void WindEstimation::stopWindEstimationSchemeCalibration() -{ - calRunning = false; - LOG_INFO(logger, "WindEstimationSchemeCalibration stopped"); - // assign value to the array only if running != false; - if (!running) - { - miosix::Lock<FastMutex> l(mutex); - wind = windCalibration; - } - windLogger.cal = true; - windLogger.vn = windCalibration[0]; - windLogger.ve = windCalibration[1]; - startWindEstimationScheme(); - logStatus(); -} - -void WindEstimation::windEstimationSchemeCalibration() -{ - - if (calRunning) - { - auto gpsData = getModule<Sensors>()->getUBXGPSLastSample(); - if (gpsData.fix != 0) - { - if (nSampleCal < config::Calibration::SAMPLE_COUNT) - { - float gpsN = 0, gpsE = 0; - - gpsN = gpsData.velocityNorth; - gpsE = gpsData.velocityEast; - calibrationMatrix(nSampleCal, 0) = gpsN; - calibrationMatrix(nSampleCal, 1) = gpsE; - calibrationV2(nSampleCal) = gpsN * gpsN + gpsE * gpsE; - - vn += gpsN; - ve += gpsE; - v2 += gpsN * gpsN + gpsE * gpsE; - nSampleCal++; - } - else - { - vn = vn / nSampleCal; - ve = ve / nSampleCal; - v2 = v2 / nSampleCal; - for (int i = 0; i < nSampleCal; i++) - { - calibrationMatrix(i, 0) = calibrationMatrix(i, 0) - vn; - calibrationMatrix(i, 1) = calibrationMatrix(i, 1) - ve; - calibrationV2(i) = 0.5f * (calibrationV2(i) - v2); - } - Eigen::MatrixXf calibrationMatrixT = - calibrationMatrix.transpose(); - windCalibration = - (calibrationMatrixT * calibrationMatrix) - .ldlt() - .solve(calibrationMatrixT * calibrationV2); - stopWindEstimationSchemeCalibration(); - } - } - } -} - -void WindEstimation::startWindEstimationScheme() -{ - if (!running) - { - running = true; - // nSample has to be reset after the calibration - nSample = nSampleCal; - LOG_INFO(logger, "WindEstimationScheme started"); - } -} - -void WindEstimation::stopWindEstimationScheme() -{ - if (running) - { - running = false; - LOG_INFO(logger, "WindEstimationScheme ended"); - } -} - -void WindEstimation::windEstimationScheme() -{ - if (running) - { - auto gpsData = getModule<Sensors>()->getUBXGPSLastSample(); - if (gpsData.fix != 0) - { - Eigen::Vector2f phi; - Eigen::Matrix<float, 1, 2> phiT; - Eigen::Vector2f temp; - float y, gpsN = 0, gpsE = 0; - gpsN = gpsData.velocityNorth; - gpsE = gpsData.velocityEast; - // update avg - nSample++; - vn = (vn * nSample + gpsN) / (nSample + 1); - ve = (ve * nSample + gpsE) / (nSample + 1); - v2 = (v2 * nSample + (gpsN * gpsN + gpsE * gpsE)) / (nSample + 1); - phi(0) = gpsN - vn; - phi(1) = gpsE - ve; - y = 0.5f * ((gpsN * gpsN + gpsE * gpsE) - v2); - - phiT = phi.transpose(); - funv = - (funv - (funv * phi * phiT * funv) / (1 + (phiT * funv * phi))); - temp = (0.5 * (funv + funv.transpose()) * phi) * - (y - phiT * getWindEstimationScheme()); - { - miosix::Lock<FastMutex> l(mutex); - wind = wind + temp; - windLogger.vn = wind[0]; - windLogger.ve = wind[1]; - windLogger.cal = false; - } - logStatus(); - } - } -} - -Eigen::Vector2f WindEstimation::getWindEstimationScheme() -{ - miosix::Lock<FastMutex> l(mutex); - return wind; -} - -void WindEstimation::logStatus() -{ - windLogger.timestamp = TimestampTimer::getTimestamp(); - Logger::getInstance().log(windLogger); -} - -} // namespace Payload diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimation.h b/src/boards/Payload/WindEstimationScheme/WindEstimation.h deleted file mode 100644 index b190beee531fb54178264490db57079bca9a81b1..0000000000000000000000000000000000000000 --- a/src/boards/Payload/WindEstimationScheme/WindEstimation.h +++ /dev/null @@ -1,130 +0,0 @@ -/* 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 <Payload/Configs/WESConfig.h> -#include <diagnostic/PrintLogger.h> -#include <utils/DependencyManager/DependencyManager.h> - -#include <Eigen/Core> -#include <atomic> - -#include "WindEstimationData.h" - -namespace Payload -{ -class BoardScheduler; -class Sensors; - -/** - * @brief This class implements the wind prediction algorithm, the first part is - * the initial setup, and then the continuos algoritms runs; - */ -class WindEstimation - : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors> -{ -public: - /** - * @brief Construct a new Wing Estimation object - */ - WindEstimation(); - - /** - * @brief Stops the wind estimation scheme and calibration tasks. - */ - ~WindEstimation(); - - bool start(); - - void startWindEstimationSchemeCalibration(); - - void stopWindEstimationSchemeCalibration(); - - void startWindEstimationScheme(); - - void stopWindEstimationScheme(); - - bool getStatus(); - - Eigen::Vector2f getWindEstimationScheme(); - -private: - /** - * @brief Creates the windCalibration matrix with the starting prediction - * value - */ - void windEstimationSchemeCalibration(); - - /** - * @brief Updates the wind matrix with the updated wind prediction values - */ - void windEstimationScheme(); - - /** - * @brief Logs the prediction - */ - void logStatus(); - - /** - * @brief Parameters needed for calibration - */ - Eigen::Vector2f windCalibration{0.0f, 0.0f}; - uint8_t nSampleCal = 0; - Eigen::Matrix<float, Config::WES::Calibration::SAMPLE_COUNT, 2> - calibrationMatrix; - Eigen::Vector<float, Config::WES::Calibration::SAMPLE_COUNT> calibrationV2; - float vn = 0; - float ve = 0; - float v2 = 0; - - /** - * @brief Parameters needed for recursive - */ - Eigen::Vector2f wind{0.0f, 0.0f}; - uint8_t nSample = 0; - Eigen::Matrix2f funv; - - /** - * @brief Mutex - */ - miosix::FastMutex mutex; - - /** - * @brief PrintLogger - */ - Boardcore::PrintLogger logger = - Boardcore::Logging::getLogger("WindEstimation"); - - /** - * @brief Logging struct - */ - WindLogging windLogger{0, 0, 0}; - - /** - * @brief Internal running state - */ - std::atomic<bool> running; - std::atomic<bool> calRunning; -}; - -} // namespace Payload diff --git a/src/boards/Payload/WindEstimationScheme/WindEstimationData.h b/src/boards/Payload/WindEstimationScheme/WindEstimationData.h deleted file mode 100644 index 3104692f7dd2e80f09209e4f2a183569c6db4fa9..0000000000000000000000000000000000000000 --- a/src/boards/Payload/WindEstimationScheme/WindEstimationData.h +++ /dev/null @@ -1,47 +0,0 @@ -/* 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 <stdint.h> - -#include <iostream> -#include <string> - -namespace Payload -{ - -struct WindLogging -{ - long long timestamp = 0; - float vn = 0, ve = 0; - bool cal = 0; - - static std::string header() { return "timestamp,vn,ve,cal\n"; } - - void print(std::ostream& os) const - { - os << timestamp << "," << vn << "," << ve << "," << cal << "\n "; - } -}; - -} // namespace Payload diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp index 278894a11ef4a050f97b6890b01ed023beb36339..aeb7b1c4c8760532a0f728afe0849b5aa0f1fc50 100644 --- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp +++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp @@ -26,7 +26,6 @@ #include <Payload/Sensors/Sensors.h> #include <Payload/StateMachines/NASController/NASController.h> #include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/WindEstimationScheme/WindEstimation.h> #include <drivers/timer/TimestampTimer.h> #include <math.h> #include <utils/AeroUtils/AeroUtils.h> @@ -57,9 +56,7 @@ void AutomaticWingAlgorithm::step() if (getModule<Sensors>()->getUBXGPSLastSample().fix != 0) { // The PI calculated result - float result = algorithmStep( - getModule<NASController>()->getNasState(), - getModule<WindEstimation>()->getWindEstimationScheme()); + float result = algorithmStep(getModule<NASController>()->getNasState()); // Actuate the result // To see how to interpret the PI output @@ -101,7 +98,7 @@ void AutomaticWingAlgorithm::step() } } -float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED) +float AutomaticWingAlgorithm::algorithmStep(const NASState& state) { float result; // For some algorithms the third component is needed! @@ -111,7 +108,6 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED) float targetAngle = guidance.calculateTargetAngle(currentPosition, heading); - // WES is currently unused Vector2f relativeVelocity(state.vn, state.ve); // Compute the angle of the current velocity diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.h b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h index 7d3eb87c73634bae18b78a4a541928a21ab1c5af..f5e979aa3222c27b2d350fb17ca1713f1203aedb 100644 --- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.h +++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.h @@ -34,13 +34,11 @@ namespace Payload { class Sensors; class NASController; -class WindEstimation; class Actuators; -class AutomaticWingAlgorithm - : public Boardcore::InjectableWithDeps< - Boardcore::InjectableBase<WingAlgorithm>, Sensors, NASController, - WindEstimation, Actuators> +class AutomaticWingAlgorithm : public Boardcore::InjectableWithDeps< + Boardcore::InjectableBase<WingAlgorithm>, + Sensors, NASController, Actuators> { public: /** @@ -66,11 +64,10 @@ protected: /** * @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 + * @return The angle to set to the servo, positive is right and negative is + * left */ - float algorithmStep(Boardcore::NASState state, Eigen::Vector2f windNED); + float algorithmStep(const Boardcore::NASState& state); /** * @brief This method implements the automatic algorithm that will steer the diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp index 90c4c6ff6ded2c09d592cb038874c2ce4f0e359f..7e9cfb18c6d5df68279ebd8cc7d34e27b2abc346 100644 --- a/src/entrypoints/Payload/payload-entry.cpp +++ b/src/entrypoints/Payload/payload-entry.cpp @@ -37,7 +37,6 @@ #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> #include <Payload/StateMachines/WingController/WingController.h> -#include <Payload/WindEstimationScheme/WindEstimation.h> #include <common/Events.h> #include <common/Topics.h> #include <diagnostic/CpuMeter/CpuMeter.h> @@ -151,8 +150,6 @@ int main() initResult &= depman.insert(altitudeTrigger); auto wingController = new WingController(); initResult &= depman.insert(wingController); - auto windEstimation = new WindEstimation(); - initResult &= depman.insert(windEstimation); // Actuators auto actuators = new Actuators(); @@ -199,7 +196,6 @@ int main() START_MODULE(nasController); START_MODULE(altitudeTrigger); START_MODULE(wingController); - START_MODULE(windEstimation); START_MODULE(actuators); START_MODULE(scheduler); diff --git a/src/scripts/logdecoder/Payload/logdecoder.cpp b/src/scripts/logdecoder/Payload/logdecoder.cpp index 84e301a5960c351830e5153523df127b02bb7e09..d068a2b551e03bb282d93bce0bea9bc08832b3b4 100644 --- a/src/scripts/logdecoder/Payload/logdecoder.cpp +++ b/src/scripts/logdecoder/Payload/logdecoder.cpp @@ -25,7 +25,6 @@ #include <Payload/StateMachines/FlightModeManager/FlightModeManagerData.h> #include <Payload/StateMachines/NASController/NASControllerData.h> #include <Payload/StateMachines/WingController/WingControllerData.h> -#include <Payload/WindEstimationScheme/WindEstimationData.h> #include <Payload/Wing/WingAlgorithmData.h> #include <Payload/Wing/WingTargetPositionData.h> #include <fmt/format.h>