def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params if enabled: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: accel = 0.0 gas, brake = 0.0, 0.0 # *** apply brake hysteresis *** pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** rate limit after the enable check *** self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL) # 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 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # tester present - w/ no response (keeps radar disabled) if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: if (frame % 10) == 0: can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) stopping = actuators.longControlState == LongCtrlState.stopping # wind brake from air resistance decel at high speed wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) # all of this is only relevant for HONDA NIDEC max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) # TODO this 1.44 is just to maintain previous behavior pcm_speed_BP = [-wind_brake, -wind_brake*(3/4), 0.0, 0.5] # The Honda ODYSSEY seems to have different PCM_ACCEL # msgs, is it other cars too? if CS.CP.enableGasInterceptor: pcm_speed = 0.0 pcm_accel = int(0.0) elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL: pcm_speed_V = [0.0, clip(CS.out.vEgo - 3.0, 0.0, 100.0), clip(CS.out.vEgo + 0.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0)] pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V) pcm_accel = int((1.0) * 0xc6) else: pcm_speed_V = [0.0, clip(CS.out.vEgo - 2.0, 0.0, 100.0), clip(CS.out.vEgo + 2.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0)] pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V) pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint)) elif CS.out.cruiseState.standstill: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX) self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, CS.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) pcm_override = True can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake self.brake = apply_brake / P.NIDEC_BRAKE_MAX if CS.CP.enableGasInterceptor: # way too aggressive at low speed without this gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0]) # 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 # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected # when you do enable. if active: self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.) else: self.gas = 0.0 can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame//10) % 4 hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required) can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud)) if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint not in HONDA_BOSCH): self.speed = pcm_speed if not CS.CP.enableGasInterceptor: self.gas = pcm_accel / 0xc6 new_actuators = actuators.copy() new_actuators.speed = self.speed new_actuators.accel = self.accel new_actuators.gas = self.gas new_actuators.brake = self.brake return new_actuators, can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \ 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 """ # TODO: Make the accord work. if CS.accord or 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.civic, CS.odyssey) # *** 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 #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(round(hud_v_cruise)), 0x01, 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 if CS.civic or CS.odyssey: is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] STEER_MAX = 0x1FFF if is_fw_modified else 0x1000 elif CS.crv: STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value else: STEER_MAX = 0xF00 GAS_OFFSET = 328 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 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 if CS.steer_not_allowed: apply_steer = 0 # Send CAN commands. can_sends = [] # Send steering command. if CS.accord: idx = frame % 2 can_sends.append(hondacan.create_accord_steering_control(apply_steer, idx)) else: idx = frame % 4 can_sends.extend(hondacan.create_steering_control(apply_steer, CS.crv, 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, CS.accord, CS.crv, CS.odyssey, idx)) # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) if CS.acura: radar_send_step = 2 else: radar_send_step = 5 if (frame % radar_send_step) == 0: idx = (frame/radar_send_step) % 4 can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.civic, CS.accord, CS.crv, CS.odyssey, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.enabled: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # Never send cancel command if we never enter cruise state (no cruise if pedal) # Cancel cmd causes brakes to release at a standstill causing grinding pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.enableCruise # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) # 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 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) 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, CS.CP.isPandaBlackDEPRECATED, CS.CP.openpilotLongitudinalControl)) # 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, CS.is_metric, idx, CS.CP.isPandaBlackDEPRECATED, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append( hondacan.create_bosch_supplemental_1( self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlackDEPRECATED)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlackDEPRECATED)) elif CS.out.cruiseState.standstill: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlackDEPRECATED)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: pass # TODO: implement else: apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int( clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append( hondacan.create_brake_command( self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlackDEPRECATED, CS.stock_brake)) 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( create_gas_command(self.packer, apply_gas, idx)) return can_sends
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** 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 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, fcw_display, acc_alert, steer_required) # **** 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.CRV_EU, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): STEER_MAX = 0x7FFF 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 # 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, CS.CP.isPandaBlack)) # 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, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud)) 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, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.stopped: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) 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( create_gas_command(self.packer, apply_gas, idx)) return can_sends
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) # *** 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., DT_CTRL) # 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 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, 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 elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): STEER_MAX = 0x7FFF elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: STEER_MAX = 0x1400 else: STEER_MAX = 0x1000 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) # return minimum of brake_last*MAX, or MAX-1, but not less than zero 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 if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: if apply_steer > 0xA00: apply_steer = (apply_steer - 0xA00) / 2 + 0xA00 elif apply_steer < -0xA00: apply_steer = (apply_steer + 0xA00) / 2 - 0xA00 # 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, CS.CP.isPandaBlack)) # 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, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud)) 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, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.stopped: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT): if CS.lead_distance > (self.prev_lead_distance + float(kegman.conf['leadDistance'])): can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH): if CS.hud_lead == 1: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: self.prev_lead_distance = CS.lead_distance else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL pump_on, self.last_pump_on_state = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_on_state, ts) # Do NOT send the cancel command if we are using the pedal. Sending cancel causes the car firmware to # turn the brake pump off, and we don't want that. Stock ACC does not send the cancel cmd when it is braking. if CS.CP.enableGasInterceptor: pcm_cancel_cmd = False can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) 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(create_gas_command(self.packer, apply_gas, idx)) return can_sends
def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params if enabled: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: accel = 0.0 gas, brake = 0.0, 0.0 # *** apply brake hysteresis *** pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.enabled: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # Never send cancel command if we never enter cruise state (no cruise if pedal) # Cancel cmd causes brakes to release at a standstill causing grinding pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise # *** rate limit after the enable check *** self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL) # 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 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] # tester present - w/ no response (keeps radar disabled) if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: if (frame % 10) == 0: can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) # TODO: pass in LoC.long_control_state and use that to decide starting/stoppping stopping = accel < 0 and CS.out.vEgo < 0.3 starting = accel > 0 and CS.out.vEgo < 0.3 # Prevent rolling backwards accel = -4.0 if stopping else accel # wind brake from air resistance decel at high speed wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) if CS.CP.carFingerprint in OLD_NIDEC_LONG_CONTROL: #pcm_speed = pcm_speed pcm_accel = int(clip(pcm_accel, 0, 1) * 0xc6) else: max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) # TODO this 1.44 is just to maintain previous behavior pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6) pcm_speed_BP = [-wind_brake, -wind_brake*(3/4), 0.0] pcm_speed_V = [0.0, clip(CS.out.vEgo + accel/2.0 - 2.0, 0.0, 100.0), clip(CS.out.vEgo + accel/2.0 + 2.0, 0.0, 100.0)] pcm_speed = interp(-brake, pcm_speed_BP, pcm_speed_V) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint)) elif CS.out.cruiseState.standstill: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: # way too aggressive at low speed without this gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0]) # 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 apply_gas = clip(gas_mult * gas, 0., 1.) can_sends.append(create_gas_command(self.packer, apply_gas, idx)) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required) # 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, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) return can_sends
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), 0x01, 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))
def update(self, sendcan, enabled, CS, frame, actuators, \ 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 """ 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 # TODO: factor this out better 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): print "INVALID HUD", 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.append( hondacan.create_brake_command(self.packer, apply_brake, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, 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 can_sends.extend( hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, 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, \ 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 != 0 else snd_chime #print("{0} {1} {2}".format(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) elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): STEER_MAX = 0x7FFF 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 not CS.lane_departure_toggle_on: 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 and CS.lkMode # 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: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): if CS.lead_distance > (self.prev_lead_distance + float(kegman.conf['leadDistance'])): can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx)) elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH): if CS.hud_lead == 1: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx)) else: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) else: self.prev_lead_distance = CS.lead_distance else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 pump_on, self.last_pump_ts = brake_pump_hysteresis( 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( create_gas_command(self.packer, apply_gas, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def update(self, sendcan, enabled, CS, frame, actuators, \ 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 """ ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars) 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 # TODO: factor this out better 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: #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): print "INVALID HUD", hud hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** STEER_MAX = 0x4000 #16384 # Prototype Angle Max. slope of 180 degree at 8.3 m/s (30 km/h) and 25 degree at 33.3 m/s (120 KM/h) # = -62x + 2314.6 # Angle max = -62*V(m/s) + 2314.6 # Gives for example: # 180 degree at 30 km/h # 145 degree at 50 km/h # 94 degree at 80 km/h # 59 degree at 100 km/h # 42 degree at 110 km/h # 25 degree at 120 km/h USER_STEER_MAX = (-62.0 * CS.v_ego) + 2314.6 # Basic highway lane change logic enable_steer_control = (not CS.right_blinker_on and not CS.left_blinker_on and enabled) # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int( clip((-actuators.steer * 100) + STEER_MAX - (CS.angle_steers * 10), STEER_MAX - USER_STEER_MAX, STEER_MAX + USER_STEER_MAX)) # Send CAN commands. can_sends = [] send_step = 5 if (frame % send_step) == 0: idx = (frame / send_step) % 16 can_sends.append( teslacan.create_steering_control(enable_steer_control, apply_steer, idx)) can_sends.append(teslacan.create_epb_enable_signal(idx)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.enabled: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) # 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 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required) # **** process the car messages **** if CS.CP.carFingerprint in HONDA_BOSCH: stopping = 0 starting = 0 accel = actuators.gas - actuators.brake if accel < 0 and CS.out.vEgo < 0.3: # prevent rolling backwards stopping = 1 accel = -1.0 elif accel > 0 and CS.out.vEgo < 0.3: starting = 1 apply_accel = interp(accel, BOSCH_ACCEL_LOOKUP_BP, BOSCH_ACCEL_LOOKUP_V) print("%s: %s" % (str(apply_accel), str(CS.out.aEgo))) apply_gas = interp(accel, BOSCH_GAS_LOOKUP_BP, BOSCH_GAS_LOOKUP_V) else: apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int( clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) lkas_active = enabled and not CS.steer_not_allowed # Send CAN commands. can_sends = [] if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl: # TODO: radar disable hacked together to see if it works if (frame % 10) == 0: # tester present - w/ no response (keeps radar disabled) can_sends.append([ 0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1 if CS.CP.isPandaBlack else 0 ]) # Send steering command. idx = frame % 4 can_sends.append( hondacan.create_steering_control( self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl)) # 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, CS.is_metric, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: idx = frame // 2 can_sends.append( hondacan.create_bosch_supplemental_1( self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.out.cruiseState.standstill: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: can_sends.extend( hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: pump_on, self.last_pump_ts = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append( hondacan.create_brake_command( self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) 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( create_gas_command(self.packer, apply_gas, idx)) return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \ 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 """ ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only 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 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 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) 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, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** BRAKE_MAX = 1024/4 STEER_MAX = 0x4000 #16384 # 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 if CS.steer_not_allowed: apply_steer = 0 # Send CAN commands. can_sends = [] # Send steering command. idx = frame % 4 can_sends.append(teslacan.create_steering_control(self.packer, enabled ,apply_steer, CS.CP.carFingerprint, idx)) # Send gas and brake commands. #if (frame % 2) == 0: # idx = (frame / 2) % 4 # can_sends.append( # teslacan.create_brake_command(self.packer, apply_brake, pcm_override, # pcm_cancel_cmd, hud.chime, hud.fcw, 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 # can_sends.append(teslacan.create_gas_command(self.packer, apply_gas, idx)) # # Send dashboard UI commands. #if (frame % 10) == 0: # idx = (frame/10) % 4 # can_sends.extend(teslacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) radar_send_step = 5 if (frame % radar_send_step) == 0: idx = (frame/radar_send_step) % 4 #print "Steer command", apply_steer # can_sends.extend(teslacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses( actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.enabled: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # *** rate limit after the enable check *** self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes and CS.lkMode: hud_lanes = 1 else: hud_lanes = 0 if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud_lanes, fcw_display, acc_alert, steer_required, CS.read_distance_lines, CS.lkMode) # **** process the car messages **** # 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 * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) apply_brake = int( clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) apply_steer = int( interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) 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, CS.CP.isPandaBlack)) # 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, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud)) 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, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.out.cruiseState.standstill: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL pump_on, self.last_pump_on_state = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_on_state, ts) # Do NOT send the cancel command if we are using the pedal. Sending cancel causes the car firmware to # turn the brake pump off, and we don't want that. Stock ACC does not send the cancel cmd when it is braking. if CS.CP.enableGasInterceptor: pcm_cancel_cmd = False can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) 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( create_gas_command(self.packer, apply_gas, idx)) return can_sends