LCOV - code coverage report
Current view: top level - src/situation - RssFormulas.cpp (source / functions) Hit Total Coverage
Test: ad_rss Lines: 123 123 100.0 %
Date: 2024-03-25 17:12:56 Functions: 10 10 100.0 %
Branches: 93 136 68.4 %

           Branch data     Line data    Source code
       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/rss/situation/RssFormulas.hpp"
      10                 :            : #include <algorithm>
      11                 :            : #include "ad/rss/situation/Physics.hpp"
      12                 :            : #include "ad/rss/situation/VehicleStateValidInputRange.hpp"
      13                 :            : 
      14                 :            : namespace ad {
      15                 :            : 
      16                 :            : namespace rss {
      17                 :            : namespace situation {
      18                 :            : 
      19                 :            : // make the code more readable
      20                 :            : using physics::Acceleration;
      21                 :            : using physics::Distance;
      22                 :            : using physics::Duration;
      23                 :            : using physics::Speed;
      24                 :            : using situation::calculateStoppingDistance;
      25                 :            : 
      26                 :     133247 : inline bool vehicleStateWithinVaildInputRange(VehicleState const &vehicleState)
      27                 :            : {
      28         [ +  + ]:     133247 :   if (!withinValidInputRange(vehicleState))
      29                 :            :   {
      30                 :      12007 :     return false;
      31                 :            :   }
      32   [ +  -  +  + ]:     121240 :   if (vehicleState.velocity.speedLon.minimum < Speed(0.))
      33                 :            :   {
      34                 :          3 :     return false;
      35                 :            :   }
      36                 :     121237 :   return true;
      37                 :            : }
      38                 :            : 
      39                 :      56786 : bool calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern(Speed const &currentSpeed,
      40                 :            :                                                                   Speed const &maxSpeedOnAcceleration,
      41                 :            :                                                                   Duration const &responseTime,
      42                 :            :                                                                   Acceleration const &acceleration,
      43                 :            :                                                                   Acceleration const &deceleration,
      44                 :            :                                                                   Distance &distanceOffset)
      45                 :            : {
      46                 :      56786 :   Speed resultingSpeed = Speed(0.);
      47                 :      56786 :   Distance distanceOffsetAfterResponseTime = Distance(0.);
      48         [ +  - ]:      56786 :   bool result = calculateAcceleratedLimitedMovement(
      49                 :            :     currentSpeed, maxSpeedOnAcceleration, acceleration, responseTime, resultingSpeed, distanceOffsetAfterResponseTime);
      50                 :            : 
      51                 :      56786 :   Distance distanceToStop = Distance(0.);
      52         [ +  + ]:      56786 :   if (std::signbit(static_cast<double>(resultingSpeed)) != std::signbit(static_cast<double>(deceleration)))
      53                 :            :   {
      54                 :            :     // if speed after stated braking pattern has the same direction as the acceleration
      55                 :            :     // further braking to full stop in that moving direction has to be added
      56   [ +  +  +  -  :      56785 :     result = result && calculateStoppingDistance(resultingSpeed, deceleration, distanceToStop);
                   +  - ]
      57                 :            :   }
      58                 :            : 
      59         [ +  + ]:      56786 :   if (result)
      60                 :            :   {
      61         [ +  - ]:      56784 :     distanceOffset = distanceOffsetAfterResponseTime + distanceToStop;
      62                 :            :   }
      63                 :            : 
      64                 :      56786 :   return result;
      65                 :            : }
      66                 :            : 
      67                 :       4097 : bool calculateLateralDistanceOffsetAfterStatedBrakingPattern(Speed const &currentSpeed,
      68                 :            :                                                              Duration const &responseTime,
      69                 :            :                                                              Acceleration const &acceleration,
      70                 :            :                                                              Acceleration const &deceleration,
      71                 :            :                                                              Distance &distanceOffset)
      72                 :            : {
      73                 :       4097 :   Speed resultingSpeed = Speed(0.);
      74                 :       4097 :   Distance distanceOffsetAfterResponseTime = Distance(0.);
      75                 :            : 
      76         [ +  - ]:       4097 :   bool result = calculateSpeedInAcceleratedMovement(currentSpeed, acceleration, responseTime, resultingSpeed);
      77                 :       4097 :   result = result
      78   [ +  +  +  -  :       4097 :     && calculateDistanceOffsetInAcceleratedMovement(
                   +  - ]
      79                 :            :              currentSpeed, acceleration, responseTime, distanceOffsetAfterResponseTime);
      80                 :            : 
      81                 :       4097 :   Distance distanceToStop = Distance(0.);
      82         [ +  + ]:       4097 :   if (std::signbit(static_cast<double>(resultingSpeed)) != std::signbit(static_cast<double>(deceleration)))
      83                 :            :   {
      84                 :            :     // if speed after stated braking pattern has the same direction as the acceleration
      85                 :            :     // further braking to full stop in that moving direction has to be added
      86   [ +  -  +  -  :       3718 :     result = result && calculateStoppingDistance(resultingSpeed, deceleration, distanceToStop);
                   +  - ]
      87                 :            :   }
      88                 :            : 
      89         [ +  + ]:       4097 :   if (result)
      90                 :            :   {
      91         [ +  - ]:       4096 :     distanceOffset = distanceOffsetAfterResponseTime + distanceToStop;
      92                 :            :   }
      93                 :            : 
      94                 :       4097 :   return result;
      95                 :            : }
      96                 :            : 
      97                 :      65591 : bool calculateSafeLongitudinalDistanceSameDirection(VehicleState const &leadingVehicle,
      98                 :            :                                                     VehicleState const &followingVehicle,
      99                 :            :                                                     Distance &safeDistance)
     100                 :            : {
     101   [ +  -  +  +  :      65591 :   if (!vehicleStateWithinVaildInputRange(leadingVehicle) || !vehicleStateWithinVaildInputRange(followingVehicle))
          +  -  +  +  +  
                      + ]
     102                 :            :   {
     103                 :      12002 :     return false;
     104                 :            :   }
     105                 :            : 
     106                 :      53589 :   Distance distanceStatedBraking = Distance(0.);
     107                 :            : 
     108                 :            :   bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
     109                 :      53589 :     followingVehicle.velocity.speedLon.maximum,
     110                 :      53589 :     followingVehicle.dynamics.maxSpeedOnAcceleration,
     111                 :      53589 :     followingVehicle.dynamics.responseTime,
     112                 :      53589 :     followingVehicle.dynamics.alphaLon.accelMax,
     113         [ +  - ]:      53589 :     followingVehicle.dynamics.alphaLon.brakeMin,
     114                 :            :     distanceStatedBraking);
     115                 :      53589 :   Distance distanceMaxBrake = Distance(0.);
     116                 :      53589 :   result = result
     117   [ +  -  +  -  :      53589 :     && calculateStoppingDistance(
                   +  - ]
     118                 :            :              leadingVehicle.velocity.speedLon.minimum, leadingVehicle.dynamics.alphaLon.brakeMax, distanceMaxBrake);
     119                 :            : 
     120         [ +  - ]:      53589 :   if (result)
     121                 :            :   {
     122         [ +  - ]:      53589 :     safeDistance = distanceStatedBraking - distanceMaxBrake;
     123         [ +  - ]:      53589 :     safeDistance = std::max(safeDistance, Distance(0.));
     124                 :            :   }
     125                 :            : 
     126                 :      53589 :   return result;
     127                 :            : }
     128                 :            : 
     129                 :      65590 : bool checkSafeLongitudinalDistanceSameDirection(VehicleState const &leadingVehicle,
     130                 :            :                                                 VehicleState const &followingVehicle,
     131                 :            :                                                 Distance const &vehicleDistance,
     132                 :            :                                                 Distance &safeDistance,
     133                 :            :                                                 bool &isDistanceSafe)
     134                 :            : {
     135   [ +  -  +  + ]:      65590 :   if (vehicleDistance < Distance(0.))
     136                 :            :   {
     137                 :          1 :     return false;
     138                 :            :   }
     139                 :            : 
     140                 :      65589 :   isDistanceSafe = false;
     141                 :      65589 :   safeDistance = std::numeric_limits<physics::Distance>::max();
     142                 :            : 
     143                 :      65589 :   bool const result = calculateSafeLongitudinalDistanceSameDirection(leadingVehicle, followingVehicle, safeDistance);
     144                 :            : 
     145         [ +  + ]:      65589 :   if (vehicleDistance > safeDistance)
     146                 :            :   {
     147                 :      51690 :     isDistanceSafe = true;
     148                 :            :   }
     149                 :      65589 :   return result;
     150                 :            : }
     151                 :            : 
     152                 :       1070 : bool calculateSafeLongitudinalDistanceOppositeDirection(VehicleState const &correctVehicle,
     153                 :            :                                                         VehicleState const &oppositeVehicle,
     154                 :            :                                                         Distance &safeDistance)
     155                 :            : {
     156   [ +  -  +  +  :       1070 :   if (!vehicleStateWithinVaildInputRange(correctVehicle) || !vehicleStateWithinVaildInputRange(oppositeVehicle))
          +  -  +  +  +  
                      + ]
     157                 :            :   {
     158                 :          4 :     return false;
     159                 :            :   }
     160                 :            : 
     161                 :       1066 :   Distance distanceStatedBrakingCorrect = Distance(0.);
     162                 :            : 
     163                 :            :   bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
     164                 :       1066 :     correctVehicle.velocity.speedLon.maximum,
     165                 :       1066 :     correctVehicle.dynamics.maxSpeedOnAcceleration,
     166                 :       1066 :     correctVehicle.dynamics.responseTime,
     167                 :       1066 :     correctVehicle.dynamics.alphaLon.accelMax,
     168         [ +  - ]:       1066 :     correctVehicle.dynamics.alphaLon.brakeMinCorrect,
     169                 :            :     distanceStatedBrakingCorrect);
     170                 :            : 
     171                 :       1066 :   Distance distanceStatedBrakingOpposite = Distance(0.);
     172                 :            : 
     173         [ +  - ]:       1066 :   if (result)
     174                 :            :   {
     175                 :            :     result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
     176                 :       1066 :       oppositeVehicle.velocity.speedLon.maximum,
     177                 :       1066 :       oppositeVehicle.dynamics.maxSpeedOnAcceleration,
     178                 :       1066 :       oppositeVehicle.dynamics.responseTime,
     179                 :       1066 :       oppositeVehicle.dynamics.alphaLon.accelMax,
     180         [ +  - ]:       1066 :       oppositeVehicle.dynamics.alphaLon.brakeMin,
     181                 :            :       distanceStatedBrakingOpposite);
     182                 :            :   }
     183                 :            : 
     184         [ +  - ]:       1066 :   if (result)
     185                 :            :   {
     186         [ +  - ]:       1066 :     safeDistance = distanceStatedBrakingCorrect + distanceStatedBrakingOpposite;
     187                 :            :   }
     188                 :            : 
     189                 :       1066 :   return result;
     190                 :            : }
     191                 :            : 
     192                 :       1069 : bool checkSafeLongitudinalDistanceOppositeDirection(VehicleState const &correctVehicle,
     193                 :            :                                                     VehicleState const &oppositeVehicle,
     194                 :            :                                                     Distance const &vehicleDistance,
     195                 :            :                                                     Distance &safeDistance,
     196                 :            :                                                     bool &isDistanceSafe)
     197                 :            : {
     198   [ +  -  +  + ]:       1069 :   if (vehicleDistance < Distance(0.))
     199                 :            :   {
     200                 :          1 :     return false;
     201                 :            :   }
     202                 :            : 
     203                 :       1068 :   isDistanceSafe = false;
     204                 :       1068 :   safeDistance = std::numeric_limits<physics::Distance>::max();
     205                 :       1068 :   bool const result = calculateSafeLongitudinalDistanceOppositeDirection(correctVehicle, oppositeVehicle, safeDistance);
     206                 :            : 
     207         [ +  + ]:       1068 :   if (vehicleDistance > safeDistance)
     208                 :            :   {
     209                 :        391 :     isDistanceSafe = true;
     210                 :            :   }
     211                 :       1068 :   return result;
     212                 :            : }
     213                 :            : 
     214                 :        848 : bool checkStopInFrontIntersection(VehicleState const &vehicle, Distance &safeDistance, bool &isDistanceSafe)
     215                 :            : {
     216         [ +  + ]:        848 :   if (!vehicleStateWithinVaildInputRange(vehicle))
     217                 :            :   {
     218                 :          1 :     return false;
     219                 :            :   }
     220                 :            : 
     221                 :        847 :   isDistanceSafe = false;
     222                 :            : 
     223                 :        847 :   safeDistance = Distance(0.);
     224                 :            :   bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
     225                 :        847 :     vehicle.velocity.speedLon.maximum,
     226                 :        847 :     vehicle.dynamics.maxSpeedOnAcceleration,
     227                 :        847 :     vehicle.dynamics.responseTime,
     228                 :        847 :     vehicle.dynamics.alphaLon.accelMax,
     229                 :        847 :     vehicle.dynamics.alphaLon.brakeMin,
     230                 :            :     safeDistance);
     231                 :            : 
     232         [ +  + ]:        847 :   if (safeDistance < vehicle.distanceToEnterIntersection)
     233                 :            :   {
     234                 :        333 :     isDistanceSafe = true;
     235                 :            :   }
     236                 :            : 
     237                 :        847 :   return result;
     238                 :            : }
     239                 :            : 
     240                 :       2043 : bool calculateSafeLateralDistance(VehicleState const &leftVehicle,
     241                 :            :                                   VehicleState const &rightVehicle,
     242                 :            :                                   Distance &safeDistance)
     243                 :            : {
     244   [ +  -  +  +  :       2043 :   if (!vehicleStateWithinVaildInputRange(leftVehicle) || !vehicleStateWithinVaildInputRange(rightVehicle))
          +  -  +  +  +  
                      + ]
     245                 :            :   {
     246                 :          3 :     return false;
     247                 :            :   }
     248                 :            : 
     249                 :       2040 :   bool result = false;
     250                 :       2040 :   Distance distanceOffsetStatedBrakingLeft = Distance(0.);
     251                 :       2040 :   Distance distanceOffsetStatedBrakingRight = Distance(0.);
     252                 :            : 
     253                 :            :   result = calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
     254                 :       2040 :     leftVehicle.velocity.speedLat.maximum,
     255                 :       2040 :     leftVehicle.dynamics.responseTime,
     256                 :       2040 :     leftVehicle.dynamics.alphaLat.accelMax,
     257         [ +  - ]:       2040 :     leftVehicle.dynamics.alphaLat.brakeMin,
     258                 :            :     distanceOffsetStatedBrakingLeft);
     259                 :            : 
     260                 :       2040 :   result = result
     261                 :            :     && calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
     262                 :       2040 :              rightVehicle.velocity.speedLat.minimum,
     263         [ +  - ]:       2040 :              rightVehicle.dynamics.responseTime,
     264         [ +  - ]:       2040 :              -rightVehicle.dynamics.alphaLat.accelMax,
     265         [ +  - ]:       2040 :              -rightVehicle.dynamics.alphaLat.brakeMin,
     266                 :            :              distanceOffsetStatedBrakingRight);
     267                 :            : 
     268         [ +  - ]:       2040 :   if (result)
     269                 :            :   {
     270                 :            :     // safe distance is the difference of both distances
     271         [ +  - ]:       2040 :     safeDistance = distanceOffsetStatedBrakingLeft - distanceOffsetStatedBrakingRight;
     272                 :            :     //  plus the lateral fluctuation margin: here we use the 0.5*my of both
     273                 :            :     safeDistance
     274   [ +  -  +  -  :       2040 :       += 0.5 * (leftVehicle.dynamics.lateralFluctuationMargin + rightVehicle.dynamics.lateralFluctuationMargin);
                   +  - ]
     275         [ +  - ]:       2040 :     safeDistance = std::max(safeDistance, Distance(0.));
     276                 :            :   }
     277                 :       2040 :   return result;
     278                 :            : }
     279                 :            : 
     280                 :       2008 : bool checkSafeLateralDistance(VehicleState const &leftVehicle,
     281                 :            :                               VehicleState const &rightVehicle,
     282                 :            :                               Distance const &vehicleDistance,
     283                 :            :                               Distance &safeDistance,
     284                 :            :                               bool &isDistanceSafe)
     285                 :            : {
     286   [ +  -  +  + ]:       2008 :   if (vehicleDistance < Distance(0.))
     287                 :            :   {
     288                 :          1 :     return false;
     289                 :            :   }
     290                 :            : 
     291                 :       2007 :   isDistanceSafe = false;
     292                 :       2007 :   safeDistance = std::numeric_limits<physics::Distance>::max();
     293                 :       2007 :   bool const result = calculateSafeLateralDistance(leftVehicle, rightVehicle, safeDistance);
     294                 :            : 
     295         [ +  + ]:       2007 :   if (vehicleDistance > safeDistance)
     296                 :            :   {
     297                 :       1355 :     isDistanceSafe = true;
     298                 :            :   }
     299                 :            : 
     300                 :       2007 :   return result;
     301                 :            : }
     302                 :            : 
     303                 :            : } // namespace situation
     304                 :            : } // namespace rss
     305                 :            : } // namespace ad

Generated by: LCOV version 1.14