add desired angle to log (#23115)

* add desired angle to log

* update ref

* bump cereal

cereal

@@ -1 +1 @@
-Subproject commit c4a1c9fa00c5915fbdf7f6c29f5968dfc1be0630
+Subproject commit 486a09a2e9a8c4ae5bb2853a9bef2b64e875f883

selfdrive/controls/lib/latcontrol_angle.py

@@ -1,4 +1,5 @@
 import math
+
 from cereal import log
 
 
@@ -21,5 +22,7 @@ class LatControlAngle():
       angle_steers_des += params.angleOffsetDeg
 
     angle_log.saturated = False
-    angle_log.steeringAngleDeg = angle_steers_des
+    angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    angle_log.steeringAngleDesiredDeg = angle_steers_des 
+
     return 0, float(angle_steers_des), angle_log

selfdrive/controls/lib/latcontrol_indi.py

@@ -95,14 +95,16 @@ class LatControlINDI():
 
     steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
     steers_des += math.radians(params.angleOffsetDeg)
+    indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
+
+    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
+    indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
+
     if CS.vEgo < 0.3 or not active:
       indi_log.active = False
       self.output_steer = 0.0
       self.steer_filter.x = 0.0
     else:
-
-      rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
-
       # Expected actuator value
       self.steer_filter.update_alpha(self.RC)
       self.steer_filter.update(self.output_steer)

selfdrive/controls/lib/latcontrol_lqr.py

@@ -57,6 +57,7 @@ class LatControlLQR():
 
     instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
     desired_angle += instant_offset  # Only add offset that originates from vehicle model errors
+    lqr_log.steeringAngleDesiredDeg = desired_angle
 
     # Update Kalman filter
     angle_steers_k = float(self.C.dot(self.x_hat))
@@ -93,7 +94,7 @@ class LatControlLQR():
     check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
     saturated = self._check_saturation(output_steer, check_saturation, steers_max)
 
-    lqr_log.steeringAngleDeg = angle_steers_k + params.angleOffsetAverageDeg
+    lqr_log.steeringAngleDeg = angle_steers_k
     lqr_log.i = self.i_lqr
     lqr_log.output = output_steer
     lqr_log.lqrOutput = lqr_output

selfdrive/controls/lib/latcontrol_pid.py

@@ -24,6 +24,7 @@ class LatControlPID():
     angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
 
+    pid_log.steeringAngleDesiredDeg = angle_steers_des 
     pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
     if CS.vEgo < 0.3 or not active:
       output_steer = 0.0

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-4d55b3974e099f2bd6e77ea12e0e0830af051041
+7ad16d3db047bbcbf91c704e8a52f91e5fc08690