def update(self, CC, CS): can_sends = [] actuators = CC.actuators hud_control = CC.hudControl main_on = CS.out.cruiseState.available steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) if CC.cruiseControl.cancel: # cancel stock ACC can_sends.append(fordcan.spam_cancel_button(self.packer)) # apply rate limits new_steer = actuators.steeringAngleDeg apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) self.steer_rate_limited = new_steer != apply_steer # send steering commands at 20Hz if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: lca_rq = 1 if CC.latActive else 0 # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented path_angle = apply_steer # convert steer angle to curvature curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0) # TODO: get other actuators curvature_rate = 0 path_offset = 0 ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately precision = 0 # 0=Comfortable, 1=Precise self.apply_steer_last = apply_steer can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature)) can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, path_offset, path_angle, curvature_rate, curvature)) send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) # send lkas ui command at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) # send acc ui command at 20Hz or if ui state changes if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive self.steer_alert_last = steer_alert new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_steer self.frame += 1 return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel): can_sends = [] steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired apply_steer = actuators.steer if self.enable_camera: if pcm_cancel: print("CANCELING!!!!") can_sends.append(spam_cancel_button(self.packer)) if (frame % 3) == 0: curvature = self.vehicle_model.calc_curvature( actuators.steerAngle * CV.DEG_TO_RAD, CS.out.vEgo) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) self.lkas_action &= 0xf else: self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy can_sends.append( create_steer_command(self.packer, apply_steer, enabled, CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action)) self.generic_toggle_last = CS.out.genericToggle if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \ (self.steer_alert_last != steer_alert): can_sends.append( create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) self.enabled_last = enabled self.main_on_last = CS.out.cruiseState.available self.steer_alert_last = steer_alert return can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel): can_sends = [] steer_alert = visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw) apply_steer = actuators.steer if pcm_cancel: #print "CANCELING!!!!" can_sends.append(spam_cancel_button(self.packer)) if (frame % 3) == 0: curvature = self.vehicle_model.calc_curvature( math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) self.lkas_action &= 0xf else: self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy can_sends.append( create_steer_command(self.packer, apply_steer, enabled, CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action)) self.generic_toggle_last = CS.out.genericToggle if (frame % 100) == 0: can_sends.append( make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0)) #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \ (self.steer_alert_last != steer_alert): can_sends.append( create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) if (frame % 200) == 0: can_sends.append( make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) if (frame % 10) == 0: can_sends.append( make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1)) can_sends.append( make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1)) can_sends.append( make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1)) can_sends.append( make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1)) can_sends.append( make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1)) can_sends.append( make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1)) can_sends.append( make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1)) can_sends.append( make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1)) can_sends.append( make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1)) can_sends.append( make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1)) can_sends.append( make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1)) can_sends.append( make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1)) static_msgs = range(1653, 1658) for addr in static_msgs: cnt = (frame % 10) + 1 can_sends.append( make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1)) self.enabled_last = enabled self.main_on_last = CS.out.cruiseState.available self.steer_alert_last = steer_alert return actuators, can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel): can_sends = [] steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired apply_steer = actuators.steer if self.enable_camera: if pcm_cancel: #print "CANCELING!!!!" can_sends.append(spam_cancel_button(self.packer)) if (frame % 3) == 0: curvature = self.vehicle_model.calc_curvature( actuators.steerAngle * 3.1415 / 180., CS.v_ego) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last) self.lkas_action &= 0xf else: self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy can_sends.append( create_steer_command(self.packer, apply_steer, enabled, CS.lkas_state, CS.angle_steers, curvature, self.lkas_action)) self.generic_toggle_last = CS.generic_toggle if (frame % 100) == 0: can_sends.append( make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0, False)) #can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False)) if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \ (self.steer_alert_last != steer_alert): can_sends.append( create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert)) if (frame % 200) == 0: can_sends.append( make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False)) if (frame % 10) == 0: can_sends.append( make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1, False)) can_sends.append( make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False)) can_sends.append( make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False)) can_sends.append( make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False)) can_sends.append( make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False)) can_sends.append( make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False)) can_sends.append( make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False)) can_sends.append( make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1, False)) can_sends.append( make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1, False)) can_sends.append( make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1, False)) can_sends.append( make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False)) can_sends.append( make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False)) static_msgs = range(1653, 1658) for addr in static_msgs: cnt = (frame % 10) + 1 can_sends.append( make_can_msg( addr, chr(cnt << 4) + '\x00\x00\x00\x00\x00\x00\x00', 1, False)) self.enabled_last = enabled self.main_on_last = CS.main_on self.steer_alert_last = steer_alert return can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel, dragonconf): can_sends = [] steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired apply_steer = actuators.steer # 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 if self.enable_camera: if pcm_cancel: #print "CANCELING!!!!" can_sends.append(spam_cancel_button(self.packer)) if (frame % 3) == 0: curvature = self.vehicle_model.calc_curvature( actuators.steerAngle * 3.1415 / 180., CS.out.vEgo) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) self.lkas_action &= 0xf else: self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy can_sends.append( create_steer_command(self.packer, apply_steer, enabled, CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action)) self.generic_toggle_last = CS.out.genericToggle if (frame % 100) == 0: can_sends.append( make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0)) #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \ (self.steer_alert_last != steer_alert): can_sends.append( create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) if (frame % 200) == 0: can_sends.append( make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) if (frame % 10) == 0: can_sends.append( make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1)) can_sends.append( make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1)) can_sends.append( make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1)) can_sends.append( make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1)) can_sends.append( make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1)) can_sends.append( make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1)) can_sends.append( make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1)) can_sends.append( make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1)) can_sends.append( make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1)) can_sends.append( make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1)) can_sends.append( make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1)) can_sends.append( make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1)) static_msgs = range(1653, 1658) for addr in static_msgs: cnt = (frame % 10) + 1 can_sends.append( make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1)) self.enabled_last = enabled self.main_on_last = CS.out.cruiseState.available self.steer_alert_last = steer_alert return can_sends