diff --git a/cereal b/cereal index c4a1c9fa00..486a09a2e9 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit c4a1c9fa00c5915fbdf7f6c29f5968dfc1be0630 +Subproject commit 486a09a2e9a8c4ae5bb2853a9bef2b64e875f883 diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index a641687b4f..8fcb9ae7bf 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index cee391d45b..50a8e22b3c 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index e445fb3f8a..16fffac279 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 5062df36d6..c7730d011c 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 640e6864da..bfef2971a7 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4d55b3974e099f2bd6e77ea12e0e0830af051041 \ No newline at end of file +7ad16d3db047bbcbf91c704e8a52f91e5fc08690 \ No newline at end of file