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>
pull/1125/head
rbiasini 5 years ago committed by GitHub
parent 09bb76eeb8
commit 0c67143c92
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
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
Split 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.cruise_enabled_prev = False
self.low_speed_alert = False
self.left_blinker_prev = False
self.right_blinker_prev = False
# *** init the major players ***
self.CS = CarState(CP)
@ -123,22 +121,7 @@ class CarInterface(CarInterfaceBase):
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# TODO: button presses
buttonEvents = []
if ret.leftBlinker != self.left_blinker_prev:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = ret.leftBlinker != 0
buttonEvents.append(be)
if ret.rightBlinker != self.right_blinker_prev:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = ret.rightBlinker != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.buttonEvents = []
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
@ -177,8 +160,6 @@ class CarInterface(CarInterfaceBase):
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
self.left_blinker_prev = ret.leftBlinker
self.right_blinker_prev = ret.rightBlinker
# copy back carState packet to CS
self.CS.out = ret.as_reader()

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

@ -101,10 +101,10 @@ class CarController():
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
lkas_enabled = enabled and not CS.steer_not_allowed and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
@ -114,7 +114,7 @@ class CarController():
if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends += gmcan.create_steering_control_ct6(self.packer_pt,
canbus, apply_steer, CS.v_ego, idx, lkas_enabled)
canbus, apply_steer, CS.out.vEgo, idx, lkas_enabled)
else:
can_sends.append(gmcan.create_steering_control(self.packer_pt,
canbus.powertrain, apply_steer, idx, lkas_enabled))
@ -142,11 +142,11 @@ class CarController():
if (frame % 4) == 0:
idx = (frame // 4) % 4
at_full_stop = enabled and CS.standstill
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
at_full_stop = enabled and CS.out.standstill
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
at_full_stop = enabled and CS.standstill
at_full_stop = enabled and CS.out.standstill
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
@ -167,7 +167,7 @@ class CarController():
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)

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.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.gm.values import DBC, CAR, \
from selfdrive.car.gm.values import DBC, CAR, AccState, \
CruiseButtons, is_eps_status_ok, \
STEER_THRESHOLD, SUPERCRUISE_CARS
@ -58,73 +58,68 @@ class CarState(CarStateBase):
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
def update(self, pt_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
self.standstill = self.v_ego_raw < 0.01
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition']
ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
if ret.brake < 10/0xd0:
ret.brake = 0.
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']
self.user_gas_pressed = self.pedal_gas > 0
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
ret.gasPressed = ret.gas > 1e-5
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
# 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)
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 1 - open, 0 - closed
self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0)
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1)
# 1 - latched
self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1
self.steer_error = False
self.brake_error = False
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False
self.main_on = False
ret.cruiseState.available = False
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
self.esp_disabled = False
self.regen_pressed = False
regen_pressed = False
self.pcm_acc_status = int(self.acc_active)
else:
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'])
self.acc_active = False
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
if self.car_fingerprint == CAR.VOLT:
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else:
self.regen_pressed = False
# Brake pedal's potentiometer returns near-zero reading
# even when pedal is not pressed.
if self.user_brake < 10:
self.user_brake = 0
regen_pressed = False
# 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):
self.pt_cp.update_strings(can_strings)
self.CS.update(self.pt_cp)
# create message
ret = car.CarState.new_message()
ret = self.CS.update(self.pt_cp)
ret.canValid = self.pt_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal information.
ret.gas = self.CS.pedal_gas / 254.0
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake / 0xd0
ret.brakePressed = self.CS.brake_pressed
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF
ret.cruiseState.enabled = cruiseEnabled
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.gearShifter = self.CS.gear_shifter
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
@ -247,7 +193,7 @@ class CarInterface(CarInterfaceBase):
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
if not (cruiseEnabled and self.CS.standstill):
if not (ret.cruiseState.enabled and ret.standstill):
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET:
be.type = ButtonType.decelCruise
@ -260,8 +206,6 @@ class CarInterface(CarInterfaceBase):
ret.buttonEvents = buttonEvents
events = []
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if ret.doorOpen:
@ -276,15 +220,13 @@ class CarInterface(CarInterfaceBase):
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid:
if ret.gearShifter != car.CarState.GearShifter.drive:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
if not ret.cruiseState.available:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.gear_shifter == 3:
if ret.gearShifter == car.CarState.GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
@ -317,11 +259,11 @@ class CarInterface(CarInterfaceBase):
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
return ret.as_reader()
# copy back carState packet to CS
self.CS.out = ret.as_reader()
return self.CS.out
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
hud_v_cruise = c.hudControl.setSpeed
if hud_v_cruise > 70:
@ -329,7 +271,7 @@ class CarInterface(CarInterfaceBase):
# For Openpilot, "enabled" includes pre-enable.
# In GM, PCM faults out if ACC command overlaps user gas.
enabled = c.enabled and not self.CS.user_gas_pressed
enabled = c.enabled and not self.CS.out.gasPressed
can_sends = self.CC.update(enabled, self.CS, self.frame, \
c.actuators,

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):
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
@ -169,7 +169,7 @@ class CarController():
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
elif CS.stopped:
elif CS.out.cruiseState.standstill:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
else:

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

@ -1,3 +1,4 @@
from cereal import car
from collections import defaultdict
from common.numpy_fast import interp
from opendbc.can.can_define import CANDefine
@ -193,9 +194,9 @@ class CarState(CarStateBase):
self.cruise_setting = 0
self.v_cruise_pcm_prev = 0
self.cruise_mode = 0
self.stopped = 0
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
# car params
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
@ -204,21 +205,19 @@ class CarState(CarStateBase):
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
# ******************* parse out can *******************
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'])
else:
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']])
self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']
ret.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
ret.doorOpen = any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']])
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] or not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'])
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']]
self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT']
@ -234,111 +233,111 @@ class CarState(CarStateBase):
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr)/4.
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
self.v_ego_raw = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
self.v_weight * v_wheel
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
# 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']
ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
ret.leftBlinker = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] != 0
ret.rightBlinker = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] != 0
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
else:
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'])
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
gear = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN):
self.car_gas = self.pedal_gas
ret.gas = self.pedal_gas / 256.
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']
self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
# 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 > 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:
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID):
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
ret.brakePressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] != 0 or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
else:
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
self.v_cruise_pcm_prev = self.v_cruise_pcm
ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] * CV.KPH_TO_MS
self.v_cruise_pcm_prev = ret.cruiseState.speed
else:
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], ret.vEgo)
ret.cruiseState.speed = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] * CV.KPH_TO_MS
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or
(self.brake_switch and self.brake_switch_prev and
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts))
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
ret.brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] != 0
ret.cruiseState.available = bool(main_on) and self.cruise_mode == 0
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
if self.user_brake > 0.05:
self.brake_pressed = 1
if ret.brake > 0.05:
ret.brakePressed = True
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_aeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
ret.stockAeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
self.stock_aeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_hud = False
self.stock_fcw = False
ret.stockFcw = False
else:
self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0)
ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
self.stock_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
return ret

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

@ -380,79 +380,16 @@ class CarInterface(CarInterfaceBase):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
ret.gas = self.CS.car_gas / 256.0
if not self.CP.enableGasInterceptor:
ret.gasPressed = self.CS.pedal_gas > 0
else:
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
ret.brakeLights = bool(self.CS.brake_switch or
c.actuators.brake > brakelights_threshold)
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
# gear shifter lever
ret.gearShifter = self.CS.gear_shifter
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringTorqueEps = self.CS.steer_torque_motor
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode)
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.stockAeb = self.CS.stock_aeb
ret.stockFcw = self.CS.stock_fcw
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
@ -504,7 +441,7 @@ class CarInterface(CarInterfaceBase):
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on or self.CS.cruise_mode:
if not ret.cruiseState.available:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
@ -569,8 +506,8 @@ class CarInterface(CarInterfaceBase):
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
return ret.as_reader()
self.CS.out = ret.as_reader()
return self.CS.out
# pass in a car.CarControl
# to be called @ 100hz

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

@ -18,7 +18,7 @@ class CarController():
### Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams)
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
@ -38,7 +38,7 @@ class CarController():
if pcm_cancel_cmd:
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5:
self.last_resume_cnt = self.cnt
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))

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

@ -1,5 +1,5 @@
from cereal import car
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
@ -127,95 +127,101 @@ def get_camera_parser(CP):
class CarState(CarStateBase):
def update(self, cp, cp_cam):
# update prevs, update must run once per Loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = True
self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw']
self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step']
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.main_on = True
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0
self.pcm_acc_status = int(self.acc_active)
self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
self.low_speed_lockout = self.v_ego_raw < 1.0
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
self.standstill = not self.v_ego_raw > 0.1
self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
self.main_on = True
self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
self.brake_error = 0
self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq']
self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq']
self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
self.user_brake = 0
ret = car.CarState.new_message()
ret.doorOpen = False # FIXME
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1
ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle']
ret.steeringRate = cp.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0
ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0
ret.steeringTorque = cp.vl["MDPS11"]['CR_Mdps_DrvTq']
ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# cruise state
ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0
ret.cruiseState.available = True
ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
if ret.cruiseState.enabled:
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
ret.cruiseState.speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
else:
ret.cruiseState.speed = 0
self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
self.brake_lights = bool(self.brake_pressed)
ret.brake = 0 # FIXME
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
ret.brakeLights = ret.brakePressed
if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1):
self.pedal_gas = 0
pedal_gas = 0
else:
self.pedal_gas = cp.vl["EMS12"]['TPS']
self.car_gas = cp.vl["EMS12"]['TPS']
pedal_gas = cp.vl["EMS12"]['TPS']
ret.gasPressed = pedal_gas > 1e-3
ret.gas = cp.vl["EMS12"]['TPS']
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear == 5:
self.gear_shifter = GearShifter.drive
gear_shifter = GearShifter.drive
elif gear == 6:
self.gear_shifter = GearShifter.neutral
gear_shifter = GearShifter.neutral
elif gear == 0:
self.gear_shifter = GearShifter.park
gear_shifter = GearShifter.park
elif gear == 7:
self.gear_shifter = GearShifter.reverse
gear_shifter = GearShifter.reverse
else:
self.gear_shifter = GearShifter.unknown
gear_shifter = GearShifter.unknown
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
self.gear_shifter_cluster = GearShifter.drive
gear_shifter_cluster = GearShifter.drive
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
self.gear_shifter_cluster = GearShifter.neutral
gear_shifter_cluster = GearShifter.neutral
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
self.gear_shifter_cluster = GearShifter.park
gear_shifter_cluster = GearShifter.park
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
self.gear_shifter_cluster = GearShifter.reverse
gear_shifter_cluster = GearShifter.reverse
else:
self.gear_shifter_cluster = GearShifter.unknown
gear_shifter_cluster = GearShifter.unknown
# Gear Selecton via TCU12
gear2 = cp.vl["TCU12"]["CUR_GR"]
if gear2 == 0:
self.gear_tcu = GearShifter.park
gear_tcu = GearShifter.park
elif gear2 == 14:
self.gear_tcu = GearShifter.reverse
gear_tcu = GearShifter.reverse
elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently
self.gear_tcu = GearShifter.drive
gear_tcu = GearShifter.drive
else:
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:
self.gear_tcu = GearShifter.unknown
ret.gearShifter = gear_shifter
# save the entire LKAS11 and CLU11
self.lkas11 = cp_cam.vl["LKAS11"]
self.clu11 = cp.vl["CLU11"]
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step']
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
self.brake_error = 0
return ret

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.vehicle_model import VehicleModel
from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser
from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS
from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FINGERPRINTS
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
@ -148,83 +148,16 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
ret.gearShifter = self.CS.gear_shifter_cluster
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
ret.gearShifter = self.CS.gear_tcu
else:
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.buttonEvents = []
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
@ -241,7 +174,7 @@ class CarInterface(CarInterfaceBase):
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
if not ret.cruiseState.available:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
@ -270,7 +203,8 @@ class CarInterface(CarInterfaceBase):
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
self.CS.out = ret.as_reader()
return self.CS.out
def apply(self, c):

2
selfdrive/car/interfaces.py
Unescape View File

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

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

@ -50,7 +50,7 @@ class CarController():
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled

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

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

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

@ -15,7 +15,7 @@ class CarInterface(CarInterfaceBase):
self.CP = CP
self.frame = 0
self.acc_active_prev = 0
self.enabled_prev = 0
self.gas_pressed_prev = False
# *** init the major players ***
@ -98,67 +98,17 @@ class CarInterface(CarInterfaceBase):
self.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings)
self.CS.update(self.pt_cp, self.cam_cp)
# create message
ret = car.CarState.new_message()
ret = self.CS.update(self.pt_cp, self.cam_cp)
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.gas = self.CS.pedal_gas / 255.
ret.gasPressed = self.CS.user_gas_pressed
# cruise state
ret.cruiseState.enabled = bool(self.CS.acc_active)
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
ret.doorOpen = self.CS.door_open
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.accelCruise
buttonEvents.append(be)
events = []
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@ -166,9 +116,9 @@ class CarInterface(CarInterfaceBase):
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.acc_active and not self.acc_active_prev:
if ret.cruiseState.enabled and not self.enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
if not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on gas pedal rising edge
@ -180,12 +130,11 @@ class CarInterface(CarInterfaceBase):
ret.events = events
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.acc_active_prev = self.CS.acc_active
self.enabled_prev = ret.cruiseState.enabled
# cast to reader so it can't be modified
return ret.as_reader()
self.CS.out = ret.as_reader()
return self.CS.out
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,

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

@ -122,7 +122,7 @@ class CarController():
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# only cut torque when steer state is a known fault
@ -143,25 +143,25 @@ class CarController():
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
angle_lim = interp(CS.out.vEgo, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
apply_angle = CS.out.steeringAngle
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
if CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
@ -170,7 +170,7 @@ class CarController():
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
self.last_standstill = CS.out.standstill
can_sends = []
@ -194,7 +194,7 @@ class CarController():
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:

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

@ -1,9 +1,10 @@
from cereal import car
from common.numpy_fast import mean
from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR, NO_STOP_TIMER_CAR
def get_can_parser(CP):
@ -91,75 +92,80 @@ class CarState(CarStateBase):
self.init_angle_offset = False
def update(self, cp, cp_cam):
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
ret = car.CarState.new_message()
self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
ret.doorOpen = any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed)
if self.CP.enableGasInterceptor:
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
ret.gasPressed = ret.gas > 15
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
ret.gasPressed = ret.gas > 1e-5
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
self.standstill = not self.v_ego_raw > 0.001
ret.standstill = ret.vEgoRaw < 0.001
if self.CP.carFingerprint in TSS2_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
elif self.CP.carFingerprint in NO_DSU_CAR:
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
# need to apply an offset as soon as the steering angle measurements are both received
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3 and not self.init_angle_offset:
self.init_angle_offset = True
self.angle_offset = self.angle_steers - angle_wheel
self.angle_offset = ret.steeringAngle - angle_wheel
else:
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else:
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
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:
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
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.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
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.vehicle_model import VehicleModel
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS
from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FINGERPRINTS
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
from selfdrive.swaglog import cloudlog
from selfdrive.car.interfaces import CarInterfaceBase
@ -343,85 +343,12 @@ class CarInterface(CarInterfaceBase):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.pedal_gas
if self.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press
ret.gasPressed = self.CS.pedal_gas > 15
else:
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringTorqueEps = self.CS.steer_torque_motor
ret.steeringPressed = self.CS.steer_override
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_active
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command
# also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.genericToggle = self.CS.generic_toggle
ret.stockAeb = self.CS.stock_aeb
ret.buttonEvents = []
# events
events = []
@ -436,7 +363,7 @@ class CarInterface(CarInterfaceBase):
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on and self.CP.openpilotLongitudinalControl:
if not ret.cruiseState.available and self.CP.openpilotLongitudinalControl:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
@ -473,7 +400,8 @@ class CarInterface(CarInterfaceBase):
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
self.CS.out = ret.as_reader()
return self.CS.out
# pass in a car.CarControl
# to be called @ 100hz

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
# commanding HCA if there's a fault, so the steering rack recovers.
if enabled and not (CS.standstill or CS.steeringFault):
if enabled and not (CS.out.standstill or CS.steeringFault):
# FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This
# is inherently handled by scaling to STEER_MAX. The rack doesn't seem
# to care about up/down rate, but we have some evidence it may do its
# own rate limiting, and matching OP helps for accurate tuning.
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steeringTorque, P)
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending
@ -116,7 +116,7 @@ class CarController():
# filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
if frame % P.LDW_STEP == 0:
hcaEnabled = True if enabled and not CS.standstill else False
hcaEnabled = True if enabled and not CS.out.standstill else False
if visual_alert == VisualAlert.steerRequired:
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
@ -124,7 +124,7 @@ class CarController():
hud_alert = MQB_LDW_MESSAGES["none"]
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, canbus.pt, hcaEnabled,
CS.steeringPressed, hud_alert, leftLaneVisible,
CS.out.steeringPressed, hud_alert, leftLaneVisible,
rightLaneVisible))
#--------------------------------------------------------------------------
@ -140,11 +140,11 @@ class CarController():
# stock ACC with OP disengagement, or to auto-resume from stop.
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
if not enabled and CS.accEnabled:
if not enabled and CS.out.cruiseState.enabled:
# Cancel ACC if it's engaged with OP disengaged.
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["cancel"] = True
elif enabled and CS.standstill:
elif enabled and CS.out.standstill:
# Blip the Resume button if we're engaged at standstill.
# FIXME: This is a naive implementation, improve with visiond or radar input.
# A subset of MQBs like to "creep" too aggressively with this implementation.

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

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

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

@ -120,60 +120,22 @@ class CarInterface(CarInterfaceBase):
events = []
buttonEvents = []
params = Params()
ret = car.CarState.new_message()
# Process the most recent CAN message traffic, and check for validity
# The camera CAN has no signals we use at this time, but we process it
# anyway so we can test connectivity with can_valid
self.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings)
self.CS.update(self.pt_cp)
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
# Wheel and vehicle speed, yaw rate
ret.wheelSpeeds.fl = self.CS.wheelSpeedFL
ret.wheelSpeeds.fr = self.CS.wheelSpeedFR
ret.wheelSpeeds.rl = self.CS.wheelSpeedRL
ret.wheelSpeeds.rr = self.CS.wheelSpeedRR
ret.vEgoRaw = self.CS.vEgoRaw
ret.vEgo = self.CS.vEgo
ret.aEgo = self.CS.aEgo
ret.standstill = self.CS.standstill
# Steering wheel position, movement, yaw rate, and driver input
ret.steeringAngle = self.CS.steeringAngle
ret.steeringRate = self.CS.steeringRate
ret.steeringTorque = self.CS.steeringTorque
ret.steeringPressed = self.CS.steeringPressed
ret = self.CS.update(self.pt_cp)
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.yawRate = self.CS.yawRate
# Gas, brakes and shifting
ret.gas = self.CS.gas
ret.gasPressed = self.CS.gasPressed
ret.brake = self.CS.brake
ret.brakePressed = self.CS.brakePressed
ret.brakeLights = self.CS.brakeLights
ret.gearShifter = self.CS.gearShifter
# Doors open, seatbelt unfastened
ret.doorOpen = self.CS.doorOpen
ret.seatbeltUnlatched = self.CS.seatbeltUnlatched
# Update the EON metric configuration to match the car at first startup,
# or if there's been a change.
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Blinker switch updates
ret.leftBlinker = self.CS.buttonStates["leftBlinker"]
ret.rightBlinker = self.CS.buttonStates["rightBlinker"]
# ACC cruise state
ret.cruiseState.available = self.CS.accAvailable
ret.cruiseState.enabled = self.CS.accEnabled
ret.cruiseState.speed = self.CS.accSetSpeed
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates:
@ -230,8 +192,8 @@ class CarInterface(CarInterfaceBase):
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
# cast to reader so it can't be modified
return ret.as_reader()
self.CS.out = ret.as_reader()
return self.CS.out
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,

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

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

2
selfdrive/test/process_replay/ref_commit
Unescape View File

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