Commit 1eb61651 authored by Uwe Woessner's avatar Uwe Woessner
Browse files

fixed issue when cars are slower than speed limit


Signed-off-by: Uwe Woessner's avatarhpcwoess <woessner@hlrs.de>
parent d1bf674d
......@@ -234,10 +234,13 @@ double SituationAssessment::AdaptionOnSpeedLimit(double SpeedLimit, double Dista
const double v_ego = SA_Input->MM_O->Ego->GetState_Ego()->velocity_long;
//v_Wish_new = v_ego + (v_Wish_target - v_ego)/Distance * (cycleTime*0.001); // please explain this to me I would rather say that the wish speed should be the speed limit when the distance reaches 0 (whell the stvo sais something else) but this should be the minimum...
//double a = ((v_Wish_target - v_ego) * (v_Wish_target - v_ego)) / (2 * Distance);
double tToSpeedlimit = 2 * Distance / sqrt(v_ego -v_Wish_target);
v_Wish_new = v_ego - (((v_ego - v_Wish_target) / tToSpeedlimit) * (cycleTime * 0.001));
if (v_Wish_new < lastSpeedLimit + commonSpeedLimit_Violation)
v_Wish_new = lastSpeedLimit + commonSpeedLimit_Violation; // just to be safe
if(v_ego - v_Wish_target > 0)
{
double tToSpeedlimit = 2 * Distance / sqrt(v_ego - v_Wish_target);
v_Wish_new = v_ego - (((v_ego - v_Wish_target) / tToSpeedlimit) * (cycleTime * 0.001));
if (v_Wish_new < lastSpeedLimit + commonSpeedLimit_Violation)
v_Wish_new = lastSpeedLimit + commonSpeedLimit_Violation; // just to be safe
}
return v_Wish_new;
}
if (Distance < 0)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment