From 5de938b0e7a1bc6db5acdfa0307ca4150608a1dd Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Thu, 7 Jul 2022 16:59:55 +0000
Subject: [PATCH] [AirBrakes] Ported and reviewd the algorithm from the obsw
---
cmake/boardcore.cmake | 1 +
src/shared/algorithms/AirBrakes/AirBrakes.cpp | 239 ++++++++++++++++++
src/shared/algorithms/AirBrakes/AirBrakes.h | 153 +++++++++++
.../algorithms/AirBrakes/AirBrakesConfig.h | 69 +++++
.../algorithms/AirBrakes/AirBrakesData.h | 41 +++
src/shared/algorithms/AirBrakes/Trajectory.h | 45 ++++
.../algorithms/AirBrakes/TrajectoryPoint.h | 89 +++++++
.../algorithms/AirBrakes/TrajectorySet.h | 44 ++++
src/shared/algorithms/Algorithm.h | 2 -
.../algorithms/{PID.h => PIController.h} | 38 ++-
src/shared/utils/AeroUtils/AeroUtils.h | 2 +-
src/shared/utils/Constants.h | 5 +
.../Kalman/test-kalman-benchmark.cpp | 6 -
13 files changed, 703 insertions(+), 31 deletions(-)
create mode 100644 src/shared/algorithms/AirBrakes/AirBrakes.cpp
create mode 100644 src/shared/algorithms/AirBrakes/AirBrakes.h
create mode 100644 src/shared/algorithms/AirBrakes/AirBrakesConfig.h
create mode 100644 src/shared/algorithms/AirBrakes/AirBrakesData.h
create mode 100644 src/shared/algorithms/AirBrakes/Trajectory.h
create mode 100644 src/shared/algorithms/AirBrakes/TrajectoryPoint.h
create mode 100644 src/shared/algorithms/AirBrakes/TrajectorySet.h
rename src/shared/algorithms/{PID.h => PIController.h} (71%)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 00caab9dc..af7bd7e40 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -33,6 +33,7 @@ foreach(OPT_BOARD ${BOARDS})
# Algorithms
${SBS_BASE}/src/shared/algorithms/ADA/ADA.cpp
+ ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakes.cpp
${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp
# Debug
diff --git a/src/shared/algorithms/AirBrakes/AirBrakes.cpp b/src/shared/algorithms/AirBrakes/AirBrakes.cpp
new file mode 100644
index 000000000..9e288ba06
--- /dev/null
+++ b/src/shared/algorithms/AirBrakes/AirBrakes.cpp
@@ -0,0 +1,239 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Author: 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.
+ */
+
+#include "AirBrakes.h"
+
+#include <logger/Logger.h>
+#include <utils/Constants.h>
+
+#include <limits>
+
+namespace Boardcore
+{
+
+AirBrakes::AirBrakes(std::function<TimedTrajectoryPoint()> getCurrentPosition,
+ TrajectorySet &trajectorySet,
+ const AirBrakesConfig &config,
+ std::function<void(float)> setActuator)
+ : getCurrentPosition(getCurrentPosition), trajectorySet(trajectorySet),
+ config(config), setActuator(setActuator),
+ pi(config.KP, config.KI, config.TS)
+{
+}
+
+bool AirBrakes::init() { return true; }
+
+void AirBrakes::begin()
+{
+ if (running)
+ return;
+
+ lastPosition = getCurrentPosition();
+ chooseTrajectory(lastPosition);
+
+ Algorithm::begin();
+}
+
+void AirBrakes::step()
+{
+ auto currentPosition = getCurrentPosition();
+
+ // Do nothing if we have no new data
+ if (lastPosition.timestamp >= currentPosition.timestamp)
+ return;
+ lastPosition = currentPosition;
+
+ auto setPoint = getSetpoint(currentPosition);
+ float rho = getRho(currentPosition.z);
+
+ float targetDrag = piStep(currentPosition, setPoint, rho);
+ float surface = getSurface(currentPosition, rho, targetDrag);
+
+ setActuator(surface / config.SURFACE);
+}
+
+void AirBrakes::chooseTrajectory(TrajectoryPoint currentPosition)
+{
+ float minDistance = std::numeric_limits<float>::infinity();
+ uint8_t trjIndexMin = trajectorySet.length() / 2;
+
+ for (uint8_t trjIndex = 0; trjIndex < trajectorySet.length(); trjIndex++)
+ {
+ Trajectory &trajectory = trajectorySet.trajectories[trjIndex];
+
+ for (uint32_t ptIndex = 0; ptIndex < trajectory.size(); ptIndex++)
+ {
+ TrajectoryPoint point = trajectory.points[ptIndex];
+ float distance =
+ TrajectoryPoint::distanceSquared(point, currentPosition);
+
+ if (distance < minDistance)
+ {
+ minDistance = distance;
+ trjIndexMin = trjIndex;
+ lastSelectedPointIndex = ptIndex;
+ chosenTrajectory = &trajectory;
+ }
+ }
+ }
+
+ chosenTrajectory = &(trajectorySet.trajectories[trjIndexMin]);
+
+ Logger::getInstance().log(AirBrakesChosenTrajectory{trjIndexMin});
+}
+
+TrajectoryPoint AirBrakes::getSetpoint(TrajectoryPoint currentPosition)
+{
+ if (chosenTrajectory == nullptr)
+ return {};
+
+ float minDistance = std::numeric_limits<float>::infinity();
+
+ uint32_t end = chosenTrajectory->size();
+ for (uint32_t ptIndex = lastSelectedPointIndex; ptIndex < end; ptIndex++)
+ {
+ float distanceFromCurrentinput = TrajectoryPoint::distanceSquared(
+ chosenTrajectory->points[ptIndex], currentPosition);
+
+ if (distanceFromCurrentinput < minDistance)
+ {
+ minDistance = distanceFromCurrentinput;
+ lastSelectedPointIndex = ptIndex;
+ }
+ }
+
+ return chosenTrajectory->points[lastSelectedPointIndex];
+}
+
+float AirBrakes::getRho(float z)
+{
+ return Constants::RHO_0 * expf(-z / Constants::Hn);
+}
+
+float AirBrakes::piStep(TimedTrajectoryPoint currentPosition,
+ TrajectoryPoint setPoint, float rho)
+{
+ const float cdMin = getCD(currentPosition, 0);
+ const float dragMin = getDrag(currentPosition, cdMin, rho);
+
+ const float cdMax = getCD(currentPosition, config.EXTENSION);
+ const float dragMax = getDrag(currentPosition, cdMax, rho);
+
+ // Get target surface percentage
+ const float cdRef = getCD(currentPosition, chosenTrajectory->extension);
+ const float dragRef = getDrag(currentPosition, cdRef, rho);
+
+ // PI update
+ const float error = currentPosition.vz - setPoint.vz;
+ const float dragPi =
+ pi.antiWindUp(pi.update(error) + dragRef, dragMin, dragMax);
+
+ return dragPi;
+}
+
+float AirBrakes::getSurface(const TimedTrajectoryPoint ¤tPosition,
+ float rho, float targetDrag)
+{
+ float bestDDrag = std::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::getMach(TimedTrajectoryPoint currentPosition)
+{
+ return currentPosition.vMod /
+ (Constants::CO + Constants::ALPHA * currentPosition.z);
+}
+
+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
new file mode 100644
index 000000000..b8d7d8b3c
--- /dev/null
+++ b/src/shared/algorithms/AirBrakes/AirBrakes.h
@@ -0,0 +1,153 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Author: 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 <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,
+ TrajectorySet &trajectorySet, const AirBrakesConfig &config,
+ std::function<void(float)> setActuator);
+
+ bool init() override;
+
+ /**
+ * @brief This method chooses the trajectory the rocket will follow and
+ * starts the algorithm.
+ */
+ void begin();
+
+ /**
+ * @brief Looks for nearest point in the current chosen trajectory and moves
+ * the airbraks according to the current rocket speed and the prediction.
+ */
+ void step() override;
+
+private:
+ /**
+ * @brief Searched all the trajectories and find the neares point to the
+ * given position. The trajectory of this point is the one choosen.
+ */
+ void chooseTrajectory(TrajectoryPoint currentPosition);
+
+ /**
+ * @brief Searches, in the choosen trajectory, the point neares to the given
+ * one. This method considers the Euclidean distance between altitude and
+ * vertical speed.
+ */
+ TrajectoryPoint getSetpoint(TrajectoryPoint currentPosition);
+
+ /**
+ * @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 Update PI to compute the target drag froce.
+ *
+ * @returns Target drag force to generate [N].
+ */
+ float piStep(TimedTrajectoryPoint currentPosition,
+ TrajectoryPoint reference, float rho);
+
+ /**
+ * @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 speed in Mach unit for the given position.
+ *
+ * @return Mach speed [M].
+ */
+ float getMach(TimedTrajectoryPoint currentPosition);
+
+ /**
+ * @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);
+
+private:
+ std::function<TimedTrajectoryPoint()> getCurrentPosition;
+ TrajectorySet &trajectorySet;
+ const AirBrakesConfig &config;
+ std::function<void(float)> setActuator;
+
+ TimedTrajectoryPoint lastPosition;
+ uint32_t lastSelectedPointIndex = 0;
+
+ PIController pi;
+
+ Trajectory *chosenTrajectory = nullptr;
+};
+
+} // namespace Boardcore
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesConfig.h b/src/shared/algorithms/AirBrakes/AirBrakesConfig.h
new file mode 100644
index 000000000..8294310a5
--- /dev/null
+++ b/src/shared/algorithms/AirBrakes/AirBrakesConfig.h
@@ -0,0 +1,69 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: 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
+
+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]
+
+ // PI parameters
+ float KP;
+ float KI;
+ float TS;
+};
+
+} // namespace Boardcore
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesData.h b/src/shared/algorithms/AirBrakes/AirBrakesData.h
new file mode 100644
index 000000000..1e9ee5c9f
--- /dev/null
+++ b/src/shared/algorithms/AirBrakes/AirBrakesData.h
@@ -0,0 +1,41 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Author: 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/Trajectory.h b/src/shared/algorithms/AirBrakes/Trajectory.h
new file mode 100644
index 000000000..0f8d17740
--- /dev/null
+++ b/src/shared/algorithms/AirBrakes/Trajectory.h
@@ -0,0 +1,45 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Author: 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 "TrajectoryPoint.h"
+
+namespace Boardcore
+{
+
+class Trajectory
+{
+public:
+ float extension; ///< AirBrakes target extension for this trajectory [m].
+ TrajectoryPoint *points;
+ uint16_t trjSize;
+
+ Trajectory(float extension, TrajectoryPoint points[], uint16_t trjSize)
+ : extension(extension), points(points), trjSize(trjSize)
+ {
+ }
+
+ uint32_t size() { return trjSize; }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/algorithms/AirBrakes/TrajectoryPoint.h b/src/shared/algorithms/AirBrakes/TrajectoryPoint.h
new file mode 100644
index 000000000..18e880d67
--- /dev/null
+++ b/src/shared/algorithms/AirBrakes/TrajectoryPoint.h
@@ -0,0 +1,89 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Author: 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 <algorithms/NAS/NASState.h>
+#include <math.h>
+#include <utils/Constants.h>
+
+#include <Eigen/Eigen>
+
+namespace Boardcore
+{
+
+class TrajectoryPoint
+{
+public:
+ float z; ///< Vertical position [m].
+ float vz; ///< Vertical velocity [m/s].
+
+ TrajectoryPoint() : TrajectoryPoint(0, 0) {}
+
+ TrajectoryPoint(float z, float vz) : z(z), vz(vz) {}
+
+ /**
+ * @brief Returns the distance squared between the two points.
+ */
+ static float distanceSquared(TrajectoryPoint a, TrajectoryPoint b)
+ {
+ return powf(a.z - b.z, 2) + powf(a.vz - b.vz, 2);
+ }
+
+ /**
+ * @brief Returns the distance in height between the two given points.
+ */
+ static float zDistance(TrajectoryPoint a, TrajectoryPoint b)
+ {
+ return abs(a.z - b.z);
+ }
+
+ /**
+ * @brief Returns the distance in vertical speed between the two points.
+ */
+ static float vzDistance(TrajectoryPoint a, TrajectoryPoint b)
+ {
+ return abs(a.vz - b.vz);
+ }
+};
+
+/**
+ * @brief Trajectory point with timestamp and velocity module.
+ */
+class TimedTrajectoryPoint : public TrajectoryPoint
+{
+public:
+ uint64_t timestamp;
+ float vMod;
+
+ TimedTrajectoryPoint() : TrajectoryPoint(), timestamp(0), vMod(0) {}
+
+ explicit TimedTrajectoryPoint(NASState state)
+ : TrajectoryPoint(-state.d, -state.vn), timestamp(state.timestamp),
+ vMod(Eigen::Vector3f{state.vn, state.ve, state.vd}.norm())
+ {
+ }
+
+ float getMac() { return vMod / (Constants::CO + Constants::ALPHA * z); }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/algorithms/AirBrakes/TrajectorySet.h b/src/shared/algorithms/AirBrakes/TrajectorySet.h
new file mode 100644
index 000000000..b720af10f
--- /dev/null
+++ b/src/shared/algorithms/AirBrakes/TrajectorySet.h
@@ -0,0 +1,44 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Author: 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 "Trajectory.h"
+
+namespace Boardcore
+{
+
+class TrajectorySet
+{
+public:
+ Trajectory* trajectories;
+ uint16_t trjSize;
+
+ TrajectorySet(Trajectory trajectories[], uint16_t trjSize)
+ : trajectories(trajectories), trjSize(trjSize)
+ {
+ }
+
+ uint32_t length() { return trjSize; }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/algorithms/Algorithm.h b/src/shared/algorithms/Algorithm.h
index 56d31830c..441f9116a 100644
--- a/src/shared/algorithms/Algorithm.h
+++ b/src/shared/algorithms/Algorithm.h
@@ -53,9 +53,7 @@ public:
void update()
{
if (running)
- {
step();
- }
}
protected:
diff --git a/src/shared/algorithms/PID.h b/src/shared/algorithms/PIController.h
similarity index 71%
rename from src/shared/algorithms/PID.h
rename to src/shared/algorithms/PIController.h
index 5f460f438..c30e3e409 100644
--- a/src/shared/algorithms/PID.h
+++ b/src/shared/algorithms/PIController.h
@@ -34,9 +34,9 @@ class PIController
{
public:
- PIController(double Kp, double Ki, double Ts = 1,
- double uMin = -std::numeric_limits<double>::infinity(),
- double uMax = std::numeric_limits<double>::infinity())
+ PIController(float Kp, float Ki, float Ts = 1,
+ float uMin = -std::numeric_limits<float>::infinity(),
+ float uMax = std::numeric_limits<float>::infinity())
: Kp(Kp), Ki(Ki), Ts(Ts), uMin(uMin), uMax(uMax)
{
}
@@ -44,26 +44,23 @@ public:
/**
* @brief Update the PI internal state.
* */
- double update(double error)
+ float update(float error)
{
-
- lastP = Kp * error;
-
if (!saturation)
- {
i = i + Ki * Ts * error;
- }
- double u = lastP + i;
+ float u = Kp * error + i;
lastOutput = antiWindUp(u);
return lastOutput;
}
+ float antiWindUp(float u) { return antiWindUp(u, uMin, uMax); }
+
/**
* @brief Anti-windup mechanism.
*/
- double antiWindUp(double u)
+ float antiWindUp(float u, float uMin, float uMax)
{
if (u < uMin)
{
@@ -83,23 +80,20 @@ public:
return u;
}
- double getI() { return i; }
-
- double getLastP() { return lastP; }
+ float getI() { return i; }
- double getLastOutput() { return lastOutput; }
+ float getLastOutput() { return lastOutput; }
bool isSaturated() { return saturation; }
- double Kp; // Proportional factor.
- double Ki; // Integral factor.
- double Ts; // Sampling period.
- double uMin, uMax; // Anti-windup limits.
+ float Kp; // Proportional factor.
+ float Ki; // Integral factor.
+ float Ts; // Sampling period.
+ float uMin, uMax; // Anti-windup limits.
private:
- double i = 0; // Integral contribution.
- double lastP = 0; // Last computer proportional contribution.
- double lastOutput; // Last computed controller output.
+ float i = 0; // Integral contribution.
+ float lastOutput; // Last computed controller output.
bool saturation = false;
};
diff --git a/src/shared/utils/AeroUtils/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h
index 80abb8dac..19e3a5a6b 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.h
+++ b/src/shared/utils/AeroUtils/AeroUtils.h
@@ -36,7 +36,7 @@ namespace Aeroutils
* given pressure, using International Standard Atmosphere model.
*
* @warning This function is valid for altitudes below 11000 meters above sea
- * level
+ * level.
* @warning This function provides a relative altitude from the reference
* altitude. It does not provide altitude above mean sea level unless the
* reference is, in fact, the sea level.
diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h
index ddc1e8109..6f8aaa90e 100644
--- a/src/shared/utils/Constants.h
+++ b/src/shared/utils/Constants.h
@@ -39,6 +39,11 @@ static constexpr float R = 287.05f; // Air gas constant [J/Kg/K]
static constexpr float n = g / (R * a);
static constexpr float nInv = (R * a) / g;
+static constexpr float CO = 340.3; // Sound speed at ground altitude [m/s]
+static constexpr float ALPHA = -3.871e-3; // Sound speed gradient [1/s]
+static constexpr float RHO_0 = 1.225; // Air density at sea level [kg/m^3]
+static constexpr float Hn = 10400.0; // Scale height [m]
+
static constexpr const float MSL_PRESSURE = 101325.0f; // [Pa]
static constexpr const float MSL_TEMPERATURE = 288.15f; // [Kelvin]
diff --git a/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp b/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp
index d690705b8..52ca7e81c 100644
--- a/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp
+++ b/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp
@@ -88,8 +88,6 @@ int main()
{
float time; // Current time as read from csv file
float T; // Time elapsed between last sample and current one
- uint64_t tick1;
- uint64_t tick2;
time = TIME[i];
T = time - lastTime;
@@ -100,15 +98,11 @@ int main()
y(0) = INPUT[i];
- tick1 = TimestampTimer::getTimestamp();
-
filter.predict(F);
if (!filter.correct(y))
printf("Correction failed at iteration : %u \n", i);
- tick2 = TimestampTimer::getTimestamp();
-
lastTime = time;
if (filter.getState()(1) < 0 && !apogeeDetected)
--
GitLab