Skip to content
Snippets Groups Projects
Commit 685bd339 authored by Matteo Pignataro's avatar Matteo Pignataro
Browse files

[ABKInterpConfig] Added final configs

[ABKInterpTest] Added working test-catch
parent 05c900c1
No related branches found
No related tags found
No related merge requests found
...@@ -132,6 +132,7 @@ add_executable(catch-tests-boardcore ...@@ -132,6 +132,7 @@ add_executable(catch-tests-boardcore
src/tests/catch/xbee/test-xbee-parser.cpp src/tests/catch/xbee/test-xbee-parser.cpp
src/tests/catch/test-modulemanager.cpp src/tests/catch/test-modulemanager.cpp
src/tests/catch/test-MEA.cpp src/tests/catch/test-MEA.cpp
src/tests/catch/test-airbrakesInterp.cpp
) )
target_compile_definitions(catch-tests-boardcore PRIVATE USE_MOCK_PERIPHERALS) target_compile_definitions(catch-tests-boardcore PRIVATE USE_MOCK_PERIPHERALS)
sbs_target(catch-tests-boardcore stm32f429zi_stm32f4discovery) sbs_target(catch-tests-boardcore stm32f429zi_stm32f4discovery)
......
...@@ -36,6 +36,8 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL ...@@ -36,6 +36,8 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
# Algorithms # Algorithms
${SBS_BASE}/src/shared/algorithms/MEA/MEA.cpp ${SBS_BASE}/src/shared/algorithms/MEA/MEA.cpp
${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakes.cpp
# Logger # Logger
${SBS_BASE}/src/shared/logger/Logger.cpp ${SBS_BASE}/src/shared/logger/Logger.cpp
......
...@@ -91,29 +91,26 @@ void AirBrakesInterp::step() ...@@ -91,29 +91,26 @@ void AirBrakesInterp::step()
// Interpolation // Interpolation
float percentage = controlInterp(currentPosition); float percentage = controlInterp(currentPosition);
// The maximum altitude is the one which is registered at the last point in
// the trajectory
float maxAltitude =
choosenOpenTrajectory->points[choosenOpenTrajectory->size() - 1].z;
// Filtering // Filtering
float filterCoeff = 0; float filterCoeff = 0;
// If the altitude is lower than the minimum one, the filter is kept at the // If the altitude is lower than the minimum one, the filter is kept at the
// same value, to avoid misleading filtering actions // same value, to avoid misleading filtering actions
if (currentPosition.z < configInterp.MINIMUM_ALTITUDE) if (currentPosition.z < configInterp.FILTER_MINIMUM_ALTITUDE)
{ {
filterCoeff = configInterp.STARTING_FILTER_VALUE; filterCoeff = configInterp.STARTING_FILTER_VALUE;
} }
else else
{ {
filterCoeff = configInterp.STARTING_FILTER_VALUE - filterCoeff =
(currentPosition.z - configInterp.MINIMUM_ALTITUDE) * configInterp.STARTING_FILTER_VALUE -
(currentPosition.z - configInterp.FILTER_MINIMUM_ALTITUDE) *
((configInterp.STARTING_FILTER_VALUE) / ((configInterp.STARTING_FILTER_VALUE) /
(maxAltitude - configInterp.MINIMUM_ALTITUDE)); (configInterp.FILTER_MAXIMUM_ALTITUDE -
configInterp.FILTER_MINIMUM_ALTITUDE));
} }
if (currentPosition.z < maxAltitude) if (currentPosition.z < configInterp.ABK_CRITICAL_ALTITUDE)
{ {
// Compute the actual value filtered // Compute the actual value filtered
percentage = percentage =
...@@ -133,8 +130,11 @@ void AirBrakesInterp::step() ...@@ -133,8 +130,11 @@ void AirBrakesInterp::step()
float AirBrakesInterp::controlInterp(TrajectoryPoint currentPosition) float AirBrakesInterp::controlInterp(TrajectoryPoint currentPosition)
{ {
// we take the index of the current point of the trajectory and we look // we take the index of the current point of the trajectory and we look
// ahead of 2 points // ahead of N points
int index_z = floor(currentPosition.z / configInterp.DZ) + 2; int index_z =
floor((currentPosition.z / configInterp.DZ)) + configInterp.N_FORWARD;
index_z = std::max(index_z, 0);
// for safety we check whether the index exceeds the maximum index of the // for safety we check whether the index exceeds the maximum index of the
// trajectory sets // trajectory sets
......
...@@ -28,13 +28,19 @@ namespace Boardcore ...@@ -28,13 +28,19 @@ namespace Boardcore
struct AirBrakesInterpConfig struct AirBrakesInterpConfig
{ {
// Minimum altitude for the algorithm to start acting // Minimum altitude for the filter to consider
float MINIMUM_ALTITUDE; float FILTER_MINIMUM_ALTITUDE;
// Maximum altitude for the filter to consider
float FILTER_MAXIMUM_ALTITUDE;
// Normalized value [0-1] that represents the minimum filtering action that // Normalized value [0-1] that represents the minimum filtering action that
// the applied filter can do. // the applied filter can do.
float STARTING_FILTER_VALUE; float STARTING_FILTER_VALUE;
// Altitude after which the output should be the maximum extension
float ABK_CRITICAL_ALTITUDE;
// The delta in altitude between consequent trajectory points // The delta in altitude between consequent trajectory points
float DZ; float DZ;
...@@ -43,6 +49,9 @@ struct AirBrakesInterpConfig ...@@ -43,6 +49,9 @@ struct AirBrakesInterpConfig
// The delta in mass between consequent trajectory sets // The delta in mass between consequent trajectory sets
float DM; float DM;
// Number of steps to look forward into the reference trajectories
uint16_t N_FORWARD;
}; };
} // namespace Boardcore } // namespace Boardcore
Source diff could not be displayed: it is too large. Options to address this: view the blob.
Source diff could not be displayed: it is too large. Options to address this: view the blob.
/* Copyright (c) 2023 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 <algorithms/AirBrakes/AirBrakesInterp.h>
#include <algorithm>
#include <catch2/catch.hpp>
#include <fstream>
#include <iostream>
#include "../algorithms/Airbrakes/test-airbrakesInterp-data.h"
#include "../algorithms/Airbrakes/test-airbrakesInterp-references.h"
using namespace Boardcore;
using namespace std;
constexpr float MINIMUM_ALTITUDE = 1000;
constexpr float MAXIMUM_ALTITUDE = 3000;
constexpr float STARTING_FILTER_VALUE = 0.9f;
constexpr float ABK_CRITICAL_ALTITUDE = 2990;
constexpr float DZ = 10;
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;
config.FILTER_MINIMUM_ALTITUDE = MINIMUM_ALTITUDE;
config.FILTER_MAXIMUM_ALTITUDE = MAXIMUM_ALTITUDE;
config.STARTING_FILTER_VALUE = STARTING_FILTER_VALUE;
config.ABK_CRITICAL_ALTITUDE = ABK_CRITICAL_ALTITUDE;
config.DZ = DZ;
config.INITIAL_MASS = INITIAL_MASS;
config.DM = DM;
config.N_FORWARD = N_FORWARD;
return config;
}
NASState getState()
{
// Index of the progressive data point
static size_t i = 0;
// Max out the counter
i = min(Z.size() - 1, i);
NASState state;
state.timestamp =
i + 1; // Increasing timestamp to let the algorithm evolve
state.d = -Z[i]; // Compute altitude AGL
state.vd = -Vz[i];
i += 1;
return state;
}
TEST_CASE("ABK Update Test")
{
AirBrakesInterp abk(
[]() { return static_cast<TimedTrajectoryPoint>(getState()); },
OPEN_TRAJECTORY_SET, CLOSED_TRAJECTORY_SET, ABK_CONFIG, getConfig(),
[&](float position)
{
static int i = 0;
// Check the output
if (position != Approx(ABK[i]).epsilon(0.01))
{
FAIL("The computed position differs from the correct one["
<< i << "]: " << position << " != " << ABK[i]);
}
i++;
});
abk.begin(28.8);
for (size_t i = 0; i < Z.size(); i++)
{
abk.update();
}
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment