You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
74 lines
3.3 KiB
74 lines
3.3 KiB
2 days ago
|
from opendbc.car import structs, Bus
|
||
|
|
from opendbc.can.parser import CANParser
|
||
|
|
from opendbc.car.common.conversions import Conversions as CV
|
||
|
|
from opendbc.car.psa.values import DBC, CarControllerParams
|
||
|
|
from opendbc.car.interfaces import CarStateBase
|
||
|
|
|
||
|
|
GearShifter = structs.CarState.GearShifter
|
||
|
|
TransmissionType = structs.CarParams.TransmissionType
|
||
|
|
|
||
|
|
|
||
|
|
class CarState(CarStateBase):
|
||
|
|
def update(self, can_parsers) -> structs.CarState:
|
||
|
|
cp = can_parsers[Bus.main]
|
||
|
|
cp_adas = can_parsers[Bus.adas]
|
||
|
|
cp_cam = can_parsers[Bus.cam]
|
||
|
|
ret = structs.CarState()
|
||
|
|
|
||
|
|
# car speed
|
||
|
|
self.parse_wheel_speeds(ret,
|
||
|
|
cp.vl['Dyn4_FRE']['P263_VehV_VPsvValWhlFrtL'],
|
||
|
|
cp.vl['Dyn4_FRE']['P264_VehV_VPsvValWhlFrtR'],
|
||
|
|
cp.vl['Dyn4_FRE']['P265_VehV_VPsvValWhlBckL'],
|
||
|
|
cp.vl['Dyn4_FRE']['P266_VehV_VPsvValWhlBckR'],
|
||
|
|
)
|
||
|
|
ret.yawRate = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VITESSE_LACET_BRUTE'] * CV.DEG_TO_RAD
|
||
|
|
ret.standstill = bool(cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VEHICLE_STANDSTILL'])
|
||
|
|
|
||
|
|
# gas
|
||
|
|
ret.gasPressed = cp.vl['Dyn_CMM']['P002_Com_rAPP'] > 0
|
||
|
|
|
||
|
|
# brake
|
||
|
|
ret.brakePressed = bool(cp_cam.vl['Dat_BSI']['P013_MainBrake'])
|
||
|
|
ret.parkingBrake = cp.vl['Dyn_EasyMove']['P337_Com_stPrkBrk'] == 1 # 0: disengaged, 1: engaged, 3: brake actuator moving
|
||
|
|
|
||
|
|
# steering wheel
|
||
|
|
ret.steeringAngleDeg = cp.vl['STEERING_ALT']['ANGLE'] # EPS
|
||
|
|
ret.steeringRateDeg = cp.vl['STEERING_ALT']['RATE'] * (2 * cp.vl['STEERING_ALT']['RATE_SIGN'] - 1) # convert [0,1] to [-1,1] EPS: rot. speed * rot. sign
|
||
|
|
ret.steeringTorque = cp.vl['STEERING']['DRIVER_TORQUE']
|
||
|
|
ret.steeringTorqueEps = cp.vl['IS_DAT_DIRA']['EPS_TORQUE']
|
||
|
|
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
||
|
|
self.eps_active = cp.vl['IS_DAT_DIRA']['EPS_STATE_LKA'] == 3 # 0: Unauthorized, 1: Authorized, 2: Available, 3: Active, 4: Defect
|
||
|
|
|
||
|
|
# cruise
|
||
|
|
ret.cruiseState.speed = cp_adas.vl['HS2_DAT_MDD_CMD_452']['SPEED_SETPOINT'] * CV.KPH_TO_MS # set to 255 when ACC is off, -2 kph offset from dash speed
|
||
|
|
ret.cruiseState.enabled = cp_adas.vl['HS2_DAT_MDD_CMD_452']['RVV_ACC_ACTIVATION_REQ'] == 1
|
||
|
|
ret.cruiseState.available = cp_adas.vl['HS2_DYN1_MDD_ETAT_2B6']['ACC_STATUS'] > 2
|
||
|
|
ret.cruiseState.nonAdaptive = cp_adas.vl['HS2_DAT_MDD_CMD_452']['LONGITUDINAL_REGULATION_TYPE'] != 3 # 0: None, 1: CC, 2: Limiter, 3: ACC
|
||
|
|
ret.cruiseState.standstill = bool(cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VEHICLE_STANDSTILL'])
|
||
|
|
ret.accFaulted = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['ACC_ETAT_DECEL_OR_ESP_STATUS'] == 3 # 0: Inhibited, 1: Waiting, 2: Active, 3: Fault
|
||
|
|
|
||
|
|
# gear
|
||
|
|
if bool(cp_cam.vl['Dat_BSI']['P103_Com_bRevGear']):
|
||
|
|
ret.gearShifter = GearShifter.reverse
|
||
|
|
else:
|
||
|
|
ret.gearShifter = GearShifter.drive
|
||
|
|
|
||
|
|
# blinkers
|
||
|
|
blinker = cp_cam.vl['HS2_DAT7_BSI_612']['CDE_CLG_ET_HDC']
|
||
|
|
ret.leftBlinker = blinker == 1
|
||
|
|
ret.rightBlinker = blinker == 2
|
||
|
|
|
||
|
|
# lock info
|
||
|
|
ret.doorOpen = any((cp_cam.vl['Dat_BSI']['DRIVER_DOOR'], cp_cam.vl['Dat_BSI']['PASSENGER_DOOR']))
|
||
|
|
ret.seatbeltUnlatched = cp_cam.vl['RESTRAINTS']['DRIVER_SEATBELT'] != 2
|
||
|
|
return ret
|
||
|
|
|
||
|
|
@staticmethod
|
||
|
|
def get_can_parsers(CP):
|
||
|
|
return {
|
||
|
|
Bus.main: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
|
||
|
|
Bus.adas: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1),
|
||
|
|
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
|
||
|
|
}
|