From 3e61c7e6ae41f18f7b87c5d3177e9f88a1e6ec28 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 13 Mar 2022 20:58:44 -0700 Subject: [PATCH] carControl: add long and lat active fields (#23859) * proof of concept * actuators packet describes which actuators are active * bump cereal * fixes * not needed for this PR * Do Toyota * add back controlsState.active * bump cereal * rest of cars * in actuators * add active back * which * use controlsState.active for now * will make an issue * Update selfdrive/controls/controlsd.py Co-authored-by: Adeeb Shihadeh * move local lat_active checks into CC.latActive * remove redundant checks * move comment move comment * fix joystick mode * get enabled from carcontrol * do standstill check in controlsd * make sure we consider the gas press case for GM * use CC.actuators * fix * capitalization * Bump cereal Bump cereal * make intermediate actuators * similar convention to before * clean that up * update refs Co-authored-by: Adeeb Shihadeh old-commit-hash: 629399a44925080b2c35b6dde35c093cc2e32035 --- cereal | 2 +- selfdrive/car/ford/interface.py | 2 +- selfdrive/car/gm/carcontroller.py | 15 +++--- selfdrive/car/gm/interface.py | 7 +-- selfdrive/car/honda/carcontroller.py | 14 +++--- selfdrive/car/honda/interface.py | 8 ++-- selfdrive/car/hyundai/carcontroller.py | 19 ++++---- selfdrive/car/hyundai/interface.py | 6 +-- selfdrive/car/mazda/carcontroller.py | 2 +- selfdrive/car/nissan/carcontroller.py | 8 ++-- selfdrive/car/nissan/interface.py | 2 +- selfdrive/car/subaru/carcontroller.py | 6 +-- selfdrive/car/subaru/interface.py | 2 +- selfdrive/car/tesla/carcontroller.py | 4 +- selfdrive/car/tesla/interface.py | 2 +- selfdrive/car/toyota/carcontroller.py | 10 ++-- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/volkswagen/carcontroller.py | 8 ++-- selfdrive/car/volkswagen/interface.py | 2 +- selfdrive/controls/controlsd.py | 57 ++++++++++++----------- selfdrive/test/process_replay/ref_commit | 2 +- 21 files changed, 86 insertions(+), 94 deletions(-) diff --git a/cereal b/cereal index 279311d0bd..84a1793eb4 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 279311d0bde60e814a697da22c55c6abe8a3b03c +Subproject commit 84a1793eb4d09821f23613b98d80e2ce4ee57b9c diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index d792e5fd55..fc6843c826 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -66,7 +66,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, - c.hudControl.visualAlert, c.cruiseControl.cancel) + c.hudControl.visualAlert, c.cruiseControl.cancel) self.frame += 1 return ret diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 93c28a1cfc..39e348ff29 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -27,8 +27,7 @@ class CarController(): self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) - def update(self, c, enabled, CS, frame, actuators, - hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): + def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params @@ -41,7 +40,7 @@ class CarController(): if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter elif (frame % P.STEER_STEP) == 0: - lkas_enabled = c.active and not (CS.out.steerFaultTemporary or CS.out.steerFaultPermanent) and CS.out.vEgo > P.MIN_STEER_SPEED + lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) @@ -58,7 +57,7 @@ class CarController(): # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: - if not c.active: + if not c.longActive: # Stock ECU sends max regen when not enabled. self.apply_gas = P.MAX_ACC_REGEN self.apply_brake = 0 @@ -68,15 +67,15 @@ class CarController(): idx = (frame // 4) % 4 - at_full_stop = enabled and CS.out.standstill - near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) + at_full_stop = c.longActive and CS.out.standstill + near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) 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)) + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: send_fcw = hud_alert == VisualAlert.fcw - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d6a2d3cfee..0aad0a7f04 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -222,12 +222,7 @@ class CarInterface(CarInterfaceBase): if hud_v_cruise > 70: hud_v_cruise = 0 - # For Openpilot, "enabled" includes pre-enable. - # In GM, PCM faults out if ACC command overlaps user gas. - enabled = c.enabled and not self.CS.out.gasPressed - - ret = self.CC.update(c, enabled, self.CS, self.frame, - c.actuators, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, hud_v_cruise, hud_control.lanesVisible, hud_control.leadVisible, hud_control.visualAlert) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 3ba8b64dd3..bf5456bc4b 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -112,12 +112,12 @@ class CarController(): self.params = CarControllerParams(CP) - def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params - if active: + if c.longActive: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: @@ -136,7 +136,7 @@ class CarController(): else: hud_lanes = 0 - if enabled: + if c.enabled: if hud_show_car: hud_car = 2 else: @@ -152,8 +152,6 @@ class CarController(): # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) - lkas_active = active - # Send CAN commands. can_sends = [] @@ -165,7 +163,7 @@ class CarController(): # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, - lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) + c.latActive, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) stopping = actuators.longControlState == LongCtrlState.stopping @@ -217,7 +215,7 @@ class CarController(): if CS.CP.carFingerprint in HONDA_BOSCH: 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, CS.CP.carFingerprint)) + can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, 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)) @@ -236,7 +234,7 @@ class CarController(): # This prevents unexpected pedal range rescaling # 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: + if c.longActive: self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.) else: self.gas = 0.0 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 94e4305909..9f09f8a719 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -427,11 +427,9 @@ class CarInterface(CarInterfaceBase): else: hud_v_cruise = 255 - ret = self.CC.update(c.enabled, c.active, self.CS, self.frame, - c.actuators, - c.cruiseControl.cancel, - hud_v_cruise, - hud_control.lanesVisible, + ret = self.CC.update(c, self.CS, self.frame, + c.actuators, c.cruiseControl.cancel, + hud_v_cruise, hud_control.lanesVisible, hud_show_car=hud_control.leadVisible, hud_alert=hud_control.visualAlert) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 8572465b6b..55fc442f6d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -46,23 +46,20 @@ class CarController(): self.last_resume_frame = 0 self.accel = 0 - def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, left_lane, right_lane, left_lane_depart, right_lane_depart): # Steering Torque new_steer = int(round(actuators.steer * self.p.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer - # disable when temp fault is active, or below LKA minimum speed - lkas_active = c.active and not CS.out.steerFaultTemporary and CS.out.vEgo >= CS.CP.minSteerSpeed - - if not lkas_active: + if not c.latActive: apply_steer = 0 self.apply_steer_last = apply_steer sys_warning, sys_state, left_lane_warning, right_lane_warning = \ - process_hud_alert(enabled, self.car_fingerprint, visual_alert, + process_hud_alert(c.enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) can_sends = [] @@ -72,8 +69,8 @@ class CarController(): if (frame % 100) == 0: can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0]) - can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, - CS.lkas11, sys_warning, sys_state, enabled, + can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, c.latActive, + CS.lkas11, sys_warning, sys_state, c.enabled, left_lane, right_lane, left_lane_warning, right_lane_warning)) @@ -89,7 +86,7 @@ class CarController(): if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl: lead_visible = False - accel = actuators.accel if c.active else 0 + accel = actuators.accel if c.longActive else 0 jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) @@ -100,7 +97,7 @@ class CarController(): 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)) + can_sends.extend(create_acc_commands(self.packer, c.enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping)) self.accel = accel # 20 Hz LFA MFA message @@ -108,7 +105,7 @@ class CarController(): CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022): - can_sends.append(create_lfahda_mfc(self.packer, enabled)) + can_sends.append(create_lfahda_mfc(self.packer, c.enabled)) # 5 Hz ACC options if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 379e6937ae..cda94e1346 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -352,8 +352,8 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible, - hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, + hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible, + hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 return ret diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index b6e4726c7f..5f3d41ae34 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -19,7 +19,7 @@ class CarController(): apply_steer = 0 self.steer_rate_limited = False - if c.active: + if c.latActive: # calculate steer and also set limits due to driver torque new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index eceb0cc75a..15cd44be92 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -18,7 +18,7 @@ class CarController(): self.packer = CANPacker(dbc_name) - def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert, + def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert, left_line, right_line, left_lane_depart, right_lane_depart): can_sends = [] @@ -30,7 +30,7 @@ class CarController(): steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 - if c.active: + if c.latActive: # # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) @@ -68,12 +68,12 @@ class CarController(): can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel)) can_sends.append(nissancan.create_steering_control( - self.packer, apply_angle, frame, enabled, self.lkas_max_torque)) + self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque)) if lkas_hud_msg and lkas_hud_info_msg: if frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg( - self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart)) + self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 50 == 0: can_sends.append(nissancan.create_lkas_hud_info_msg( diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index c32fb13780..63b4dad7fd 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index afc91f4755..fd439356e9 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -15,7 +15,7 @@ class CarController(): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): can_sends = [] @@ -30,7 +30,7 @@ class CarController(): apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer - if not c.active: + if not c.latActive: apply_steer = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: @@ -69,7 +69,7 @@ class CarController(): self.es_distance_cnt = CS.es_distance_msg["Counter"] if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: - 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)) + can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, c.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() diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 8c6d188643..fdd077ac49 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 69caf5876e..d5a5bb8629 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -12,12 +12,12 @@ class CarController(): self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, c, enabled, CS, frame, actuators, cruise_cancel): + def update(self, c, CS, frame, actuators, cruise_cancel): can_sends = [] # Temp disable steering on a hands_on_fault, and allow for user override hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3) - lkas_enabled = c.active and (not hands_on_fault) + lkas_enabled = c.latActive and (not hands_on_fault) if lkas_enabled: apply_angle = actuators.steeringAngleDeg diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 746cd356ea..3bd1a0efca 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -71,6 +71,6 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) self.frame += 1 return ret diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 87ba0055f0..5ec69488d5 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -22,11 +22,11 @@ class CarController(): self.gas = 0 self.accel = 0 - def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert, + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): # gas and brake - if CS.CP.enableGasInterceptor and active: + if CS.CP.enableGasInterceptor and c.longActive: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): @@ -49,7 +49,7 @@ class CarController(): self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) - if not active or CS.steer_state in (9, 25): + if not c.latActive or CS.steer_state in (9, 25): apply_steer = 0 apply_steer_req = 0 else: @@ -57,7 +57,7 @@ class CarController(): # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different - if not enabled and CS.pcm_acc_status: + if not c.enabled and CS.pcm_acc_status: pcm_cancel_cmd = 1 # on entering standstill, send standstill request @@ -122,7 +122,7 @@ class CarController(): send_ui = True if (frame % 100 == 0 or send_ui): - can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled)) + can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, c.enabled)) if frame % 100 == 0 and CS.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 18e15bfefd..b6a01bcc46 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -273,7 +273,7 @@ class CarInterface(CarInterfaceBase): # to be called @ 100hz def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, c.active, self.CS, self.frame, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leadVisible, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 63d79aaf05..5726c98285 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -21,7 +21,7 @@ class CarController(): self.steer_rate_limited = False - def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): + def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ can_sends = [] @@ -39,7 +39,7 @@ class CarController(): # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. - if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerFaultPermanent or CS.out.steerFaultTemporary): + if c.latActive: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer @@ -77,7 +77,7 @@ class CarController(): else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled, + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled, CS.out.steeringPressed, hud_alert, left_lane_visible, right_lane_visible, CS.ldw_stock_values, left_lane_depart, right_lane_depart)) @@ -92,7 +92,7 @@ class CarController(): # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True - elif enabled and CS.esp_hold_confirmation: + elif c.enabled and CS.esp_hold_confirmation: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. self.graButtonStatesToSend = BUTTON_STATES.copy() diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 961cfed7fe..976c4459e3 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -211,7 +211,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, + ret = self.CC.update(c, self.CS, self.frame, self.ext_bus, c.actuators, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7887c2428b..cf403b50b2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -459,7 +459,7 @@ class Controls: self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) # Check if actuators are enabled - self.active = self.state == State.enabled or self.state == State.softDisabling + self.active = self.state in (State.enabled, State.softDisabling) if self.active: self.current_alert_types.append(ET.WARNING) @@ -467,7 +467,7 @@ class Controls: self.enabled = self.active or self.state == State.preEnabled def state_control(self, CS): - """Given the state, this function returns an actuators packet""" + """Given the state, this function returns a CarControl packet""" # Update VehicleModel params = self.sm['liveParameters'] @@ -478,7 +478,14 @@ class Controls: lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] - actuators = car.CarControl.Actuators.new_message() + CC = car.CarControl.new_message() + CC.enabled = self.enabled + # Check which actuators can be enabled + CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + CS.vEgo > self.CP.minSteerSpeed and not CS.standstill + CC.longActive = self.active + + actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state if CS.leftBlinker or CS.rightBlinker: @@ -486,37 +493,40 @@ class Controls: # State specific actions - if not self.active: + if not CC.latActive: self.LaC.reset() + if not CC.longActive: self.LoC.reset(v_pid=CS.vEgo) if not self.joystick_mode: # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL - actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) + actuators.accel = self.LoC.update(CC.longActive, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC - lat_active = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and CS.vEgo > self.CP.minSteerSpeed desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, 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, self.last_actuators, - desired_curvature, desired_curvature_rate) + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, + params, self.last_actuators, desired_curvature, + desired_curvature_rate) else: lac_log = log.ControlsState.LateralDebugState.new_message() - if self.sm.rcv_frame['testJoystick'] > 0 and self.active: - actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) + if self.sm.rcv_frame['testJoystick'] > 0: + if CC.longActive: + actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) - steer = clip(self.sm['testJoystick'].axes[1], -1, 1) - # max angle is 45 for angle-based cars - actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. + if CC.latActive: + steer = clip(self.sm['testJoystick'].axes[1], -1, 1) + # max angle is 45 for angle-based cars + actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. - lac_log.active = True + lac_log.active = self.active lac_log.steeringAngleDeg = CS.steeringAngleDeg - lac_log.output = steer - lac_log.saturated = abs(steer) >= 0.9 + lac_log.output = actuators.steer + lac_log.saturated = abs(actuators.steer) >= 0.9 # Send a "steering required alert" if saturation count has reached the limit if lac_log.active and lac_log.saturated and not CS.steeringPressed: @@ -540,7 +550,7 @@ class Controls: cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") setattr(actuators, p, 0.0) - return actuators, lac_log + return CC, lac_log def update_button_timers(self, buttonEvents): # increment timer for buttons still pressed @@ -552,14 +562,9 @@ class Controls: if b.type.raw in self.button_timers: self.button_timers[b.type.raw] = 1 if b.pressed else 0 - def publish_logs(self, CS, start_time, actuators, lac_log): + def publish_logs(self, CS, start_time, CC, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" - CC = car.CarControl.new_message() - CC.enabled = self.enabled - CC.active = self.active - CC.actuators = actuators - orientation_value = self.sm['liveLocationKalman'].orientationNED.value if len(orientation_value) > 2: CC.roll = orientation_value[0] @@ -580,7 +585,7 @@ class Controls: recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ - and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED + and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction @@ -719,12 +724,12 @@ class Controls: self.prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - actuators, lac_log = self.state_control(CS) + CC, lac_log = self.state_control(CS) self.prof.checkpoint("State Control") # Publish data - self.publish_logs(CS, start_time, actuators, lac_log) + self.publish_logs(CS, start_time, CC, lac_log) self.prof.checkpoint("Sent") self.update_button_timers(CS.buttonEvents) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1993b05fb7..d81fe18ac1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -595f118aa4f6eebacaef4db77179c108ec725483 \ No newline at end of file +c6d7afb13c183852f8d61d5f79495b828ed401ed \ No newline at end of file