diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 93220fd124..b51cf28213 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -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() diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 53eccf1f38..a502bc15a6 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -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) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f924c66f17..358464efc2 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -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 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 450fce8538..42cb6f940f 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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, diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 9b0e0706ce..4b5354c47c 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f7b8d57790..d7a010fea3 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index b055bc5592..92224ff287 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0a6144bab6..9f368d329f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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)) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 5305396f30..5efa38188d 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -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 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 2db3cd45e0..695a0d0686 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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): diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e98b2681b7..bd6625ca89 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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]]) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index dce4df810b..bddef6468e 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -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 diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 10500ce6fa..e68d586b49 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -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 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index d4388727bb..dc692ee9f2 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -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, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 24c3bfed75..a5f86cf0ea 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index cb172b591c..3c05f657af 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index ff397e4429..df7eb2c52f 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 610e1d5a09..1691d2f697 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -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. diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 79cd063106..0b9065b989 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -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 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 0e8b5206d3..5b40730e11 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -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, diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 045e03ae33..970e33fed5 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -19,8 +19,6 @@ class CarControllerParams: STEER_DRIVER_FACTOR = 1 # from dbc BUTTON_STATES = { - "leftBlinker": False, - "rightBlinker": False, "accelCruise": False, "decelCruise": False, "cancel": False, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1414556bff..0bd156c6e0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -917e6889be1691fb96e7566a92e0c6bbefc861a4 \ No newline at end of file +d0e2657d3a57ba231e3775d3bc901221d8794281 \ No newline at end of file