diff --git a/src/shared/algorithms/AirBrakes/AirBrakes.cpp b/src/shared/algorithms/AirBrakes/AirBrakes.cpp
index 0354d738467094f06a5d6900c292c2396877a7d0..87f6615cfa0ad0b1c719e80772a676eb9fb599a5 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakes.cpp
+++ b/src/shared/algorithms/AirBrakes/AirBrakes.cpp
@@ -37,11 +37,10 @@ namespace Boardcore
{
AirBrakes::AirBrakes(function<TimedTrajectoryPoint()> getCurrentPosition,
- const TrajectorySet &trajectorySet,
const AirBrakesConfig &config,
std::function<void(float)> setActuator)
- : getCurrentPosition(getCurrentPosition), trajectorySet(trajectorySet),
- config(config), setActuator(setActuator)
+ : getCurrentPosition(getCurrentPosition), config(config),
+ setActuator(setActuator)
{
}
diff --git a/src/shared/algorithms/AirBrakes/AirBrakes.h b/src/shared/algorithms/AirBrakes/AirBrakes.h
index 249dce7e1a7a7cb0b40cd432273ac39bafd966cf..86115a382eb893a52211e21e02b0d3b425a1966e 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakes.h
+++ b/src/shared/algorithms/AirBrakes/AirBrakes.h
@@ -40,7 +40,7 @@ class AirBrakes : public Algorithm
{
public:
AirBrakes(std::function<TimedTrajectoryPoint()> getCurrentPosition,
- const TrajectorySet &trajectorySet, const AirBrakesConfig &config,
+ const AirBrakesConfig &config,
std::function<void(float)> setActuator);
protected:
@@ -96,7 +96,6 @@ protected:
protected:
std::function<TimedTrajectoryPoint()> getCurrentPosition;
- const TrajectorySet &trajectorySet;
const AirBrakesConfig &config;
std::function<void(float)> setActuator;
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp b/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
index 70f7644a63143eab3c33d045ceb92bbc232f5109..f24a505a11c7d240b2a696ddf10863c0e0d66e82 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
+++ b/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
@@ -35,23 +35,48 @@ namespace Boardcore
AirBrakesInterp::AirBrakesInterp(
std::function<TimedTrajectoryPoint()> getCurrentPosition,
- const TrajectorySet &trajectorySet, const AirBrakesConfig &config,
+ const TrajectorySet &trajectoryOpenSet,
+ const TrajectorySet &trajectoryCloseSet, const AirBrakesConfig &config,
const AirBrakesInterpConfig &configInterp,
- std::function<void(float)> setActuator, float dz)
- : AirBrakes(getCurrentPosition, trajectorySet, config, setActuator),
- configInterp(configInterp), tLiftoff(-1), lastPercentage(0),
+ std::function<void(float)> setActuator)
+ : AirBrakes(getCurrentPosition, config, setActuator),
+ trajectoryOpenSet(trajectoryOpenSet),
+ trajectoryCloseSet(trajectoryCloseSet), configInterp(configInterp),
+ tLiftoff(0), lastPercentage(0),
filter_coeff(configInterp.INITIAL_FILTER_COEFF),
- Tfilter(configInterp.INITIAL_T_FILTER), filter(false), dz(dz)
+ Tfilter(configInterp.INITIAL_T_FILTER), filter(false),
+ dz(configInterp.DZ), dm(configInterp.DM),
+ initialMass(configInterp.INITIAL_MASS)
{
}
bool AirBrakesInterp::init() { return true; }
-void AirBrakesInterp::begin()
+void AirBrakesInterp::begin(float currentMass)
{
if (running)
return;
+ // Choose the best trajectories depending on the mass and the delta mass
+ if (dm == 0)
+ {
+ // Compatibility in case the mass information is not provided
+ choosenOpenTrajectory = &trajectoryOpenSet.trajectories[0];
+ choosenCloseTrajectory = &trajectoryCloseSet.trajectories[0];
+ }
+ else
+ {
+ int index = round((currentMass - initialMass) / dm);
+
+ // Bound the index in order to have an indexable element
+ index = std::max(index, 0);
+ uint32_t boundedIndex = std::min(static_cast<uint32_t>(index),
+ trajectoryOpenSet.length() - 1);
+
+ choosenOpenTrajectory = &trajectoryOpenSet.trajectories[boundedIndex];
+ choosenCloseTrajectory = &trajectoryCloseSet.trajectories[boundedIndex];
+ }
+
Algorithm::begin();
}
@@ -106,17 +131,15 @@ float AirBrakesInterp::controlInterp(TrajectoryPoint currentPosition)
// ahead of 2 points
int index_z = floor(currentPosition.z / dz) + 2;
- Boardcore::Trajectory t_closed = trajectorySet.trajectories[0];
- Boardcore::Trajectory t_opened =
- trajectorySet.trajectories[trajectorySet.trjSize - 1];
-
// for safety we check whether the index exceeds the maximum index of the
// trajectory sets
- index_z =
- std::min(index_z, std::min(t_closed.trjSize - 1, t_opened.trjSize - 1));
+ index_z = std::min(index_z, std::min(choosenCloseTrajectory->trjSize - 1,
+ choosenOpenTrajectory->trjSize - 1));
- Boardcore::TrajectoryPoint trjPointClosed = t_closed.points[index_z];
- Boardcore::TrajectoryPoint trjPointOpen = t_opened.points[index_z];
+ Boardcore::TrajectoryPoint trjPointClosed =
+ choosenCloseTrajectory->points[index_z];
+ Boardcore::TrajectoryPoint trjPointOpen =
+ choosenOpenTrajectory->points[index_z];
// TRACE("z: %+.3f, curr: %+.3f, clos: %+.3f, open: %+.3f\n",
// currentPosition.z, currentPosition.vz, trjPointClosed.vz,
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterp.h b/src/shared/algorithms/AirBrakes/AirBrakesInterp.h
index c83af72882af736153d66dfd71ac6db1a65aedca..7550fb8245a2eb536f0495dbe298595983b6e1df 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesInterp.h
+++ b/src/shared/algorithms/AirBrakes/AirBrakesInterp.h
@@ -40,24 +40,21 @@ class AirBrakesInterp : public AirBrakes
{
public:
AirBrakesInterp(std::function<TimedTrajectoryPoint()> getCurrentPosition,
- const TrajectorySet &trajectorySet,
+ const TrajectorySet &trajectoryOpenSet,
+ const TrajectorySet &trajectoryCloseSet,
const AirBrakesConfig &config,
const AirBrakesInterpConfig &configInterp,
- std::function<void(float)> setActuator, float dz);
+ std::function<void(float)> setActuator);
bool init() override;
/**
- * @brief This method chooses the trajectory the rocket will follow and
- * starts the algorithm.
+ * @brief This method chooses the trajectory set that will be used to
+ * control the algorithm and starts it.
+ *
+ * @param currentMass The current estimated rocket mass.
*/
- 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;
+ void begin(float currentMass);
/**
* @brief registers the timestamp of liftoff
@@ -65,6 +62,12 @@ public:
void setLiftoffTimestamp();
private:
+ /**
+ * @brief Looks for nearest point in the current chosen trajectory and moves
+ * the airbrakes according to the current rocket speed and the prediction.
+ */
+ void step() override;
+
/**
* @brief Calculates the percentage of aperture of the airbrakes
* interpolating the trajectory points of the fully closed and fully opened
@@ -72,6 +75,16 @@ private:
*/
float controlInterp(TrajectoryPoint currentPosition);
+ // 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.
+ const TrajectorySet &trajectoryOpenSet;
+ const TrajectorySet &trajectoryCloseSet;
+
+ // Choosen trajectories from the begin function
+ Trajectory *choosenCloseTrajectory = nullptr;
+ Trajectory *choosenOpenTrajectory = nullptr;
+
const AirBrakesInterpConfig &configInterp; ///< specialized config
uint64_t tLiftoff; ///< timestamp of the liftoff
float lastPercentage; ///< last opening of the airbrakes
@@ -79,6 +92,10 @@ private:
float Tfilter; ///< [s] time from liftoff when to update filter
bool filter = false; ///< whether to apply the filter or not
float dz; ///< [m] the distance between two consecutive Trajectory points
+ float dm; ///< [kg] the distance in mass between two consecutive trajectory
+ ///< sets
+ float initialMass; ///< [kg] the mass correspondent to the first trajectory
+ ///< set
};
} // namespace Boardcore
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h b/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h
index 7fbb963dc2d1340b1144016b3e25a3ed34a33bed..9ca44b15d7c2243fd2969f84d123f4716785a0f3 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h
+++ b/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h
@@ -35,6 +35,11 @@ struct AirBrakesInterpConfig
float DELTA_T_FILTER; ///< after how much time we change the filter
///< coefficient
float FILTER_RATIO; ///< how much the filter coefficient is reduced
+
+ float DZ; /// The delta in altitude between consequent trajectory points
+
+ float INITIAL_MASS; /// The mass correspondent to the first trajectory
+ float DM; /// The delta in mass between consequent trajectory sets
};
} // namespace Boardcore
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp b/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp
index 3bfccf3526e7b7b6ece5deafdaa8e786cfc4ab22..4e4f868ee05ffbb16f48237b13e0344296217544 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp
+++ b/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp
@@ -39,8 +39,8 @@ AirBrakesPI::AirBrakesPI(
std::function<TimedTrajectoryPoint()> getCurrentPosition,
const TrajectorySet &trajectorySet, const AirBrakesConfig &config,
const AirBrakesPIConfig &configPI, std::function<void(float)> setActuator)
- : AirBrakes(getCurrentPosition, trajectorySet, config, setActuator),
- pi(configPI.KP, configPI.KI, configPI.TS)
+ : AirBrakes(getCurrentPosition, config, setActuator),
+ pi(configPI.KP, configPI.KI, configPI.TS), trajectorySet(trajectorySet)
{
}
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesPI.h b/src/shared/algorithms/AirBrakes/AirBrakesPI.h
index 50c3d4f3733806da61d8459fa85cf695a96a5ae0..25ad500c19ab1acca21e02792f0855a8452d56a5 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesPI.h
+++ b/src/shared/algorithms/AirBrakes/AirBrakesPI.h
@@ -85,6 +85,9 @@ private:
private:
PIController pi;
+ // Trajectory set from which the used trajectory can be choosen
+ const TrajectorySet &trajectorySet;
+
Trajectory *chosenTrajectory = nullptr;
};