def test_correctness(self): # Test all commands, randomize the params. for _ in range(1000): has_relay = False car_fingerprint = HONDA_BOSCH[0] apply_brake = (random.randint(0, 2) % 2 == 0) pump_on = (random.randint(0, 2) % 2 == 0) pcm_override = (random.randint(0, 2) % 2 == 0) pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0) fcw = random.randint(0, 65536) idx = random.randint(0, 65536) m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay) m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay) self.assertEqual(m_old, m) apply_steer = (random.randint(0, 2) % 2 == 0) lkas_active = (random.randint(0, 2) % 2 == 0) idx = random.randint(0, 65536) m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, has_relay) m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, has_relay) self.assertEqual(m_old, m) pcm_speed = random.randint(0, 65536) hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536), 0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536)) idx = random.randint(0, 65536) is_metric = (random.randint(0, 2) % 2 == 0) m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay) m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay) self.assertEqual(m_old, m) button_val = random.randint(0, 65536) idx = random.randint(0, 65536) m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, has_relay) m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, has_relay) self.assertEqual(m_old, m)
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 **** # 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.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: 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.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, \ 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, 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 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, self.speed_units) # **** 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 # 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: 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)) 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_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( create_gas_command(self.packer, apply_gas, 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): # *** 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: 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 **** # *** 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.) 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)) 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 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) pump_on, self.last_pump_on_state = brake_pump_hysteresis( apply_brake, self.apply_brake_last, self.last_pump_on_state, ts) 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, c, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params if c.longActive: 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 c.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)) # 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, c.latActive, 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, c.enabled, c.longActive, 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 c.longActive: 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 """ 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 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) # **** 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)) 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())
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.) # return minimum of brake_last*MAX, or MAX-1, but not less than zero 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.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 ''' 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
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.pcmCruise # *** 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) # **** 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)) accel = actuators.gas - actuators.brake # 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 = -1.0 if stopping else accel if CS.CP.carFingerprint in HONDA_BOSCH: apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V) else: apply_accel = interp(accel, P.NIDEC_ACCEL_LOOKUP_BP, P.NIDEC_ACCEL_LOOKUP_V) # 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) pcm_accel = int(clip(apply_accel / 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 + apply_accel / 2.0 - 2.0, 0.0, 100.0), clip(CS.out.vEgo + apply_accel / 2.0 + 2.0, 0.0, 100.0) ] pcm_speed = interp(accel, 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: apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend( hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_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 * actuators.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, 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): # *** 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 or 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 # 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 # 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("{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.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) elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): STEER_MAX = 0x7FFF else: STEER_MAX = 0x1000 if self.BP2 is None: self.BP2 = int(kegman.conf['BP2']) if int( kegman.conf['BP2']) != 0 else STEER_MAX self.BP1 = int( kegman.conf['BP1']) if int(kegman.conf['BP1']) != 0 else 2560 print("torque breakpoints set!", self.BP1, self.BP2) # 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, -self.BP2, self.BP2)) apply_steer = int( interp(apply_steer, [-self.BP2, -self.BP1, self.BP1, self.BP2], [-STEER_MAX, -self.BP1, self.BP1, STEER_MAX])) lkas_active = 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, CS.CP.isPandaBlack)) can_sends.append( hondacan.create_steering_control_x2(self.packer, 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)) 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.standstill and enabled and CS.stopped and CS.lead_distance < 200: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT): rough_lead_speed = self.rough_speed(CS.lead_distance) if CS.lead_distance > (self.stopped_lead_distance + 15.0) or rough_lead_speed > 0.1: #self.stopped_lead_distance = 0.0 can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) print("spamming") print(self.stopped_lead_distance, CS.lead_distance, rough_lead_speed) elif False and CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): print(' this is a civic or crv') 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: print(' doing the regular SPAM RESUME') can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.v_ego == 0: #print(' not quite "stopped" yet, but lead distance is %f' % CS.lead_distance) self.stopped_lead_distance = CS.lead_distance self.prev_lead_distance = CS.lead_distance else: self.stopped_lead_distance = CS.lead_distance 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_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.chime, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) 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, dragonconf, 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.pcmCruise # *** 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.lkMode) # **** 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 and CS.lkMode # 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)) # dp blinker_on = CS.out.leftBlinker or CS.out.rightBlinker if not enabled: self.blinker_end_frame = 0 if self.last_blinker_on and not blinker_on: self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay apply_steer = common_controller_ctrl( enabled, dragonconf, blinker_on or frame < self.blinker_end_frame, apply_steer, CS.out.vEgo) self.last_blinker_on = blinker_on # 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)) # Send dashboard UI commands. if not dragonconf.dpAtl and (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)) 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 dragonconf.dpAtl: pass # If using stock ACC, spam cancel command to kill gas when OP disengages. elif not dragonconf.dpAllowGas and pcm_cancel_cmd: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint)) elif CS.out.cruiseState.standstill: if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.INSIGHT): rough_lead_speed = self.rough_speed(CS.lead_distance) if CS.lead_distance > (self.stopped_lead_distance + 15.0) or rough_lead_speed > 0.1: self.stopped_lead_distance = 0.0 can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): if CS.hud_lead == 1: can_sends.append( hondacan.spam_buttons_command( self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: can_sends.append( hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint)) else: self.stopped_lead_distance = CS.lead_distance self.prev_lead_distance = CS.lead_distance else: # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL if dragonconf.dpAtl: pass elif CS.CP.carFingerprint in HONDA_BOSCH: accel = actuators.gas - actuators.brake # 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 = -1.0 if stopping else accel apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V) apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend( hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint)) 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.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