sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Carstate returns capnp struct (#1115)

Browse Source 
* 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>

old-commit-hash: 0c67143c92
commatwo_master
rbiasini 5 years ago committed by GitHub
parent 20e09fe5de
commit 6209ad5120
22 changed files with 369 additions and 733 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      selfdrive/car/chrysler/interface.py
  2. 14
      selfdrive/car/gm/carcontroller.py
  3. 85
      selfdrive/car/gm/carstate.py
  4. 80
      selfdrive/car/gm/interface.py
  5. 6
      selfdrive/car/honda/carcontroller.py
  6. 129
      selfdrive/car/honda/carstate.py
  7. 73
      selfdrive/car/honda/interface.py
  8. 4
      selfdrive/car/hyundai/carcontroller.py
  9. 134
      selfdrive/car/hyundai/carstate.py
  10. 78
      selfdrive/car/hyundai/interface.py
  11. 2
      selfdrive/car/interfaces.py
  12. 2
      selfdrive/car/subaru/carcontroller.py
  13. 67
      selfdrive/car/subaru/carstate.py
  14. 67
      selfdrive/car/subaru/interface.py
  15. 16
      selfdrive/car/toyota/carcontroller.py
  16. 100
      selfdrive/car/toyota/carstate.py
  17. 86
      selfdrive/car/toyota/interface.py
  18. 12
      selfdrive/car/volkswagen/carcontroller.py
  19. 76
      selfdrive/car/volkswagen/carstate.py
  20. 46
      selfdrive/car/volkswagen/interface.py
  21. 2
      selfdrive/car/volkswagen/values.py
  22. 2
      selfdrive/test/process_replay/ref_commit

21
selfdrive/car/chrysler/interface.py
Unescape View File

@ -20,8 +20,6 @@ class CarInterface(CarInterfaceBase):
self.brake_pressed_prev = False self.brake_pressed_prev = False
self.cruise_enabled_prev = False self.cruise_enabled_prev = False
self.low_speed_alert = False self.low_speed_alert = False
self.left_blinker_prev = False
self.right_blinker_prev = False
# *** init the major players *** # *** init the major players ***
self.CS = CarState(CP) 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.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 ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# TODO: button presses ret.buttonEvents = []
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
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed) self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
@ -177,8 +160,6 @@ class CarInterface(CarInterfaceBase):
self.gas_pressed_prev = ret.gasPressed self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled 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 # copy back carState packet to CS
self.CS.out = ret.as_reader() self.CS.out = ret.as_reader()

14
selfdrive/car/gm/carcontroller.py
Unescape View File

@ -101,10 +101,10 @@ class CarController():
### STEER ### ### STEER ###
if (frame % P.STEER_STEP) == 0: 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: if lkas_enabled:
new_steer = actuators.steer * P.STEER_MAX 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 self.steer_rate_limited = new_steer != apply_steer
else: else:
apply_steer = 0 apply_steer = 0
@ -114,7 +114,7 @@ class CarController():
if self.car_fingerprint in SUPERCRUISE_CARS: if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends += gmcan.create_steering_control_ct6(self.packer_pt, 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: else:
can_sends.append(gmcan.create_steering_control(self.packer_pt, can_sends.append(gmcan.create_steering_control(self.packer_pt,
canbus.powertrain, apply_steer, idx, lkas_enabled)) canbus.powertrain, apply_steer, idx, lkas_enabled))
@ -142,11 +142,11 @@ class CarController():
if (frame % 4) == 0: if (frame % 4) == 0:
idx = (frame // 4) % 4 idx = (frame // 4) % 4
at_full_stop = enabled and CS.standstill at_full_stop = enabled and CS.out.standstill
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) 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)) 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)) 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 # Send dashboard UI commands (ACC status), 25hz
@ -167,7 +167,7 @@ class CarController():
if frame % speed_and_accelerometer_step == 0: if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4 idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx)) 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: if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain) can_sends += gmcan.create_adas_keepalive(canbus.powertrain)

85
selfdrive/car/gm/carstate.py
Unescape View File

@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
from opendbc.can.can_define import CANDefine from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase 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, \ CruiseButtons, is_eps_status_ok, \
STEER_THRESHOLD, SUPERCRUISE_CARS STEER_THRESHOLD, SUPERCRUISE_CARS
@ -58,73 +58,68 @@ class CarState(CarStateBase):
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"] self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
def update(self, pt_cp): def update(self, pt_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons'] self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS ret.wheelSpeeds.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]) ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
self.standstill = self.v_ego_raw < 0.01 ret.standstill = ret.vEgoRaw < 0.01
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None)) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] 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'] ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
self.user_gas_pressed = self.pedal_gas > 0 ret.gasPressed = ret.gas > 1e-5
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq'] ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 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 # 1 - open, 0 - closed
self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0) pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1)
# 1 - latched # 1 - latched
self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1 ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
self.steer_error = False ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
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: if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False self.park_brake = False
self.main_on = False ret.cruiseState.available = False
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive'] self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
self.esp_disabled = False self.esp_disabled = False
self.regen_pressed = False regen_pressed = False
self.pcm_acc_status = int(self.acc_active) self.pcm_acc_status = int(self.acc_active)
else: else:
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'] ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'])
self.acc_active = False self.acc_active = False
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
if self.car_fingerprint == CAR.VOLT: if self.car_fingerprint == CAR.VOLT:
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else: else:
self.regen_pressed = False 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 # Regen braking is braking
self.brake_pressed = self.user_brake > 10 or self.regen_pressed 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)
self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive return ret

80
selfdrive/car/gm/interface.py
Unescape View File

@ -175,68 +175,14 @@ class CarInterface(CarInterfaceBase):
def update(self, c, can_strings): def update(self, c, can_strings):
self.pt_cp.update_strings(can_strings) self.pt_cp.update_strings(can_strings)
self.CS.update(self.pt_cp) ret = self.CS.update(self.pt_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = self.pt_cp.can_valid ret.canValid = self.pt_cp.can_valid
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
# 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.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False 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 = [] 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: 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 = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown be.type = ButtonType.unknown
@ -247,7 +193,7 @@ class CarInterface(CarInterfaceBase):
be.pressed = False be.pressed = False
but = self.CS.prev_cruise_buttons but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL: 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. be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET: elif but == CruiseButtons.DECEL_SET:
be.type = ButtonType.decelCruise be.type = ButtonType.decelCruise
@ -260,8 +206,6 @@ class CarInterface(CarInterfaceBase):
ret.buttonEvents = buttonEvents ret.buttonEvents = buttonEvents
events = [] 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: if self.CS.steer_not_allowed:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if ret.doorOpen: if ret.doorOpen:
@ -276,15 +220,13 @@ class CarInterface(CarInterfaceBase):
events.append(create_event('pcmDisable', [ET.USER_DISABLE])) events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else: else:
if self.CS.brake_error: if ret.gearShifter != car.CarState.GearShifter.drive:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled: if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) 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])) 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])) events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed: if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY])) events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
@ -317,11 +259,11 @@ class CarInterface(CarInterfaceBase):
self.gas_pressed_prev = ret.gasPressed self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified # copy back carState packet to CS
return ret.as_reader() self.CS.out = ret.as_reader()
return self.CS.out
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c): def apply(self, c):
hud_v_cruise = c.hudControl.setSpeed hud_v_cruise = c.hudControl.setSpeed
if hud_v_cruise > 70: if hud_v_cruise > 70:
@ -329,7 +271,7 @@ class CarInterface(CarInterfaceBase):
# For Openpilot, "enabled" includes pre-enable. # For Openpilot, "enabled" includes pre-enable.
# In GM, PCM faults out if ACC command overlaps user gas. # 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, \ can_sends = self.CC.update(enabled, self.CS, self.frame, \
c.actuators, c.actuators,

6
selfdrive/car/honda/carcontroller.py
Unescape View File

@ -95,10 +95,10 @@ class CarController():
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
# *** apply brake hysteresis *** # *** 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 *** # *** 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 # 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 pcm_cancel_cmd = True
@ -169,7 +169,7 @@ class CarController():
# If using stock ACC, spam cancel command to kill gas when OP disengages. # If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd: if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) 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)) can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
else: else:

129
selfdrive/car/honda/carstate.py
Unescape View File

@ -1,3 +1,4 @@
from cereal import car
from collections import defaultdict from collections import defaultdict
from common.numpy_fast import interp from common.numpy_fast import interp
from opendbc.can.can_define import CANDefine from opendbc.can.can_define import CANDefine
@ -193,9 +194,9 @@ class CarState(CarStateBase):
self.cruise_setting = 0 self.cruise_setting = 0
self.v_cruise_pcm_prev = 0 self.v_cruise_pcm_prev = 0
self.cruise_mode = 0 self.cruise_mode = 0
self.stopped = 0
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
ret = car.CarState.new_message()
# car params # car params
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping 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 # update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting 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 ******************* # ******************* 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 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 ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'] ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'])
else: else:
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] ret.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'], 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']]) 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.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']] 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'] 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'] self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
speed_factor = SPEED_FACTOR[self.CP.carFingerprint] speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor ret.wheelSpeeds.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 ret.wheelSpeeds.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 ret.wheelSpeeds.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 ret.wheelSpeeds.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. 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 # 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) 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 + \ ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
self.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']
# 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_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] 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'] ret.leftBlinker = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] != 0
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] ret.rightBlinker = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] != 0
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] 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): 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.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
else: else:
self.park_brake = 0 # TODO self.park_brake = 0 # TODO
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) gear = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control # 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): 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 ret.gas = self.pedal_gas / 256.
else: else:
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] ret.gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] / 256.
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] # this is a hack for the interceptor. This is now only used in the simulation
self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE'] # TODO: Replace tests by toyota so this can go away
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] 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 > 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:
ret.gasPressed = self.pedal_gas > 1e-5
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.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0
if self.CP.radarOffCan: if self.CP.radarOffCan:
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL'] self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID): if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID):
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] ret.brakePressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] != 0 or \
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \ (self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
else: 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. # 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'] 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 = self.v_cruise_pcm self.v_cruise_pcm_prev = ret.cruiseState.speed
else: else:
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], ret.vEgo)
self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego) ret.cruiseState.speed = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] * CV.KPH_TO_MS
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
# brake switch has shown some single time step noise, so only considered when # brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples # switch is on for at least 2 consecutive CAN samples
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or
(self.brake_switch and self.brake_switch_prev and \ (self.brake_switch and self.brake_switch_prev and
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts))
self.brake_switch_prev = self.brake_switch self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] ret.brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] 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 # 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.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
if self.user_brake > 0.05: if ret.brake > 0.05:
self.brake_pressed = 1 ret.brakePressed = True
# TODO: discover the CAN msg that has the imperial unit bit for all other cars # 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 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: 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: 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: if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_hud = False self.stock_hud = False
self.stock_fcw = False ret.stockFcw = False
else: 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_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"] self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
return ret

73
selfdrive/car/honda/interface.py
Unescape View File

@ -380,79 +380,16 @@ class CarInterface(CarInterfaceBase):
self.cp.update_strings(can_strings) self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings) self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam) ret = self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
# 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
# FIXME: read sendcan for brakelights # FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
ret.brakeLights = bool(self.CS.brake_switch or ret.brakeLights = bool(self.CS.brake_switch or
c.actuators.brake > brakelights_threshold) 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 = [] 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: if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message() be = car.CarState.ButtonEvent.new_message()
@ -504,7 +441,7 @@ class CarInterface(CarInterfaceBase):
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled: if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) 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])) events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse: if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) 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.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified self.CS.out = ret.as_reader()
return ret.as_reader() return self.CS.out
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz

4
selfdrive/car/hyundai/carcontroller.py
Unescape View File

@ -18,7 +18,7 @@ class CarController():
### Steering Torque ### Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX 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 self.steer_rate_limited = new_steer != apply_steer
if not enabled: if not enabled:
@ -38,7 +38,7 @@ class CarController():
if pcm_cancel_cmd: if pcm_cancel_cmd:
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) 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 self.last_resume_cnt = self.cnt
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))

134
selfdrive/car/hyundai/carstate.py
Unescape View File

@ -1,5 +1,5 @@
from cereal import car 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 selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
@ -127,95 +127,101 @@ def get_camera_parser(CP):
class CarState(CarStateBase): class CarState(CarStateBase):
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
# update prevs, update must run once per Loop ret = car.CarState.new_message()
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on ret.doorOpen = False # FIXME
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
self.door_all_closed = True
self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] 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
self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step'] 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.
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
self.main_on = True
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0 ret.standstill = ret.vEgoRaw < 0.1
self.pcm_acc_status = int(self.acc_active)
ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle']
self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS ret.steeringRate = cp.vl["SAS11"]['SAS_Speed']
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0
self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. ret.steeringTorque = cp.vl["MDPS11"]['CR_Mdps_DrvTq']
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) 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
# cruise state
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS ret.cruiseState.available = True
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
self.standstill = not self.v_ego_raw > 0.1 if ret.cruiseState.enabled:
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] ret.cruiseState.speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] else:
self.main_on = True ret.cruiseState.speed = 0
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'] ret.brake = 0 # FIXME
self.brake_lights = bool(self.brake_pressed) ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
ret.brakeLights = ret.brakePressed
if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1): if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1):
self.pedal_gas = 0 pedal_gas = 0
else: else:
self.pedal_gas = cp.vl["EMS12"]['TPS'] pedal_gas = cp.vl["EMS12"]['TPS']
self.car_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 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"] gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear == 5: if gear == 5:
self.gear_shifter = GearShifter.drive gear_shifter = GearShifter.drive
elif gear == 6: elif gear == 6:
self.gear_shifter = GearShifter.neutral gear_shifter = GearShifter.neutral
elif gear == 0: elif gear == 0:
self.gear_shifter = GearShifter.park gear_shifter = GearShifter.park
elif gear == 7: elif gear == 7:
self.gear_shifter = GearShifter.reverse gear_shifter = GearShifter.reverse
else: 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. # 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: 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: 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: 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: elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
self.gear_shifter_cluster = GearShifter.reverse gear_shifter_cluster = GearShifter.reverse
else: else:
self.gear_shifter_cluster = GearShifter.unknown gear_shifter_cluster = GearShifter.unknown
# Gear Selecton via TCU12 # Gear Selecton via TCU12
gear2 = cp.vl["TCU12"]["CUR_GR"] gear2 = cp.vl["TCU12"]["CUR_GR"]
if gear2 == 0: if gear2 == 0:
self.gear_tcu = GearShifter.park gear_tcu = GearShifter.park
elif gear2 == 14: elif gear2 == 14:
self.gear_tcu = GearShifter.reverse gear_tcu = GearShifter.reverse
elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently
self.gear_tcu = GearShifter.drive gear_tcu = GearShifter.drive
else:
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: else:
self.gear_tcu = GearShifter.unknown ret.gearShifter = gear_shifter
# save the entire LKAS11 and CLU11 # save the entire LKAS11 and CLU11
self.lkas11 = cp_cam.vl["LKAS11"] self.lkas11 = cp_cam.vl["LKAS11"]
self.clu11 = cp.vl["CLU11"] 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

78
selfdrive/car/hyundai/interface.py
Unescape View File

@ -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.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel 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.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 import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
@ -148,83 +148,16 @@ class CarInterface(CarInterfaceBase):
return ret return ret
# returns a car.CarState
def update(self, c, can_strings): def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings) self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings) self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam) ret = self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid 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 # TODO: button presses
buttonEvents = [] ret.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
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) # 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.: 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])) events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled: if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) 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])) events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse: if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) 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.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled 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): def apply(self, c):

2
selfdrive/car/interfaces.py
Unescape View File

@ -51,8 +51,6 @@ class CarStateBase:
def __init__(self, CP): def __init__(self, CP):
self.CP = CP self.CP = CP
self.car_fingerprint = CP.carFingerprint self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = 0
self.right_blinker_on = 0
self.cruise_buttons = 0 self.cruise_buttons = 0
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])

2
selfdrive/car/subaru/carcontroller.py
Unescape View File

@ -50,7 +50,7 @@ class CarController():
# limits due to driver torque # limits due to driver torque
new_steer = int(round(apply_steer)) 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 self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled lkas_enabled = enabled

67
selfdrive/car/subaru/carstate.py
Unescape View File

@ -1,4 +1,5 @@
import copy import copy
from cereal import car
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
@ -85,54 +86,50 @@ def get_camera_can_parser(CP):
class CarState(CarStateBase): class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
super().__init__(CP) super().__init__(CP)
# initialize can parser
self.left_blinker_cnt = 0 self.left_blinker_cnt = 0
self.right_blinker_cnt = 0 self.right_blinker_cnt = 0
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
ret = car.CarState.new_message()
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
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
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal'] # continuous blinker signals for assisted lane change
self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal'] self.left_blinker_cnt = 50 if cp.vl["Dashlights"]['LEFT_BLINKER'] else max(self.left_blinker_cnt - 1, 0)
self.user_gas_pressed = self.pedal_gas > 0 ret.leftBlinker = self.left_blinker_cnt > 0
self.brake_pressed = self.brake_pressure > 0 self.right_blinker_cnt = 50 if cp.vl["Dashlights"]['RIGHT_BLINKER'] else max(self.right_blinker_cnt - 1, 0)
self.brake_lights = bool(self.brake_pressed) ret.rightBlinker = self.right_blinker_cnt > 0
self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle']
self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint]
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'] 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 # 1 = imperial, 6 = metric
if cp.vl["Dash_State"]['Units'] == 1: if cp.vl["Dash_State"]['Units'] == 1:
self.v_cruise_pcm *= CV.MPH_TO_KPH ret.cruiseState.speed *= 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.
# 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 ret.seatbeltUnlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
self.prev_right_blinker_on = self.right_blinker_on ret.doorOpen = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'],
# 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
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
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'],
cp.vl["BodyInfo"]['DOOR_OPEN_RL'], cp.vl["BodyInfo"]['DOOR_OPEN_RL'],
cp.vl["BodyInfo"]['DOOR_OPEN_FR'], cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
cp.vl["BodyInfo"]['DOOR_OPEN_FL']]) cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"]) self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
return ret

67
selfdrive/car/subaru/interface.py
Unescape View File

@ -15,7 +15,7 @@ class CarInterface(CarInterfaceBase):
self.CP = CP self.CP = CP
self.frame = 0 self.frame = 0
self.acc_active_prev = 0 self.enabled_prev = 0
self.gas_pressed_prev = False self.gas_pressed_prev = False
# *** init the major players *** # *** init the major players ***
@ -98,67 +98,17 @@ class CarInterface(CarInterfaceBase):
self.pt_cp.update_strings(can_strings) self.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings) self.cam_cp.update_strings(can_strings)
self.CS.update(self.pt_cp, self.cam_cp) ret = self.CS.update(self.pt_cp, self.cam_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid 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.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
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
buttonEvents = [] 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 = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.accelCruise be.type = ButtonType.accelCruise
buttonEvents.append(be) buttonEvents.append(be)
events = [] events = []
if ret.seatbeltUnlatched: if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@ -166,9 +116,9 @@ class CarInterface(CarInterfaceBase):
if ret.doorOpen: if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) 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])) 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])) events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on gas pedal rising edge # disable on gas pedal rising edge
@ -180,12 +130,11 @@ class CarInterface(CarInterfaceBase):
ret.events = events ret.events = events
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed 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 self.CS.out = ret.as_reader()
return ret.as_reader() return self.CS.out
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,

16
selfdrive/car/toyota/carcontroller.py
Unescape View File

@ -122,7 +122,7 @@ class CarController():
# steer torque # steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) 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 self.steer_rate_limited = new_steer != apply_steer
# only cut torque when steer state is a known fault # only cut torque when steer state is a known fault
@ -143,25 +143,25 @@ class CarController():
# steer angle # steer angle
if self.steer_angle_enabled and CS.ipas_active: if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle 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) apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower # windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): 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: 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) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else: else:
apply_angle = CS.angle_steers apply_angle = CS.out.steeringAngle
if not enabled and CS.pcm_acc_status: 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 # 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 pcm_cancel_cmd = 1
# on entering standstill, send standstill request # 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 self.standstill_req = True
if CS.pcm_acc_status != 8: if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled # pcm entered standstill or it's disabled
@ -170,7 +170,7 @@ class CarController():
self.last_steer = apply_steer self.last_steer = apply_steer
self.last_angle = apply_angle self.last_angle = apply_angle
self.last_accel = apply_accel self.last_accel = apply_accel
self.last_standstill = CS.standstill self.last_standstill = CS.out.standstill
can_sends = [] can_sends = []
@ -194,7 +194,7 @@ class CarController():
# we can spam can to cancel the system even if we are using lat only control # 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): 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 # Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:

100
selfdrive/car/toyota/carstate.py
Unescape View File

@ -1,9 +1,10 @@
from cereal import car
from common.numpy_fast import mean from common.numpy_fast import mean
from opendbc.can.can_define import CANDefine from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV 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): def get_can_parser(CP):
@ -91,75 +92,80 @@ class CarState(CarStateBase):
self.init_angle_offset = False self.init_angle_offset = False
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
# update prevs, update must run once per loop ret = car.CarState.new_message()
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], 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']]) 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.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: 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: else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] ret.gasPressed = ret.gas > 1e-5
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS ret.wheelSpeeds.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 ret.wheelSpeeds.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 ret.wheelSpeeds.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 ret.wheelSpeeds.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]) ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) 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: 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: 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. # 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 # 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'] 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.init_angle_offset = True
self.angle_offset = self.angle_steers - angle_wheel self.angle_offset = ret.steeringAngle - angle_wheel
else: else:
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] ret.steeringAngle = 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.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR']) can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
if self.CP.carFingerprint == CAR.LEXUS_IS: ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON'] ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
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
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
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 # 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 ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
self.user_brake = 0
if self.CP.carFingerprint == CAR.LEXUS_IS: if self.CP.carFingerprint == CAR.LEXUS_IS:
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED'] 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 self.low_speed_lockout = False
else: else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] 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.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_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed) # 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: if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else: else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) 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.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) return ret

86
selfdrive/car/toyota/interface.py
Unescape View File

@ -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.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel 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.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.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
@ -343,85 +343,12 @@ class CarInterface(CarInterfaceBase):
self.cp.update_strings(can_strings) self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings) self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam) ret = self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
# 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.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.buttonEvents = []
# 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
# events # events
events = [] events = []
@ -436,7 +363,7 @@ class CarInterface(CarInterfaceBase):
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl: if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) 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])) events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl: if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) 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.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled 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 # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz

12
selfdrive/car/volkswagen/carcontroller.py
Unescape View File

@ -49,14 +49,14 @@ class CarController():
# FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop # FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop
# commanding HCA if there's a fault, so the steering rack recovers. # 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 # 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 # 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 # 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. # own rate limiting, and matching OP helps for accurate tuning.
new_steer = int(round(actuators.steer * P.STEER_MAX)) 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 self.steer_rate_limited = new_steer != apply_steer
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending # 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. # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
if frame % P.LDW_STEP == 0: 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: if visual_alert == VisualAlert.steerRequired:
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
@ -124,7 +124,7 @@ class CarController():
hud_alert = MQB_LDW_MESSAGES["none"] hud_alert = MQB_LDW_MESSAGES["none"]
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, canbus.pt, hcaEnabled, 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)) rightLaneVisible))
#-------------------------------------------------------------------------- #--------------------------------------------------------------------------
@ -140,11 +140,11 @@ class CarController():
# stock ACC with OP disengagement, or to auto-resume from stop. # stock ACC with OP disengagement, or to auto-resume from stop.
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: 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. # Cancel ACC if it's engaged with OP disengaged.
self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["cancel"] = True 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. # Blip the Resume button if we're engaged at standstill.
# FIXME: This is a naive implementation, improve with visiond or radar input. # FIXME: This is a naive implementation, improve with visiond or radar input.
# A subset of MQBs like to "creep" too aggressively with this implementation. # A subset of MQBs like to "creep" too aggressively with this implementation.

76
selfdrive/car/volkswagen/carstate.py
Unescape View File

@ -1,4 +1,5 @@
import numpy as np import numpy as np
from cereal import car
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
@ -105,45 +106,46 @@ class CarState(CarStateBase):
self.buttonStates = BUTTON_STATES.copy() self.buttonStates = BUTTON_STATES.copy()
def update(self, pt_cp): def update(self, pt_cp):
ret = car.CarState.new_message()
# Update vehicle speed and acceleration from ABS wheel speeds. # Update vehicle speed and acceleration from ABS wheel speeds.
self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS ret.wheelSpeeds.fl = 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 ret.wheelSpeeds.fr = 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 ret.wheelSpeeds.rl = 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.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])) ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
self.vEgo, self.aEgo = self.update_speed_kf(self.vEgoRaw) 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 # Update steering angle, rate, yaw rate, and driver input torque. VW send
# the sign/direction in a separate signal so they must be recombined. # 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'])] ret.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'])] ret.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'])] ret.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 ret.steeringPressed = abs(ret.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.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. # Update gas, brakes, and gearshift.
self.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 ret.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
self.gasPressed = self.gas > 0 ret.gasPressed = ret.gas > 0
self.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects ret.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']) ret.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
self.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
# Update gear and/or clutch position data. # Update gear and/or clutch position data.
can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe']) 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. # Update door and trunk/hatch lid open status.
self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_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_BT_offen'],
pt_cp.vl["Gateway_72"]['ZV_HFS_offen'], pt_cp.vl["Gateway_72"]['ZV_HFS_offen'],
pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'], pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'],
pt_cp.vl["Gateway_72"]['ZV_HD_offen']]) pt_cp.vl["Gateway_72"]['ZV_HD_offen']])
# Update seatbelt fastened status. # 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 # Update driver preference for metric. VW stores many different unit
# preferences, including separate units for for distance vs. speed. # preferences, including separate units for for distance vs. speed.
@ -155,38 +157,39 @@ class CarState(CarStateBase):
if accStatus == 1: if accStatus == 1:
# ACC okay but disabled # ACC okay but disabled
self.accFault = False self.accFault = False
self.accAvailable = False ret.cruiseState.available = False
self.accEnabled = False ret.cruiseState.enabled = False
elif accStatus == 2: elif accStatus == 2:
# ACC okay and enabled, but not currently engaged # ACC okay and enabled, but not currently engaged
self.accFault = False self.accFault = False
self.accAvailable = True ret.cruiseState.available = True
self.accEnabled = False ret.cruiseState.enabled = False
elif accStatus in [3, 4, 5]: 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) # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5)
self.accFault = False self.accFault = False
self.accAvailable = True ret.cruiseState.available = True
self.accEnabled = True ret.cruiseState.enabled = True
else: else:
# ACC fault of some sort. Seen statuses 6 or 7 for CAN comms disruptions, visibility issues, etc. # ACC fault of some sort. Seen statuses 6 or 7 for CAN comms disruptions, visibility issues, etc.
self.accFault = True self.accFault = True
self.accAvailable = False ret.cruiseState.available = False
self.accEnabled = False ret.cruiseState.enabled = False
# Update ACC setpoint. When the setpoint is zero or there's an error, the # 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. # radar sends a set-speed of ~90.69 m/s / 203mph.
self.accSetSpeed = pt_cp.vl["ACC_02"]['SetSpeed'] ret.cruiseState.speed = pt_cp.vl["ACC_02"]['SetSpeed']
if self.accSetSpeed > 90: self.accSetSpeed = 0 if ret.cruiseState.speed > 90:
ret.cruiseState.speed = 0
# Update control button states for turn signals and ACC controls. # 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["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["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["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["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["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme'])
self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke']) 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 # Read ACC hardware button type configuration info that has to pass thru
# to the radar. Ends up being different for steering wheel buttons vs # 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.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'] self.stabilityControlDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv']
return ret

46
selfdrive/car/volkswagen/interface.py
Unescape View File

@ -120,60 +120,22 @@ class CarInterface(CarInterfaceBase):
events = [] events = []
buttonEvents = [] buttonEvents = []
params = Params() params = Params()
ret = car.CarState.new_message()
# Process the most recent CAN message traffic, and check for validity # 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 # The camera CAN has no signals we use at this time, but we process it
# anyway so we can test connectivity with can_valid # anyway so we can test connectivity with can_valid
self.pt_cp.update_strings(can_strings) self.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings) self.cam_cp.update_strings(can_strings)
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 = self.CS.update(self.pt_cp)
ret.wheelSpeeds.fl = self.CS.wheelSpeedFL ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
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.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, # Update the EON metric configuration to match the car at first startup,
# or if there's been a change. # or if there's been a change.
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev: if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0") 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 # Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons. # the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates: for button in self.CS.buttonStates:
@ -230,8 +192,8 @@ class CarInterface(CarInterfaceBase):
self.displayMetricUnitsPrev = self.CS.displayMetricUnits self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy() self.buttonStatesPrev = self.CS.buttonStates.copy()
# cast to reader so it can't be modified self.CS.out = ret.as_reader()
return ret.as_reader() return self.CS.out
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,

2
selfdrive/car/volkswagen/values.py
Unescape View File

@ -19,8 +19,6 @@ class CarControllerParams:
STEER_DRIVER_FACTOR = 1 # from dbc STEER_DRIVER_FACTOR = 1 # from dbc
BUTTON_STATES = { BUTTON_STATES = {
"leftBlinker": False,
"rightBlinker": False,
"accelCruise": False, "accelCruise": False,
"decelCruise": False, "decelCruise": False,
"cancel": False, "cancel": False,

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
917e6889be1691fb96e7566a92e0c6bbefc861a4 d0e2657d3a57ba231e3775d3bc901221d8794281
Write Preview
Loading…
Cancel
Save