diff --git a/cmake/boardcore-host.cmake b/cmake/boardcore-host.cmake index 6f22f2b1fde182ffdb17fb4b6470bc51de995bb8..116277fb3aaff2ab1fef29923c2f6414165fbb21 100644 --- a/cmake/boardcore-host.cmake +++ b/cmake/boardcore-host.cmake @@ -34,8 +34,8 @@ set(BOARDCORE_HOST_SRC # Algorithms ${SBS_BASE}/src/shared/algorithms/MEA/MEA.cpp + ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp - ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakes.cpp # Logger ${SBS_BASE}/src/shared/logger/Logger.cpp diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index 1f725d32bd35de35b3412b66125c39c2b3956e3a..5a53153f43b849cb58d00a2589228ea5ae847241 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -38,7 +38,6 @@ set(BOARDCORE_SRC # Algorithms ${BOARDCORE_PATH}/src/shared/algorithms/ADA/ADA.cpp ${BOARDCORE_PATH}/src/shared/algorithms/MEA/MEA.cpp - ${BOARDCORE_PATH}/src/shared/algorithms/AirBrakes/AirBrakes.cpp ${BOARDCORE_PATH}/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp ${BOARDCORE_PATH}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp ${BOARDCORE_PATH}/src/shared/algorithms/NAS/NAS.cpp diff --git a/src/shared/algorithms/AirBrakes/AirBrakes.cpp b/src/shared/algorithms/AirBrakes/AirBrakes.cpp deleted file mode 100644 index 87f6615cfa0ad0b1c719e80772a676eb9fb599a5..0000000000000000000000000000000000000000 --- a/src/shared/algorithms/AirBrakes/AirBrakes.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/* Copyright (c) 2021-2022 Skyward Experimental Rocketry - * Authors: Vincenzo Santomarco, Alberto Nidasio, Emilio Corigliano - * - * 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 "AirBrakes.h" - -#include <logger/Logger.h> -#include <math.h> -#include <utils/Constants.h> - -#include <limits> - -#include "drivers/timer/TimestampTimer.h" -#include "utils/Debug.h" - -using namespace std; - -namespace Boardcore -{ - -AirBrakes::AirBrakes(function<TimedTrajectoryPoint()> getCurrentPosition, - const AirBrakesConfig &config, - std::function<void(float)> setActuator) - : getCurrentPosition(getCurrentPosition), config(config), - setActuator(setActuator) -{ -} - -float AirBrakes::getRho(float z) -{ - return Constants::RHO_0 * expf(-z / Constants::Hn); -} - -float AirBrakes::getSurface(const TimedTrajectoryPoint ¤tPosition, - float rho, float targetDrag) -{ - float bestDDrag = numeric_limits<float>::infinity(); - float bestSurface = 0; - - // TODO: Drags are monotone, here the algorithm can be more efficient - for (uint8_t step = 0; step < config.DRAG_STEPS; step++) - { - float surface = (step / (config.DRAG_STEPS - 1)) * config.SURFACE; - - float extension = getExtension(surface); - float cd = getCD(currentPosition, extension); - float drag = getDrag(currentPosition, cd, rho); - float dDrag = abs(targetDrag - drag); - - if (dDrag < bestDDrag) - { - bestDDrag = dDrag; - bestSurface = surface; - } - } - - return bestSurface; -} - -float AirBrakes::getExtension(float surface) -{ - // clang-format off - return - config.EXT_POL_1 * powf(surface, 4) + - config.EXT_POL_2 * powf(surface, 3) + - config.EXT_POL_3 * powf(surface, 2) + - config.EXT_POL_4 * surface; - // clang-format on -} - -float AirBrakes::getCD(TimedTrajectoryPoint currentPosition, float extension) -{ - const float mach1 = currentPosition.getMac(); - const float mach2 = powf(mach1, 2); - const float mach3 = powf(mach1, 3); - const float mach4 = powf(mach1, 4); - const float mach5 = powf(mach1, 5); - const float mach6 = powf(mach1, 6); - - const float extension2 = powf(extension, 2); - - // clang-format off - return - config.N000 + - config.N100 * mach1 + - config.N200 * mach2 + - config.N300 * mach3 + - config.N400 * mach4 + - config.N500 * mach5 + - config.N600 * mach6 + - config.N010 * extension + - config.N020 * extension2 + - config.N110 * extension * mach1 + - config.N120 * extension2 * mach1 + - config.N210 * extension * mach2 + - config.N220 * extension2 * mach2 + - config.N310 * extension * mach3 + - config.N320 * extension2 * mach3 + - config.N410 * extension * mach4 + - config.N420 * extension2 * mach4 + - config.N510 * extension * mach5 + - config.N520 * extension2 * mach5 + - config.N001 * currentPosition.z; - // clang-format on -} - -float AirBrakes::getDrag(TimedTrajectoryPoint currentPosition, float cd, - float rho) -{ - return 0.5 * rho * config.S0 * cd * currentPosition.vz * - currentPosition.vMod; -} - -} // namespace Boardcore diff --git a/src/shared/algorithms/AirBrakes/AirBrakes.h b/src/shared/algorithms/AirBrakes/AirBrakes.h deleted file mode 100644 index 86115a382eb893a52211e21e02b0d3b425a1966e..0000000000000000000000000000000000000000 --- a/src/shared/algorithms/AirBrakes/AirBrakes.h +++ /dev/null @@ -1,106 +0,0 @@ -/* Copyright (c) 2021-2022 Skyward Experimental Rocketry - * Authors: Vincenzo Santomarco, Alberto Nidasio, Emilio Corigliano - * - * 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 <algorithms/Algorithm.h> -#include <algorithms/PIController.h> - -#include <functional> -#include <vector> - -#include "AirBrakesConfig.h" -#include "AirBrakesData.h" -#include "TrajectorySet.h" - -namespace Boardcore -{ - -class AirBrakes : public Algorithm -{ -public: - AirBrakes(std::function<TimedTrajectoryPoint()> getCurrentPosition, - const AirBrakesConfig &config, - std::function<void(float)> setActuator); - -protected: - /** - * @brief Returns the air density at the current altitude using the basic - * atmosphere model. - * - * @param z The current altitude [m]. - * @return The density of air according to current altitude [Kg/m^3] - */ - float getRho(float z); - - /** - * @brief Compute the necessary airbrakes surface to match the - * given drag force from the Pid as closely as possible. - * - * @param currentPosition Current rocket position. - * @param rho Air density [kg/m^2]. - * @param drag Target drag force. - * @return AirBrakes surface [m]; - */ - float getSurface(const TimedTrajectoryPoint ¤tPosition, float rho, - float drag); - - /** - * @brief Computes the airbrakes extension given the desired area. - * - * @param surface Desired airbrakes surface [m^2]. - * @return The radial extension [m]. - */ - float getExtension(float surface); - - /** - * @brief Returns the coefficient of drag for the airbrakes at the given - * position and with the given extension. - * - * @param currentPosition Current rocket position. - * @param extension AirBrakes extension [m]. - * @return The coefficient drag [1]. - */ - float getCD(TimedTrajectoryPoint currentPosition, float extension); - - /** - * @brief Return the drag generated from the AirBrakes in the given - * conditions. - * - * @param currentPosition Current position of the rocket. - * @param cd Coefficient of drag [1]. - * @param rho Air density [kg/m^2]. - * @return Generated drag [N]. - */ - float getDrag(TimedTrajectoryPoint currentPosition, float cd, float rho); - -protected: - std::function<TimedTrajectoryPoint()> getCurrentPosition; - const AirBrakesConfig &config; - std::function<void(float)> setActuator; - - TimedTrajectoryPoint lastPosition; - uint32_t lastSelectedPointIndex = 0; -}; - -} // namespace Boardcore diff --git a/src/shared/algorithms/AirBrakes/AirBrakesConfig.h b/src/shared/algorithms/AirBrakes/AirBrakesConfig.h deleted file mode 100644 index 17171252214bc4385d8052ad6315c2356aa32080..0000000000000000000000000000000000000000 --- a/src/shared/algorithms/AirBrakes/AirBrakesConfig.h +++ /dev/null @@ -1,96 +0,0 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Alberto Nidasio, Emilio Corigliano - * - * 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 - -namespace Boardcore -{ - -struct AirBrakesConfig -{ - // Coefficient of drag coefficients. - float N000; - float N100; - float N200; - float N300; - float N400; - float N500; - float N600; - float N010; - float N020; - float N110; - float N120; - float N210; - float N220; - float N310; - float N320; - float N410; - float N420; - float N510; - float N520; - float N001; - - // Aibrakes extension. - float EXTENSION; ///< [m] - uint8_t DRAG_STEPS; - float EXT_POL_1; - float EXT_POL_2; - float EXT_POL_3; - float EXT_POL_4; - - float S0; ///< Rocket surface [m^2] - float SURFACE; ///< AirBrakes max surface [m^2] - - AirBrakesConfig(const AirBrakesConfig &abkConfig) - : N000(abkConfig.N000), N100(abkConfig.N100), N200(abkConfig.N200), - N300(abkConfig.N300), N400(abkConfig.N400), N500(abkConfig.N500), - N600(abkConfig.N600), N010(abkConfig.N010), N020(abkConfig.N020), - N110(abkConfig.N110), N120(abkConfig.N120), N210(abkConfig.N210), - N220(abkConfig.N220), N310(abkConfig.N310), N320(abkConfig.N320), - N410(abkConfig.N410), N420(abkConfig.N420), N510(abkConfig.N510), - N520(abkConfig.N520), N001(abkConfig.N001), - EXTENSION(abkConfig.EXTENSION), DRAG_STEPS(abkConfig.DRAG_STEPS), - EXT_POL_1(abkConfig.EXT_POL_1), EXT_POL_2(abkConfig.EXT_POL_2), - EXT_POL_3(abkConfig.EXT_POL_3), EXT_POL_4(abkConfig.EXT_POL_4), - S0(abkConfig.S0), SURFACE(abkConfig.SURFACE) - { - } - - AirBrakesConfig(float N000, float N100, float N200, float N300, float N400, - float N500, float N600, float N010, float N020, float N110, - float N120, float N210, float N220, float N310, float N320, - float N410, float N420, float N510, float N520, float N001, - float EXTENSION, uint8_t DRAG_STEPS, float EXT_POL_1, - float EXT_POL_2, float EXT_POL_3, float EXT_POL_4, float S0, - float SURFACE) - : N000(N000), N100(N100), N200(N200), N300(N300), N400(N400), - N500(N500), N600(N600), N010(N010), N020(N020), N110(N110), - N120(N120), N210(N210), N220(N220), N310(N310), N320(N320), - N410(N410), N420(N420), N510(N510), N520(N520), N001(N001), - EXTENSION(EXTENSION), DRAG_STEPS(DRAG_STEPS), EXT_POL_1(EXT_POL_1), - EXT_POL_2(EXT_POL_2), EXT_POL_3(EXT_POL_3), EXT_POL_4(EXT_POL_4), - S0(S0), SURFACE(SURFACE) - { - } -}; - -} // namespace Boardcore diff --git a/src/shared/algorithms/AirBrakes/AirBrakesData.h b/src/shared/algorithms/AirBrakes/AirBrakesData.h deleted file mode 100644 index e4a1ee8275ff564fdcaf45fd06a97af3a6a6f653..0000000000000000000000000000000000000000 --- a/src/shared/algorithms/AirBrakes/AirBrakesData.h +++ /dev/null @@ -1,41 +0,0 @@ -/* Copyright (c) 2021-2022 Skyward Experimental Rocketry - * Authors: Vincenzo Santomarco, Alberto Nidasio - * - * 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> - -namespace Boardcore -{ - -struct AirBrakesChosenTrajectory -{ - uint8_t trajectory; - - static std::string header() { return "trajectory\n"; } - - void print(std::ostream& os) const { os << (int)trajectory << "\n"; } -}; - -} // namespace Boardcore diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp b/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp index 334c2ac34dbac815cebb29fa2983c03c868a34e1..4fa04719f61ec7850a0c23116906266635b0c0d4 100644 --- a/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp +++ b/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp @@ -36,15 +36,13 @@ namespace Boardcore AirBrakesInterp::AirBrakesInterp( std::function<TimedTrajectoryPoint()> getCurrentPosition, const TrajectorySet &trajectoryOpenSet, - const TrajectorySet &trajectoryCloseSet, const AirBrakesConfig &config, + const TrajectorySet &trajectoryCloseSet, const AirBrakesInterpConfig &configInterp, std::function<void(float)> setActuator) - : AirBrakes(getCurrentPosition, config, setActuator), + : getCurrentPosition(getCurrentPosition), setActuator(setActuator), trajectoryOpenSet(trajectoryOpenSet), trajectoryCloseSet(trajectoryCloseSet), configInterp(configInterp) { - // Initial values to avoid UB - lastPercentage = 0; } bool AirBrakesInterp::init() { return true; } diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterp.h b/src/shared/algorithms/AirBrakes/AirBrakesInterp.h index 29b1a6fcabe6fabc4f667a3d5372f083daeac748..f2e9ee6f397ed9f1ba91a07bddf9a693d3228c2f 100644 --- a/src/shared/algorithms/AirBrakes/AirBrakesInterp.h +++ b/src/shared/algorithms/AirBrakes/AirBrakesInterp.h @@ -22,27 +22,23 @@ #pragma once -#include <actuators/Servo/Servo.h> -#include <algorithms/PIController.h> +#include <algorithms/Algorithm.h> #include <functional> #include <vector> -#include "AirBrakes.h" -#include "AirBrakesData.h" #include "AirBrakesInterpConfig.h" #include "TrajectorySet.h" namespace Boardcore { -class AirBrakesInterp : public AirBrakes +class AirBrakesInterp : public Algorithm { public: AirBrakesInterp(std::function<TimedTrajectoryPoint()> getCurrentPosition, const TrajectorySet &trajectoryOpenSet, const TrajectorySet &trajectoryCloseSet, - const AirBrakesConfig &config, const AirBrakesInterpConfig &configInterp, std::function<void(float)> setActuator); @@ -70,6 +66,11 @@ private: */ float controlInterp(TrajectoryPoint currentPosition); + std::function<TimedTrajectoryPoint()> getCurrentPosition; + std::function<void(float)> setActuator; + + TimedTrajectoryPoint lastPosition; + // Trajectory sets (open and closed) from which the algorithm will choose at // the beginning the tuple with which interpolate the data. The selection // depends on the rocket mass. @@ -81,7 +82,7 @@ private: Trajectory *choosenOpenTrajectory = nullptr; AirBrakesInterpConfig configInterp; ///< specialized config - float lastPercentage; ///< last opening of the airbrakes + float lastPercentage = 0; ///< last opening of the airbrakes }; } // namespace Boardcore diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h b/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h index ec05294b7e7ad1b6439e526d8241dcbaa82cb26a..c3d96ac301998460524322992bcd7590f473bea4 100644 --- a/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h +++ b/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h @@ -21,7 +21,6 @@ */ #pragma once -#include "AirBrakesConfig.h" namespace Boardcore { diff --git a/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp b/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp index 4e4f868ee05ffbb16f48237b13e0344296217544..bd1866637aacfe068184598755f2a450d5d1fed6 100644 --- a/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp +++ b/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp @@ -30,6 +30,7 @@ #include "drivers/timer/TimestampTimer.h" #include "utils/Debug.h" + using namespace std; namespace Boardcore @@ -37,10 +38,11 @@ namespace Boardcore AirBrakesPI::AirBrakesPI( std::function<TimedTrajectoryPoint()> getCurrentPosition, - const TrajectorySet &trajectorySet, const AirBrakesConfig &config, - const AirBrakesPIConfig &configPI, std::function<void(float)> setActuator) - : AirBrakes(getCurrentPosition, config, setActuator), - pi(configPI.KP, configPI.KI, configPI.TS), trajectorySet(trajectorySet) + const TrajectorySet &trajectorySet, const AirBrakesPIConfig &config, + std::function<void(float)> setActuator) + : getCurrentPosition(getCurrentPosition), config(config), + setActuator(setActuator), pi(config.KP, config.KI, config.TS), + trajectorySet(trajectorySet) { } @@ -74,6 +76,91 @@ void AirBrakesPI::step() setActuator(surface / config.SURFACE); } +float AirBrakesPI::getRho(float z) +{ + return Constants::RHO_0 * expf(-z / Constants::Hn); +} + +float AirBrakesPI::getSurface(const TimedTrajectoryPoint ¤tPosition, + float rho, float targetDrag) +{ + float bestDDrag = numeric_limits<float>::infinity(); + float bestSurface = 0; + + // TODO: Drags are monotone, here the algorithm can be more efficient + for (uint8_t step = 0; step < config.DRAG_STEPS; step++) + { + float surface = (step / (config.DRAG_STEPS - 1)) * config.SURFACE; + + float extension = getExtension(surface); + float cd = getCD(currentPosition, extension); + float drag = getDrag(currentPosition, cd, rho); + float dDrag = abs(targetDrag - drag); + + if (dDrag < bestDDrag) + { + bestDDrag = dDrag; + bestSurface = surface; + } + } + + return bestSurface; +} + +float AirBrakesPI::getExtension(float surface) +{ + // clang-format off + return + config.EXT_POL_1 * powf(surface, 4) + + config.EXT_POL_2 * powf(surface, 3) + + config.EXT_POL_3 * powf(surface, 2) + + config.EXT_POL_4 * surface; + // clang-format on +} + +float AirBrakesPI::getCD(TimedTrajectoryPoint currentPosition, float extension) +{ + const float mach1 = currentPosition.getMac(); + const float mach2 = powf(mach1, 2); + const float mach3 = powf(mach1, 3); + const float mach4 = powf(mach1, 4); + const float mach5 = powf(mach1, 5); + const float mach6 = powf(mach1, 6); + + const float extension2 = powf(extension, 2); + + // clang-format off + return + config.N000 + + config.N100 * mach1 + + config.N200 * mach2 + + config.N300 * mach3 + + config.N400 * mach4 + + config.N500 * mach5 + + config.N600 * mach6 + + config.N010 * extension + + config.N020 * extension2 + + config.N110 * extension * mach1 + + config.N120 * extension2 * mach1 + + config.N210 * extension * mach2 + + config.N220 * extension2 * mach2 + + config.N310 * extension * mach3 + + config.N320 * extension2 * mach3 + + config.N410 * extension * mach4 + + config.N420 * extension2 * mach4 + + config.N510 * extension * mach5 + + config.N520 * extension2 * mach5 + + config.N001 * currentPosition.z; + // clang-format on +} + +float AirBrakesPI::getDrag(TimedTrajectoryPoint currentPosition, float cd, + float rho) +{ + return 0.5 * rho * config.S0 * cd * currentPosition.vz * + currentPosition.vMod; +} + void AirBrakesPI::chooseTrajectory(TrajectoryPoint currentPosition) { float minDistance = numeric_limits<float>::infinity(); @@ -100,8 +187,6 @@ void AirBrakesPI::chooseTrajectory(TrajectoryPoint currentPosition) } chosenTrajectory = &(trajectorySet.trajectories[trjIndexMin]); - - Logger::getInstance().log(AirBrakesChosenTrajectory{trjIndexMin}); } TrajectoryPoint AirBrakesPI::getSetpoint(TrajectoryPoint currentPosition) diff --git a/src/shared/algorithms/AirBrakes/AirBrakesPI.h b/src/shared/algorithms/AirBrakes/AirBrakesPI.h index 25ad500c19ab1acca21e02792f0855a8452d56a5..ce72b5a42198aff80ab8644ef87202ddb03db39d 100644 --- a/src/shared/algorithms/AirBrakes/AirBrakesPI.h +++ b/src/shared/algorithms/AirBrakes/AirBrakesPI.h @@ -29,21 +29,18 @@ #include <functional> #include <vector> -#include "AirBrakes.h" -#include "AirBrakesData.h" #include "AirBrakesPIConfig.h" #include "TrajectorySet.h" namespace Boardcore { -class AirBrakesPI : public AirBrakes +class AirBrakesPI : public Algorithm { public: AirBrakesPI(std::function<TimedTrajectoryPoint()> getCurrentPosition, const TrajectorySet &trajectorySet, - const AirBrakesConfig &config, - const AirBrakesPIConfig &configPI, + const AirBrakesPIConfig &config, std::function<void(float)> setActuator); bool init() override; @@ -61,6 +58,56 @@ public: void step() override; private: + /** + * @brief Returns the air density at the current altitude using the basic + * atmosphere model. + * + * @param z The current altitude [m]. + * @return The density of air according to current altitude [Kg/m^3] + */ + float getRho(float z); + + /** + * @brief Compute the necessary airbrakes surface to match the + * given drag force from the Pid as closely as possible. + * + * @param currentPosition Current rocket position. + * @param rho Air density [kg/m^2]. + * @param drag Target drag force. + * @return AirBrakes surface [m]; + */ + float getSurface(const TimedTrajectoryPoint ¤tPosition, float rho, + float drag); + + /** + * @brief Computes the airbrakes extension given the desired area. + * + * @param surface Desired airbrakes surface [m^2]. + * @return The radial extension [m]. + */ + float getExtension(float surface); + + /** + * @brief Returns the coefficient of drag for the airbrakes at the given + * position and with the given extension. + * + * @param currentPosition Current rocket position. + * @param extension AirBrakes extension [m]. + * @return The coefficient drag [1]. + */ + float getCD(TimedTrajectoryPoint currentPosition, float extension); + + /** + * @brief Return the drag generated from the AirBrakes in the given + * conditions. + * + * @param currentPosition Current position of the rocket. + * @param cd Coefficient of drag [1]. + * @param rho Air density [kg/m^2]. + * @return Generated drag [N]. + */ + float getDrag(TimedTrajectoryPoint currentPosition, float cd, float rho); + /** * @brief Searched all the trajectories and find the neares point to the * given position. The trajectory of this point is the one choosen. @@ -83,6 +130,13 @@ private: TrajectoryPoint reference, float rho); private: + std::function<TimedTrajectoryPoint()> getCurrentPosition; + const AirBrakesPIConfig &config; + std::function<void(float)> setActuator; + + TimedTrajectoryPoint lastPosition; + uint32_t lastSelectedPointIndex = 0; + PIController pi; // Trajectory set from which the used trajectory can be choosen diff --git a/src/shared/algorithms/AirBrakes/AirBrakesPIConfig.h b/src/shared/algorithms/AirBrakes/AirBrakesPIConfig.h index 8765cf647f353d2d259724c45cec4d21a726794c..c25473d7a53139b14be0a238cecf56edf54d0860 100644 --- a/src/shared/algorithms/AirBrakes/AirBrakesPIConfig.h +++ b/src/shared/algorithms/AirBrakes/AirBrakesPIConfig.h @@ -22,13 +22,44 @@ #pragma once -#include "AirBrakesConfig.h" - namespace Boardcore { struct AirBrakesPIConfig { + // Coefficient of drag coefficients. + float N000; + float N100; + float N200; + float N300; + float N400; + float N500; + float N600; + float N010; + float N020; + float N110; + float N120; + float N210; + float N220; + float N310; + float N320; + float N410; + float N420; + float N510; + float N520; + float N001; + + // Aibrakes extension. + float EXTENSION; ///< [m] + uint8_t DRAG_STEPS; + float EXT_POL_1; + float EXT_POL_2; + float EXT_POL_3; + float EXT_POL_4; + + float S0; ///< Rocket surface [m^2] + float SURFACE; ///< AirBrakes max surface [m^2] + // PI parameters float KP; float KI; diff --git a/src/tests/catch/test-airbrakesInterp.cpp b/src/tests/catch/test-airbrakesInterp.cpp index 8f774cf5dd9db49fc7b5bd0944b3f6d6d5b8ede8..20137849b471ddf2b647bcc0469687e26a68438c 100644 --- a/src/tests/catch/test-airbrakesInterp.cpp +++ b/src/tests/catch/test-airbrakesInterp.cpp @@ -42,19 +42,6 @@ constexpr float INITIAL_MASS = 28; constexpr float DM = 0.2f; constexpr uint16_t N_FORWARD = 1; -static const Boardcore::AirBrakesConfig ABK_CONFIG{ - 0.4884, -1.4391, 6.6940, - -18.4272, 29.1044, -24.5585, - 8.6058, 9.0426, 159.5995, - 4.8188, -208.4471, 47.0771, - 1.9433e+03, -205.6689, -6.4634e+03, - 331.0332, 8.8763e+03, -161.8111, - -3.9917e+03, 2.8025e-06, 0.0373, - 20, -0.009216, 0.02492, - -0.01627, 0.03191, 0.017671458676443, - 0, -}; - AirBrakesInterpConfig getConfig() { AirBrakesInterpConfig config; @@ -91,7 +78,7 @@ TEST_CASE("ABK Update Test") { AirBrakesInterp abk( []() { return static_cast<TimedTrajectoryPoint>(getState()); }, - OPEN_TRAJECTORY_SET, CLOSED_TRAJECTORY_SET, ABK_CONFIG, getConfig(), + OPEN_TRAJECTORY_SET, CLOSED_TRAJECTORY_SET, getConfig(), [&](float position) { static int i = 0;