diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6d4f92246..a19e317a9 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -76,8 +76,17 @@ class LongControl: self.reset() elif self.long_control_state == LongCtrlState.starting: - output_accel = self.CP.startAccel - self.reset() + if self.CP.useCarBrakeHoldState: + if CS.brakeHoldActive: + output_accel = self.CP.startAccel + self.reset() + else: + error = a_target - CS.aEgo + output_accel = self.pid.update(error, speed=CS.vEgo, + feedforward=a_target) + else: + output_accel = self.CP.startAccel + self.reset() else: # LongCtrlState.pid error = a_target - CS.aEgo