Co-authored-by: Cameron Clough <cameronjclough@gmail.com>
pull/27089/head
Shane Smiskol 2 years ago
parent 9170f75819
commit e445112f17
  1. 22
      selfdrive/car/ford/carcontroller.py

@ -2,7 +2,7 @@ import math
from cereal import car from cereal import car
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from opendbc.can.packer import CANPacker from opendbc.can.packer import CANPacker
from selfdrive.car.ford import fordcan from selfdrive.car.ford.fordcan import create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg
from selfdrive.car.ford.values import CANBUS, CarControllerParams from selfdrive.car.ford.values import CANBUS, CarControllerParams
VisualAlert = car.CarControl.HUDControl.VisualAlert VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -46,16 +46,15 @@ class CarController:
### acc buttons ### ### acc buttons ###
if CC.cruiseControl.cancel: if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, cancel=True)) can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main)) can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main))
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, resume=True)) can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True))
can_sends.append(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main)) can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main))
# if stock lane centering isn't off, send a button press to toggle it off # if stock lane centering isn't off, send a button press to toggle it off
# the stock system checks for steering pressed, and eventually disengages cruise control # the stock system checks for steering pressed, and eventually disengages cruise control
elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0: elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True)) can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True))
### lateral control ### ### lateral control ###
if CC.latActive: if CC.latActive:
@ -84,21 +83,20 @@ class CarController:
precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
self.apply_angle_last = apply_angle self.apply_angle_last = apply_angle
can_sends.append(fordcan.create_lka_msg(self.packer, 0, 0)) can_sends.append(create_lka_msg(self.packer, 0, 0))
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, lca_rq, ramp_type, precision, can_sends.append(create_lat_ctl_msg(self.packer, lca_rq, ramp_type, precision,
0, path_angle, 0, 0)) 0, path_angle, 0, 0))
### ui ### ### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
# send lkas ui command at 1Hz or if ui state changes # send lkas ui command at 1Hz or if ui state changes
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) can_sends.append(create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
# send acc ui command at 20Hz or if ui state changes # send acc ui command at 20Hz or if ui state changes
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values))
self.main_on_last = main_on self.main_on_last = main_on
self.lkas_enabled_last = CC.latActive self.lkas_enabled_last = CC.latActive

Loading…
Cancel
Save