diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp b/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
index f24a505a11c7d240b2a696ddf10863c0e0d66e82..26b5923b69eaa94de2bb4c10eedbacf720fff36a 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
+++ b/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
@@ -41,13 +41,10 @@ AirBrakesInterp::AirBrakesInterp(
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(configInterp.DZ), dm(configInterp.DM),
- initialMass(configInterp.INITIAL_MASS)
+ trajectoryCloseSet(trajectoryCloseSet), configInterp(configInterp)
{
+ // Initial values to avoid UB
+ lastPercentage = 0;
}
bool AirBrakesInterp::init() { return true; }
@@ -58,7 +55,7 @@ void AirBrakesInterp::begin(float currentMass)
return;
// Choose the best trajectories depending on the mass and the delta mass
- if (dm == 0)
+ if (configInterp.DM == 0)
{
// Compatibility in case the mass information is not provided
choosenOpenTrajectory = &trajectoryOpenSet.trajectories[0];
@@ -66,7 +63,8 @@ void AirBrakesInterp::begin(float currentMass)
}
else
{
- int index = round((currentMass - initialMass) / dm);
+ int index =
+ round((currentMass - configInterp.INITIAL_MASS) / configInterp.DM);
// Bound the index in order to have an indexable element
index = std::max(index, 0);
@@ -90,46 +88,53 @@ void AirBrakesInterp::step()
lastPosition = currentPosition;
- // interpolation
+ // Interpolation
float percentage = controlInterp(currentPosition);
- // filter the aperture value only from the second step
- if (filter)
+ // 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
+ float filterCoeff = 0;
+
+ // If the altitude is lower than the minimum one, the filter is kept at the
+ // same value, to avoid misleading filtering actions
+ if (currentPosition.z < configInterp.MINIMUM_ALTITUDE)
+ {
+ filterCoeff = configInterp.STARTING_FILTER_VALUE;
+ }
+ else
{
+ filterCoeff = configInterp.STARTING_FILTER_VALUE -
+ (currentPosition.z - configInterp.MINIMUM_ALTITUDE) *
+ ((configInterp.STARTING_FILTER_VALUE) /
+ (maxAltitude - configInterp.MINIMUM_ALTITUDE));
+ }
+
+ if (currentPosition.z < maxAltitude)
+ {
+ // Compute the actual value filtered
percentage =
- lastPercentage + (percentage - lastPercentage) * filter_coeff;
-
- // if the time elapsed from liftoff is greater or equal than the
- // Tfilter (converted in microseconds as for the timestamp), we
- // update the filter
- uint64_t currentTimestamp = TimestampTimer::getTimestamp();
- if (currentTimestamp - tLiftoff >= Tfilter * 1e6)
- {
- Tfilter += configInterp.DELTA_T_FILTER;
- filter_coeff /= configInterp.FILTER_RATIO;
- }
+ lastPercentage + (percentage - lastPercentage) * filterCoeff;
}
else
{
- TRACE("START FILTERING\n");
- TRACE("tLiftoff: %llu\n", tLiftoff);
- filter = true;
+ // If the height is beyond the target one, the algorithm tries to brake
+ // as much as possible
+ percentage = 1;
}
lastPercentage = percentage;
setActuator(percentage);
}
-void AirBrakesInterp::setLiftoffTimestamp()
-{
- this->tLiftoff = TimestampTimer::getTimestamp();
-}
-
float AirBrakesInterp::controlInterp(TrajectoryPoint currentPosition)
{
// we take the index of the current point of the trajectory and we look
// ahead of 2 points
- int index_z = floor(currentPosition.z / dz) + 2;
+ int index_z = floor(currentPosition.z / configInterp.DZ) + 2;
// for safety we check whether the index exceeds the maximum index of the
// trajectory sets
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterp.h b/src/shared/algorithms/AirBrakes/AirBrakesInterp.h
index 7550fb8245a2eb536f0495dbe298595983b6e1df..29b1a6fcabe6fabc4f667a3d5372f083daeac748 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesInterp.h
+++ b/src/shared/algorithms/AirBrakes/AirBrakesInterp.h
@@ -56,11 +56,6 @@ public:
*/
void begin(float currentMass);
- /**
- * @brief registers the timestamp of liftoff
- */
- void setLiftoffTimestamp();
-
private:
/**
* @brief Looks for nearest point in the current chosen trajectory and moves
@@ -85,17 +80,8 @@ private:
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
- float filter_coeff; ///< how much the new aperture impacts the real one
- 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
+ AirBrakesInterpConfig configInterp; ///< specialized config
+ float lastPercentage; ///< last opening of the airbrakes
};
} // namespace Boardcore
diff --git a/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h b/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h
index 9ca44b15d7c2243fd2969f84d123f4716785a0f3..df77a38e311e5e251005fa735912fdbcc6cf8024 100644
--- a/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h
+++ b/src/shared/algorithms/AirBrakes/AirBrakesInterpConfig.h
@@ -28,18 +28,21 @@ namespace Boardcore
struct AirBrakesInterpConfig
{
- // interp parameters
- float INITIAL_FILTER_COEFF;
- float INITIAL_T_FILTER; ///< after how much time we change the filter
- ///< coefficient
- 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
+ // Minimum altitude for the algorithm to start acting
+ float MINIMUM_ALTITUDE;
+
+ // Normalized value [0-1] that represents the minimum filtering action that
+ // the applied filter can do.
+ float STARTING_FILTER_VALUE;
+
+ // The delta in altitude between consequent trajectory points
+ float DZ;
+
+ // The mass correspondent to the first trajectory
+ float INITIAL_MASS;
+
+ // The delta in mass between consequent trajectory sets
+ float DM;
};
} // namespace Boardcore