def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert): #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) # *** compute control surfaces *** # gas and brake apply_gas = clip(actuators.gas, 0., 1.) if CS.CP.enableGasInterceptor: # send only send brake values if interceptor is detected. otherwise, send the regular value # +0.06 offset to reduce ABS pump usage when OP is engaged apply_accel = 0.06 - actuators.brake else: apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # 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) apply_steer = int(round(alca_steer * STEER_MAX)) # steer torque #apply_steer = int(round(actuators.steer * STEER_MAX)) max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX) min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX) apply_steer = clip(apply_steer, min_lim, max_lim) # slow rate if steer torque increases in magnitude if self.last_steer > 0: apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP) else: apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP)) # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota # cuts steer torque immediately anyway TODO: monitor if this is a real issue # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # Cut steering for 2s after fault if not enabled or (frame - self.last_fault_frame < 200): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active # steer angle if self.steer_angle_enabled and CS.ipas_active: apply_angle = alca_angle #apply_angle = actuators.steerAngle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) else: apply_angle = CS.angle_steers if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 # on entering standstill, send standstill request #if CS.standstill and not self.last_standstill: # self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer self.last_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.standstill can_sends = [] # Enable blindspot debug mode once if BLINDSPOTDEBUG: self.blindspot_poll_counter += 1 if self.blindspot_poll_counter > 1000: # 10 seconds after start if CS.left_blinker_on: self.blindspot_blink_counter_left += 1 #print "debug Left Blinker on" elif CS.right_blinker_on: self.blindspot_blink_counter_right += 1 else: self.blindspot_blink_counter_left = 0 self.blindspot_blink_counter_right = 0 #print "debug Left Blinker off" if self.blindspot_debug_enabled_left: can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, False)) self.blindspot_debug_enabled_left = False #print "debug Left blindspot debug disabled" if self.blindspot_debug_enabled_right: can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, False)) self.blindspot_debug_enabled_right = False #print "debug Right blindspot debug disabled" if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, True)) #print "debug Left blindspot debug enabled" self.blindspot_debug_enabled_left = True if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode if CS.v_ego > 6: #polling at low speeds switches camera off can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, True)) #print "debug Right blindspot debug enabled" self.blindspot_debug_enabled_right = True if self.blindspot_debug_enabled_left: if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001: # Poll blindspots at 5 Hz can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT)) if self.blindspot_debug_enabled_right: if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005: # Poll blindspots at 5 Hz can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT)) #*** control msgs *** #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages if ECU.CAM in self.fake_ecus: if self.angle_control: can_sends.append(create_steer_command(self.packer, 0., 0, frame)) else: if CS.lane_departure_toggle_on: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) else: can_sends.append(create_steer_command(self.packer, 0., 0, frame)) # rav4h with dsu disconnected if self.angle_control: if CS.lane_departure_toggle_on: can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) else: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) if CS.cstm_btns.get_button_status("tr") > 0: distance = 0b01110011 #x73 comma with toggle - toggle is 5th bit from right CS.cstm_btns.set_button_status("tr", 0) else: distance = 0b01100011 #x63 comma with toggle - toggle is 5th bit from right # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): if ECU.DSU in self.fake_ecus: can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, distance)) else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, distance)) 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)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus and self.car_fingerprint not in NO_DSU_CAR: for addr in TARGET_IDS: can_sends.append(create_video_target(frame/10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying alert_out = process_hud_alert(hud_alert, audible_alert) steer, fcw, sound1, sound2 = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append(create_ui_command(self.packer, steer, sound1, sound2)) can_sends.append(create_fcw_command(self.packer, fcw)) #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame / 5) % 7) + 1) << 5 vl = chr(cnt) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) cnt = ((frame/100)%0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 vl += chr(cnt) can_sends.append(make_can_msg(addr, vl, bus, False)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead): #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) # *** compute control surfaces *** # gas and brake apply_gas = clip(actuators.gas, 0., 1.) if CS.CP.enableGasInterceptor: # send only negative accel if interceptor is detected. otherwise, send the regular value # +0.06 offset to reduce ABS pump usage when OP is engaged apply_accel = 0.06 - actuators.brake else: apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis( apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # 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) apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX)) apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: self.last_fault_frame = frame # Cut steering for 2s after fault if not enabled or (frame - self.last_fault_frame < 200): apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active # steer angle if self.steer_angle_enabled and CS.ipas_active: apply_angle = alca_angle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs( self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) else: apply_angle = CS.angle_steers if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 # on entering standstill, send standstill request if CS.standstill and not self.last_standstill: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer self.last_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.standstill can_sends = [] #*** control msgs *** #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages if ECU.CAM in self.fake_ecus: if self.angle_control: can_sends.append( create_steer_command(self.packer, 0., 0, frame)) else: can_sends.append( create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if self.angle_control: can_sends.append( create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or ( pcm_cancel_cmd and ECU.CAM in self.fake_ecus): lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged if ECU.DSU in self.fake_ecus: can_sends.append( create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead)) else: can_sends.append( create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) 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)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera: for addr in TARGET_IDS: can_sends.append(create_video_target(frame / 10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying alert_out = process_hud_alert(hud_alert, audible_alert) steer, fcw, sound1, sound2 = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append( create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line)) if frame % 100 == 0 and ECU.DSU in self.fake_ecus: can_sends.append(create_fcw_command(self.packer, fcw)) #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not ( ecu == ECU.CAM and forwarding_camera): # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame / 5) % 7) + 1) << 5 vl = chr(cnt) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) cnt = ((frame / 100) % 0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 vl += chr(cnt) can_sends.append(make_can_msg(addr, vl, bus, False)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert): # *** compute control surfaces *** # gas and brake apply_gas = clip(actuators.gas, 0., 1.) if CS.CP.enableGasInterceptor: # send only send brake values if interceptor is detected. otherwise, send the regular value # +0.06 offset to reduce ABS pump usage when OP is engaged apply_accel = 0.06 - actuators.brake else: apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis( apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # steer torque apply_steer = int(round(actuators.steer * STEER_MAX)) max_lim = min( max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX) min_lim = max( min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX) apply_steer = clip(apply_steer, min_lim, max_lim) # slow rate if steer torque increases in magnitude if self.last_steer > 0: apply_steer = clip( apply_steer, max(self.last_steer - STEER_DELTA_DOWN, -STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP) else: apply_steer = clip( apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP)) # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota # cuts steer torque immediately anyway TODO: monitor if this is a real issue # only cut torque when steer state is a known fault if not enabled or CS.steer_state in [9, 25]: apply_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active # steer angle if self.steer_angle_enabled and CS.ipas_active: apply_angle = actuators.steerAngle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs( self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) else: apply_angle = CS.angle_steers if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 # on entering standstill, send standstill request if CS.standstill and not self.last_standstill: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer self.last_angle = apply_angle self.last_accel = apply_accel self.last_standstill = CS.standstill can_sends = [] #*** control msgs *** #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages if ECU.CAM in self.fake_ecus: if self.angle_control: can_sends.append( create_steer_command(self.packer, 0., 0, frame)) else: can_sends.append( create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) if self.angle_control: can_sends.append( create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or ( pcm_cancel_cmd and ECU.CAM in self.fake_ecus): if ECU.DSU in self.fake_ecus: can_sends.append( create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req)) else: can_sends.append( create_accel_command(self.packer, 0, pcm_cancel_cmd, False)) 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)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus and self.car_fingerprint not in NO_DSU_CAR: for addr in TARGET_IDS: can_sends.append(create_video_target(frame / 10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying alert_out = process_hud_alert(hud_alert, audible_alert) steer, fcw, sound1, sound2 = alert_out if (any(alert_out) and not self.alert_active) or \ (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append( create_ui_command(self.packer, steer, sound1, sound2)) can_sends.append(create_fcw_command(self.packer, fcw)) #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame / 5) % 7) + 1) << 5 vl = chr(cnt) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) cnt = ((frame / 100) % 0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 vl += chr(cnt) can_sends.append(make_can_msg(addr, vl, bus, False)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())