def update(self, enabled, CS, frame, actuators, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # Send CAN commands. can_sends = [] alert_out = process_hud_alert(hud_alert) steer = alert_out ### STEER ### if (frame % P.STEER_STEP) == 0: lkas_enabled = enabled and not CS.steer_warning and CS.lkMode and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = actuators.steer * P.STEER_MAX apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer else: apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame // P.STEER_STEP) % 4 if self.car_fingerprint in SUPERCRUISE_CARS: can_sends += gmcan.create_steering_control_ct6( self.packer_pt, CanBus, apply_steer, CS.out.vEgo, idx, lkas_enabled) else: can_sends.append( gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) ### GAS/BRAKE ### if self.car_fingerprint not in SUPERCRUISE_CARS: # no output if not enabled, but keep sending keepalive messages # treat pedals as one final_pedal = actuators.gas - actuators.brake # *** apply pedal hysteresis *** final_brake, self.brake_steady = actuator_hystereses( final_pedal, self.pedal_steady) if not enabled: # Stock ECU sends max regen when not enabled. apply_gas = P.MAX_ACC_REGEN apply_brake = 0 else: apply_gas = int( round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) apply_brake = int( round( interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: idx = (frame // 4) % 4 car_stopping = apply_gas < P.ZERO_GAS standstill = CS.pcm_acc_status == AccState.STANDSTILL at_full_stop = enabled and standstill and car_stopping near_stop = enabled and ( CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) and car_stopping can_sends.append( gmcan.create_friction_brake_command( self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) # Auto-resume from full stop by resetting ACC control acc_enabled = enabled if standstill and not car_stopping: acc_enabled = False can_sends.append( gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, acc_enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz follow_level = CS.get_follow_level() if (frame % 4) == 0: send_fcw = 0 if self.fcw_count > 0: self.fcw_count -= 1 send_fcw = 0x3 can_sends.append( gmcan.create_acc_dashboard_command( self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, follow_level, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) time_and_headlights_step = 10 tt = frame * DT_CTRL if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 can_sends.append( gmcan.create_adas_time_status( CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) can_sends.append( gmcan.create_adas_headlights_status(CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame // speed_and_accelerometer_step) % 4 can_sends.append( gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) can_sends.append( gmcan.create_adas_accelerometer_speed_status( CanBus.OBSTACLE, CS.out.vEgo, idx)) if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. # Conveniently, sending camera message periodically also works as a keepalive. lka_active = CS.lkas_status == 1 lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ or lka_icon_status != self.lka_icon_status_last: can_sends.append( gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer)) self.lka_icon_status_last = lka_icon_status return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \ hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt): """ Controls thread """ # Sanity check. if not self.allow_controls: return P = self.params # Send CAN commands. can_sends = [] canbus = self.canbus ### STEER ### if (frame % P.STEER_STEP) == 0: lkas_enabled = enabled and not CS.steer_not_allowed and CS.lkMode and CS.v_ego > P.MIN_STEER_SPEED and not CS.left_blinker_on and not CS.right_blinker_on if lkas_enabled: apply_steer = actuators.steer * P.STEER_MAX apply_steer = apply_std_steer_torque_limits( apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) else: apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame / P.STEER_STEP) % 4 if self.car_fingerprint in SUPERCRUISE_CARS: can_sends += gmcan.create_steering_control_ct6( self.packer_pt, canbus, apply_steer, CS.v_ego, idx, lkas_enabled) else: can_sends.append( gmcan.create_steering_control(self.packer_pt, canbus.powertrain, apply_steer, idx, lkas_enabled)) ### GAS/BRAKE ### if self.car_fingerprint not in SUPERCRUISE_CARS: # no output if not enabled, but keep sending keepalive messages # treat pedals as one final_pedal = actuators.gas - actuators.brake # *** apply pedal hysteresis *** final_brake, self.brake_steady = actuator_hystereses( final_pedal, self.pedal_steady) if not enabled: # Stock ECU sends max regen when not enabled. apply_gas = P.MAX_ACC_REGEN apply_brake = 0 else: apply_gas = int( round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) apply_brake = int( round( interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: idx = (frame / 4) % 4 car_stopping = apply_gas < P.ZERO_GAS standstill = CS.pcm_acc_status == AccState.STANDSTILL at_full_stop = enabled and standstill and car_stopping near_stop = enabled and ( CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping can_sends.append( gmcan.create_friction_brake_command( self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop)) # Auto-resume from full stop by resetting ACC control acc_enabled = enabled if standstill and not car_stopping: acc_enabled = False can_sends.append( gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, acc_enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz follow_level = CS.get_follow_level() if (frame % 4) == 0: can_sends.append( gmcan.create_acc_dashboard_command( self.packer_pt, canbus.powertrain, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, follow_level)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) time_and_headlights_step = 10 tt = sec_since_boot() if frame % time_and_headlights_step == 0: idx = (frame / time_and_headlights_step) % 4 can_sends.append( gmcan.create_adas_time_status( canbus.obstacle, int((tt - self.start_time) * 60), idx)) can_sends.append( gmcan.create_adas_headlights_status(canbus.obstacle)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame / speed_and_accelerometer_step) % 4 can_sends.append( gmcan.create_adas_steering_status(canbus.obstacle, idx)) can_sends.append( gmcan.create_adas_accelerometer_speed_status( canbus.obstacle, CS.v_ego, idx)) if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(canbus.powertrain) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. # Conveniently, sending camera message periodically also works as a keepalive. lka_active = CS.lkas_status == 1 lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ or lka_icon_status != self.lka_icon_status_last: can_sends.append( gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical)) self.lka_icon_status_last = lka_icon_status # Send chimes if self.chime != chime: duration = 0x3c # There is no 'repeat forever' chime command # TODO: Manage periodic re-issuing of chime command # and chime cancellation if chime_cnt == -1: chime_cnt = 10 if chime != 0: can_sends.append( gmcan.create_chime_command(canbus.sw_gmlan, chime, duration, chime_cnt)) # If canceling a repeated chime, cancel command must be # issued for the same chime type and duration self.chime = chime sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # Send CAN commands. can_sends = [] # STEER if (frame % P.STEER_STEP) == 0: lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = actuators.steer * P.STEER_MAX apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer else: apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame // P.STEER_STEP) % 4 can_sends.append( gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) # GAS/BRAKE # no output if not enabled, but keep sending keepalive messages # treat pedals as one final_pedal = actuators.gas - actuators.brake if not enabled: # Stock ECU sends max regen when not enabled. apply_gas = P.MAX_ACC_REGEN apply_brake = 0 else: apply_gas = int( round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) apply_brake = int( round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: idx = (frame // 4) % 4 at_full_stop = enabled and CS.out.standstill near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) can_sends.append( gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) can_sends.append( gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: send_fcw = hud_alert == VisualAlert.fcw can_sends.append( gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) time_and_headlights_step = 10 tt = frame * DT_CTRL if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 can_sends.append( gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) can_sends.append( gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame // speed_and_accelerometer_step) % 4 can_sends.append( gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) can_sends.append( gmcan.create_adas_accelerometer_speed_status( CanBus.OBSTACLE, CS.out.vEgo, idx)) if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. # Conveniently, sending camera message periodically also works as a keepalive. lka_active = CS.lkas_status == 1 lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last: steer_alert = hud_alert == VisualAlert.steerRequired can_sends.append( gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status return can_sends
def update(self, CC, CS): actuators = CC.actuators hud_control = CC.hudControl hud_alert = hud_control.visualAlert hud_v_cruise = hud_control.setSpeed if hud_v_cruise > 70: hud_v_cruise = 0 # Send CAN commands. can_sends = [] # Steering (50Hz) # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the # next Panda loopback confirmation in the current CS frame. if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter elif (self.frame % self.params.STEER_STEP) == 0: if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) else: apply_steer = 0 self.apply_steer_last = apply_steer # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks. idx = (CS.lka_steering_cmd_counter + 1) % 4 can_sends.append( gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive)) if self.CP.openpilotLongitudinalControl: # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: if not CC.longActive: # Stock ECU sends max regen when not enabled self.apply_gas = self.params.MAX_ACC_REGEN self.apply_brake = 0 else: if self.CP.carFingerprint in EV_CAR: self.apply_gas = int( round( interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) self.apply_brake = int( round( interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) else: self.apply_gas = int( round( interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) self.apply_brake = int( round( interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) idx = (self.frame // 4) % 4 at_full_stop = CC.longActive and CS.out.standstill near_stop = CC.longActive and ( CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation can_sends.append( gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) can_sends.append( gmcan.create_friction_brake_command( self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw can_sends.append( gmcan.create_acc_dashboard_command( self.packer_pt, CanBus.POWERTRAIN, CC.enabled, hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) if not self.CP.radarOffCan: tt = self.frame * DT_CTRL time_and_headlights_step = 10 if self.frame % time_and_headlights_step == 0: idx = (self.frame // time_and_headlights_step) % 4 can_sends.append( gmcan.create_adas_time_status( CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) can_sends.append( gmcan.create_adas_headlights_status( self.packer_obj, CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if self.frame % speed_and_accelerometer_step == 0: idx = (self.frame // speed_and_accelerometer_step) % 4 can_sends.append( gmcan.create_adas_steering_status( CanBus.OBSTACLE, idx)) can_sends.append( gmcan.create_adas_accelerometer_speed_status( CanBus.OBSTACLE, CS.out.vEgo, idx)) if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) else: # Stock longitudinal, integrated at camera if (self.frame - self.last_button_frame) * DT_CTRL > 0.04: if CC.cruiseControl.cancel: self.last_button_frame = self.frame can_sends.append( gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL)) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. # Conveniently, sending camera message periodically also works as a keepalive. lka_active = CS.lkas_status == 1 lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) # SW_GMLAN not yet on cam harness, no HUD alerts if self.CP.networkLocation != NetworkLocation.fwdCamera and ( self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last): steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) can_sends.append( gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.gas = self.apply_gas new_actuators.brake = self.apply_brake self.frame += 1 return new_actuators, can_sends
def test_correctness(self): # Test all cars' commands, randomize the params. for _ in xrange(1000): bus = random.randint(0, 65536) apply_steer = (random.randint(0, 2) % 2 == 0) idx = random.randint(0, 65536) lkas_active = (random.randint(0, 2) % 2 == 0) m_old = gmcan.create_steering_control(self.gm_cp_old, bus, apply_steer, idx, lkas_active) m = gmcan.create_steering_control(self.gm_cp, bus, apply_steer, idx, lkas_active) self.assertEqual(m_old, m) canbus = GMCanBus() apply_steer = (random.randint(0, 2) % 2 == 0) v_ego = random.randint(0, 65536) idx = random.randint(0, 65536) enabled = (random.randint(0, 2) % 2 == 0) m_old = gmcan.create_steering_control_ct6(self.ct6_cp_old, canbus, apply_steer, v_ego, idx, enabled) m = gmcan.create_steering_control_ct6(self.ct6_cp, canbus, apply_steer, v_ego, idx, enabled) self.assertEqual(m_old, m) bus = random.randint(0, 65536) throttle = random.randint(0, 65536) idx = random.randint(0, 65536) acc_engaged = (random.randint(0, 2) % 2 == 0) at_full_stop = (random.randint(0, 2) % 2 == 0) m_old = gmcan.create_gas_regen_command(self.gm_cp_old, bus, throttle, idx, acc_engaged, at_full_stop) m = gmcan.create_gas_regen_command(self.gm_cp, bus, throttle, idx, acc_engaged, at_full_stop) self.assertEqual(m_old, m) bus = random.randint(0, 65536) apply_brake = (random.randint(0, 2) % 2 == 0) idx = random.randint(0, 65536) near_stop = (random.randint(0, 2) % 2 == 0) at_full_stop = (random.randint(0, 2) % 2 == 0) m_old = gmcan.create_friction_brake_command( self.ct6_cp_old, bus, apply_brake, idx, near_stop, at_full_stop) m = gmcan.create_friction_brake_command(self.ct6_cp, bus, apply_brake, idx, near_stop, at_full_stop) self.assertEqual(m_old, m) bus = random.randint(0, 65536) acc_engaged = (random.randint(0, 2) % 2 == 0) target_speed_kph = random.randint(0, 65536) lead_car_in_sight = (random.randint(0, 2) % 2 == 0) m_old = gmcan.create_acc_dashboard_command(self.gm_cp_old, bus, acc_engaged, target_speed_kph, lead_car_in_sight) m = gmcan.create_acc_dashboard_command(self.gm_cp, bus, acc_engaged, target_speed_kph, lead_car_in_sight) self.assertEqual(m_old, m)
def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # Send CAN commands. can_sends = [] # Steering (50Hz) # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the # next Panda loopback confirmation in the current CS frame. if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter elif (frame % P.STEER_STEP) == 0: lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits( new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer else: apply_steer = 0 self.apply_steer_last = apply_steer # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks. idx = (CS.lka_steering_cmd_counter + 1) % 4 can_sends.append( gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: if not c.longActive: # Stock ECU sends max regen when not enabled. self.apply_gas = P.MAX_ACC_REGEN self.apply_brake = 0 else: self.apply_gas = int( round( interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) self.apply_brake = int( round( interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) idx = (frame // 4) % 4 at_full_stop = c.longActive and CS.out.standstill near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) can_sends.append( gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) can_sends.append( gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: send_fcw = hud_alert == VisualAlert.fcw can_sends.append( gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) time_and_headlights_step = 10 tt = frame * DT_CTRL if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 can_sends.append( gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) can_sends.append( gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame // speed_and_accelerometer_step) % 4 can_sends.append( gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) can_sends.append( gmcan.create_adas_accelerometer_speed_status( CanBus.OBSTACLE, CS.out.vEgo, idx)) if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. # Conveniently, sending camera message periodically also works as a keepalive. lka_active = CS.lkas_status == 1 lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last: steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) can_sends.append( gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / P.STEER_MAX new_actuators.gas = self.apply_gas new_actuators.brake = self.apply_brake return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params # Send CAN commands. can_sends = [] canbus = self.canbus alert_out = process_hud_alert(hud_alert) steer = alert_out ### STEER ### if (frame % P.STEER_STEP) == 0: lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED if lkas_enabled: apply_steer = actuators.steer * P.STEER_MAX apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) else: apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame // P.STEER_STEP) % 4 if self.car_fingerprint in SUPERCRUISE_CARS: can_sends += gmcan.create_steering_control_ct6(self.packer_pt, canbus, apply_steer, CS.v_ego, idx, lkas_enabled) else: can_sends.append(gmcan.create_steering_control(self.packer_pt, canbus.powertrain, apply_steer, idx, lkas_enabled)) ### GAS/BRAKE ### if self.car_fingerprint not in SUPERCRUISE_CARS: # no output if not enabled, but keep sending keepalive messages # treat pedals as one final_pedal = actuators.gas - actuators.brake # *** apply pedal hysteresis *** final_brake, self.brake_steady = actuator_hystereses( final_pedal, self.pedal_steady) if not enabled: # Stock ECU sends max regen when not enabled. apply_gas = P.MAX_ACC_REGEN apply_brake = 0 else: apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) #TODO: there may be other GM cars that lack ACC if not self.car_fingerprint in NO_ASCM_CARS: # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: idx = (frame // 4) % 4 at_full_stop = enabled and CS.standstill near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop)) at_full_stop = enabled and CS.standstill can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, canbus.powertrain, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) time_and_headlights_step = 10 tt = frame * DT_CTRL if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 can_sends.append(gmcan.create_adas_time_status(canbus.obstacle, int((tt - self.start_time) * 60), idx)) can_sends.append(gmcan.create_adas_headlights_status(canbus.obstacle)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame // speed_and_accelerometer_step) % 4 can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx)) can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx)) if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(canbus.powertrain) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. # If not sent again, LKA icon disappears in about 5 seconds. # Conveniently, sending camera message periodically also works as a keepalive. lka_active = CS.lkas_status == 1 lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ or lka_icon_status != self.lka_icon_status_last: can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer)) self.lka_icon_status_last = lka_icon_status return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \ hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt): """ Controls thread """ P = self.params # Send CAN commands. can_sends = [] canbus = self.canbus ### STEER ### if (frame % P.STEER_STEP) == 0: final_steer = actuators.steer if enabled else 0. apply_steer = final_steer * P.STEER_MAX apply_steer = apply_std_steer_torque_limits( apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3. if not lkas_enabled: apply_steer = 0 self.apply_steer_last = apply_steer idx = (frame / P.STEER_STEP) % 4 if self.car_fingerprint == CAR.VOLT: can_sends.append( gmcan.create_steering_control(self.packer_pt, canbus.powertrain, apply_steer, idx, lkas_enabled)) if self.car_fingerprint == CAR.CADILLAC_CT6: can_sends += gmcan.create_steering_control_ct6( self.packer_pt, canbus, apply_steer, CS.v_ego, idx, lkas_enabled) ### GAS/BRAKE ### if self.car_fingerprint == CAR.VOLT: # no output if not enabled, but keep sending keepalive messages # threat pedals as one final_pedal = actuators.gas - actuators.brake # *** apply pedal hysteresis *** final_brake, self.brake_steady = actuator_hystereses( final_pedal, self.pedal_steady) if not enabled: apply_gas = P.MAX_ACC_REGEN # TODO: do we really need to send max regen when not enabled? apply_brake = 0 else: apply_gas = int( round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) apply_brake = int( round( interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: idx = (frame / 4) % 4 at_full_stop = enabled and CS.standstill near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) can_sends.append( gmcan.create_friction_brake_command( self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop)) at_full_stop = enabled and CS.standstill can_sends.append( gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: can_sends.append( gmcan.create_acc_dashboard_command( canbus.powertrain, enabled, hud_v_cruise / CV.MS_TO_KPH, hud_show_car)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) time_and_headlights_step = 10 tt = sec_since_boot() if frame % time_and_headlights_step == 0: idx = (frame / time_and_headlights_step) % 4 can_sends.append( gmcan.create_adas_time_status( canbus.obstacle, int((tt - self.start_time) * 60), idx)) can_sends.append( gmcan.create_adas_headlights_status(canbus.obstacle)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame / speed_and_accelerometer_step) % 4 can_sends.append( gmcan.create_adas_steering_status(canbus.obstacle, idx)) can_sends.append( gmcan.create_adas_accelerometer_speed_status( canbus.obstacle, CS.v_ego, idx)) # Send ADAS keepalive, 10hz if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(canbus.powertrain) # Send chimes if self.chime != chime: duration = 0x3c # There is no 'repeat forever' chime command # TODO: Manage periodic re-issuing of chime command # and chime cancellation if chime_cnt == -1: chime_cnt = 10 if chime != 0: can_sends.append( gmcan.create_chime_command(canbus.sw_gmlan, chime, duration, chime_cnt)) # If canceling a repeated chime, cancel command must be # issued for the same chime type and duration self.chime = chime sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())