Chrysler: carstate returns capnp struct directly

selfdrive/car/chrysler/carcontroller.py

@@ -35,14 +35,14 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     # 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 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     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

selfdrive/car/chrysler/carstate.py

@@ -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,54 @@ def __init__(self, CP):
 
   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
-    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)
+    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.
 
-    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
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+    ret.standstill = not ret.vEgoRaw > 0.001
 
-    self.generic_toggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
+    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 ret.vEgo > self.CP.minSteerSpeed)
+
+    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

selfdrive/car/chrysler/interface.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import copy
 from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
@@ -20,6 +21,8 @@ def __init__(self, CP, CarController):
     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 +116,34 @@ def update(self, c, can_strings):
     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.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
+    ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
 
-    # 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
+    ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else 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 +154,7 @@ def update(self, c, can_strings):
       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 +179,13 @@ def update(self, c, can_strings):
     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 copy.copy(self.CS.out)
 
   # pass in a car.CarControl
   # to be called @ 100hz

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-bc89e6f25e88a904ad905296d516aaebb77e2207
+3f9473e0a7d25ecca0f24bea48f5412ce5070fdb