|
|
|
@ -35,40 +35,40 @@ class CarController(): |
|
|
|
|
# **** Acceleration and Braking Controls ******************************** # |
|
|
|
|
|
|
|
|
|
if CS.CP.openpilotLongitudinalControl: |
|
|
|
|
if CS.tsk_status in [2, 3, 4, 5]: |
|
|
|
|
acc_status = 3 if enabled else 2 |
|
|
|
|
else: |
|
|
|
|
acc_status = CS.tsk_status |
|
|
|
|
|
|
|
|
|
accel = actuators.accel if enabled else 0 |
|
|
|
|
accel = clip(accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0 |
|
|
|
|
|
|
|
|
|
# FIXME: this needs to become a proper state machine |
|
|
|
|
acc_hold_request, acc_hold_release = False, False |
|
|
|
|
if actuators.longControlState == LongCtrlState.stopping: |
|
|
|
|
self.acc_stopping = True |
|
|
|
|
acc_hold_request = not CS.esp_hold_confirmation # Send hold request for ABS/ESP brake-hold until acked |
|
|
|
|
elif enabled: |
|
|
|
|
if self.acc_stopping: |
|
|
|
|
self.acc_starting = True |
|
|
|
|
self.acc_stopping = False |
|
|
|
|
self.acc_starting &= CS.out.vEgo < CS.CP.vEgoStarting |
|
|
|
|
acc_hold_release = CS.esp_hold_confirmation # Send release request for ABS/ESP brake-hold until acked |
|
|
|
|
else: |
|
|
|
|
self.acc_stopping, self.acc_starting = False, False |
|
|
|
|
if frame % P.ACC_CONTROL_STEP == 0: |
|
|
|
|
if CS.tsk_status in [2, 3, 4, 5]: |
|
|
|
|
acc_status = 3 if enabled else 2 |
|
|
|
|
else: |
|
|
|
|
acc_status = CS.tsk_status |
|
|
|
|
|
|
|
|
|
accel = actuators.accel if enabled else 0 |
|
|
|
|
accel = clip(accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0 |
|
|
|
|
|
|
|
|
|
# FIXME: this needs to become a proper state machine |
|
|
|
|
acc_hold_request, acc_hold_release = False, False |
|
|
|
|
if actuators.longControlState == LongCtrlState.stopping: |
|
|
|
|
self.acc_stopping = True |
|
|
|
|
acc_hold_request = not CS.esp_hold_confirmation # Send hold request for ABS/ESP brake-hold until acked |
|
|
|
|
elif enabled: |
|
|
|
|
if self.acc_stopping: |
|
|
|
|
self.acc_starting = True |
|
|
|
|
self.acc_stopping = False |
|
|
|
|
self.acc_starting &= CS.out.vEgo < CS.CP.vEgoStarting |
|
|
|
|
acc_hold_release = CS.esp_hold_confirmation # Send release request for ABS/ESP brake-hold until acked |
|
|
|
|
else: |
|
|
|
|
self.acc_stopping, self.acc_starting = False, False |
|
|
|
|
|
|
|
|
|
# Hopefully, have PI yank the lag out and let the drivetrain coordinator make it smooth again |
|
|
|
|
cb_pos = 0.2 |
|
|
|
|
cb_neg = 0.2 |
|
|
|
|
# Hopefully, have PI yank the lag out and let the drivetrain coordinator make it smooth again |
|
|
|
|
cb_pos = 0.2 |
|
|
|
|
cb_neg = 0.2 |
|
|
|
|
|
|
|
|
|
if acc_hold_request: |
|
|
|
|
weird_value = 0x88 |
|
|
|
|
elif self.acc_stopping: |
|
|
|
|
weird_value = 0x95 |
|
|
|
|
else: |
|
|
|
|
weird_value = 0x7F |
|
|
|
|
if acc_hold_request: |
|
|
|
|
weird_value = 0x88 |
|
|
|
|
elif self.acc_stopping: |
|
|
|
|
weird_value = 0x95 |
|
|
|
|
else: |
|
|
|
|
weird_value = 0x7F |
|
|
|
|
|
|
|
|
|
if frame % P.ACC_CONTROL_STEP == 0: |
|
|
|
|
idx = (frame / P.ACC_CONTROL_STEP) % 16 |
|
|
|
|
can_sends.append(volkswagencan.create_mqb_acc_06_control(self.packer_pt, CANBUS.pt, enabled, acc_status, |
|
|
|
|
accel, self.acc_stopping, self.acc_starting, |
|
|
|
@ -77,6 +77,11 @@ class CarController(): |
|
|
|
|
accel, self.acc_stopping, self.acc_starting, |
|
|
|
|
acc_hold_request, acc_hold_release, weird_value, idx)) |
|
|
|
|
|
|
|
|
|
if frame % P.ACC_HUD_STEP == 0: |
|
|
|
|
idx = (frame / P.ACC_HUD_STEP) % 16 |
|
|
|
|
can_sends.append(volkswagencan.create_mqb_acc_hud_control(self.packer_pt, CANBUS.pt, CS.tsk_status, |
|
|
|
|
set_speed * CV.MS_TO_KPH, idx)) |
|
|
|
|
|
|
|
|
|
# **** Steering Controls ************************************************ # |
|
|
|
|
|
|
|
|
|
if frame % P.HCA_STEP == 0: |
|
|
|
@ -133,12 +138,6 @@ class CarController(): |
|
|
|
|
right_lane_visible, CS.ldw_stock_values, |
|
|
|
|
left_lane_depart, right_lane_depart)) |
|
|
|
|
|
|
|
|
|
if CS.CP.openpilotLongitudinalControl: |
|
|
|
|
if frame % P.ACC_HUD_STEP == 0: |
|
|
|
|
idx = (frame / P.ACC_HUD_STEP) % 16 |
|
|
|
|
can_sends.append(volkswagencan.create_mqb_acc_hud_control(self.packer_pt, CANBUS.pt, CS.tsk_status, |
|
|
|
|
set_speed * CV.MS_TO_KPH, idx)) |
|
|
|
|
|
|
|
|
|
# **** ACC Button Controls ********************************************** # |
|
|
|
|
|
|
|
|
|
# FIXME: this entire section is in desperate need of refactoring |
|
|
|
|