diff --git a/tools/plotjuggler/layouts/carcontrol.xml b/tools/plotjuggler/layouts/carcontrol.xml index 01edfeeddf7174..2263e00607b2a2 100644 --- a/tools/plotjuggler/layouts/carcontrol.xml +++ b/tools/plotjuggler/layouts/carcontrol.xml @@ -1,188 +1,216 @@ - - + + - + - - + + - - - + + + - - - + + + - - + + - - - - - + + + + + - - + + - - - - - + + + + + - - + + - - - - + + + + - - + + - + - + - + - - + + - - + + + + + + + + + + - - + + - - - - + + + + + + + + + + - - + + - - + + + + + + + + + + + + + + + + - + - + - - + + - + - - + + - - - + + + - - + + - + - + - + - - + + - - - + + + - - + + - - + + - - + + - + - + - + - - + + - - + + - - + + - + - - + + - + - + @@ -191,15 +219,55 @@ - - - - - - - - + + engage_delay = 5 +last_bad_time = -engage_delay + accel = value +brake = v1 +gas = v2 +enabled = v3 + +if (brake ~= 0 or gas ~= 0 or enabled == 0) then + last_bad_time = time +end + +if (time > last_bad_time + engage_delay) then + return value +else + return 0 +end + /carControl/actuators/accel + + /carState/brakePressed + /carState/gasPressed + /carControl/enabled + + + + engage_delay = 5 +last_bad_time = -engage_delay + accel = value +brake = v1 +gas = v2 +enabled = v3 + +if (brake ~= 0 or gas ~= 0 or enabled == 0) then + last_bad_time = time +end + +if (time > last_bad_time + engage_delay) then + return value +else + return 0 +end + /carState/aEgo + + /carState/brakePressed + /carState/gasPressed + /carControl/enabled + + MS_TO_KPH = 3.6 return value/3.6 @@ -216,13 +284,28 @@ /sendcan/1/CruiseControlStalk/minus5 - - - return (value * v1 ^ 2) - (v2 * 9.81) - /controlsState/curvature + + engage_delay = 5 +last_bad_time = -engage_delay + accel = value +brake = v1 +gas = v2 +enabled = v3 + +if (brake ~= 0 or gas ~= 0 or enabled == 0) then + last_bad_time = time +end + +if (time > last_bad_time + engage_delay) then + return value +else + return 0 +end + /longitudinalPlan/accels/0 - /carState/vEgo - /liveParameters/roll + /carState/brakePressed + /carState/gasPressed + /carControl/enabled @@ -241,11 +324,42 @@ return value * 3.6 /carState/vEgo + + + return (value * v1 ^ 2) - (v2 * 9.81) + /controlsState/curvature + + /carState/vEgo + /liveParameters/roll + + return value * 2.23694 /carState/vEgo + + engage_delay = 5 +last_bad_time = -engage_delay + curvature = value +pressed = v1 +enabled = v2 + +if (pressed == 1 or enabled == 0) then + last_bad_time = time +end + +if (time > last_bad_time + engage_delay) then + return value +else + return 0 +end + /controlsState/curvature + + /carState/steeringPressed + /carControl/enabled + + return (value * v1 ^ 2) - (v2 * 9.81) @@ -288,6 +402,28 @@ return ret /carState/steeringTorqueEps + + engage_delay = 5 +last_bad_time = -engage_delay + curvature = value +pressed = v1 +enabled = v2 + +if (pressed == 1 or enabled == 0) then + last_bad_time = time +end + +if (time > last_bad_time + engage_delay) then + return value +else + return 0 +end + /lateralPlan/curvatures/0 + + /carState/steeringPressed + /carControl/enabled + + prevX = 0 calcDesiredSpeed = 0 @@ -313,6 +449,29 @@ return calcDesiredSpeed /carState/vEgo + + engage_delay = 5 +last_bad_time = -engage_delay + curvature = value / v3 +pressed = v1 +enabled = v2 + +if (pressed == 1 or enabled == 0) then + last_bad_time = time +end + +if (time > last_bad_time + engage_delay) then + return curvature +else + return 0 +end + /liveLocationKalman/angularVelocityCalibrated/value/2 + + /carState/steeringPressed + /carControl/enabled + /liveLocationKalman/velocityCalibrated/value/0 + +