sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 64 Packages Projects Releases Wiki Activity

Chrysler: carstate returns capnp struct directly (#1110)

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

* ops, missed this conflict

* not sure why this got deleted

* no need to copy

* remove copy import

Co-authored-by: Willem Melching <willem.melching@gmail.com>
pull/214/head
rbiasini 6 years ago committed by GitHub
parent 11ec8e64fd
commit 703eb511da
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 60 additions and 95 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. 10
      selfdrive/car/chrysler/carcontroller.py
  2. 75
      selfdrive/car/chrysler/carstate.py
  3. 68
      selfdrive/car/chrysler/interface.py
  4. 2
      selfdrive/test/process_replay/ref_commit

10
selfdrive/car/chrysler/carcontroller.py
Unescape View File

@ -35,14 +35,14 @@ class CarController():
# steer torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
CS.steer_torque_motor, SteerLimitParams)
CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020_HYBRID, CAR.JEEP_CHEROKEE_2019):
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
@ -65,7 +65,7 @@ class CarController():
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(
self.packer, CS.gear_shifter, lkas_active, hud_alert,
self.packer, CS.out.gearShifter, lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1

75
selfdrive/car/chrysler/carstate.py
Unescape View File

@ -1,5 +1,7 @@
from cereal import car
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
@ -69,52 +71,53 @@ class CarState(CarStateBase):
def update(self, cp, cp_cam):
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
ret = car.CarState.new_message()
self.frame_23b = int(cp.vl["WHEEL_BUTTONS"]['COUNTER'])
self.frame = int(cp.vl["EPS_STATUS"]['COUNTER'])
self.door_all_closed = not any([cp.vl["DOORS"]['DOOR_OPEN_FL'],
cp.vl["DOORS"]['DOOR_OPEN_FR'],
cp.vl["DOORS"]['DOOR_OPEN_RL'],
cp.vl["DOORS"]['DOOR_OPEN_RR']])
self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
ret.doorOpen = any([cp.vl["DOORS"]['DOOR_OPEN_FL'],
cp.vl["DOORS"]['DOOR_OPEN_FR'],
cp.vl["DOORS"]['DOOR_OPEN_RL'],
cp.vl["DOORS"]['DOOR_OPEN_RR']])
ret.seatbeltUnlatched = cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 1
ret.brakePressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
ret.brake = 0
ret.brakeLights = ret.brakePressed
ret.gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
ret.gasPressed = ret.gas > 1e-5
self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
self.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
self.v_ego_raw = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
self.standstill = not self.v_ego_raw > 0.001
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None))
self.main_on = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
self.steer_torque_driver = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
self.steer_torque_motor = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
ret.wheelSpeeds.fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
ret.wheelSpeeds.rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
ret.wheelSpeeds.rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
ret.wheelSpeeds.fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
ret.vEgoRaw = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringAngle = cp.vl["STEERING"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING"]['STEERING_RATE']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None))
ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
ret.cruiseState.available = ret.cruiseState.enabled # FIXME: for now same as enabled
ret.cruiseState.speed = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH'] * CV.KPH_TO_MS
ret.steeringTorque = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"]
self.steer_error = steer_state == 4 or (steer_state == 0 and self.v_ego > self.CP.minSteerSpeed)
self.user_brake = 0
self.brake_lights = self.brake_pressed
self.v_cruise_pcm = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH']
self.pcm_acc_status = self.main_on
self.steer_error = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
self.generic_toggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]['COUNTER']
self.lkas_car_model = cp_cam.vl["LKAS_HUD"]['CAR_MODEL']
self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK']
return ret

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

@ -20,6 +20,8 @@ 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)
@ -113,78 +115,33 @@ 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
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed
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.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_status # same as main_on
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = self.CS.main_on
ret.cruiseState.speedOffset = 0.
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
if ret.leftBlinker != self.left_blinker_prev:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
be.pressed = ret.leftBlinker != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
if ret.rightBlinker != self.right_blinker_prev:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
be.pressed = ret.rightBlinker != 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
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
ret.genericToggle = self.CS.generic_toggle
#ret.lkasCounter = self.CS.lkas_counter
#ret.lkasCarModel = self.CS.lkas_car_model
# events
events = []
if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)):
@ -195,7 +152,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]))
@ -220,8 +177,13 @@ 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()
return ret.as_reader()
return self.CS.out
# pass in a car.CarControl
# to be called @ 100hz

2
selfdrive/test/process_replay/ref_commit
Unescape View File

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