LateralTorqueState: log controller version and desired lateral jerk (#36421)

This commit is contained in:
felsager
2025-10-22 10:56:34 -07:00
committed by GitHub
parent d2bb8fe537
commit a2e7f3788f
3 changed files with 6 additions and 1 deletions

View File

@@ -918,6 +918,8 @@ struct ControlsState @0x97ff69c53601abf1 {
saturated @7 :Bool;
actualLateralAccel @9 :Float32;
desiredLateralAccel @10 :Float32;
desiredLateralJerk @11 :Float32;
version @12 :Int32;
}
struct LateralLQRState {

View File

@@ -29,6 +29,7 @@ KD = 0.0
LP_FILTER_CUTOFF_HZ = 1.2
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
VERSION = 0
class LatControlTorque(LatControl):
def __init__(self, CP, CI, dt):
@@ -56,6 +57,7 @@ class LatControlTorque(LatControl):
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = VERSION
if not active:
output_torque = 0.0
pid_log.active = False
@@ -106,6 +108,7 @@ class LatControlTorque(LatControl):
pid_log.output = float(-output_torque) # TODO: log lat accel?
pid_log.actualLateralAccel = float(measurement)
pid_log.desiredLateralAccel = float(setpoint)
pid_log.desiredLateralJerk= float(desired_lateral_jerk)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
# TODO left is positive in this convention

View File

@@ -1 +1 @@
55e82ab6370865a1427ebc1d559921a5354d9cbf
1841c3c365689310f7f337a2aa6966d79bc4a60c