openpilot v0.9.6 release
date: 2024-01-12T10:13:37 master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
166
selfdrive/car/gm/carstate.py
Normal file
166
selfdrive/car/gm/carstate.py
Normal file
@@ -0,0 +1,166 @@
|
||||
import copy
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import mean
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD
|
||||
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
NetworkLocation = car.CarParams.NetworkLocation
|
||||
STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
|
||||
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
|
||||
self.cluster_min_speed = CV.KPH_TO_MS / 2.
|
||||
|
||||
self.loopback_lka_steering_cmd_updated = False
|
||||
self.loopback_lka_steering_cmd_ts_nanos = 0
|
||||
self.pt_lka_steering_cmd_counter = 0
|
||||
self.cam_lka_steering_cmd_counter = 0
|
||||
self.buttons_counter = 0
|
||||
|
||||
def update(self, pt_cp, cam_cp, loopback_cp):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
|
||||
self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0
|
||||
|
||||
# Variables used for avoiding LKAS faults
|
||||
self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
|
||||
if self.loopback_lka_steering_cmd_updated:
|
||||
self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
|
||||
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)
|
||||
# sample rear wheel speeds, standstill=True if ECM allows engagement with brake
|
||||
ret.standstill = ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
|
||||
|
||||
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
|
||||
ret.gearShifter = self.parse_gear_shifter("T")
|
||||
else:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
|
||||
|
||||
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0
|
||||
else:
|
||||
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
|
||||
# that the brake is being intermittently pressed without user interaction.
|
||||
# To avoid a cruise fault we need to use a conservative brake position threshold
|
||||
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
|
||||
ret.brakePressed = ret.brake >= 8
|
||||
|
||||
# Regen braking is braking
|
||||
if self.CP.transmissionType == TransmissionType.direct:
|
||||
ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
|
||||
|
||||
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
|
||||
|
||||
# 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
|
||||
|
||||
ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
|
||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
|
||||
pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1)
|
||||
|
||||
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
|
||||
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
|
||||
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
|
||||
# openpilot controls nonAdaptive when not pcmCruise
|
||||
if self.CP.pcmCruise:
|
||||
ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3)
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
messages = []
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
messages += [
|
||||
("AEBCmd", 10),
|
||||
("ASCMLKASteeringCmd", 10),
|
||||
("ASCMActiveCruiseControlStatus", 25),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.CAMERA)
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
("BCMTurnSignals", 1),
|
||||
("ECMPRDNL2", 10),
|
||||
("PSCMStatus", 10),
|
||||
("ESPStatus", 10),
|
||||
("BCMDoorBeltStatus", 10),
|
||||
("BCMGeneralPlatformStatus", 10),
|
||||
("EBCMWheelSpdFront", 20),
|
||||
("EBCMWheelSpdRear", 20),
|
||||
("EBCMFrictionBrakeStatus", 20),
|
||||
("AcceleratorPedal2", 33),
|
||||
("ASCMSteeringButton", 33),
|
||||
("ECMEngineStatus", 100),
|
||||
("PSCMSteeringAngle", 100),
|
||||
("ECMAcceleratorPos", 80),
|
||||
]
|
||||
|
||||
# Used to read back last counter sent to PT by camera
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
messages += [
|
||||
("ASCMLKASteeringCmd", 0),
|
||||
]
|
||||
|
||||
if CP.transmissionType == TransmissionType.direct:
|
||||
messages.append(("EBCMRegenPaddle", 50))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN)
|
||||
|
||||
@staticmethod
|
||||
def get_loopback_can_parser(CP):
|
||||
messages = [
|
||||
("ASCMLKASteeringCmd", 0),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.LOOPBACK)
|
||||
Reference in New Issue
Block a user