From 1b26b026805f1a2be3ddbcf1040800cdcea45b0a Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 15:31:40 +0100 Subject: [PATCH 01/17] return actuators from carcontroller --- cereal | 2 +- selfdrive/car/chrysler/carcontroller.py | 4 ++-- selfdrive/car/chrysler/interface.py | 6 ++---- selfdrive/car/ford/carcontroller.py | 2 +- selfdrive/car/ford/interface.py | 4 ++-- selfdrive/car/gm/carcontroller.py | 2 +- selfdrive/car/gm/interface.py | 10 +++++----- selfdrive/car/honda/carcontroller.py | 2 +- selfdrive/car/honda/interface.py | 16 ++++++++-------- selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/interface.py | 8 ++++---- selfdrive/car/interfaces.py | 2 +- selfdrive/car/mazda/carcontroller.py | 2 +- selfdrive/car/mazda/interface.py | 4 ++-- selfdrive/car/mock/interface.py | 3 ++- selfdrive/car/nissan/carcontroller.py | 2 +- selfdrive/car/nissan/interface.py | 10 +++++----- selfdrive/car/subaru/carcontroller.py | 2 +- selfdrive/car/subaru/interface.py | 8 ++++---- selfdrive/car/tesla/carcontroller.py | 2 +- selfdrive/car/tesla/interface.py | 4 ++-- selfdrive/car/toyota/carcontroller.py | 7 ++----- selfdrive/car/toyota/interface.py | 13 ++++++------- selfdrive/car/volkswagen/carcontroller.py | 2 +- selfdrive/car/volkswagen/interface.py | 14 +++++++------- selfdrive/controls/controlsd.py | 4 +++- 26 files changed, 67 insertions(+), 70 deletions(-) diff --git a/cereal b/cereal index e5a04ab458afd5..0cdd2177e6a90e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e5a04ab458afd52cf630cc9e35ccdc10efba6688 +Subproject commit 0cdd2177e6a90ef0e643e7aa4b5b5a138a2ba1d6 diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 46681b13e2db84..db13f308b3ffae 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -20,7 +20,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): # this seems needed to avoid steering faults and to force the sync with the EPS counter frame = CS.lkas_counter if self.prev_frame == frame: - return [] + return actuators, [] # *** compute control surfaces *** # steer torque @@ -67,4 +67,4 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): self.ccframe += 1 self.prev_frame = frame - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 1822d76a1af000..c159211dd118f0 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -78,8 +78,6 @@ def update(self, c, can_strings): def apply(self, c): if (self.CS.frame == -1): - return [] # if we haven't seen a frame 220, then do not update. + return c.actuators, [] # if we haven't seen a frame 220, then do not update. - can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert) - - return can_sends + return self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 06c5f257976c86..2eaacd301cd4fe 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -83,4 +83,4 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel): self.main_on_last = CS.out.cruiseState.available self.steer_alert_last = steer_alert - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 8c0b1e1faaaceb..7155b3ea82d887 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -63,8 +63,8 @@ def update(self, c, can_strings): # to be called @ 100hz def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.hudControl.visualAlert, c.cruiseControl.cancel) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 8f4d0f27ca332d..2212cebb28c66e 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -106,4 +106,4 @@ def update(self, enabled, CS, frame, actuators, can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index b06e5373aa1009..d9a63d32e34f04 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -220,10 +220,10 @@ def apply(self, c): # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.out.gasPressed - can_sends = self.CC.update(enabled, self.CS, self.frame, - c.actuators, - hud_v_cruise, c.hudControl.lanesVisible, - c.hudControl.leadVisible, c.hudControl.visualAlert) + ret = self.CC.update(enabled, self.CS, self.frame, + c.actuators, + hud_v_cruise, c.hudControl.lanesVisible, + c.hudControl.leadVisible, c.hudControl.visualAlert) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index e1488eda2a79b2..b3609186615b8b 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -246,4 +246,4 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, idx = (frame//10) % 4 can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud)) - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 03074b875c107a..274f4ed5f63e9f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -438,13 +438,13 @@ def apply(self, c): else: hud_v_cruise = 255 - can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame, - c.actuators, - c.cruiseControl.cancel, - hud_v_cruise, - c.hudControl.lanesVisible, - hud_show_car=c.hudControl.leadVisible, - hud_alert=c.hudControl.visualAlert) + ret = self.CC.update(c.enabled, c.active, self.CS, self.frame, + c.actuators, + c.cruiseControl.cancel, + hud_v_cruise, + c.hudControl.lanesVisible, + hud_show_car=c.hudControl.leadVisible, + hud_alert=c.hudControl.visualAlert) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index a8b70fcf9021f9..6dd1f6699c8421 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -116,4 +116,4 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.append(create_frt_radar_opt(self.packer)) - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 57b313e5344c6a..091523ac1a9ada 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -347,8 +347,8 @@ def update(self, c, can_strings): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible, - c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) + ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible, + c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index bd5b98c8e18a77..8bf42cd4022ab7 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -103,7 +103,7 @@ def get_std_params(candidate, fingerprint): def update(self, c, can_strings): raise NotImplementedError - # return sendcan, pass in a car.CarControl + # returns (car.CarControl.Actuators, sendcan), pass in a car.CarControl def apply(self, c): raise NotImplementedError diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 06c5eb0948ae9a..81b4ee2e5925d3 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -58,4 +58,4 @@ def update(self, c, CS, frame): # send steering command can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, frame, apply_steer, CS.cam_lkas)) - return can_sends + return c.actuators, can_sends diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 8364bf000c942b..b4ae938228b885 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -95,6 +95,6 @@ def update(self, c, can_strings): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c, self.CS, self.frame) + ret = self.CC.update(c, self.CS, self.frame) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 999e735c73dec9..bc1f6dcf6bc60d 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -88,4 +88,5 @@ def update(self, c, can_strings): def apply(self, c): # in mock no carcontrols - return [] + actuators = car.CarControl.Actuators.new_message() + return actuators, [] diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 41ba9f6599cbdf..0b635422c53071 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -87,4 +87,4 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert, self.packer, lkas_hud_info_msg, steer_hud_alert )) - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 18ae885f8379b0..004ea8dd0a8117 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -78,9 +78,9 @@ def update(self, c, can_strings): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, c.hudControl.visualAlert, - c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, - c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) + ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + c.cruiseControl.cancel, c.hudControl.visualAlert, + c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, + c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index a2d0c2ebdda2ba..55747f890e683b 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -72,4 +72,4 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index f5b0886a95eaec..9537f293ada614 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -122,8 +122,8 @@ def update(self, c, can_strings): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, c.hudControl.visualAlert, - c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) + ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + c.cruiseControl.cancel, c.hudControl.visualAlert, + c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 7e6a2f2e9a8e70..e5a663c1cf774f 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -62,4 +62,4 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel): # TODO: HUD control - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 45dc0a72393db7..4b5b2117877a4a 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -71,6 +71,6 @@ def update(self, c, can_strings): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) + ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b35081c70353a4..c2a4789cc94455 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -23,8 +23,6 @@ def __init__(self, dbc_name, CP, VM): def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): - # *** compute control surfaces *** - # gas and brake if CS.CP.enableGasInterceptor and enabled: MAX_INTERCEPTOR_GAS = 0.5 @@ -125,10 +123,9 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler if frame % 100 == 0 and CS.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) - #*** static msgs *** - + # *** static msgs *** for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS: if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index f9d6b586fddb45..6c1f3e9f41a8df 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -312,12 +312,11 @@ def update(self, c, can_strings): # pass in a car.CarControl # to be called @ 100hz def apply(self, c): - - can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame, - c.actuators, c.cruiseControl.cancel, - c.hudControl.visualAlert, c.hudControl.leftLaneVisible, - c.hudControl.rightLaneVisible, c.hudControl.leadVisible, - c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) + ret = self.CC.update(c.enabled, c.active, self.CS, self.frame, + c.actuators, c.cruiseControl.cancel, + c.hudControl.visualAlert, c.hudControl.leftLaneVisible, + c.hudControl.rightLaneVisible, c.hudControl.leadVisible, + c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 977818dbd5dc9e..6f6863733ef233 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -110,4 +110,4 @@ def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane self.graButtonStatesToSend = None self.graMsgSentCount = 0 - return can_sends + return actuators, can_sends diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 560e64ce2d143f..bef1c37185a519 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -216,11 +216,11 @@ def update(self, c, can_strings): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, - c.hudControl.visualAlert, - c.hudControl.leftLaneVisible, - c.hudControl.rightLaneVisible, - c.hudControl.leftLaneDepart, - c.hudControl.rightLaneDepart) + ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, + c.hudControl.visualAlert, + c.hudControl.leftLaneVisible, + c.hudControl.rightLaneVisible, + c.hudControl.leftLaneDepart, + c.hudControl.rightLaneDepart) self.frame += 1 - return can_sends + return ret diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 19fe328dbf8712..e50539d0a435c9 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -159,6 +159,7 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = False self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} + self.last_actuators = car.CarControl.Actuators.new_message() # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True @@ -621,8 +622,9 @@ def publish_logs(self, CS, start_time, actuators, lac_log): if not self.read_only and self.initialized: # send car controls over can - can_sends = self.CI.apply(CC) + self.last_actuators, can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + # CC.actuatorsFinal = self.last_actuators force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) From b8df2a44396a19fc37736f052b402a02f1bb804d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 15:35:59 +0100 Subject: [PATCH 02/17] log it --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e50539d0a435c9..21ca4dab5aae89 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -624,7 +624,7 @@ def publish_logs(self, CS, start_time, actuators, lac_log): # send car controls over can self.last_actuators, can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) - # CC.actuatorsFinal = self.last_actuators + CC.actuatorsFinal = self.last_actuators force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) From 1f8cd49d68216ce4b8890fcaaffe276a7a16312d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 15:37:13 +0100 Subject: [PATCH 03/17] pass to latcontrol --- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 4 ++-- selfdrive/controls/lib/latcontrol_indi.py | 2 +- selfdrive/controls/lib/latcontrol_lqr.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 4 ++-- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21ca4dab5aae89..b7ecd77d4b0cbe 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -511,7 +511,7 @@ def state_control(self, CS): lat_plan.psis, lat_plan.curvatures, lat_plan.curvatureRates) - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators, desired_curvature, desired_curvature_rate) else: lac_log = log.ControlsState.LateralDebugState.new_message() diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 8fcb9ae7bfd066..5975fa66b758f4 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -10,7 +10,7 @@ def __init__(self, CP): def reset(self): pass - def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): angle_log = log.ControlsState.LateralAngleState.new_message() if CS.vEgo < 0.3 or not active: @@ -23,6 +23,6 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur angle_log.saturated = False angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) - angle_log.steeringAngleDesiredDeg = angle_steers_des + 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 50a8e22b3cfa02..169d504d5b83ff 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -82,7 +82,7 @@ def _check_saturation(self, control, check_saturation, limit): return self.sat_count > self.sat_limit - def update(self, active, CS, CP, VM, params, curvature, curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 16fffac2791b1c..9f58aea2e78db2 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -44,7 +44,7 @@ def _check_saturation(self, control, check_saturation, limit): return self.sat_count > self.sat_limit - def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index c7730d011cea79..ca78d5fa4e322c 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -16,7 +16,7 @@ def __init__(self, CP, CI): def reset(self): self.pid.reset() - def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -24,7 +24,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur 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.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = angle_steers_des - CS.steeringAngleDeg if CS.vEgo < 0.3 or not active: output_steer = 0.0 From 96df1976e717a0998f5b57c60a88c7c4489a4fd7 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 15:39:53 +0100 Subject: [PATCH 04/17] chrysler --- selfdrive/car/chrysler/carcontroller.py | 9 ++++++--- selfdrive/car/chrysler/interface.py | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index db13f308b3ffae..d491ccd4b9a4c7 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,3 +1,4 @@ +from cereal import car from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \ create_wheel_buttons @@ -20,9 +21,8 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): # this seems needed to avoid steering faults and to force the sync with the EPS counter frame = CS.lkas_counter if self.prev_frame == frame: - return actuators, [] + return car.CarControl.Actuators.new_message(), [] - # *** compute control surfaces *** # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, @@ -67,4 +67,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): self.ccframe += 1 self.prev_frame = frame - return actuators, can_sends + new_actuators = actuators.copy() + new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + + return new_actuators, can_sends diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index c159211dd118f0..a893b222f8b0bc 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -78,6 +78,6 @@ def update(self, c, can_strings): def apply(self, c): if (self.CS.frame == -1): - return c.actuators, [] # if we haven't seen a frame 220, then do not update. + return car.CarControl.Actuators.new_message(), [] # if we haven't seen a frame 220, then do not update. return self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert) From 7c7e8707d792db665af77262bc381eeacf3c0f7f Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 15:46:02 +0100 Subject: [PATCH 05/17] gm --- selfdrive/car/gm/carcontroller.py | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 2212cebb28c66e..1a7cceeaf56731 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -14,6 +14,9 @@ class CarController(): def __init__(self, dbc_name, CP, VM): self.start_time = 0. self.apply_steer_last = 0 + self.apply_gas = 0 + self.apply_brake = 0 + self.lka_steering_cmd_counter_last = -1 self.lka_icon_status_last = (False, False) self.steer_rate_limited = False @@ -31,6 +34,7 @@ def update(self, enabled, CS, frame, actuators, # Send CAN commands. can_sends = [] + new_actuators = actuators.copy() # Steering (50Hz) # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the @@ -52,23 +56,27 @@ def update(self, enabled, CS, frame, actuators, idx = (CS.lka_steering_cmd_counter + 1) % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) - - if not enabled: - # Stock ECU sends max regen when not enabled. - apply_gas = P.MAX_ACC_REGEN - apply_brake = 0 - else: - apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) - apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) + new_actuators.steer = self.apply_steer_last / P.STEER_MAX # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: + if not enabled: + # Stock ECU sends max regen when not enabled. + self.apply_gas = P.MAX_ACC_REGEN + self.apply_brake = 0 + else: + self.apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) + idx = (frame // 4) % 4 at_full_stop = enabled and CS.out.standstill near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) - can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop)) + + new_actuators.gas = self.apply_gas + new_actuators.brake = self.apply_brake # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: @@ -106,4 +114,4 @@ def update(self, enabled, CS, frame, actuators, can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status - return actuators, can_sends + return new_actuators, can_sends From 084a71d788820e9a732d552096ad27ac4f1f5bae Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 16:08:28 +0100 Subject: [PATCH 06/17] honda --- cereal | 2 +- selfdrive/car/honda/carcontroller.py | 38 ++++++++++++++++++++-------- 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/cereal b/cereal index 0cdd2177e6a90e..065f562ebc805c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 0cdd2177e6a90ef0e643e7aa4b5b5a138a2ba1d6 +Subproject commit 065f562ebc805ce58702d480ed0ed32c9755ccd5 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b3609186615b8b..365eb5729b51b8 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -105,6 +105,11 @@ def __init__(self, dbc_name, CP, VM): self.last_pump_ts = 0. self.packer = CANPacker(dbc_name) + self.accel = 0 + self.speed = 0 + self.gas = 0 + self.brake = 0 + self.params = CarControllerParams(CP) def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, @@ -151,6 +156,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, # Send CAN commands. can_sends = [] + new_actuators = actuators.copy() # tester present - w/ no response (keeps radar disabled) if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: @@ -211,10 +217,9 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: - accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX) - bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) - can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint)) - + self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX) + self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) + can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, starting, CS.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1)) @@ -224,6 +229,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake + self.brake = apply_brake / P.NIDEC_BRAKE_MAX if CS.CP.enableGasInterceptor: # way too aggressive at low speed without this @@ -233,17 +239,27 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected # when you do enable. if active: - apply_gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.) + self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.) else: - apply_gas = 0.0 - can_sends.append(create_gas_interceptor_command(self.packer, apply_gas, idx)) - - hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, - hud_lanes, fcw_display, acc_alert, steer_required) + self.gas = 0.0 + can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame//10) % 4 + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, + hud_lanes, fcw_display, acc_alert, steer_required) can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud)) - return actuators, can_sends + if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint not in HONDA_BOSCH): + self.speed = pcm_speed + + if not CS.CP.enableGasInterceptor: + self.gas = pcm_accel / 0xc6 + + new_actuators.speed = self.speed + new_actuators.accel = self.accel + new_actuators.gas = self.gas + new_actuators.brake = self.brake + + return new_actuators, can_sends From a81ad118f2f125e7766f9a5c42665e9d1e0a5779 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 16:13:29 +0100 Subject: [PATCH 07/17] more brands --- selfdrive/car/hyundai/carcontroller.py | 5 +++++ selfdrive/car/mazda/carcontroller.py | 5 ++++- selfdrive/car/nissan/carcontroller.py | 4 +++- selfdrive/car/subaru/carcontroller.py | 2 ++ selfdrive/car/tesla/carcontroller.py | 2 ++ 5 files changed, 16 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 6dd1f6699c8421..fe71595a2f3245 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -44,6 +44,7 @@ def __init__(self, dbc_name, CP, VM): self.car_fingerprint = CP.carFingerprint self.steer_rate_limited = False self.last_resume_frame = 0 + self.accel = 0 def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, left_lane, right_lane, left_lane_depart, right_lane_depart): @@ -65,6 +66,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu left_lane, right_lane, left_lane_depart, right_lane_depart) can_sends = [] + new_actuators = actuators.copy() + new_actuators.steer = apply_steer / self.p.STEER_MAX # tester present - w/ no response (keeps radar disabled) if CS.CP.openpilotLongitudinalControl: @@ -100,6 +103,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu stopping = (actuators.longControlState == LongCtrlState.stopping) set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping)) + self.accel = accel # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, @@ -116,4 +120,5 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.append(create_frt_radar_opt(self.packer)) + new_actuators.accel = self.accel return actuators, can_sends diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 81b4ee2e5925d3..7f516a497bfc18 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -58,4 +58,7 @@ def update(self, c, CS, frame): # send steering command can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, frame, apply_steer, CS.cam_lkas)) - return c.actuators, can_sends + + new_actuators = c.actuators.copy() + new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + return new_actuators, can_sends diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 0b635422c53071..86a2883f0b9297 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -87,4 +87,6 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert, self.packer, lkas_hud_info_msg, steer_hud_alert )) - return actuators, can_sends + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + return new_actuators, can_sends diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 55747f890e683b..2a3a17b33fd958 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -72,4 +72,6 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / CarControllerParams.STEER_MAX return actuators, can_sends diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index e5a663c1cf774f..fe8b2f72fe5fa6 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -62,4 +62,6 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel): # TODO: HUD control + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle return actuators, can_sends From 7882807fa3d78b07d971bbbf8e2ed6c9051b6ca0 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 16:20:11 +0100 Subject: [PATCH 08/17] rest of the brands --- selfdrive/car/honda/carcontroller.py | 2 +- selfdrive/car/toyota/carcontroller.py | 11 ++++++++++- selfdrive/car/volkswagen/carcontroller.py | 4 +++- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 365eb5729b51b8..9a424a1e689c7c 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -156,7 +156,6 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, # Send CAN commands. can_sends = [] - new_actuators = actuators.copy() # tester present - w/ no response (keeps radar disabled) if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: @@ -257,6 +256,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, if not CS.CP.enableGasInterceptor: self.gas = pcm_accel / 0xc6 + new_actuators = actuators.copy() new_actuators.speed = self.speed new_actuators.accel = self.accel new_actuators.gas = self.gas diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c2a4789cc94455..506d7547291e2a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -19,6 +19,8 @@ def __init__(self, dbc_name, CP, VM): self.steer_rate_limited = False self.packer = CANPacker(dbc_name) + self.gas = 0 + self.accel = 0 def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): @@ -94,6 +96,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) + self.accel = pcm_accel_cmd else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type)) @@ -101,6 +104,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. # This prevents unexpected pedal range rescaling can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2)) + self.gas = interceptor_gas_cmd # ui mesg is at 100Hz but we send asap if: # - there is something to display @@ -128,4 +132,9 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) - return actuators, can_sends + new_actuators = actuators.copy() + new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.accel = self.accel + new_actuators.gas = self.gas + + return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 6f6863733ef233..057460807dbc97 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -110,4 +110,6 @@ def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane self.graButtonStatesToSend = None self.graMsgSentCount = 0 - return actuators, can_sends + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / P.STEER_MAX + return new_actuators, can_sends From 986709dddb6bb69718037884c60964024348f79b Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 16:25:08 +0100 Subject: [PATCH 09/17] gm cleanup --- selfdrive/car/gm/carcontroller.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 1a7cceeaf56731..71587b17d96141 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -34,7 +34,6 @@ def update(self, enabled, CS, frame, actuators, # Send CAN commands. can_sends = [] - new_actuators = actuators.copy() # Steering (50Hz) # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the @@ -56,7 +55,6 @@ def update(self, enabled, CS, frame, actuators, idx = (CS.lka_steering_cmd_counter + 1) % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) - new_actuators.steer = self.apply_steer_last / P.STEER_MAX # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: @@ -75,9 +73,6 @@ def update(self, enabled, CS, frame, actuators, can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop)) - new_actuators.gas = self.apply_gas - new_actuators.brake = self.apply_brake - # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: send_fcw = hud_alert == VisualAlert.fcw @@ -114,4 +109,9 @@ def update(self, enabled, CS, frame, actuators, can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / P.STEER_MAX + new_actuators.gas = self.apply_gas + new_actuators.brake = self.apply_brake + return new_actuators, can_sends From 741565a5e9d814b710b3bb7029b8de23f9abe53a Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 16:26:52 +0100 Subject: [PATCH 10/17] hyundai cleanup --- selfdrive/car/hyundai/carcontroller.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index fe71595a2f3245..a6812754e88581 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -66,8 +66,6 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu left_lane, right_lane, left_lane_depart, right_lane_depart) can_sends = [] - new_actuators = actuators.copy() - new_actuators.steer = apply_steer / self.p.STEER_MAX # tester present - w/ no response (keeps radar disabled) if CS.CP.openpilotLongitudinalControl: @@ -120,5 +118,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.append(create_frt_radar_opt(self.packer)) + new_actuators = actuators.copy() + new_actuators.steer = apply_steer / self.p.STEER_MAX new_actuators.accel = self.accel - return actuators, can_sends + + return new_actuators, can_sends From 245b0baacf72012bd08e094152a1c30fd7ac0267 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Dec 2021 16:34:52 +0100 Subject: [PATCH 11/17] update ref --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 0301f0900de0bc..8415bc8de7d86f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c84310c94ccab023e36d02e9b85430ad7d878868 \ No newline at end of file +8118420b292f4c67ce7e196fc2121571da330e65 \ No newline at end of file From 083e53ae7977b1d6b694935652b621a51102205b Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 16 Dec 2021 11:40:58 +0100 Subject: [PATCH 12/17] rename field --- cereal | 2 +- selfdrive/controls/controlsd.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cereal b/cereal index 065f562ebc805c..457f445da2d2a8 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 065f562ebc805ce58702d480ed0ed32c9755ccd5 +Subproject commit 457f445da2d2a8c71a386a70d148b71c50801944 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b7ecd77d4b0cbe..cc449537a6d38d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -624,7 +624,7 @@ def publish_logs(self, CS, start_time, actuators, lac_log): # send car controls over can self.last_actuators, can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) - CC.actuatorsFinal = self.last_actuators + CC.actuatorsOutput = self.last_actuators force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) From 55a86173a73fa6939bff1aa482867ac0cb0f11a7 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 16 Dec 2021 11:41:57 +0100 Subject: [PATCH 13/17] fix subaru --- selfdrive/car/subaru/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 2a3a17b33fd958..1aa6e3ca21b4a5 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -73,5 +73,5 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le self.es_lkas_cnt = CS.es_lkas_msg["Counter"] new_actuators = actuators.copy() - new_actuators.steer = self.apply_steer_last / CarControllerParams.STEER_MAX + new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX return actuators, can_sends From b9133ea4b523f347acaef8c72cae34099580c9b0 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 16 Dec 2021 11:53:58 +0100 Subject: [PATCH 14/17] add types --- selfdrive/car/interfaces.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 8bf42cd4022ab7..2ff26891ed4c9f 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,6 +1,7 @@ import os import time -from typing import Dict +from abc import abstractmethod +from typing import Dict, Tuple, List from cereal import car from common.kalman.simple_kalman import KF1D @@ -48,8 +49,9 @@ def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @staticmethod + @abstractmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): - raise NotImplementedError + pass @staticmethod def init(CP, logcan, sendcan): @@ -99,13 +101,13 @@ def get_std_params(candidate, fingerprint): ret.longitudinalActuatorDelayUpperBound = 0.15 return ret - # returns a car.CarState, pass in car.CarControl - def update(self, c, can_strings): - raise NotImplementedError + @abstractmethod + def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState: + pass - # returns (car.CarControl.Actuators, sendcan), pass in a car.CarControl - def apply(self, c): - raise NotImplementedError + @abstractmethod + def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]: + pass def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pcm_enable=True): events = Events() From c101db26293234eb7c5f95823675988eb7bee94d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 16 Dec 2021 11:59:28 +0100 Subject: [PATCH 15/17] more subaru fixes #23240 --- selfdrive/car/subaru/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 1aa6e3ca21b4a5..7f9ff465381aac 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -74,4 +74,4 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX - return actuators, can_sends + return new_actuators, can_sends From f33eeb24dac511e4c4009c67821a0cf24529f1a4 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 16 Dec 2021 12:01:41 +0100 Subject: [PATCH 16/17] consistent whitespace --- selfdrive/car/mazda/carcontroller.py | 1 + selfdrive/car/nissan/carcontroller.py | 1 + selfdrive/car/subaru/carcontroller.py | 1 + selfdrive/car/tesla/carcontroller.py | 1 + selfdrive/car/volkswagen/carcontroller.py | 1 + 5 files changed, 5 insertions(+) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 7f516a497bfc18..c65ff72ed52248 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -61,4 +61,5 @@ def update(self, c, CS, frame): new_actuators = c.actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + return new_actuators, can_sends diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 86a2883f0b9297..8b40a050c19efa 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -89,4 +89,5 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert, new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_angle + return new_actuators, can_sends diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 7f9ff465381aac..72b07f919261bd 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -74,4 +74,5 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX + return new_actuators, can_sends diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index fe8b2f72fe5fa6..4efd1c1fe7c4bf 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -64,4 +64,5 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel): new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_angle + return actuators, can_sends diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 057460807dbc97..a0119e2a52ed3d 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -112,4 +112,5 @@ def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / P.STEER_MAX + return new_actuators, can_sends From d1b5d528803ad00ccb1b831031a4afda06303291 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 16 Dec 2021 12:23:58 +0100 Subject: [PATCH 17/17] bump cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 457f445da2d2a8..f1c5c8ef7cab05 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 457f445da2d2a8c71a386a70d148b71c50801944 +Subproject commit f1c5c8ef7cab05b66892d67b6992f5ac1238ad4f