LateralTorqueState: log controller version and desired lateral jerk (#36421)
This commit is contained in:
@@ -918,6 +918,8 @@ struct ControlsState @0x97ff69c53601abf1 {
|
||||
saturated @7 :Bool;
|
||||
actualLateralAccel @9 :Float32;
|
||||
desiredLateralAccel @10 :Float32;
|
||||
desiredLateralJerk @11 :Float32;
|
||||
version @12 :Int32;
|
||||
}
|
||||
|
||||
struct LateralLQRState {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -1 +1 @@
|
||||
55e82ab6370865a1427ebc1d559921a5354d9cbf
|
||||
1841c3c365689310f7f337a2aa6966d79bc4a60c
|
||||
Reference in New Issue
Block a user