def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): """ Controls thread """ if not self.enable_camera: return # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep is not 0 else snd_chime #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024 / 4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) else: STEER_MAX = 0x1000 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int( clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5 lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame / 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) elif CS.stopped: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = (frame / 2) % 4 can_sends.extend( hondacan.create_brake_command(self.packer, apply_brake, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, CS.CP.carFingerprint, idx)) if CS.CP.enableGasInterceptor: # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling can_sends.append( hondacan.create_gas_command(self.packer, apply_gas, idx)) # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) if CS.CP.carFingerprint == CAR.ACURA_ILX: radar_send_step = 2 else: radar_send_step = 5 if (frame % radar_send_step) == 0: idx = (frame / radar_send_step) % 4 if not self.new_radar_config: # only change state once self.new_radar_config = car.RadarState.Error.wrongConfig in radar_error can_sends.extend( hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, self.new_radar_config, idx)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime): """ Controls thread """ # *** apply brake hysteresis *** final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic) # *** no output if not enabled *** if not enabled: final_gas = 0. final_brake = 0. final_steer = 0. # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated if CS.pcm_acc_status: pcm_cancel_cmd = True # *** rate limit after the enable check *** final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100) self.final_brake_last = final_brake # vehicle hud display, wait for one update from 10Hz 0x304 msg #TODO: use enum!! if hud_show_lanes: hud_lanes = 0x04 else: hud_lanes = 0x00 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 0xe0 else: hud_car = 0xd0 else: hud_car = 0xc0 #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(hud_v_cruise), 0x41, hud_car, 0xc1, 0x41, hud_lanes + steer_required, int(snd_beep), 0x48, (snd_chime << 5) + fcw_display, acc_alert) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print "INVALID HUD", hud hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0) # **** process the car messages **** # *** compute control surfaces *** tt = sec_since_boot() GAS_MAX = 1004 BRAKE_MAX = 1024/4 STEER_MAX = 0xF00 GAS_OFFSET = 328 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = int(np.clip(final_gas*GAS_MAX, 0, GAS_MAX-1)) apply_brake = int(np.clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1)) apply_steer = int(np.clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX)) # no gas if you are hitting the brake or the user is if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed): print "CANCELLING GAS", apply_brake apply_gas = 0 # no computer brake if the gas is being pressed if CS.car_gas > 0 and apply_brake != 0: print "CANCELLING BRAKE" apply_brake = 0 # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5 if CS.steer_not_allowed: print "STEER ALERT, TORQUE INHIBITED" apply_steer = 0 # *** entry into controls state *** if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \ CS.cruise_buttons == 0 and not self.controls_allowed: print "CONTROLS ARE LIVE" self.controls_allowed = True # *** exit from controls state on cancel, gas, or brake *** if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed: print "CONTROLS ARE DEAD" self.controls_allowed = False # *** controls fail on steer error, brake error, or invalid can *** if CS.steer_error: print "STEER ERROR" self.controls_allowed = False if CS.brake_error: print "BRAKE ERROR" self.controls_allowed = False if not CS.can_valid and self.controls_allowed: # 200 ms print "CAN INVALID" self.controls_allowed = False # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(apply_steer, idx)) # Send gas and brake commands. if (frame % 2) == 0: idx = (frame / 2) % 4 can_sends.append( hondacan.create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, hud.chime, idx)) if not CS.brake_only: # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0) can_sends.append(hondacan.create_gas_command(gas_amount, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame/10) % 4 can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.civic, idx)) # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) if CS.civic: radar_send_step = 5 else: radar_send_step = 2 if (frame % radar_send_step) == 0: idx = (frame/radar_send_step) % 4 can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.civic, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): """ Controls thread """ if not self.enable_camera: return # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep is not 0 else snd_chime # Do not send audible alert when steering is disabled or blinkers on #if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on: # snd_chime = 0 #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines, CS.lkMode) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024 / 4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) else: STEER_MAX = 0x1000 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int( clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame / 10) % 4 can_sends.extend( hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) elif CS.stopped: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = (frame / 2) % 4 pump_on, self.last_pump_ts = brake_pump_hysteresys( apply_brake, self.apply_brake_last, self.last_pump_ts) can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling can_sends.append( hondacan.create_gas_command(self.packer, apply_gas, idx)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): """ Controls thread """ if not self.enable_camera: return # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep is not 0 else snd_chime #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024/4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) else: STEER_MAX = 0x1000 #update custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 1000 == 0): CS.cstm_btns.send_button_info() CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name) # Get the angle from ALCA. alca_enabled = False alca_steer = 0. alca_angle = 0. turn_signal_needed = 0 # Update ALCA status and custom button every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) if (frame % 10 == 0): self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) # steer torque alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators) # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX)) if CS.cstm_btns.get_button_status("lka") == 0: apply_steer = 0 # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5 lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame/10) % 4 can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) elif CS.stopped: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = (frame / 2) % 4 pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())