mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-09 07:32:06 +08:00
Adeeb Shihadeh
63bbcd7279
* better steer fault names * bump cereal old-commit-hash: 87849f93b165507ad980dbefdcf9c376d5165a18
140 lines
5.4 KiB
Python
140 lines
5.4 KiB
Python
from cereal import car
|
|
from common.numpy_fast import mean
|
|
from opendbc.can.can_define import CANDefine
|
|
from opendbc.can.parser import CANParser
|
|
from selfdrive.car.interfaces import CarStateBase
|
|
from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, STEER_THRESHOLD
|
|
|
|
|
|
class CarState(CarStateBase):
|
|
def __init__(self, CP):
|
|
super().__init__(CP)
|
|
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
|
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
|
|
self.lka_steering_cmd_counter = 0
|
|
|
|
def update(self, pt_cp, loopback_cp):
|
|
ret = car.CarState.new_message()
|
|
|
|
self.prev_cruise_buttons = self.cruise_buttons
|
|
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
|
|
|
ret.wheelSpeeds = self.get_wheel_speeds(
|
|
pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
|
|
pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
|
|
pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
|
|
pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
|
|
)
|
|
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
|
|
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
|
ret.standstill = ret.vEgoRaw < 0.01
|
|
|
|
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]["PRNDL"], None))
|
|
ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["Brake_Pressed"] != 0
|
|
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
|
|
|
|
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
|
|
ret.gasPressed = ret.gas > 1e-5
|
|
|
|
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
|
|
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"]
|
|
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
|
|
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
|
|
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
|
self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
|
|
|
|
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
|
|
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
|
|
ret.steerFaultTemporary = self.lkas_status == 2
|
|
ret.steerFaultPermanent = self.lkas_status == 3
|
|
|
|
# 1 - open, 0 - closed
|
|
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
|
|
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
|
|
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
|
|
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
|
|
|
|
# 1 - latched
|
|
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
|
|
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
|
|
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
|
|
|
|
self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"]
|
|
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
|
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
|
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
|
|
|
|
# Regen braking is braking
|
|
if self.car_fingerprint == CAR.VOLT:
|
|
ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
|
|
|
|
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
|
|
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
|
|
|
|
return ret
|
|
|
|
@staticmethod
|
|
def get_can_parser(CP):
|
|
signals = [
|
|
# sig_name, sig_address
|
|
("BrakePedalPosition", "EBCMBrakePedalPosition"),
|
|
("FrontLeftDoor", "BCMDoorBeltStatus"),
|
|
("FrontRightDoor", "BCMDoorBeltStatus"),
|
|
("RearLeftDoor", "BCMDoorBeltStatus"),
|
|
("RearRightDoor", "BCMDoorBeltStatus"),
|
|
("LeftSeatBelt", "BCMDoorBeltStatus"),
|
|
("RightSeatBelt", "BCMDoorBeltStatus"),
|
|
("TurnSignals", "BCMTurnSignals"),
|
|
("AcceleratorPedal2", "AcceleratorPedal2"),
|
|
("CruiseState", "AcceleratorPedal2"),
|
|
("ACCButtons", "ASCMSteeringButton"),
|
|
("SteeringWheelAngle", "PSCMSteeringAngle"),
|
|
("SteeringWheelRate", "PSCMSteeringAngle"),
|
|
("FLWheelSpd", "EBCMWheelSpdFront"),
|
|
("FRWheelSpd", "EBCMWheelSpdFront"),
|
|
("RLWheelSpd", "EBCMWheelSpdRear"),
|
|
("RRWheelSpd", "EBCMWheelSpdRear"),
|
|
("PRNDL", "ECMPRDNL"),
|
|
("LKADriverAppldTrq", "PSCMStatus"),
|
|
("LKATorqueDelivered", "PSCMStatus"),
|
|
("LKATorqueDeliveredStatus", "PSCMStatus"),
|
|
("TractionControlOn", "ESPStatus"),
|
|
("EPBClosed", "EPBStatus"),
|
|
("CruiseMainOn", "ECMEngineStatus"),
|
|
("Brake_Pressed", "ECMEngineStatus"),
|
|
]
|
|
|
|
checks = [
|
|
("BCMTurnSignals", 1),
|
|
("ECMPRDNL", 10),
|
|
("PSCMStatus", 10),
|
|
("ESPStatus", 10),
|
|
("BCMDoorBeltStatus", 10),
|
|
("EPBStatus", 20),
|
|
("EBCMWheelSpdFront", 20),
|
|
("EBCMWheelSpdRear", 20),
|
|
("AcceleratorPedal2", 33),
|
|
("ASCMSteeringButton", 33),
|
|
("ECMEngineStatus", 100),
|
|
("PSCMSteeringAngle", 100),
|
|
("EBCMBrakePedalPosition", 100),
|
|
]
|
|
|
|
if CP.carFingerprint == CAR.VOLT:
|
|
signals.append(("RegenPaddle", "EBCMRegenPaddle"))
|
|
checks.append(("EBCMRegenPaddle", 50))
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN)
|
|
|
|
@staticmethod
|
|
def get_loopback_can_parser(CP):
|
|
signals = [
|
|
("RollingCounter", "ASCMLKASteeringCmd"),
|
|
]
|
|
|
|
checks = [
|
|
("ASCMLKASteeringCmd", 50),
|
|
]
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK)
|