if value > .3 then return 1 end return 0 Angle Error firstX = 0 firstY = 0 is_first = true secondX = 0 secondY = 0 is_second = false -- Wait for initial values if (is_first) then is_first = false is_second = true firstX = time firstY = value end if (is_second) then is_second = false secondX = time secondY = value end -- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x) dx = time - firstX dy = value - firstY -- Increment firstX = secondX firstY = secondY secondX = time secondY = value return dy/dx /can/1/LFA_ALT/LKAS_ANGLE_CMD min=0 max=250 max_from_speed=96 k1=200 k2=30 k3=1 k4=1 k5=10 function sign(number) return number > 0 and 1 or (number == 0 and 0 or -1) end return 250 - value * 20 desired lateral jark min=0 max=250 max_from_speed=96 rate_lim = 500 la_deadzone = 0.38 k1=200 k2=20 k3=1.0 k4=1 k5=10 old = 0 function sign(number) return number > 0 and 1 or (number == 0 and 0 or -1) end function apply_rate_limit(old, new, limit) return math.min(math.max(new, old - limit), old + limit) end function apply_deadzone(val, deadzone) if math.abs(val) <= deadzone then return 0.0 elseif val < 0.0 then return val + deadzone else return val - deadzone end end la = apply_deadzone(v2, la_deadzone) lj = v3 if la == 0.0 then lj = 0.0 end fla = math.min(math.abs(k1 * la)^k3, max) flj = math.min(math.abs(k2 * lj)^k4, max) out = fla flv = math.min(max_from_speed, k5 * v4) out = out + flv out = math.max(math.min(out, max), min) if sign(la) == sign(lj) then out = out - flj else out = out + flj end if v5 == 1.0 then out = 0.0 end out = math.max(math.min(out, max), min) out = apply_rate_limit(old, out, rate_lim) old = out return out /can/1/LFA_ALT/LKAS_ANGLE_CMD ang_cmd rate desired lat accel desired lateral jark /carState/vEgo /can/1/LFA_ALT/LKAS_ANGLE_ACTIVE return value * 2.23694 /carState/vEgo return math.abs(value) desired lat accel if (v3 == 0 and v4 == 1) then return (value * v1 ^ 2) - (v2 * 9.81) end return 0 /controlsState/curvature /carState/vEgo /liveParameters/roll /carState/steeringPressed /carControl/latActive return (0) /carState/canValid return value * -9.8 /can/1/IMU_01_10ms/IMU_LatAccelVal /carState/steeringPressed /carControl/latActive return value * v1^2 /controlsState/desiredCurvature /carState/vEgo 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 last_angle_requested = 0 angle_error = last_angle_requested - v1 last_angle_requested = value return angle_error /carControl/actuators/steeringAngleDeg /carState/steeringAngleDeg return value * 3.6 /carState/vEgo 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/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 firstX = 0 firstY = 0 is_first = true secondX = 0 secondY = 0 is_second = false -- Wait for initial values if (is_first) then is_first = false is_second = true firstX = time firstY = value end if (is_second) then is_second = false secondX = time secondY = value end -- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x) dx = time - firstX dy = value - firstY -- Increment firstX = secondX firstY = secondY secondX = time secondY = value return dy/dx desired lat accel 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 return math.abs(value) /can/1/LFA_ALT/LKAS_ANGLE_CMD min=0 max=250 max_from_speed=96 rate_lim = 500 la_deadzone = 0.38 k1=200 k2=20 k3=1.0 k4=1 k5=10 old = 0 function sign(number) return number > 0 and 1 or (number == 0 and 0 or -1) end function apply_rate_limit(old, new, limit) return math.min(math.max(new, old - limit), old + limit) end function apply_deadzone(val, deadzone) if math.abs(val) <= deadzone then return 0.0 elseif val < 0.0 then return val + deadzone else return val - deadzone end end return 0 /carState/aEgo 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) /controlsState/curvature /carState/vEgo /liveParameters/roll if (v1 == 0 and v2 == 1) then return (value * -9.8) - (v3 * 9.81) end --return 0 return (value * -9.8) - (v3 * 9.81) /can/1/IMU_01_10ms/IMU_LatAccelVal /carState/steeringPressed /carControl/latActive /liveParameters/roll if (v1 == 0 and v2 == 1) then return value * -9.8 end return 0 /can/1/IMU_01_10ms/IMU_LatAccelVal /carState/steeringPressed /carControl/latActive 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 return value * -9.8 /can/1/IMU_01_10ms/IMU_LatAccelVal /carState/steeringPressed /carControl/latActive return (value * v1 ^ 2) - (v2 * 9.81) /controlsState/desiredCurvature /carState/vEgo /liveParameters/roll