mawei
/
openpilot
mirror of https://github.com/1okko/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ford: correct messages frequencies (#28027) 

* ford: correct messages frequencies

* fix STEER_STEP

* fix comment

* cleanup
This commit is contained in:
Cameron Clough 2023-04-26 00:00:08 -07:00 committed by GitHub
parent 7f856eafd9
commit 42f2e39213
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
selfdrive/car/ford/carcontroller.py
View File

@ -43,7 +43,7 @@ class CarController:
can_sends.append(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 ###
# send steering commands at 20Hz # send steer msg at 20Hz
if (self.frame % CarControllerParams.STEER_STEP) == 0: if (self.frame % CarControllerParams.STEER_STEP) == 0:
if CC.latActive: if CC.latActive:
# apply limits to curvature and clip to signal range # apply limits to curvature and clip to signal range
@ -53,7 +53,6 @@ class CarController:
apply_curvature = 0. apply_curvature = 0.
self.apply_curvature_last = apply_curvature self.apply_curvature_last = apply_curvature
can_sends.append(create_lka_msg(self.packer))
if self.CP.carFingerprint in CANFD_CARS: if self.CP.carFingerprint in CANFD_CARS:
# TODO: extended mode # TODO: extended mode
@ -63,8 +62,12 @@ class CarController:
else: else:
can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.))
# send lka msg at 33Hz
if (self.frame % CarControllerParams.LKA_STEP) == 0:
can_sends.append(create_lka_msg(self.packer))
### longitudinal control ### ### longitudinal control ###
# send acc command at 50Hz # send acc msg at 50Hz
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
@ -80,12 +83,10 @@ class CarController:
### 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 msg 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(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 msg at 5Hz 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(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))

16
selfdrive/car/ford/values.py
View File

@ -12,16 +12,12 @@ Ecu = car.CarParams.Ecu
class CarControllerParams: class CarControllerParams:
# Messages: Lane_Assist_Data1, LateralMotionControl STEER_STEP = 5 # LateralMotionControl, 20Hz
STEER_STEP = 5 LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
# Message: ACCDATA ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz
ACC_CONTROL_STEP = 2 LKAS_UI_STEP = 100 # IPMA_Data, 1Hz
# Message: IPMA_Data ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
LKAS_UI_STEP = 100 BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
# Message: ACCDATA_3
ACC_UI_STEP = 5
# Message: Steering_Data_FD1, but send twice as fast
BUTTONS_STEP = 10 / 2
CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1 CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm