[ABKInterp] Added new continous filtering

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)
-    {
-        percentage =
-            lastPercentage + (percentage - lastPercentage) * filter_coeff;
+    // 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 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)
+    // 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
     {
-            Tfilter += configInterp.DELTA_T_FILTER;
-            filter_coeff /= configInterp.FILTER_RATIO;
+        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) * 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

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
+    AirBrakesInterpConfig configInterp;  ///< specialized config
     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
 };
 
 }  // namespace Boardcore

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