diff --git a/panda b/panda index a140406a2e52de..2447aeb2545146 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit a140406a2e52dedccf1ff0fee51f837762b2efa4 +Subproject commit 2447aeb25451460dbaee019b4e8d7d431c68114a diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 96722087d3ae1a..a7804520d42f7a 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -31,6 +31,8 @@ def __init__(self, dbc_name, CP, VM): self.last_button_frame = 0 self.accel_last = 0 self.motor2_frame = 0 + self.AWV_brake = 0 + self.AWV_enable = 0 self.deviationBP = [-0.13, -0.1, -0.05, 0.] # accel (m/s squared) self.deviationV = [0., 0.08, 0.14, 0.15] # comfort-band (m/s squared) @@ -150,22 +152,24 @@ def update(self, CC, CS, now_nanos): if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive, CC.cruiseControl.override) - accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping) - accel_diff = (accel - self.accel_last) * 50 - clip(accel_diff, self.rateLimitBP[0], self.rateLimitBP[3]) - self.accel_diff_smoothed = (self.longSignalSmooth * accel_diff) + (1 - self.longSignalSmooth) * getattr(self, 'accel_diff_smoothed', 0) - deviation_lookup = interp(self.accel_diff_smoothed, self.deviationBP, self.deviationV) - ratelimit_lookup = interp(self.accel_diff_smoothed, self.rateLimitBP, self.ratelimitV) - self.long_deviation = (0.019 * deviation_lookup) + (1 - 0.019) * getattr(self, 'long_deviation', 0) # 100 SMA equivalence - self.long_ratelimit = (0.007 * ratelimit_lookup) + (1 - 0.007) * getattr(self, 'long_ratelimit', 0) # 250 SMA equivalence - self.long_deviation = self.long_deviation * interp(CS.out.vEgo, [0,4,7,8], [0,0,1,1]) # 1x - 0x interpolation for RegelAbw during FTS - # self.long_ratelimit = self.long_ratelimit * interp(CS.out.vEgo, [0,1,20,21], [.5,.5,1,1]) # .5x - 1x interpolation for AendGrad during 0-100kph - clip(self.long_deviation, self.deviationV[0], self.deviationV[3]) - clip(self.long_ratelimit, self.ratelimitV[3], self.ratelimitV[0]) - self.accel_last = self.accel_last if accel == self.CCP.ACCEL_MIN else accel - + if self.CCS == pqcan and CC.longActive and (clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) <= 0) and CS.out.vEgoRaw <= 6: + accel = 0 + if not self.AWV_enable: + self.AWV_enable = 1 + self.AWV_brake = 0 + else: + self.AWV_brake = clip(actuators.accel, self.CCP.ACCEL_MIN, 0) + else: + accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 + self.AWV_enable = 0 + self.AWV_brake = 0 + self.long_deviation = 0 # TODO: make dynamic again + self.long_ratelimit = 4 # TODO: make dynamic again + + if self.CCS == pqcan: + can_sends.append(self.CCS.create_awv_control(self.packer_pt, CANBUS.pt, self.AWV_brake, self.AWV_enable)) can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, accel, acc_control, stopping, starting, CS.esp_hold_confirmation, self.long_deviation, self.long_ratelimit)) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 28adc88c33f698..500e76906d80ec 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -99,6 +99,17 @@ def create_acc_accel_control(packer, bus, acc_type, accel, acc_control, stopping return commands +def create_awv_control(packer, bus, apply_brake, enabled): + + values = { + "AWV_1_Parameter": 1 if enabled else 0, + "ANB_Teilbremsung_Freigabe": enabled, + "ANB_Ziel_Teilbrems_Verz_Anf": apply_brake if enabled else 0, + "AWV_Halten": 0, # Hold at stop, configure later? Not needed? + } + + return packer.make_can_msg("AWV", bus, values) + def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): values = {