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 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 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 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 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 return math.abs(value) desired lat accel return value * v1^2 /controlsState/desiredCurvature /carState/vEgo return math.abs(value) /can/1/LFA_ALT/LKAS_ANGLE_CMD