From 0c0dd4c7435d3bccf88953e33ea741c17ffd65a2 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Mon, 17 Feb 2020 13:35:51 -0800 Subject: [PATCH] Chrysler: carstate returns capnp struct directly (#1110) * 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 old-commit-hash: 703eb511da31d80e93d1f009d6d8de76c863f623 --- selfdrive/car/chrysler/carcontroller.py | 10 ++-- selfdrive/car/chrysler/carstate.py | 75 ++++++++++++------------ selfdrive/car/chrysler/interface.py | 68 +++++---------------- selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 60 insertions(+), 95 deletions(-) diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 2bc4e9680f..f6b55b9e19 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -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 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index bbfbdbac43..bab58b9637 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,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 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 468acea936..93220fd124 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7418267c78..1414556bff 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -bc89e6f25e88a904ad905296d516aaebb77e2207 \ No newline at end of file +917e6889be1691fb96e7566a92e0c6bbefc861a4 \ No newline at end of file