Carstate returns capnp struct (#1115)
* Before abstraction, adding speed init from VW as well * strting to abstract carstate class * fix bug and update lock? * revert pipfile change * another bug * fix linter * bug fix * remove a bunch of diplicated kf code * better to not have class vars. will abstract __init__ anyway later * abstract common instance vars in carstate init and a generic gear parser static method * abstract gear parser for chrysler * abstract gm gear parser too * remove unnecessary random vars * Chrysler: carstate returns capnp struct directly * revert ref commit * test ref * WIP * more WIP * ops, missed this conflict * ford as well * not sure why this got deleted * no need to copy * remove copy * remove copy import * remove unnecessary intermediate variable * remove obsolete comments * GM: have carstate returning capnp struct directly * Honda carstate also outputing capnp struct * hyundai too now returns capnp from carstate * ops, not meant this * Subaru carstate also returning capnp * Toyota: capnp struct as output of carstate * fix bool * minor simplififcation in Honda * no need to negate * VW carstate returning capnp struct (#1118) * VW carstate also returning capnp struct * fixed typo * Remove unused blinker button (#1119) * remove unused blinker button * ops, this wasn't meant * remove blinker button for VW as well * update ref Co-authored-by: Willem Melching <willem.melching@gmail.com>albatross
rbiasini
GitHub
parent
09bb76eeb8
commit
0c67143c92
|
|
@ -20,8 +20,6 @@ class CarInterface(CarInterfaceBase):
|
|||
self.brake_pressed_prev = False
|
||||
self.cruise_enabled_prev = False
|
||||
self.low_speed_alert = False
|
||||
self.left_blinker_prev = False
|
||||
self.right_blinker_prev = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
|
|
@ -123,22 +121,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
|
||||
if ret.leftBlinker != self.left_blinker_prev:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = ret.leftBlinker != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if ret.rightBlinker != self.right_blinker_prev:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = ret.rightBlinker != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.buttonEvents = []
|
||||
|
||||
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
|
||||
|
||||
|
|
@ -177,8 +160,6 @@ class CarInterface(CarInterfaceBase):
|
|||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
self.left_blinker_prev = ret.leftBlinker
|
||||
self.right_blinker_prev = ret.rightBlinker
|
||||
|
||||
# copy back carState packet to CS
|
||||
self.CS.out = ret.as_reader()
|
||||
|
|
|
|||
|
|
@ -101,10 +101,10 @@ class CarController():
|
|||
### STEER ###
|
||||
|
||||
if (frame % P.STEER_STEP) == 0:
|
||||
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
|
||||
lkas_enabled = enabled and not CS.steer_not_allowed and CS.out.vEgo > P.MIN_STEER_SPEED
|
||||
if lkas_enabled:
|
||||
new_steer = actuators.steer * P.STEER_MAX
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
else:
|
||||
apply_steer = 0
|
||||
|
|
@ -114,7 +114,7 @@ class CarController():
|
|||
|
||||
if self.car_fingerprint in SUPERCRUISE_CARS:
|
||||
can_sends += gmcan.create_steering_control_ct6(self.packer_pt,
|
||||
canbus, apply_steer, CS.v_ego, idx, lkas_enabled)
|
||||
canbus, apply_steer, CS.out.vEgo, idx, lkas_enabled)
|
||||
else:
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt,
|
||||
canbus.powertrain, apply_steer, idx, lkas_enabled))
|
||||
|
|
@ -142,11 +142,11 @@ class CarController():
|
|||
if (frame % 4) == 0:
|
||||
idx = (frame // 4) % 4
|
||||
|
||||
at_full_stop = enabled and CS.standstill
|
||||
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
|
||||
at_full_stop = enabled and CS.out.standstill
|
||||
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
|
||||
|
||||
at_full_stop = enabled and CS.standstill
|
||||
at_full_stop = enabled and CS.out.standstill
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
|
||||
|
||||
# Send dashboard UI commands (ACC status), 25hz
|
||||
|
|
@ -167,7 +167,7 @@ class CarController():
|
|||
if frame % speed_and_accelerometer_step == 0:
|
||||
idx = (frame // speed_and_accelerometer_step) % 4
|
||||
can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx))
|
||||
can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx))
|
||||
can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.out.vEgo, idx))
|
||||
|
||||
if frame % P.ADAS_KEEPALIVE_STEP == 0:
|
||||
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
|
|||
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, \
|
||||
from selfdrive.car.gm.values import DBC, CAR, AccState, \
|
||||
CruiseButtons, is_eps_status_ok, \
|
||||
STEER_THRESHOLD, SUPERCRUISE_CARS
|
||||
|
||||
|
|
@ -58,73 +58,68 @@ class CarState(CarStateBase):
|
|||
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
|
||||
|
||||
def update(self, pt_cp):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
|
||||
|
||||
self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
self.standstill = self.v_ego_raw < 0.01
|
||||
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
|
||||
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
|
||||
|
||||
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
|
||||
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
|
||||
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition']
|
||||
ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
|
||||
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
|
||||
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
|
||||
if ret.brake < 10/0xd0:
|
||||
ret.brake = 0.
|
||||
|
||||
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']
|
||||
self.user_gas_pressed = self.pedal_gas > 0
|
||||
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
|
||||
# 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
|
||||
|
||||
if self.car_fingerprint in SUPERCRUISE_CARS:
|
||||
self.park_brake = False
|
||||
ret.cruiseState.available = False
|
||||
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
|
||||
self.esp_disabled = False
|
||||
regen_pressed = False
|
||||
self.pcm_acc_status = int(self.acc_active)
|
||||
else:
|
||||
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
|
||||
ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'])
|
||||
self.acc_active = False
|
||||
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
|
||||
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
|
||||
else:
|
||||
regen_pressed = False
|
||||
|
||||
# Regen braking is braking
|
||||
ret.brakePressed = ret.brake > 1e-5 or regen_pressed
|
||||
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
|
||||
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
|
||||
|
||||
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
|
||||
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
|
||||
self.steer_not_allowed = not is_eps_status_ok(self.lkas_status, self.car_fingerprint)
|
||||
|
||||
# 1 - open, 0 - closed
|
||||
self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0)
|
||||
|
||||
# 1 - latched
|
||||
self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1
|
||||
|
||||
self.steer_error = False
|
||||
|
||||
self.brake_error = False
|
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
|
||||
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
|
||||
|
||||
if self.car_fingerprint in SUPERCRUISE_CARS:
|
||||
self.park_brake = False
|
||||
self.main_on = False
|
||||
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
|
||||
self.esp_disabled = False
|
||||
self.regen_pressed = False
|
||||
self.pcm_acc_status = int(self.acc_active)
|
||||
else:
|
||||
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
|
||||
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
|
||||
self.acc_active = False
|
||||
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
|
||||
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
|
||||
else:
|
||||
self.regen_pressed = False
|
||||
|
||||
# Brake pedal's potentiometer returns near-zero reading
|
||||
# even when pedal is not pressed.
|
||||
if self.user_brake < 10:
|
||||
self.user_brake = 0
|
||||
|
||||
# Regen braking is braking
|
||||
self.brake_pressed = self.user_brake > 10 or self.regen_pressed
|
||||
|
||||
self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -175,68 +175,14 @@ class CarInterface(CarInterfaceBase):
|
|||
def update(self, c, can_strings):
|
||||
self.pt_cp.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.pt_cp)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
ret = self.CS.update(self.pt_cp)
|
||||
|
||||
ret.canValid = self.pt_cp.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gas pedal information.
|
||||
ret.gas = self.CS.pedal_gas / 254.0
|
||||
ret.gasPressed = self.CS.user_gas_pressed
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake / 0xd0
|
||||
ret.brakePressed = self.CS.brake_pressed
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
|
||||
# torque and user override. Driver awareness
|
||||
# timer resets when the user uses the steering wheel.
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF
|
||||
ret.cruiseState.enabled = cruiseEnabled
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
|
||||
|
||||
ret.leftBlinker = self.CS.left_blinker_on
|
||||
ret.rightBlinker = self.CS.right_blinker_on
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
buttonEvents = []
|
||||
|
||||
# blinkers
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.unknown
|
||||
|
|
@ -247,7 +193,7 @@ class CarInterface(CarInterfaceBase):
|
|||
be.pressed = False
|
||||
but = self.CS.prev_cruise_buttons
|
||||
if but == CruiseButtons.RES_ACCEL:
|
||||
if not (cruiseEnabled and self.CS.standstill):
|
||||
if not (ret.cruiseState.enabled and ret.standstill):
|
||||
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
|
||||
elif but == CruiseButtons.DECEL_SET:
|
||||
be.type = ButtonType.decelCruise
|
||||
|
|
@ -260,8 +206,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.buttonEvents = buttonEvents
|
||||
|
||||
events = []
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if self.CS.steer_not_allowed:
|
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
|
||||
if ret.doorOpen:
|
||||
|
|
@ -276,15 +220,13 @@ class CarInterface(CarInterfaceBase):
|
|||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
else:
|
||||
if self.CS.brake_error:
|
||||
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if not self.CS.gear_shifter_valid:
|
||||
if ret.gearShifter != car.CarState.GearShifter.drive:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on:
|
||||
if not ret.cruiseState.available:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if self.CS.gear_shifter == 3:
|
||||
if ret.gearShifter == car.CarState.GearShifter.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
|
||||
|
|
@ -317,11 +259,11 @@ class CarInterface(CarInterfaceBase):
|
|||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
# copy back carState packet to CS
|
||||
self.CS.out = ret.as_reader()
|
||||
|
||||
return self.CS.out
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
hud_v_cruise = c.hudControl.setSpeed
|
||||
if hud_v_cruise > 70:
|
||||
|
|
@ -329,7 +271,7 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
# For Openpilot, "enabled" includes pre-enable.
|
||||
# In GM, PCM faults out if ACC command overlaps user gas.
|
||||
enabled = c.enabled and not self.CS.user_gas_pressed
|
||||
enabled = c.enabled and not self.CS.out.gasPressed
|
||||
|
||||
can_sends = self.CC.update(enabled, self.CS, self.frame, \
|
||||
c.actuators,
|
||||
|
|
|
|||
|
|
@ -95,10 +95,10 @@ class CarController():
|
|||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
|
||||
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint)
|
||||
|
||||
# *** no output if not enabled ***
|
||||
if not enabled and CS.pcm_acc_status:
|
||||
if not enabled and CS.out.cruiseState.enabled:
|
||||
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
|
||||
pcm_cancel_cmd = True
|
||||
|
||||
|
|
@ -169,7 +169,7 @@ class CarController():
|
|||
# If using stock ACC, spam cancel command to kill gas when OP disengages.
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
|
||||
elif CS.stopped:
|
||||
elif CS.out.cruiseState.standstill:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
|
||||
|
||||
else:
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
from cereal import car
|
||||
from collections import defaultdict
|
||||
from common.numpy_fast import interp
|
||||
from opendbc.can.can_define import CANDefine
|
||||
|
|
@ -193,9 +194,9 @@ class CarState(CarStateBase):
|
|||
self.cruise_setting = 0
|
||||
self.v_cruise_pcm_prev = 0
|
||||
self.cruise_mode = 0
|
||||
self.stopped = 0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
# car params
|
||||
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
|
|
@ -204,21 +205,19 @@ class CarState(CarStateBase):
|
|||
# update prevs, update must run once per loop
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.prev_cruise_setting = self.cruise_setting
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
# ******************* parse out can *******************
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc
|
||||
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'])
|
||||
else:
|
||||
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
|
||||
self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']
|
||||
ret.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
|
||||
ret.doorOpen = any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']])
|
||||
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] or not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'])
|
||||
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']]
|
||||
self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT']
|
||||
|
|
@ -234,111 +233,111 @@ class CarState(CarStateBase):
|
|||
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
|
||||
|
||||
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr)/4.
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
|
||||
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4.
|
||||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
|
||||
self.v_ego_raw = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
|
||||
self.v_weight * v_wheel
|
||||
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
|
||||
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
|
||||
ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
|
||||
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
|
||||
|
||||
ret.leftBlinker = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] != 0
|
||||
ret.rightBlinker = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] != 0
|
||||
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
|
||||
|
||||
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
else:
|
||||
self.park_brake = 0 # TODO
|
||||
main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
|
||||
gear = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
|
||||
|
||||
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
|
||||
# crv doesn't include cruise control
|
||||
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN):
|
||||
ret.gas = self.pedal_gas / 256.
|
||||
else:
|
||||
ret.gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] / 256.
|
||||
|
||||
# this is a hack for the interceptor. This is now only used in the simulation
|
||||
# TODO: Replace tests by toyota so this can go away
|
||||
if self.CP.enableGasInterceptor:
|
||||
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
||||
|
||||
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']
|
||||
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
|
||||
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
|
||||
|
||||
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
|
||||
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
|
||||
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
|
||||
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
|
||||
|
||||
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
||||
ret.gasPressed = self.user_gas_pressed
|
||||
else:
|
||||
self.park_brake = 0 # TODO
|
||||
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
ret.gasPressed = self.pedal_gas > 1e-5
|
||||
|
||||
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
|
||||
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
|
||||
ret.steeringTorque = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
|
||||
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint]
|
||||
|
||||
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
|
||||
# crv doesn't include cruise control
|
||||
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN):
|
||||
self.car_gas = self.pedal_gas
|
||||
else:
|
||||
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
|
||||
|
||||
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
|
||||
self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
|
||||
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0
|
||||
|
||||
if self.CP.radarOffCan:
|
||||
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
|
||||
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
|
||||
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
|
||||
ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
|
||||
ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
|
||||
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID):
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
|
||||
ret.brakePressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] != 0 or \
|
||||
(self.brake_switch and self.brake_switch_prev and \
|
||||
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
||||
self.brake_switch_prev = self.brake_switch
|
||||
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
else:
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
|
||||
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
|
||||
self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
|
||||
self.v_cruise_pcm_prev = self.v_cruise_pcm
|
||||
ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] * CV.KPH_TO_MS
|
||||
self.v_cruise_pcm_prev = ret.cruiseState.speed
|
||||
else:
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego)
|
||||
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
|
||||
ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], ret.vEgo)
|
||||
ret.cruiseState.speed = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] * CV.KPH_TO_MS
|
||||
# brake switch has shown some single time step noise, so only considered when
|
||||
# switch is on for at least 2 consecutive CAN samples
|
||||
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
|
||||
(self.brake_switch and self.brake_switch_prev and \
|
||||
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
||||
ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or
|
||||
(self.brake_switch and self.brake_switch_prev and
|
||||
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts))
|
||||
self.brake_switch_prev = self.brake_switch
|
||||
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
|
||||
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
|
||||
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
|
||||
ret.brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
|
||||
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] != 0
|
||||
ret.cruiseState.available = bool(main_on) and self.cruise_mode == 0
|
||||
|
||||
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
|
||||
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
|
||||
if self.user_brake > 0.05:
|
||||
self.brake_pressed = 1
|
||||
if ret.brake > 0.05:
|
||||
ret.brakePressed = True
|
||||
|
||||
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
|
||||
self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.stock_aeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
|
||||
ret.stockAeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
|
||||
else:
|
||||
self.stock_aeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
|
||||
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.stock_hud = False
|
||||
self.stock_fcw = False
|
||||
ret.stockFcw = False
|
||||
else:
|
||||
self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0)
|
||||
ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
|
||||
self.stock_hud = cp_cam.vl["ACC_HUD"]
|
||||
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
|
||||
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -380,79 +380,16 @@ class CarInterface(CarInterfaceBase):
|
|||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
ret = self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas / 256.0
|
||||
if not self.CP.enableGasInterceptor:
|
||||
ret.gasPressed = self.CS.pedal_gas > 0
|
||||
else:
|
||||
ret.gasPressed = self.CS.user_gas_pressed
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed != 0
|
||||
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
|
||||
# FIXME: read sendcan for brakelights
|
||||
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
|
||||
ret.brakeLights = bool(self.CS.brake_switch or
|
||||
c.actuators.brake > brakelights_threshold)
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate
|
||||
|
||||
# gear shifter lever
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringTorqueEps = self.CS.steer_torque_motor
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode)
|
||||
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
|
||||
ret.cruiseState.standstill = False
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
ret.stockAeb = self.CS.stock_aeb
|
||||
ret.stockFcw = self.CS.stock_fcw
|
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
|
|
@ -504,7 +441,7 @@ class CarInterface(CarInterfaceBase):
|
|||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on or self.CS.cruise_mode:
|
||||
if not ret.cruiseState.available:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
|
|
@ -569,8 +506,8 @@ class CarInterface(CarInterfaceBase):
|
|||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
self.CS.out = ret.as_reader()
|
||||
return self.CS.out
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ class CarController():
|
|||
|
||||
### Steering Torque
|
||||
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams)
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
if not enabled:
|
||||
|
|
@ -38,7 +38,7 @@ class CarController():
|
|||
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
|
||||
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
|
||||
elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5:
|
||||
self.last_resume_cnt = self.cnt
|
||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
from cereal import car
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
|
@ -127,95 +127,101 @@ def get_camera_parser(CP):
|
|||
class CarState(CarStateBase):
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# update prevs, update must run once per Loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.door_all_closed = True
|
||||
self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw']
|
||||
ret.doorOpen = False # FIXME
|
||||
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
|
||||
|
||||
self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
|
||||
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step']
|
||||
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
|
||||
self.main_on = True
|
||||
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0
|
||||
self.pcm_acc_status = int(self.acc_active)
|
||||
ret.standstill = ret.vEgoRaw < 0.1
|
||||
|
||||
self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
|
||||
self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle']
|
||||
ret.steeringRate = cp.vl["SAS11"]['SAS_Speed']
|
||||
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
|
||||
ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0
|
||||
ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0
|
||||
ret.steeringTorque = cp.vl["MDPS11"]['CR_Mdps_DrvTq']
|
||||
ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq']
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
|
||||
self.low_speed_lockout = self.v_ego_raw < 1.0
|
||||
|
||||
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
|
||||
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
|
||||
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
|
||||
self.standstill = not self.v_ego_raw > 0.1
|
||||
|
||||
self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
|
||||
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
|
||||
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
|
||||
self.main_on = True
|
||||
self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
|
||||
self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
|
||||
self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD
|
||||
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
|
||||
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
|
||||
self.brake_error = 0
|
||||
self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq']
|
||||
self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq']
|
||||
self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
|
||||
|
||||
self.user_brake = 0
|
||||
|
||||
self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
|
||||
self.brake_lights = bool(self.brake_pressed)
|
||||
if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1):
|
||||
self.pedal_gas = 0
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0
|
||||
ret.cruiseState.available = True
|
||||
ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
|
||||
if ret.cruiseState.enabled:
|
||||
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
|
||||
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
|
||||
else:
|
||||
self.pedal_gas = cp.vl["EMS12"]['TPS']
|
||||
self.car_gas = cp.vl["EMS12"]['TPS']
|
||||
ret.cruiseState.speed = 0
|
||||
|
||||
ret.brake = 0 # FIXME
|
||||
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
|
||||
ret.brakeLights = ret.brakePressed
|
||||
if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1):
|
||||
pedal_gas = 0
|
||||
else:
|
||||
pedal_gas = cp.vl["EMS12"]['TPS']
|
||||
ret.gasPressed = pedal_gas > 1e-3
|
||||
ret.gas = cp.vl["EMS12"]['TPS']
|
||||
|
||||
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
|
||||
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
|
||||
if gear == 5:
|
||||
self.gear_shifter = GearShifter.drive
|
||||
gear_shifter = GearShifter.drive
|
||||
elif gear == 6:
|
||||
self.gear_shifter = GearShifter.neutral
|
||||
gear_shifter = GearShifter.neutral
|
||||
elif gear == 0:
|
||||
self.gear_shifter = GearShifter.park
|
||||
gear_shifter = GearShifter.park
|
||||
elif gear == 7:
|
||||
self.gear_shifter = GearShifter.reverse
|
||||
gear_shifter = GearShifter.reverse
|
||||
else:
|
||||
self.gear_shifter = GearShifter.unknown
|
||||
gear_shifter = GearShifter.unknown
|
||||
|
||||
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
|
||||
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.drive
|
||||
gear_shifter_cluster = GearShifter.drive
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.neutral
|
||||
gear_shifter_cluster = GearShifter.neutral
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.park
|
||||
gear_shifter_cluster = GearShifter.park
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.reverse
|
||||
gear_shifter_cluster = GearShifter.reverse
|
||||
else:
|
||||
self.gear_shifter_cluster = GearShifter.unknown
|
||||
gear_shifter_cluster = GearShifter.unknown
|
||||
|
||||
# Gear Selecton via TCU12
|
||||
gear2 = cp.vl["TCU12"]["CUR_GR"]
|
||||
if gear2 == 0:
|
||||
self.gear_tcu = GearShifter.park
|
||||
gear_tcu = GearShifter.park
|
||||
elif gear2 == 14:
|
||||
self.gear_tcu = GearShifter.reverse
|
||||
gear_tcu = GearShifter.reverse
|
||||
elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently
|
||||
self.gear_tcu = GearShifter.drive
|
||||
gear_tcu = GearShifter.drive
|
||||
else:
|
||||
self.gear_tcu = GearShifter.unknown
|
||||
gear_tcu = GearShifter.unknown
|
||||
|
||||
# gear shifter
|
||||
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
|
||||
ret.gearShifter = gear_shifter_cluster
|
||||
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
|
||||
ret.gearShifter = gear_tcu
|
||||
else:
|
||||
ret.gearShifter = gear_shifter
|
||||
|
||||
# save the entire LKAS11 and CLU11
|
||||
self.lkas11 = cp_cam.vl["LKAS11"]
|
||||
self.clu11 = cp.vl["CLU11"]
|
||||
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step']
|
||||
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
|
||||
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
|
||||
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
|
||||
self.brake_error = 0
|
||||
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
|
|||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser
|
||||
from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS
|
||||
from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
|
@ -148,83 +148,16 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
ret = self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.yawRate = self.CS.yaw_rate
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
|
||||
ret.gearShifter = self.CS.gear_shifter_cluster
|
||||
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
|
||||
ret.gearShifter = self.CS.gear_tcu
|
||||
else:
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas
|
||||
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed != 0
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
|
||||
if self.CS.pcm_acc_status != 0:
|
||||
ret.cruiseState.speed = self.CS.cruise_set_speed
|
||||
else:
|
||||
ret.cruiseState.speed = 0
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
ret.cruiseState.standstill = False
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
ret.buttonEvents = []
|
||||
|
||||
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
|
||||
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
|
||||
|
|
@ -241,7 +174,7 @@ class CarInterface(CarInterfaceBase):
|
|||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on:
|
||||
if not ret.cruiseState.available:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
|
|
@ -270,7 +203,8 @@ class CarInterface(CarInterfaceBase):
|
|||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
return ret.as_reader()
|
||||
self.CS.out = ret.as_reader()
|
||||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
|
||||
|
|
|
|||
|
|
@ -51,8 +51,6 @@ class CarStateBase:
|
|||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
self.cruise_buttons = 0
|
||||
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ class CarController():
|
|||
# limits due to driver torque
|
||||
|
||||
new_steer = int(round(apply_steer))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
lkas_enabled = enabled
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
import copy
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
|
|
@ -85,54 +86,50 @@ def get_camera_can_parser(CP):
|
|||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
# initialize can parser
|
||||
self.left_blinker_cnt = 0
|
||||
self.right_blinker_cnt = 0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal']
|
||||
self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal']
|
||||
self.user_gas_pressed = self.pedal_gas > 0
|
||||
self.brake_pressed = self.brake_pressure > 0
|
||||
self.brake_lights = bool(self.brake_pressed)
|
||||
ret.gas = cp.vl["Throttle"]['Throttle_Pedal'] / 255.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 1e-5
|
||||
ret.brakeLights = ret.brakePressed
|
||||
|
||||
self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
|
||||
|
||||
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed']
|
||||
# 1 = imperial, 6 = metric
|
||||
if cp.vl["Dash_State"]['Units'] == 1:
|
||||
self.v_cruise_pcm *= CV.MPH_TO_KPH
|
||||
|
||||
self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
|
||||
self.standstill = self.v_ego_raw < 0.01
|
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgoRaw < 0.01
|
||||
|
||||
# continuous blinker signals for assisted lane change
|
||||
self.left_blinker_cnt = 50 if cp.vl["Dashlights"]['LEFT_BLINKER'] else max(self.left_blinker_cnt - 1, 0)
|
||||
self.left_blinker_on = self.left_blinker_cnt > 0
|
||||
|
||||
ret.leftBlinker = self.left_blinker_cnt > 0
|
||||
self.right_blinker_cnt = 50 if cp.vl["Dashlights"]['RIGHT_BLINKER'] else max(self.right_blinker_cnt - 1, 0)
|
||||
self.right_blinker_on = self.right_blinker_cnt > 0
|
||||
ret.rightBlinker = self.right_blinker_cnt > 0
|
||||
|
||||
self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
|
||||
self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
|
||||
self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated']
|
||||
self.main_on = cp.vl["CruiseControl"]['Cruise_On']
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint]
|
||||
self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle']
|
||||
self.door_open = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'],
|
||||
ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle']
|
||||
ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint]
|
||||
|
||||
ret.cruiseState.enabled = cp.vl["CruiseControl"]['Cruise_Activated'] != 0
|
||||
ret.cruiseState.available = cp.vl["CruiseControl"]['Cruise_On'] != 0
|
||||
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] * CV.KPH_TO_MS
|
||||
# 1 = imperial, 6 = metric
|
||||
if cp.vl["Dash_State"]['Units'] == 1:
|
||||
ret.cruiseState.speed *= CV.MPH_TO_KPH
|
||||
|
||||
ret.seatbeltUnlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
|
||||
ret.doorOpen = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'],
|
||||
cp.vl["BodyInfo"]['DOOR_OPEN_RL'],
|
||||
cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
|
||||
cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
|
||||
|
||||
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
|
||||
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
|
||||
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ class CarInterface(CarInterfaceBase):
|
|||
self.CP = CP
|
||||
|
||||
self.frame = 0
|
||||
self.acc_active_prev = 0
|
||||
self.enabled_prev = 0
|
||||
self.gas_pressed_prev = False
|
||||
|
||||
# *** init the major players ***
|
||||
|
|
@ -98,67 +98,17 @@ class CarInterface(CarInterfaceBase):
|
|||
self.pt_cp.update_strings(can_strings)
|
||||
self.cam_cp.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.pt_cp, self.cam_cp)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
ret = self.CS.update(self.pt_cp, self.cam_cp)
|
||||
|
||||
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
|
||||
# torque and user override. Driver awareness
|
||||
# timer resets when the user uses the steering wheel.
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
ret.gas = self.CS.pedal_gas / 255.
|
||||
ret.gasPressed = self.CS.user_gas_pressed
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = bool(self.CS.acc_active)
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
ret.cruiseState.speedOffset = 0.
|
||||
|
||||
ret.leftBlinker = self.CS.left_blinker_on
|
||||
ret.rightBlinker = self.CS.right_blinker_on
|
||||
ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
|
||||
ret.doorOpen = self.CS.door_open
|
||||
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
|
||||
|
||||
buttonEvents = []
|
||||
|
||||
# blinkers
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.accelCruise
|
||||
buttonEvents.append(be)
|
||||
|
||||
|
||||
events = []
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
|
@ -166,9 +116,9 @@ class CarInterface(CarInterfaceBase):
|
|||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if self.CS.acc_active and not self.acc_active_prev:
|
||||
if ret.cruiseState.enabled and not self.enabled_prev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
if not self.CS.acc_active:
|
||||
if not ret.cruiseState.enabled:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
# disable on gas pedal rising edge
|
||||
|
|
@ -180,12 +130,11 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
ret.events = events
|
||||
|
||||
# update previous brake/gas pressed
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.acc_active_prev = self.CS.acc_active
|
||||
self.enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
self.CS.out = ret.as_reader()
|
||||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ class CarController():
|
|||
|
||||
# steer torque
|
||||
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# only cut torque when steer state is a known fault
|
||||
|
|
@ -143,25 +143,25 @@ class CarController():
|
|||
# steer angle
|
||||
if self.steer_angle_enabled and CS.ipas_active:
|
||||
apply_angle = actuators.steerAngle
|
||||
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
|
||||
angle_lim = interp(CS.out.vEgo, ANGLE_MAX_BP, ANGLE_MAX_V)
|
||||
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
|
||||
|
||||
# windup slower
|
||||
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
|
||||
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
|
||||
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_V)
|
||||
else:
|
||||
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
|
||||
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
|
||||
|
||||
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
|
||||
else:
|
||||
apply_angle = CS.angle_steers
|
||||
apply_angle = CS.out.steeringAngle
|
||||
|
||||
if not enabled and CS.pcm_acc_status:
|
||||
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
|
||||
pcm_cancel_cmd = 1
|
||||
|
||||
# on entering standstill, send standstill request
|
||||
if CS.standstill and not self.last_standstill:
|
||||
if CS.out.standstill and not self.last_standstill:
|
||||
self.standstill_req = True
|
||||
if CS.pcm_acc_status != 8:
|
||||
# pcm entered standstill or it's disabled
|
||||
|
|
@ -170,7 +170,7 @@ class CarController():
|
|||
self.last_steer = apply_steer
|
||||
self.last_angle = apply_angle
|
||||
self.last_accel = apply_accel
|
||||
self.last_standstill = CS.standstill
|
||||
self.last_standstill = CS.out.standstill
|
||||
|
||||
can_sends = []
|
||||
|
||||
|
|
@ -194,7 +194,7 @@ class CarController():
|
|||
|
||||
# we can spam can to cancel the system even if we are using lat only control
|
||||
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
|
||||
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
|
||||
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
|
||||
|
||||
# Lexus IS uses a different cancellation message
|
||||
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
|
|
|
|||
|
|
@ -1,9 +1,10 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
|
||||
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR, NO_STOP_TIMER_CAR
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
|
@ -91,75 +92,80 @@ class CarState(CarStateBase):
|
|||
self.init_angle_offset = False
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
|
||||
ret.doorOpen = any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
|
||||
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0
|
||||
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
|
||||
ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed)
|
||||
if self.CP.enableGasInterceptor:
|
||||
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
ret.gasPressed = ret.gas > 15
|
||||
else:
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
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)
|
||||
|
||||
self.standstill = not self.v_ego_raw > 0.001
|
||||
ret.standstill = ret.vEgoRaw < 0.001
|
||||
|
||||
if self.CP.carFingerprint in TSS2_CAR:
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
elif self.CP.carFingerprint in NO_DSU_CAR:
|
||||
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
|
||||
# need to apply an offset as soon as the steering angle measurements are both received
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
|
||||
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
|
||||
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
|
||||
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3 and not self.init_angle_offset:
|
||||
self.init_angle_offset = True
|
||||
self.angle_offset = self.angle_steers - angle_wheel
|
||||
self.angle_offset = ret.steeringAngle - angle_wheel
|
||||
else:
|
||||
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
|
||||
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
|
||||
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
|
||||
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
|
||||
else:
|
||||
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
|
||||
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
|
||||
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
|
||||
# we could use the override bit from dbc, but it's triggered at too high torque values
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0
|
||||
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED'] * CV.KPH_TO_MS
|
||||
self.low_speed_lockout = False
|
||||
else:
|
||||
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0
|
||||
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] * CV.KPH_TO_MS
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
|
||||
# ignore standstill in hybrid vehicles, since pcm allows to restart without
|
||||
# receiving any special command. Also if interceptor is detected
|
||||
ret.cruiseState.standstill = False
|
||||
else:
|
||||
ret.cruiseState.standstill = self.pcm_acc_status == 7
|
||||
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
|
||||
|
||||
if self.CP.carFingerprint == CAR.PRIUS:
|
||||
ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
|
||||
else:
|
||||
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
|
||||
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
||||
|
||||
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
|
||||
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
|
||||
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
|
||||
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
|
||||
self.brake_error = 0
|
||||
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
|
||||
# we could use the override bit from dbc, but it's triggered at too high torque values
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
|
||||
self.user_brake = 0
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
|
||||
self.low_speed_lockout = False
|
||||
else:
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
|
||||
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||
if self.CP.carFingerprint == CAR.PRIUS:
|
||||
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
|
||||
else:
|
||||
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
|
||||
|
||||
self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
|
|||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||
from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS
|
||||
from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
|
@ -343,85 +343,12 @@ class CarInterface(CarInterfaceBase):
|
|||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
ret = self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.pedal_gas
|
||||
if self.CP.enableGasInterceptor:
|
||||
# use interceptor values to disengage on pedal press
|
||||
ret.gasPressed = self.CS.pedal_gas > 15
|
||||
else:
|
||||
ret.gasPressed = self.CS.pedal_gas > 0
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed != 0
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringTorqueEps = self.CS.steer_torque_motor
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_active
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
ret.cruiseState.speedOffset = 0.
|
||||
|
||||
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
|
||||
# ignore standstill in hybrid vehicles, since pcm allows to restart without
|
||||
# receiving any special command
|
||||
# also if interceptor is detected
|
||||
ret.cruiseState.standstill = False
|
||||
else:
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
|
||||
|
||||
buttonEvents = []
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
ret.genericToggle = self.CS.generic_toggle
|
||||
ret.stockAeb = self.CS.stock_aeb
|
||||
ret.buttonEvents = []
|
||||
|
||||
# events
|
||||
events = []
|
||||
|
|
@ -436,7 +363,7 @@ class CarInterface(CarInterfaceBase):
|
|||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on and self.CP.openpilotLongitudinalControl:
|
||||
if not ret.cruiseState.available and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
|
|
@ -473,7 +400,8 @@ class CarInterface(CarInterfaceBase):
|
|||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
return ret.as_reader()
|
||||
self.CS.out = ret.as_reader()
|
||||
return self.CS.out
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
|
|
|
|||
|
|
@ -49,14 +49,14 @@ class CarController():
|
|||
|
||||
# FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop
|
||||
# commanding HCA if there's a fault, so the steering rack recovers.
|
||||
if enabled and not (CS.standstill or CS.steeringFault):
|
||||
if enabled and not (CS.out.standstill or CS.steeringFault):
|
||||
|
||||
# FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This
|
||||
# is inherently handled by scaling to STEER_MAX. The rack doesn't seem
|
||||
# to care about up/down rate, but we have some evidence it may do its
|
||||
# own rate limiting, and matching OP helps for accurate tuning.
|
||||
new_steer = int(round(actuators.steer * P.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steeringTorque, P)
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending
|
||||
|
|
@ -116,7 +116,7 @@ class CarController():
|
|||
# filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
|
||||
|
||||
if frame % P.LDW_STEP == 0:
|
||||
hcaEnabled = True if enabled and not CS.standstill else False
|
||||
hcaEnabled = True if enabled and not CS.out.standstill else False
|
||||
|
||||
if visual_alert == VisualAlert.steerRequired:
|
||||
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
|
||||
|
|
@ -124,7 +124,7 @@ class CarController():
|
|||
hud_alert = MQB_LDW_MESSAGES["none"]
|
||||
|
||||
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, canbus.pt, hcaEnabled,
|
||||
CS.steeringPressed, hud_alert, leftLaneVisible,
|
||||
CS.out.steeringPressed, hud_alert, leftLaneVisible,
|
||||
rightLaneVisible))
|
||||
|
||||
#--------------------------------------------------------------------------
|
||||
|
|
@ -140,11 +140,11 @@ class CarController():
|
|||
# stock ACC with OP disengagement, or to auto-resume from stop.
|
||||
|
||||
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
|
||||
if not enabled and CS.accEnabled:
|
||||
if not enabled and CS.out.cruiseState.enabled:
|
||||
# Cancel ACC if it's engaged with OP disengaged.
|
||||
self.graButtonStatesToSend = BUTTON_STATES.copy()
|
||||
self.graButtonStatesToSend["cancel"] = True
|
||||
elif enabled and CS.standstill:
|
||||
elif enabled and CS.out.standstill:
|
||||
# Blip the Resume button if we're engaged at standstill.
|
||||
# FIXME: This is a naive implementation, improve with visiond or radar input.
|
||||
# A subset of MQBs like to "creep" too aggressively with this implementation.
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
import numpy as np
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
|
|
@ -105,45 +106,46 @@ class CarState(CarStateBase):
|
|||
self.buttonStates = BUTTON_STATES.copy()
|
||||
|
||||
def update(self, pt_cp):
|
||||
ret = car.CarState.new_message()
|
||||
# Update vehicle speed and acceleration from ABS wheel speeds.
|
||||
self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedFR = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedRL = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
|
||||
self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR]))
|
||||
self.vEgo, self.aEgo = self.update_speed_kf(self.vEgoRaw)
|
||||
ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
self.standstill = self.vEgoRaw < 0.1
|
||||
ret.standstill = ret.vEgoRaw < 0.1
|
||||
|
||||
# Update steering angle, rate, yaw rate, and driver input torque. VW send
|
||||
# the sign/direction in a separate signal so they must be recombined.
|
||||
self.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
self.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
self.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])]
|
||||
self.steeringPressed = abs(self.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
self.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD
|
||||
ret.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
ret.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
ret.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD
|
||||
|
||||
# Update gas, brakes, and gearshift.
|
||||
self.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
|
||||
self.gasPressed = self.gas > 0
|
||||
self.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
self.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
|
||||
self.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
|
||||
ret.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
|
||||
ret.gasPressed = ret.gas > 0
|
||||
ret.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
ret.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
|
||||
ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
|
||||
|
||||
# Update gear and/or clutch position data.
|
||||
can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe'])
|
||||
self.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
|
||||
|
||||
# Update door and trunk/hatch lid open status.
|
||||
self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_BT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HD_offen']])
|
||||
ret.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_BT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HD_offen']])
|
||||
|
||||
# Update seatbelt fastened status.
|
||||
self.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True
|
||||
ret.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True
|
||||
|
||||
# Update driver preference for metric. VW stores many different unit
|
||||
# preferences, including separate units for for distance vs. speed.
|
||||
|
|
@ -155,38 +157,39 @@ class CarState(CarStateBase):
|
|||
if accStatus == 1:
|
||||
# ACC okay but disabled
|
||||
self.accFault = False
|
||||
self.accAvailable = False
|
||||
self.accEnabled = False
|
||||
ret.cruiseState.available = False
|
||||
ret.cruiseState.enabled = False
|
||||
elif accStatus == 2:
|
||||
# ACC okay and enabled, but not currently engaged
|
||||
self.accFault = False
|
||||
self.accAvailable = True
|
||||
self.accEnabled = False
|
||||
ret.cruiseState.available = True
|
||||
ret.cruiseState.enabled = False
|
||||
elif accStatus in [3, 4, 5]:
|
||||
# ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5)
|
||||
self.accFault = False
|
||||
self.accAvailable = True
|
||||
self.accEnabled = True
|
||||
ret.cruiseState.available = True
|
||||
ret.cruiseState.enabled = True
|
||||
else:
|
||||
# ACC fault of some sort. Seen statuses 6 or 7 for CAN comms disruptions, visibility issues, etc.
|
||||
self.accFault = True
|
||||
self.accAvailable = False
|
||||
self.accEnabled = False
|
||||
ret.cruiseState.available = False
|
||||
ret.cruiseState.enabled = False
|
||||
|
||||
# Update ACC setpoint. When the setpoint is zero or there's an error, the
|
||||
# radar sends a set-speed of ~90.69 m/s / 203mph.
|
||||
self.accSetSpeed = pt_cp.vl["ACC_02"]['SetSpeed']
|
||||
if self.accSetSpeed > 90: self.accSetSpeed = 0
|
||||
ret.cruiseState.speed = pt_cp.vl["ACC_02"]['SetSpeed']
|
||||
if ret.cruiseState.speed > 90:
|
||||
ret.cruiseState.speed = 0
|
||||
|
||||
# Update control button states for turn signals and ACC controls.
|
||||
self.buttonStates["leftBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li'])
|
||||
self.buttonStates["rightBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re'])
|
||||
self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch'])
|
||||
self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter'])
|
||||
self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Abbrechen'])
|
||||
self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen'])
|
||||
self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme'])
|
||||
self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke'])
|
||||
ret.leftBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li'])
|
||||
ret.rightBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re'])
|
||||
|
||||
# Read ACC hardware button type configuration info that has to pass thru
|
||||
# to the radar. Ends up being different for steering wheel buttons vs
|
||||
|
|
@ -207,3 +210,4 @@ class CarState(CarStateBase):
|
|||
self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well
|
||||
self.stabilityControlDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv']
|
||||
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -120,60 +120,22 @@ class CarInterface(CarInterfaceBase):
|
|||
events = []
|
||||
buttonEvents = []
|
||||
params = Params()
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
# Process the most recent CAN message traffic, and check for validity
|
||||
# The camera CAN has no signals we use at this time, but we process it
|
||||
# anyway so we can test connectivity with can_valid
|
||||
self.pt_cp.update_strings(can_strings)
|
||||
self.cam_cp.update_strings(can_strings)
|
||||
self.CS.update(self.pt_cp)
|
||||
|
||||
ret = self.CS.update(self.pt_cp)
|
||||
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
|
||||
|
||||
# Wheel and vehicle speed, yaw rate
|
||||
ret.wheelSpeeds.fl = self.CS.wheelSpeedFL
|
||||
ret.wheelSpeeds.fr = self.CS.wheelSpeedFR
|
||||
ret.wheelSpeeds.rl = self.CS.wheelSpeedRL
|
||||
ret.wheelSpeeds.rr = self.CS.wheelSpeedRR
|
||||
ret.vEgoRaw = self.CS.vEgoRaw
|
||||
ret.vEgo = self.CS.vEgo
|
||||
ret.aEgo = self.CS.aEgo
|
||||
ret.standstill = self.CS.standstill
|
||||
|
||||
# Steering wheel position, movement, yaw rate, and driver input
|
||||
ret.steeringAngle = self.CS.steeringAngle
|
||||
ret.steeringRate = self.CS.steeringRate
|
||||
ret.steeringTorque = self.CS.steeringTorque
|
||||
ret.steeringPressed = self.CS.steeringPressed
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
ret.yawRate = self.CS.yawRate
|
||||
|
||||
# Gas, brakes and shifting
|
||||
ret.gas = self.CS.gas
|
||||
ret.gasPressed = self.CS.gasPressed
|
||||
ret.brake = self.CS.brake
|
||||
ret.brakePressed = self.CS.brakePressed
|
||||
ret.brakeLights = self.CS.brakeLights
|
||||
ret.gearShifter = self.CS.gearShifter
|
||||
|
||||
# Doors open, seatbelt unfastened
|
||||
ret.doorOpen = self.CS.doorOpen
|
||||
ret.seatbeltUnlatched = self.CS.seatbeltUnlatched
|
||||
|
||||
# Update the EON metric configuration to match the car at first startup,
|
||||
# or if there's been a change.
|
||||
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
|
||||
params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0")
|
||||
|
||||
# Blinker switch updates
|
||||
ret.leftBlinker = self.CS.buttonStates["leftBlinker"]
|
||||
ret.rightBlinker = self.CS.buttonStates["rightBlinker"]
|
||||
|
||||
# ACC cruise state
|
||||
ret.cruiseState.available = self.CS.accAvailable
|
||||
ret.cruiseState.enabled = self.CS.accEnabled
|
||||
ret.cruiseState.speed = self.CS.accSetSpeed
|
||||
|
||||
# Check for and process state-change events (button press or release) from
|
||||
# the turn stalk switch or ACC steering wheel/control stalk buttons.
|
||||
for button in self.CS.buttonStates:
|
||||
|
|
@ -230,8 +192,8 @@ class CarInterface(CarInterfaceBase):
|
|||
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
|
||||
self.buttonStatesPrev = self.CS.buttonStates.copy()
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
self.CS.out = ret.as_reader()
|
||||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
|
|
|
|||
|
|
@ -19,8 +19,6 @@ class CarControllerParams:
|
|||
STEER_DRIVER_FACTOR = 1 # from dbc
|
||||
|
||||
BUTTON_STATES = {
|
||||
"leftBlinker": False,
|
||||
"rightBlinker": False,
|
||||
"accelCruise": False,
|
||||
"decelCruise": False,
|
||||
"cancel": False,
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
917e6889be1691fb96e7566a92e0c6bbefc861a4
|
||||
d0e2657d3a57ba231e3775d3bc901221d8794281
|
||||
Loading…
Reference in New Issue