From 82d32b948eb36e4e86daf607b76d8f47ae690cc7 Mon Sep 17 00:00:00 2001 From: Riccardo Biasini Date: Sun, 16 Feb 2020 10:12:13 -0800 Subject: [PATCH] Chrysler: carstate returns capnp struct directly --- selfdrive/car/chrysler/carcontroller.py | 10 ++-- selfdrive/car/chrysler/carstate.py | 76 +++++++++++++----------- selfdrive/car/chrysler/interface.py | 70 ++++++---------------- selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 63 insertions(+), 95 deletions(-) diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 2bc4e9680f3530..f6b55b9e19412a 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/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 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index bbfbdbac4392c4..3efedb248f8cc7 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/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 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 468acea9361fe7..ae433752710eac 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7418267c78a8b1..ade30ce6374b67 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -bc89e6f25e88a904ad905296d516aaebb77e2207 \ No newline at end of file +3f9473e0a7d25ecca0f24bea48f5412ce5070fdb \ No newline at end of file