mawei/openpilot
1
0
Fork 0
mirror of https://jihulab.com/mr-one/openpilot.git synced 2026-10-26 20:43:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

carstate.py:硬编码福特carstate

Browse Source 
(cherry picked from commit 4745c79357)
This commit is contained in:
openpilot
2026-04-15 20:16:18 +08:00
committed by 1okko
parent f71ac0e03d
commit 0ceb907523
1 changed files with 19 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/ford/carstate.py
+19 -4
View File
@@ -11,6 +11,7 @@ ButtonType = structs.CarState.ButtonEvent.Type
GearShifter = structs.CarState.GearShifter
TransmissionType = structs.CarParams.TransmissionType
FordFlagsALT_STEER_ANGLE = True
class CarState(CarStateBase, MadsCarState):
def __init__(self, CP, CP_SP):
@@ -30,9 +31,16 @@ class CarState(CarStateBase, MadsCarState):
ret = structs.CarState()
ret_sp = structs.CarStateSP()
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
# The vehicle usually recovers out of this state within a minute of normal driving
ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3
if FordFlagsALT_STEER_ANGLE:
self.vehicle_sensors_valid = (
int((cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"] + 1000) * 10) not in (32766, 32767)
and cp.vl["ParkAid_Data"]["EPASExtAngleStatReq"] == 0
and cp.vl["ParkAid_Data"]["ApaSys_D_Stat"] in (0, 1)
)
else:
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
# The vehicle usually recovers out of this state within a minute of normal driving
ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3
# car speed
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
@@ -48,7 +56,14 @@ class CarState(CarStateBase, MadsCarState):
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
# steering wheel
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
if FordFlagsALT_STEER_ANGLE:
steering_angle_init = cp.vl["SteeringPinion_Data_Alt"]["StePinRelInit_An_Sns"]
if self.vehicle_sensors_valid:
steering_angle_est = cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"]
self.steering_angle_offset_deg = steering_angle_est - steering_angle_init
ret.steeringAngleDeg = steering_angle_init + self.steering_angle_offset_deg
else:
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1