diff --git a/cereal b/cereal index e5a04ab45..f1c5c8ef7 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e5a04ab458afd52cf630cc9e35ccdc10efba6688 +Subproject commit f1c5c8ef7cab05b66892d67b6992f5ac1238ad4f diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 46681b13e..d491ccd4b 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 @@ class CarController(): # 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 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 @@ class CarController(): self.ccframe += 1 self.prev_frame = frame - return 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 1822d76a1..a893b222f 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -78,8 +78,6 @@ class CarInterface(CarInterfaceBase): def apply(self, c): if (self.CS.frame == -1): - return [] # 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. - 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 06c5f2579..2eaacd301 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -83,4 +83,4 @@ class CarController(): 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 8c0b1e1fa..7155b3ea8 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -63,8 +63,8 @@ class CarInterface(CarInterfaceBase): # 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 8f4d0f27c..71587b17d 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 @@ -53,22 +56,22 @@ class CarController(): 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))) - # 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)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: @@ -106,4 +109,9 @@ class CarController(): 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 + 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 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index b06e5373a..d9a63d32e 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -220,10 +220,10 @@ class CarInterface(CarInterfaceBase): # 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 e1488eda2..9a424a1e6 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -105,6 +105,11 @@ class CarController(): 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, @@ -211,10 +216,9 @@ class CarController(): 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 +228,7 @@ class CarController(): 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 +238,28 @@ class CarController(): # 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 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 = actuators.copy() + 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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 03074b875..274f4ed5f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -438,13 +438,13 @@ class CarInterface(CarInterfaceBase): 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 a8b70fcf9..a6812754e 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -44,6 +44,7 @@ class CarController(): 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): @@ -100,6 +101,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)) + 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 +118,8 @@ class CarController(): if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.append(create_frt_radar_opt(self.packer)) - return can_sends + new_actuators = actuators.copy() + new_actuators.steer = apply_steer / self.p.STEER_MAX + new_actuators.accel = self.accel + + return new_actuators, can_sends diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 57b313e53..091523ac1 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -347,8 +347,8 @@ class CarInterface(CarInterfaceBase): 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 bd5b98c8e..2ff26891e 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 @@ class CarInterfaceBase(): 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 @@ class CarInterfaceBase(): 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 - # return 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() diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 06c5eb094..c65ff72ed 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -58,4 +58,8 @@ class CarController(): # send steering command can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, frame, apply_steer, CS.cam_lkas)) - return 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/mazda/interface.py b/selfdrive/car/mazda/interface.py index 8364bf000..b4ae93822 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -95,6 +95,6 @@ class CarInterface(CarInterfaceBase): 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 999e735c7..bc1f6dcf6 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -88,4 +88,5 @@ class CarInterface(CarInterfaceBase): 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 41ba9f659..8b40a050c 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -87,4 +87,7 @@ class CarController(): self.packer, lkas_hud_info_msg, steer_hud_alert )) - return can_sends + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + + return new_actuators, can_sends diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 18ae885f8..004ea8dd0 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -78,9 +78,9 @@ class CarInterface(CarInterfaceBase): 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 a2d0c2ebd..72b07f919 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -72,4 +72,7 @@ class CarController(): 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 + 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/subaru/interface.py b/selfdrive/car/subaru/interface.py index f5b0886a9..9537f293a 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -122,8 +122,8 @@ class CarInterface(CarInterfaceBase): 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 7e6a2f2e9..4efd1c1fe 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -62,4 +62,7 @@ class CarController(): # TODO: HUD control - return can_sends + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + + return actuators, can_sends diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 45dc0a723..4b5b21178 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): - 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 b35081c70..506d75472 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -19,12 +19,12 @@ class CarController(): 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): - # *** compute control surfaces *** - # gas and brake if CS.CP.enableGasInterceptor and enabled: MAX_INTERCEPTOR_GAS = 0.5 @@ -96,6 +96,7 @@ class CarController(): 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)) @@ -103,6 +104,7 @@ class CarController(): # 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 @@ -125,10 +127,14 @@ class CarController(): 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 + 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/toyota/interface.py b/selfdrive/car/toyota/interface.py index f9d6b586f..6c1f3e9f4 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -312,12 +312,11 @@ class CarInterface(CarInterfaceBase): # 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 977818dbd..a0119e2a5 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -110,4 +110,7 @@ class CarController(): self.graButtonStatesToSend = None self.graMsgSentCount = 0 - return can_sends + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / P.STEER_MAX + + return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 560e64ce2..bef1c3718 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -216,11 +216,11 @@ class CarInterface(CarInterfaceBase): 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 19fe328db..cc449537a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -159,6 +159,7 @@ class Controls: 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 @@ -510,7 +511,7 @@ class Controls: 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() @@ -621,8 +622,9 @@ class Controls: 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.actuatorsOutput = self.last_actuators force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 8fcb9ae7b..5975fa66b 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -10,7 +10,7 @@ class LatControlAngle(): 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 @@ class LatControlAngle(): 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 50a8e22b3..169d504d5 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -82,7 +82,7 @@ class LatControlINDI(): 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 16fffac27..9f58aea2e 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -44,7 +44,7 @@ class LatControlLQR(): 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 c7730d011..ca78d5fa4 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -16,7 +16,7 @@ class LatControlPID(): 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 @@ 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.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 0301f0900..8415bc8de 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