diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index cfff37f6d..00fb46122 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -3,8 +3,8 @@ from common.numpy_fast import clip from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.ford.fordcan import create_acc_command, 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 + create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg +from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -54,7 +54,14 @@ class CarController: self.apply_curvature_last = apply_curvature can_sends.append(create_lka_msg(self.packer)) - can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + + if self.CP.carFingerprint in CANFD_CARS: + # TODO: extended mode + mode = 1 if CC.latActive else 0 + counter = self.frame // CarControllerParams.STEER_STEP + can_sends.append(create_lat_ctl2_msg(self.packer, mode, 0., 0., -apply_curvature, 0., counter)) + else: + can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) ### longitudinal control ### # send acc command at 50Hz @@ -71,7 +78,6 @@ class CarController: can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel)) - ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index d9c37426f..594d50f59 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -4,6 +4,15 @@ from selfdrive.car.ford.values import CANBUS HUDControl = car.CarControl.HUDControl +def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): + checksum = mode + counter + checksum += dat[2] + ((dat[3] & 0xE0) >> 5) # curvature + checksum += dat[6] + ((dat[7] & 0xE0) >> 5) # curvature rate + checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2) # path angle + checksum += (dat[4] & 0x3) + dat[5] # path offset + return 0xFF - (checksum & 0xFF) + + def create_lka_msg(packer): """ Creates an empty CAN message for the Ford LKA Command. @@ -16,7 +25,8 @@ def create_lka_msg(packer): return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) -def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): +def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float, + curvature_rate: float): """ Creates a CAN message for the Ford TJA/LCA Command. @@ -55,6 +65,38 @@ def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) +def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float, curvature: float, + curvature_rate: float, counter: int): + """ + Create a CAN message for the new Ford Lane Centering command. + + This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has + additional signals for a counter and checksum. + + Frequency is 20Hz. + """ + + values = { + "LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode, + # 3=SafeRampOut, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians + "LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter + "LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2 + "HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1] + "LatCtlPath_No_Cnt": counter, # [0|15] + "LatCtlPath_No_Cs": 0, # [0|255] + } + + # calculate checksum + dat = packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)[2] + values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) + + return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values) + + def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool): """ Creates a CAN message for the Ford ACC Command. diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 7b075f154..4cabdb11e 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,7 +1,7 @@ from collections import defaultdict from dataclasses import dataclass from enum import Enum -from typing import Dict, List, Union +from typing import Dict, List, Set, Union from cereal import car from selfdrive.car import AngleRateLimit, dbc_dict @@ -52,6 +52,9 @@ class CAR: MAVERICK_MK1 = "FORD MAVERICK 1ST GEN" +CANFD_CARS: Set[str] = set() + + class RADAR: DELPHI_ESR = 'ford_fusion_2018_adas' DELPHI_MRR = 'FORD_CADS'