def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params, plan, path_plan): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events CS = CI.update(CC) events = list(CS.events) enabled = isEnabled(state) # Receive from sockets td = None cal = None hh = None dm = None for socket, event in poller.poll(0): if socket is thermal: td = messaging.recv_one(socket) elif socket is calibration: cal = messaging.recv_one(socket) elif socket is health: hh = messaging.recv_one(socket) elif socket is driver_monitor: dm = messaging.recv_one(socket) elif socket is plan_sock: plan = messaging.recv_one(socket) elif socket is path_plan_sock: path_plan = messaging.recv_one(socket) if td is not None: overtemp = td.thermal.thermalStatus >= ThermalStatus.red free_space = td.thermal.freeSpace < 0.07 # under 7% of space free no enable allowed low_battery = td.thermal.batteryPercent < 1 # at zero percent battery, OP should not be allowed # Create events for battery, temperature and disk space if low_battery: events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if overtemp: events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) # Handle calibration if cal is not None: cal_status = cal.liveCalibration.calStatus cal_perc = cal.liveCalibration.calPerc if cal_status != Calibration.CALIBRATED: if cal_status == Calibration.UNCALIBRATED: events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) else: events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket than the CAN messages, therefore one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not enabled: mismatch_counter = 0 if hh is not None: controls_allowed = hh.health.controlsAllowed if not controls_allowed and enabled: mismatch_counter += 1 if mismatch_counter >= 2: events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) # Driver monitoring if dm is not None: driver_status.get_pose(dm.driverMonitoring, params) return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan
def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp) # create message ret = car.CarState.new_message() # 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.car_gas 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.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) ret.cruiseState.speedOffset = 0. if self.CP.carFingerprint == CAR.RAV4H: # ignore standstill in hybrid rav4, since pcm allows to restart without # receiving any special command ret.cruiseState.standstill = False else: ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 # TODO: button presses buttonEvents = [] if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = '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 = '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 # events events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if not ret.gearShifter == 'drive' and self.CP.enableDsu: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled and self.CP.enableDsu: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on and self.CP.enableDsu: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse' and self.CP.enableDsu: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steer_error: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.low_speed_lockout and self.CP.enableDsu: events.append( create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if c.actuators.gas > 0.1: # some margin on the actuator to not false trigger cancellation while stopping events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) if ret.vEgo < 0.001: # while in standstill, send a user alert events.append(create_event('manualRestart', [ET.WARNING])) # enable request in prius is simple, as we activate when Toyota is active (rising edge) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader()
def controlsd_thread(sm=None, pm=None, can_sock=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() is_metric = params.get("IsMetric", encoding='utf8') == "1" is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" passive = params.get("Passive", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" passive = passive or not openpilot_enabled_toggle # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock('can', timeout=can_timeout) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType has_relay = hw_type in [HwType.blackPanda, HwType.uno] print("Waiting for CAN messages...") messaging.get_one_can(can_sock) CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay) car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive community_feature_disallowed = CP.communityFeature and not community_feature_toggle read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed if read_only: CP.safetyModel = car.CarParams.SafetyModel.noOutput # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() startup_alert = get_startup_alert(car_recognized, controller_available) AM.add(sm.frame, startup_alert, False) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) if CP.lateralTuning.which() == 'pid': LaC = LatControlPID(CP) elif CP.lateralTuning.which() == 'indi': LaC = LatControlINDI(CP) elif CP.lateralTuning.which() == 'lqr': LaC = LatControlLQR(CP) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd = bool(int(is_rhd)) state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 mismatch_counter = 0 last_blinker_frame = 0 events_prev = [] sm['liveCalibration'].calStatus = Calibration.INVALID sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True sm['thermal'].freeSpace = 1. # detect sound card presence sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None prof = Profiler(False) # off by default while True: start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_perc, mismatch_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") # Create alerts if not sm.all_alive_and_valid(): events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['pathPlan'].mpcSolutionValid: events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid: events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) if not sm['pathPlan'].posenetValid: events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING])) if not sm['plan'].radarValid: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sounds_available: events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) if internet_needed: events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) if community_feature_disallowed: events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT])) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled) prof.checkpoint("Sent") rk.monitor_time() prof.display()
def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) self.ch_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.pt_cp, self.ch_cp) # create message ret = car.CarState.new_message() # 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 ret.brakeLights = self.CS.frictionBrakesActive # steering wheel ret.steeringAngle = self.CS.angle_steers + self.angleSteersoffset # 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 # 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 = False 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 ret.readdistancelines = self.CS.follow_level ret.genericToggle = False ret.laneDepartureToggle = False ret.distanceToggle = self.CS.follow_level ret.accSlowToggle = False ret.blindspot = self.CS.blind_spot_on buttonEvents = [] # blinkers if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = '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 = 'rightBlinker' be.pressed = self.CS.right_blinker_on buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (cruiseEnabled and self.CS.standstill): be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' if not cruiseEnabled and not self.CS.lkMode: self.CS.lkMode = True elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) ret.buttonEvents = buttonEvents if self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button: if self.CS.lkMode: self.CS.lkMode = False else: self.CS.lkMode = True if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button: self.CS.follow_level -= 1 if self.CS.follow_level < 1: self.CS.follow_level = 3 kegman.save({'lastTrMode': int(self.CS.follow_level) }) # write last distance bar setting to file ret.gasbuttonstatus = self.CS.gasMode events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if ret.cruiseState.enabled and not self.cruise_enabled_prev: disengage_event = True else: disengage_event = False 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 and disengage_event: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched and disengage_event: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.car_fingerprint in SUPERCRUISE_CARS: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: 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: 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: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.gear_shifter == 3: 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])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == "cancel" and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled # cast to reader so it can't be modified return ret.as_reader()
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.canValid = True #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.car_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.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 # events events = [] # if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera: # events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT])) if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: 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: 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])) if self.CS.steer_error: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: events.append( create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if c.actuators.gas > 0.1: # some margin on the actuator to not false trigger cancellation while stopping events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) if ret.vEgo < 0.001: # while in standstill, send a user alert events.append(create_event('manualRestart', [ET.WARNING])) # enable request in prius is simple, as we activate when Toyota is active (rising edge) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader()
def update(self, c, can_strings): # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp2.update_strings(can_strings) self.cp_cam.update_strings(can_strings) self.CS.update(self.cp, self.cp2, self.cp_cam) # create message ret = car.CarState.new_message() ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.canValid = self.cp.can_valid # TODO: check cp_cam validity ret.canValid = True # 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 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 # most HKG cars has no long control, it is safer and easier to engage by main on ret.cruiseState.enabled = ( self.CS.pcm_acc_status != 0) if self.CC.longcontrol else bool( self.CS.main_on) 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 # Optima only has blinker flash signal if self.CP.carFingerprint in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: self.CS.left_blinker_on = self.CS.left_blinker_flash or self.CS.prev_left_blinker_on and self.CC.turning_signal_timer self.CS.right_blinker_on = self.CS.right_blinker_flash or self.CS.prev_right_blinker_on and self.CC.turning_signal_timer ret.lcaLeft = self.CS.lca_left != 0 ret.lcaRight = self.CS.lca_right != 0 # 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 = self.CS.seatbelt ret.seatbeltUnlatched = False # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 0.2) and self.CP.minSteerSpeed > 10.: self.low_speed_alert = True if ret.vEgo > (self.CP.minSteerSpeed + 0.7): self.low_speed_alert = False # turning indicator alert logic self.turning_indicator_alert = True if self.CC.turning_signal_timer and self.CS.v_ego < 16.7 else False # LKAS button alert logic self.lkas_button_alert = True if not self.CC.lkas_button else False events = [] if not ret.gearShifter == GearShifter.drive: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: 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: 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.USER_DISABLE])) if self.CS.steer_error or abs(self.CS.angle_steers) > 90.: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if ((ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1))) and self.CC.longcontrol: events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed and self.CC.longcontrol: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if self.low_speed_alert and not self.CS.mdps_bus: events.append(create_event('belowSteerSpeed', [ET.WARNING])) if self.turning_indicator_alert: events.append(create_event('turningIndicatorOn', [ET.WARNING])) if self.lkas_button_alert: events.append(create_event('lkasButtonOff', [ET.WARNING])) #TODO Varible for min Speed for LCA if ret.rightBlinker and ret.lcaRight and self.CS.v_ego > ( 45 * CV.MPH_TO_MS): events.append(create_event('rightLCAbsm', [ET.WARNING])) if ret.leftBlinker and ret.lcaLeft and self.CS.v_ego > (45 * CV.MPH_TO_MS): events.append(create_event('leftLCAbsm', [ET.WARNING])) ret.events = events self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader()
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.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.car_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 # 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: 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 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 if ret.cruiseState.enabled and not self.cruise_enabled_prev: disengage_event = True else: disengage_event = False # events events = [] if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)): events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen and disengage_event: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched and disengage_event: 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: 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])) if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC # from a 3+ second stop. #if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0): # events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.low_speed_alert: events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader()
def update(self, c, can_strings): ret_arne182 = arne182.CarStateArne182.new_message() self.cp.update_strings(can_strings) ret = self.CS.update(self.cp) ret.canValid = self.cp.can_valid 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 buttonEvents = [] 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 if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: 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 elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) ret.buttonEvents = buttonEvents events, eventsArne182 = self.create_common_events(ret, pcm_enable=False) if ret.brakePressed: events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in [ButtonType.accelCruise, ButtonType.decelCruise ] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == ButtonType.cancel and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events ret_arne182.events = eventsArne182 # copy back carState packet to CS self.CS.out = ret.as_reader() return self.CS.out, ret_arne182.as_reader()
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.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 # cruise state ret.cruiseState.available = bool(self.CS.main_on) cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF if not self.cruise_enabled_prev: ret.cruiseState.enabled = cruiseEnabled else: ret.cruiseState.enabled = bool(self.CS.main_on) if not cruiseEnabled: ret.brakePressed = True 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: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (cruiseEnabled and self.CS.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 elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) ret.buttonEvents = buttonEvents if ret.cruiseState.enabled and not self.cruise_enabled_prev: disengage_event = True else: disengage_event = False 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 and disengage_event: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched and disengage_event: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.car_fingerprint in SUPERCRUISE_CARS: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: 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: 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: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.gear_shifter == 3: 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])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if ((ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed)) and disengage_event: # and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed and disengage_event: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in [ButtonType.accelCruise, ButtonType.decelCruise ] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == ButtonType.cancel and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled # cast to reader so it can't be modified return ret.as_reader()
def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = None l20 = None gps_planner_plan = None for socket, event in self.poller.poll(0): if socket is self.model: md = messaging.recv_one(socket) elif socket is self.live20: l20 = messaging.recv_one(socket) elif socket is self.gps_planner_plan: gps_planner_plan = messaging.recv_one(socket) if gps_planner_plan is not None: self.last_gps_planner_plan = gps_planner_plan if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False self.PP.update(CS.vEgo, md) if self.last_gps_planner_plan is not None: plan = self.last_gps_planner_plan.gpsPlannerPlan self.gps_planner_active = plan.valid if plan.valid: self.PP.d_poly = plan.poly self.PP.p_poly = plan.poly self.PP.c_poly = plan.poly self.PP.l_prob = 0.0 self.PP.r_prob = 0.0 self.PP.c_prob = 1.0 if l20 is not None: self.perception_state = copy(l20.live20) self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time self.lead_1 = l20.live20.leadOne self.lead_2 = l20.live20.leadTwo enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 # Calculate speed for normal cruise control if enabled: accel_limits = map( float, calc_cruise_accel_limits(CS.vEgo, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else CS.vEgo reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.v_acc_sol = reset_speed self.a_acc_sol = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.leftBlinker or CS.rightBlinker self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw, blinkers) \ and not CS.brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) if cur_time - self.last_model > 0.5: self.model_dead = True if cur_time - self.last_l20 > 0.5: self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') events = [] if self.model_dead: events.append( create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.radar_dead or 'commIssue' in self.radar_errors: events.append( create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if LaC.mpc_solution[ 0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge events.append( create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # Interpolation of trajectory dt = min( cur_time - self.acc_start_time, _DT_MPC + _DT ) + _DT # no greater than dt mpc + dt, to prevent too high extraps self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.model_dead plan_send.plan.dPoly = map(float, self.PP.d_poly) plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vTarget = self.v_acc_sol plan_send.plan.aTarget = self.a_acc_sol plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.gpsPlannerActive = self.gps_planner_active # Send out fcw fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) return plan_send
def update(self, c, can_strings): canMonoTimes = [] 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, self.cam_cp) ret.canValid = self.pt_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.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: if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: be = car.CarState.ButtonEvent.new_message() be.type = button be.pressed = self.CS.buttonStates[button] buttonEvents.append(be) # Vehicle operation safety checks and events if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.gearShifter == GEAR.reverse: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not ret.gearShifter in [GEAR.drive, GEAR.eco, GEAR.sport]: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.stabilityControlDisabled: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.parkingBrakeSet: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) # Vehicle health and operation safety checks if self.CS.steeringFault: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) # Per the Comma safety model, disable on pedals rising edge or when brake # is pressed and speed isn't zero. if (ret.gasPressed and not self.gasPressedPrev) or \ (ret.brakePressed and (not self.brakePressedPrev or not ret.standstill)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # Engagement and longitudinal control using stock ACC. Make sure OP is # disengaged if stock ACC is disengaged. if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # Attempt OP engagement only on rising edge of stock ACC engagement. elif not self.cruiseStateEnabledPrev: events.append(create_event('pcmEnable', [ET.ENABLE])) ret.events = events ret.buttonEvents = buttonEvents ret.canMonoTimes = canMonoTimes # update previous car states self.gasPressedPrev = ret.gasPressed self.brakePressedPrev = ret.brakePressed self.cruiseStateEnabledPrev = ret.cruiseState.enabled self.displayMetricUnitsPrev = self.CS.displayMetricUnits self.buttonStatesPrev = self.CS.buttonStates.copy() # cast to reader so it can't be modified return ret.as_reader()
def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.cp_cam.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() # 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 = '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 = '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 # 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.: self.low_speed_alert = True if ret.vEgo > (self.CP.minSteerSpeed + 4.): self.low_speed_alert = False # events events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if not ret.gearShifter == 'drive': events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: 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: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse': events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steer_error: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) # enable request in prius is simple, as we activate when Toyota is active (rising edge) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if self.low_speed_alert: events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events ret.canMonoTimes = canMonoTimes self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader()
def controlsd_thread(sm=None, pm=None, can_sock=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() is_metric = params.get("IsMetric", encoding='utf8') == "1" is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" passive = params.get("Passive", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" passive = passive or not openpilot_enabled_toggle # Passive if internet needed internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None passive = passive or internet_needed # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster([ 'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams' ]) if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \ 'model']) if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock('can', timeout=can_timeout) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType has_relay = hw_type in [HwType.blackPanda, HwType.uno] print("Waiting for CAN messages...") messaging.get_one_can(can_sock) CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay) car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive community_feature_disallowed = CP.communityFeature and not community_feature_toggle read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed if read_only: CP.safetyModel = car.CarParams.SafetyModel.noOutput # Write CarParams for radard and boardd safety mode cp_bytes = CP.to_bytes() params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) put_nonblocking("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() startup_alert = get_startup_alert(car_recognized, controller_available) AM.add(sm.frame, startup_alert, False) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) if CP.lateralTuning.which() == 'pid': LaC = LatControlPID(CP) elif CP.lateralTuning.which() == 'indi': LaC = LatControlINDI(CP) elif CP.lateralTuning.which() == 'lqr': LaC = LatControlLQR(CP) state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 mismatch_counter = 0 can_error_counter = 0 last_blinker_frame = 0 saturated_count = 0 events_prev = [] sm['liveCalibration'].calStatus = Calibration.INVALID sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True sm['thermal'].freeSpace = 1. sm['dMonitoringState'].events = [] sm['dMonitoringState'].awarenessStatus = 1. sm['dMonitoringState'].faceDetected = False # detect sound card presence sounds_available = not os.path.isfile('/EON') or ( os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) prof = Profiler(False) # off by default # dp ts_last_check = 0. dragon_toyota_stock_dsu = False dragon_lat_control = True dragon_display_steering_limit_alert = True dragon_stopped_has_lead_count = 0 dragon_lead_car_moving_alert = False dp_last_modified = None dp_camera_offset = CAMERA_OFFSET hyundai_lkas = read_only while True: start_time = sec_since_boot() # dp ts = start_time if ts - ts_last_check >= 5.: modified = get_last_modified() if dp_last_modified != modified: try: dp_camera_offset = int( params.get("DragonCameraOffset", encoding='utf8')) * 0.01 except (TypeError, ValueError): dp_camera_offset = CAMERA_OFFSET dragon_toyota_stock_dsu = True if params.get( "DragonToyotaStockDSU", encoding='utf8') == "1" else False dragon_lat_control = False if params.get( "DragonLatCtrl", encoding='utf8') == "0" else True if dragon_lat_control: dragon_display_steering_limit_alert = False if params.get( "DragonDisplaySteeringLimitAlert", encoding='utf8') == "0" else True else: dragon_display_steering_limit_alert = False dragon_lead_car_moving_alert = True if params.get( "DragonEnableLeadCarMovingAlert", encoding='utf8') == "1" else False dp_last_modified = modified ts_last_check = ts prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample( CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params, dragon_toyota_stock_dsu) if read_only: hyundai_lkas = read_only elif CS.cruiseState.enabled: # elif state == State.enabled: hyundai_lkas = False prof.checkpoint("Sample") # Create alerts if not sm.alive['plan'] and sm.alive[ 'pathPlan']: # only plan not being received: radar not communicating events.append( create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) elif not sm.all_alive_and_valid(): events.append( create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['pathPlan'].mpcSolutionValid: events.append( create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None: events.append( create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) if not sm['pathPlan'].posenetValid: events.append( create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING])) if not sm['plan'].radarValid: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: events.append( create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: if dragon_toyota_stock_dsu: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) else: events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sounds_available: events.append( create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) if internet_needed: events.append( create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) if community_feature_disallowed: events.append( create_event('communityFeatureDisallowed', [ET.PERMANENT])) if read_only and not passive: events.append(create_event('carUnrecognized', [ET.PERMANENT])) if log.HealthData.FaultType.relayMalfunction in sm['health'].faults: events.append( create_event( 'relayMalfunction', [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE])) # Only allow engagement with brake pressed when stopped behind another stopped car if not dragon_toyota_stock_dsu and CS.brakePressed and sm[ 'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append( create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # dp if dragon_lead_car_moving_alert: # when car has a lead and is standstill and lead is barely moving, we start counting if not CP.radarOffCan and sm[ 'plan'].hasLead and CS.vEgo <= 0.01 and 0.3 >= abs( sm['plan'].vTarget) >= 0: dragon_stopped_has_lead_count += 1 else: dragon_stopped_has_lead_count = 0 # when we detect lead car over 3 secs and the lead car is started moving, we are ready to send alerts # once the condition is triggered, we want to keep the trigger if dragon_stopped_has_lead_count >= 300: if abs(sm['plan'].vTargetFuture) >= 0.1: events.append(create_event('leadCarMoving', [ET.WARNING])) else: events.append(create_event('leadCarDetected', [ET.WARNING])) # we remove alert once our car is moving if CS.vEgo > 0.: dragon_stopped_has_lead_count = 0 if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, LaC, LoC, hyundai_lkas, is_metric, cal_perc, last_blinker_frame, saturated_count, dragon_lat_control, dragon_display_steering_limit_alert, dragon_lead_car_moving_alert) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, LaC, LoC, hyundai_lkas, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled, can_error_counter, dp_camera_offset) prof.checkpoint("Sent") rk.monitor_time() prof.display() if not CS.cruiseState.enabled: # if state == State.disabled: hyundai_lkas = True
def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp) # create message ret = car.CarState.new_message() #2018.09.05 add generic toggle for testing steering max# #ret.genericToggle = self.CS.generic_toggle # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw #2018.09.04 change to vehicle yaw rate values ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ## ret.yawRate = self.CS.yaw_rate 2018.09.10 use yaw calculation from angle 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 ret.gas = self.CS.car_gas #2018.09.13 12:39AM change gas to car_gas without deviding #TODO 2018.09.05 check Throttle report operator override match this 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 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.SOUL else 0.1 #2018.09.03 change CIVIC to soul 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 #2018.09.11 TODO some can duplication error? or not reading # gear shifter lever (#2018.09.04 define in carstate.py ret.gearShifter = self.CS.gear_shifter #2018.09.05 #TODO find steering torque value from steering wheel ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override #2018.09.02 this come values py when steering operator override =1 #2018.09.02 TODO need to check is matter, we not using kia cruise control # cruise state #ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.enabled = False #2018.09.11 if use Openpilot cruise, no need, this should set to 0, need set to False #ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS #2018.09.11 check in controlsd.py if not enable cruise, this not needed ret.cruiseState.speed = 0 # 2018.09.11 check in controlsd.py if not enable cruise, this not needed, set to 0 ret.cruiseState.available = bool(self.CS.main_on) #2018.09.11 not use anywhere else beside interface.py ret.cruiseState.speedOffset = self.CS.cruise_speed_offset #2018.09.11 finally not use in controlsd.py if we use interceptor and enablecruise=false 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 if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = '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 = '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() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: 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: events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == "reverse": events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) #remove brake hold and #electric parking brake if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed 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()
def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.pt_cp) # create message ret = car.CarState.new_message() # 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 # cruise state ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 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 = '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 = 'rightBlinker' be.pressed = self.CS.right_blinker_on buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) ret.buttonEvents = buttonEvents events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 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: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.car_fingerprint == CAR.VOLT: 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: 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: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.gear_shifter == 3: 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])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == "cancel" and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) if self.CS.car_fingerprint == CAR.CADILLAC_CT6: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active 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()
def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_location, poller, cal_status, overtemp, free_space, driver_status, geofence, state, mismatch_counter, params): # *** read can and compute car states *** CS = CI.update(CC) events = list(CS.events) enabled = isEnabled(state) td = None cal = None hh = None dm = None gps = None for socket, event in poller.poll(0): if socket is thermal: td = messaging.recv_one(socket) elif socket is calibration: cal = messaging.recv_one(socket) elif socket is health: hh = messaging.recv_one(socket) elif socket is driver_monitor: dm = messaging.recv_one(socket) elif socket is gps_location: gps = messaging.recv_one(socket) # *** thermal checking logic *** # thermal data, checked every second if td is not None: overtemp = td.thermal.thermalStatus >= ThermalStatus.red # under 15% of space free no enable allowed free_space = td.thermal.freeSpace < 0.15 if overtemp: events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) # *** read calibration status *** if cal is not None: cal_status = cal.liveCalibration.calStatus if cal_status != Calibration.CALIBRATED: if cal_status == Calibration.UNCALIBRATED: events.append( create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE])) else: events.append( create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not enabled: mismatch_counter = 0 # *** health checking logic *** if hh is not None: controls_allowed = hh.health.controlsAllowed if not controls_allowed and enabled: mismatch_counter += 1 if mismatch_counter >= 2: events.append( create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) if dm is not None: driver_status.get_pose(dm.driverMonitoring, params) if geofence is not None and gps is not None: geofence.update_geofence_status(gps.gpsLocationExternal, params) if geofence is not None and not geofence.in_geofence: events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING])) return CS, events, cal_status, overtemp, free_space, mismatch_counter
def update(self, c, can_strings): self.dp_load_params('gm') self.cp.update_strings(can_strings) ret = self.CS.update(self.cp) ret.canValid = self.cp.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False ret.cruiseState.available = self.CS.main_on #ret.cruiseState.enabled = self.CS.main_on if not self.CS.regen_pressed else False ret.cruiseState.enabled = self.CS.main_on buttonEvents = [] 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 if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: 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 elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) ret.buttonEvents = buttonEvents events = self.create_common_events(ret) if not ret.cruiseState.available: events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.regen_pressed: events.append(create_event('manualSteeringRequired', [ET.WARNING])) if ret.cruiseState.enabled and not self.cruise_enable_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) # handle button presses for b in ret.buttonEvents: if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events self.cruise_enable_prev = ret.cruiseState.enabled # copy back carState packet to CS self.CS.out = ret.as_reader() return self.CS.out
def update(self, c, can_strings): 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.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.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 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])) 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: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on gas pedal rising edge if (ret.gasPressed and not self.gas_pressed_prev): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.acc_active_prev = self.CS.acc_active # cast to reader so it can't be modified return ret.as_reader()
def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw 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.enableGas: 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 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.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.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) 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) if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = '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 = '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() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if self.CS.steer_error: events.append( create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) elif self.CS.steer_not_allowed: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: events.append( create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not ret.gearShifter == 'drive': events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.door_all_closed: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.seatbelt: 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: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse': events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_hold: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if not self.CS.civic and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: print "enabled pressed at", cur_time self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed 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()
def update(self, c, can_strings): self.cp.update_strings(can_strings) self.cp2.update_strings(can_strings) self.cp_cam.update_strings(can_strings) ret = self.CS.update(self.cp, self.cp2, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp2.can_valid and self.cp_cam.can_valid # most HKG cars has no long control, it is safer and easier to engage by main on ret.cruiseState.enabled = ret.cruiseState.available if not self.CC.longcontrol else ret.cruiseState.enabled # some Optima only has blinker flash signal if self.CP.carFingerprint == CAR.KIA_OPTIMA: ret.leftBlinker = self.CS.left_blinker_flash or self.CS.prev_left_blinker and self.CC.turning_signal_timer ret.rightBlinker = self.CS.right_blinker_flash or self.CS.prev_right_blinker and self.CC.turning_signal_timer # turning indicator alert logic if (ret.leftBlinker or ret.rightBlinker or self.CC.turning_signal_timer): self.turning_indicator_alert = True else: self.turning_indicator_alert = False # LKAS button alert logic: reverse on/off #if not self.CS.lkas_error and self.CS.lkas_button_on != self.CS.prev_lkas_button_on: #self.CC.lkas_button_on = not self.CC.lkas_button_on #self.lkas_button_alert = not self.CC.lkas_button_on # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 0.2) and self.CP.minSteerSpeed > 10.: self.low_speed_alert = True if ret.vEgo > (self.CP.minSteerSpeed + 0.7): self.low_speed_alert = False ret.buttonEvents = [] events = [] if not ret.gearShifter == GearShifter.drive: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.espDisabled: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) 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.USER_DISABLE])) #if ret.steerWarning or abs(ret.steeringAngle) > 120.: # events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if ((ret.gasPressed and not self.CS.out.gasPressed) or \ (ret.brakePressed and (not self.CS.out.brakePressed or ret.vEgoRaw > 0.1))) and self.CC.longcontrol: events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed and self.CC.longcontrol: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if self.low_speed_alert and not self.CS.mdps_bus: events.append(create_event('belowSteerSpeed', [ET.WARNING])) if self.turning_indicator_alert: events.append(create_event('turningIndicatorOn', [ET.WARNING])) if self.lkas_button_alert: events.append(create_event('lkasButtonOff', [ET.WARNING])) #TODO Varible for min Speed for LCA if ret.rightBlinker and ret.rightBlindspot and ret.vEgo > ( 45 * CV.MPH_TO_MS): events.append(create_event('rightLCAbsm', [ET.WARNING])) if ret.leftBlinker and ret.leftBlindspot and ret.vEgo > (45 * CV.MPH_TO_MS): events.append(create_event('leftLCAbsm', [ET.WARNING])) ret.events = events self.CS.out = ret.as_reader() return self.CS.out
def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data): """Gets called when new live20 is available""" cur_time = live20.logMonoTime / 1e9 v_ego = CS.carState.vEgo long_control_state = live100.live100.longControlState v_cruise_kph = live100.live100.vCruise force_slow_decel = live100.live100.forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS self.last_md_ts = md.logMonoTime self.radar_errors = list(live20.live20.radarErrors) self.lead_1 = live20.live20.leadOne self.lead_2 = live20.live20.leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 self.v_speedlimit = NO_CURVATURE_SPEED self.v_curvature = NO_CURVATURE_SPEED self.map_valid = live_map_data.liveMapData.mapValid # Speed limit and curvature set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active: if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit offset = float(self.params.get("SpeedLimitOffset")) self.v_speedlimit = speed_limit + offset if live_map_data.liveMapData.curvatureValid: curvature = abs(live_map_data.liveMapData.curvature) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = math.sqrt(a_y_max / max(1e-4, curvature)) self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature) self.decel_for_turn = bool(self.v_curvature < min( [v_cruise_setpoint, self.v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min( [v_cruise_setpoint, self.v_curvature, self.v_speedlimit]) # Calculate speed for normal cruise control if enabled: accel_limits = map(float, calc_cruise_accel_limits(v_ego, following)) jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits = limit_accel_in_turns(v_ego, CS.carState.steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) # Change accel limits based on time remaining to turn if self.decel_for_turn: time_to_turn = max( 1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(CS.carState.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker self.fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw, blinkers) and not CS.carState.brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5 # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') # TODO: Move all these events to controlsd. This has nothing to do with planning events = [] if model_dead: events.append( create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) plan_send.plan.events = events plan_send.plan.mdMonoTime = md.logMonoTime plan_send.plan.l20MonoTime = live20.logMonoTime # longitudal plan plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = self.v_curvature plan_send.plan.decelForTurn = self.decel_for_turn plan_send.plan.mapValid = self.map_valid # Send out fcw fcw = self.fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration dt = 0.05 # s a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + dt * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.cp_cam.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() # 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.car_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 # 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. # ignore standstill in hybrid rav4, since pcm allows to restart without # receiving any special command ret.cruiseState.standstill = False ret.readdistancelines = 1 ret.genericToggle = False ret.laneDepartureToggle = False ret.distanceToggle = 1 ret.accSlowToggle = False ret.blindspot = 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 = '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 = '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 self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed) ret.genericToggle = self.CS.generic_toggle if ret.cruiseState.enabled and not self.cruise_enabled_prev: disengage_event = True else: disengage_event = False ret.gasbuttonstatus = self.CS.gasMode # events events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if not (ret.gearShifter in ('drive', 'low')): events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen and disengage_event: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched and disengage_event: 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: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse': events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC # from a 3+ second stop. if self.CS.cstm_btns.get_button_status("mad") == 0: if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.low_speed_alert: events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events ret.canMonoTimes = canMonoTimes self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader()
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.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 # 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() be.type = ButtonType.unknown if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = ButtonType.accelCruise elif but == CruiseButtons.DECEL_SET: be.type = ButtonType.decelCruise elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = ButtonType.altButton1 # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events events = [] if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == GearShifter.drive: events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: 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: 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])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = self.frame * DT_CTRL enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enable if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events # update previous brake/gas pressed 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()
def update(self, c, can_strings): # ******************* do can recv ******************* self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.cp) # create message ret = car.CarState.new_message() ret.canValid = self.cp.can_valid # speeds ret.vEgo = self.CS.v_ego ret.vEgoRaw = self.CS.v_ego_raw 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 ret.steeringPressed = self.CS.steer_override # gas pedal ret.gas = self.CS.user_gas / 100. ret.gasPressed = self.CS.user_gas > 0.0001 ret.brakePressed = self.CS.brake_pressed ret.brakeLights = self.CS.brake_lights ret.cruiseState.enabled = not (self.CS.pcm_acc_status in [0, 3]) ret.cruiseState.speed = self.CS.v_cruise_pcm ret.cruiseState.available = self.CS.pcm_acc_status != 0 ret.genericToggle = self.CS.generic_toggle # events events = [] if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) # enable request in prius is simple, as we activate when Toyota is active (rising edge) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if self.CS.lkas_state not in [ 2, 3 ] and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled: events.append( create_event('steerTempUnavailableMute', [ET.WARNING])) ret.events = events self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled return ret.as_reader()
def update(self, c, can_strings): self.pt_cp.update_strings(can_strings) self.ch_cp.update_strings(can_strings) self.CS.update(self.pt_cp, self.ch_cp) # create message ret = car.CarState.new_message() 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 ret.brakeLights = self.CS.frictionBrakesActive # 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 # 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 = False 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 ret.readdistancelines = self.CS.follow_level 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 if self.CS.cruise_buttons != CruiseButtons.UNPRESS: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: if not (cruiseEnabled and self.CS.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: if not cruiseEnabled and not self.CS.lkMode: self.lkMode = True be.type = ButtonType.decelCruise elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) ret.buttonEvents = buttonEvents if cruiseEnabled and self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button: self.CS.lkMode = not self.CS.lkMode if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button: self.CS.follow_level -= 1 if self.CS.follow_level < 1: self.CS.follow_level = 3 events = [] if not self.CS.lkMode: events.append(create_event('manualSteeringRequired', [ET.WARNING])) #if cruiseEnabled and (self.CS.left_blinker_on or self.CS.right_blinker_on): # events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) 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: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.car_fingerprint in SUPERCRUISE_CARS: if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: 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: 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: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.gear_shifter == 3: 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])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: events.append( create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons # The ECM will fault if resume triggers an enable while speed is set to 0 if b.type == ButtonType.accelCruise and c.hudControl.setSpeed > 0 and c.hudControl.setSpeed < 70 and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) if b.type == ButtonType.decelCruise and not b.pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) # do disable on button down if b.type == ButtonType.cancel and b.pressed: events.append( create_event('buttonCancel', [ET.USER_DISABLE])) # The ECM independently tracks a ‘speed is set’ state that is reset on main off. # To keep controlsd in sync with the ECM state, generate a RESET_V_CRUISE event on main cruise presses. if b.type == ButtonType.altButton3 and b.pressed: events.append( create_event('buttonCancel', [ET.RESET_V_CRUISE, ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active 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()
def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) sm.update(0) events = list(CS.events) events += list(sm['dMonitoringState'].events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) lane_change_bsm = sm['pathPlan'].laneChangeBSM # Check for CAN timeout if not can_strs: can_error_counter += 1 events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red free_space = sm[ 'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed low_battery = sm['thermal'].batteryPercent < 1 and sm[ 'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed mem_low = sm['thermal'].memUsedPercent > 90 #bsm alerts if lane_change_bsm == LaneChangeBSM.left: events.append(create_event('preventLCA', [ET.WARNING])) if lane_change_bsm == LaneChangeBSM.right: events.append(create_event('preventLCA', [ET.WARNING])) # Create events for battery, temperature and disk space if low_battery: events.append( create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if overtemp: events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) if mem_low: events.append( create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) if CS.stockAeb: events.append(create_event('stockAeb', [])) # Handle calibration cal_status = sm['liveCalibration'].calStatus cal_perc = sm['liveCalibration'].calPerc if cal_status != Calibration.CALIBRATED: if cal_status == Calibration.UNCALIBRATED: events.append( create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) else: events.append( create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if CS.vEgo > 92 * CV.MPH_TO_MS: events.append( create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not enabled: mismatch_counter = 0 controls_allowed = sm['health'].controlsAllowed if not controls_allowed and enabled: mismatch_counter += 1 if mismatch_counter >= 200: events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) return CS, events, cal_perc, mismatch_counter, can_error_counter
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() CC.enabled = isEnabled(state) CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled) # Some override values for Honda brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = sm['plan'].hasLead right_lane_visible = sm['pathPlan'].rProb > 0.5 left_lane_visible = sm['pathPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state) md = sm['model'] if len(md.meta.desirePrediction): l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) if ldw_allowed: CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: AM.add(sm.frame, 'ldwPermanent', False) events.append(create_event('ldw', [ET.PERMANENT])) AM.process_alerts(sm.frame) CC.hudControl.visualAlert = AM.visual_alert if not read_only: # send car controls over can can_sends = CI.apply(CC) pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = driver_status.awareness < 0. # controlsState dat = messaging.new_message() dat.init('controlsState') dat.valid = CS.canValid dat.controlsState = { "alertText1": AM.alert_text_1, "alertText2": AM.alert_text_2, "alertSize": AM.alert_size, "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "alertType": AM.alert_type, "alertSound": AM.audible_alert, "awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0, "driverMonitoringOn": bool(driver_status.face_detected), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": sm.logMonoTime['plan'], "pathPlanMonoTime": sm.logMonoTime['pathPlan'], "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), "longControlState": LoC.long_control_state, "vPid": float(LoC.v_pid), "vCruise": float(v_cruise_kph), "upAccelCmd": float(LoC.pid.p), "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), "vTargetLead": float(v_acc), "aTarget": float(a_acc), "jerkFactor": float(sm['plan'].jerkFactor), "gpsPlannerActive": sm['plan'].gpsPlannerActive, "vCurvature": sm['plan'].vCurvature, "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, "cumLagMs": -rk.remaining * 1000., "startMonoTime": int(start_time * 1e9), "mapValid": sm['plan'].mapValid, "forceDecel": bool(force_decel), } if CP.lateralTuning.which() == 'pid': dat.controlsState.lateralControlState.pidState = lac_log elif CP.lateralTuning.which() == 'lqr': dat.controlsState.lateralControlState.lqrState = lac_log elif CP.lateralTuning.which() == 'indi': dat.controlsState.lateralControlState.indiState = lac_log pm.send('controlsState', dat) # carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = events pm.send('carState', cs_send) # carEvents - logged every second or on change events_bytes = events_to_bytes(events) if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): ce_send = messaging.new_message() ce_send.init('carEvents', len(events)) ce_send.carEvents = events pm.send('carEvents', ce_send) # carParams - logged every 50 seconds (> 1 per segment) if (sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message() cp_send.init('carParams') cp_send.carParams = CP pm.send('carParams', cp_send) # carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC pm.send('carControl', cc_send) return CC, events_bytes
def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.pt_cp) # create message ret = car.CarState.new_message() # 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 # cruise state ret.cruiseState.available = bool(self.CS.main_on) ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.seatbeltUnlatched = self.CS.seatbelt_unlatched buttonEvents = [] # blinkers if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = '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 = 'rightBlinker' be.pressed = self.CS.right_blinker_on buttonEvents.append(be) be = car.CarState.ButtonEvent.new_message() be.type = 'accelCruise' buttonEvents.append(be) events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) ## handle button presses #for b in ret.buttonEvents: # # do enable on both accel and decel buttons # if b.type in ["accelCruise", "decelCruise"] and not b.pressed: # events.append(create_event('buttonEnable', [ET.ENABLE])) # # do disable on button down # if b.type == "cancel" and b.pressed: # events.append(create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active # cast to reader so it can't be modified return ret.as_reader()
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) sm.update(0) events = list(CS.events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) # Check for CAN timeout if not can_strs: events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red free_space = sm['thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed mem_low = sm['thermal'].memUsedPercent > 90 # Create events for battery, temperature and disk space if low_battery: events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if overtemp: events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) if mem_low: events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) if CS.stockAeb: events.append(create_event('stockAeb', [])) # GPS coords RHD parsing, once every restart if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked: is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude) driver_status.is_rhd_region = is_rhd driver_status.is_rhd_region_checked = True put_nonblocking("IsRHD", "1" if is_rhd else "0") # Handle calibration cal_status = sm['liveCalibration'].calStatus cal_perc = sm['liveCalibration'].calPerc cal_rpy = [0,0,0] #if cal_status != Calibration.CALIBRATED: # if cal_status == Calibration.UNCALIBRATED: # events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) # else: # events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) #else: # rpy = sm['liveCalibration'].rpyCalib # if len(rpy) == 3: # cal_rpy = rpy # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not enabled: mismatch_counter = 0 controls_allowed = sm['health'].controlsAllowed if not controls_allowed and enabled: mismatch_counter += 1 if mismatch_counter >= 200: events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) # Driver monitoring if sm.updated['model']: driver_status.set_policy(sm['model']) if sm.updated['driverMonitoring']: driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled) if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) return CS, events, cal_perc, mismatch_counter
def controlsd_thread(gctx=None, rate=100): gc.disable() # start the loop set_realtime_priority(3) context = zmq.Context() params = Params() # Pub Sockets live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" # No sendcan if passive if not passive: sendcan = messaging.pub_sock(context, service_list['sendcan'].port) else: sendcan = None # Sub sockets poller = zmq.Poller() thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller) health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller) plan_sock = messaging.sub_sock(context, service_list['plan'].port, conflate=True, poller=poller) path_plan_sock = messaging.sub_sock(context, service_list['pathPlan'].port, conflate=True, poller=poller) logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) if CI is None: raise Exception("unsupported car") # if stock camera is connected, then force passive behavior if not CP.enableCamera: passive = True sendcan = None if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(CP) AM = AlertManager() driver_status = DriverStatus() if not passive: AM.add("startup", False) # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 overtemp = False free_space = False cal_status = Calibration.INVALID cal_perc = 0 mismatch_counter = 0 low_battery = False plan = messaging.new_message() plan.init('plan') path_plan = messaging.new_message() path_plan.init('pathPlan') rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) controls_params = params.get("ControlsParams") # Read angle offset from previous drive if controls_params is not None: controls_params = json.loads(controls_params) angle_offset = controls_params['angle_offset'] else: angle_offset = 0. prof = Profiler(False) # off by default while True: start_time = int(sec_since_boot() * 1e9) prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\ data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params, plan, path_plan) prof.checkpoint("Sample") path_plan_age = (start_time - path_plan.logMonoTime) / 1e9 plan_age = (start_time - plan.logMonoTime) / 1e9 if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5: events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events += list(plan.plan.events) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not passive: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \ state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc) prof.checkpoint("State Control") # Publish data CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc) prof.checkpoint("Sent") rk.keep_time() # Run at 100Hz prof.display()