From acb109c290a3a776a201e0e0e7855ea9a4ec4ffb Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 8 Jun 2025 10:00:54 +0200 Subject: [PATCH] adding plotjuggler stuff --- .../analyzing-panda-block-angle-hkg.xml | 131 ++++++++ .../layouts/analyzing-torque-angle-hkg.xml | 145 ++++++++- .../plotjuggler/layouts/hkg_angle_control.xml | 281 ++++++++++++++++++ .../layouts/safety-limits-angle-kkg.xml | 175 +++++++++++ 4 files changed, 716 insertions(+), 16 deletions(-) create mode 100644 tools/plotjuggler/layouts/analyzing-panda-block-angle-hkg.xml create mode 100644 tools/plotjuggler/layouts/hkg_angle_control.xml create mode 100644 tools/plotjuggler/layouts/safety-limits-angle-kkg.xml diff --git a/tools/plotjuggler/layouts/analyzing-panda-block-angle-hkg.xml b/tools/plotjuggler/layouts/analyzing-panda-block-angle-hkg.xml new file mode 100644 index 0000000000..6f60d52f36 --- /dev/null +++ b/tools/plotjuggler/layouts/analyzing-panda-block-angle-hkg.xml @@ -0,0 +1,131 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/plotjuggler/layouts/analyzing-torque-angle-hkg.xml b/tools/plotjuggler/layouts/analyzing-torque-angle-hkg.xml index 88e716a7c6..7245ae248d 100644 --- a/tools/plotjuggler/layouts/analyzing-torque-angle-hkg.xml +++ b/tools/plotjuggler/layouts/analyzing-torque-angle-hkg.xml @@ -1,33 +1,33 @@ - + - + - + - + - + - + @@ -37,40 +37,76 @@ - + - + - - + + + - + - + + - + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + - + @@ -89,7 +125,84 @@ - + + + + return (value * v1 ^ 2) - (v2 * 9.81) + /controlsState/desiredCurvature + + /carState/vEgo + /liveParameters/roll + + + + + 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 + + + + + return value * -9.8 + /can/1/IMU_01_10ms/IMU_LatAccelVal + + /carState/steeringPressed + /carControl/latActive + + + + + 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 + + + + + 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 value * -9.8 + /can/1/IMU_01_10ms/IMU_LatAccelVal + + /carState/steeringPressed + /carControl/latActive + + + diff --git a/tools/plotjuggler/layouts/hkg_angle_control.xml b/tools/plotjuggler/layouts/hkg_angle_control.xml new file mode 100644 index 0000000000..ac77a0fb16 --- /dev/null +++ b/tools/plotjuggler/layouts/hkg_angle_control.xml @@ -0,0 +1,281 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + + + diff --git a/tools/plotjuggler/layouts/safety-limits-angle-kkg.xml b/tools/plotjuggler/layouts/safety-limits-angle-kkg.xml new file mode 100644 index 0000000000..b4033150ef --- /dev/null +++ b/tools/plotjuggler/layouts/safety-limits-angle-kkg.xml @@ -0,0 +1,175 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + return value * -9.8 + /can/1/IMU_01_10ms/IMU_LatAccelVal + + /carState/steeringPressed + /carControl/latActive + + + + + 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 + + + + + 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 + + + + + return value * -9.8 + /can/1/IMU_01_10ms/IMU_LatAccelVal + + /carState/steeringPressed + /carControl/latActive + + + + + + +if (v1 == 0 and v2 == 1) then + return (value * -9.8) - (v3 * 9.81) +end +return 0 + /can/1/IMU_01_10ms/IMU_LatAccelVal + + /carState/steeringPressed + /carControl/latActive + /liveParameters/roll + + + + + return (value * v1 ^ 2) - (v2 * 9.81) + /controlsState/curvature + + /carState/vEgo + /liveParameters/roll + + + + + return (value * v1 ^ 2) - (v2 * 9.81) + /controlsState/desiredCurvature + + /carState/vEgo + /liveParameters/roll + + + + + + +