Skip to content

Commit

Permalink
Add Subaru SNG support
Browse files Browse the repository at this point in the history
  • Loading branch information
@martinl
martinl committed Jan 4, 2022
1 parent 5264485 commit c249cbd
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 2 deletions.
57 changes: 57 additions & 0 deletions selfdrive/car/subaru/carcontroller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,14 @@ def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.es_distance_cnt = -1
self.es_lkas_cnt = -1
self.throttle_cnt = -1
self.cruise_button_prev = 0
self.steer_rate_limited = False
self.sng_acc_resume = False
self.sng_acc_resume_cnt = -1
self.manual_hold = False
self.prev_cruise_state = 0
self.prev_close_distance = 0

self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
Expand Down Expand Up @@ -40,6 +46,49 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le

self.apply_steer_last = apply_steer

# *** stop and go ***

throttle_cmd = False

if CS.CP.carFingerprint in PREGLOBAL_CARS:
if (enabled # ACC active
and CS.car_follow == 1 # lead car
and CS.out.standstill # must be standing still
and CS.close_distance > 3 # acc resume trigger threshold
and CS.close_distance < 4.5 # max operating distance to filter false positives
and CS.close_distance > self.prev_close_distance): # distance with lead car is increasing
self.sng_acc_resume = True
elif CS.CP.carFingerprint not in PREGLOBAL_CARS:
# Record manual hold set while in standstill and no car in front
if CS.out.standstill and self.prev_cruise_state == 1 and CS.cruise_state == 3 and CS.car_follow == 0:
self.manual_hold = True
# Cancel manual hold when car starts moving
if not CS.out.standstill:
self.manual_hold = False
if (enabled # ACC active
and not self.manual_hold
and CS.car_follow == 1 # lead car
and CS.cruise_state == 3 # ACC HOLD (only with EPB)
and CS.out.standstill # must be standing still
and CS.close_distance > 150 # acc resume trigger threshold
and CS.close_distance < 255 # ignore max value
and CS.close_distance > self.prev_close_distance): # distance with lead car is increasing
self.sng_acc_resume = True
self.prev_cruise_state = CS.cruise_state

if self.sng_acc_resume:
if self.sng_acc_resume_cnt < 5:
throttle_cmd = True
self.sng_acc_resume_cnt += 1
else:
self.sng_acc_resume = False
self.sng_acc_resume_cnt = -1

# Cancel ACC if stopped, brake pressed and not stopped behind another car
if enabled and CS.out.brakePressed and CS.car_follow == 0 and CS.out.standstill:
pcm_cancel_cmd = True

self.prev_close_distance = CS.close_distance

# *** alerts and pcm cancel ***

Expand All @@ -63,6 +112,10 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
self.es_distance_cnt = CS.es_distance_msg["Counter"]

if self.throttle_cnt != CS.throttle_msg["Counter"]:
can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg, throttle_cmd))
self.throttle_cnt = CS.throttle_msg["Counter"]

else:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
Expand All @@ -72,6 +125,10 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]

if self.throttle_cnt != CS.throttle_msg["Counter"]:
can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd))
self.throttle_cnt = CS.throttle_msg["Counter"]

new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX

Expand Down
33 changes: 31 additions & 2 deletions selfdrive/car/subaru/carstate.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,10 @@ def update(self, cp, cp_cam):
ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"]
self.car_follow = cp_cam.vl["ES_Distance"]["Car_Follow"]
self.close_distance = cp_cam.vl["ES_Distance"]["Close_Distance"]
self.throttle_msg = copy.copy(cp.vl["Throttle"])
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])

return ret
Expand All @@ -85,7 +89,6 @@ def get_can_parser(CP):
("Steer_Error_1", "Steering_Torque", 0),
("Cruise_On", "CruiseControl", 0),
("Cruise_Activated", "CruiseControl", 0),
("Brake_Pedal", "Brake_Pedal", 0),
("Throttle_Pedal", "Throttle", 0),
("LEFT_BLINKER", "Dashlights", 0),
("RIGHT_BLINKER", "Dashlights", 0),
Expand All @@ -105,7 +108,6 @@ def get_can_parser(CP):
# sig_address, frequency
("Throttle", 100),
("Dashlights", 10),
("Brake_Pedal", 50),
("Wheel_Speeds", 50),
("Transmission", 100),
("Steering_Torque", 50),
Expand All @@ -125,6 +127,16 @@ def get_can_parser(CP):

if CP.carFingerprint not in PREGLOBAL_CARS:
signals += [
("Counter", "Throttle", 0),
("Signal1", "Throttle", 0),
("Engine_RPM", "Throttle", 0),
("Signal2", "Throttle", 0),
("Throttle_Pedal", "Throttle", 0),
("Throttle_Cruise", "Throttle", 0),
("Throttle_Combo", "Throttle", 0),
("Signal1", "Throttle", 0),
("Off_Accel", "Throttle", 0),

("Steer_Warning", "Steering_Torque", 0),
("Brake", "Brake_Status", 0),
("UNITS", "Dashlights", 0),
Expand All @@ -138,11 +150,27 @@ def get_can_parser(CP):
]
else:
signals += [
("Throttle_Pedal", "Throttle", 0),
("Counter", "Throttle", 0),
("Signal1", "Throttle", 0),
("Not_Full_Throttle", "Throttle", 0),
("Signal2", "Throttle", 0),
("Engine_RPM", "Throttle", 0),
("Off_Throttle", "Throttle", 0),
("Signal3", "Throttle", 0),
("Throttle_Cruise", "Throttle", 0),
("Throttle_Combo", "Throttle", 0),
("Throttle_Body", "Throttle", 0),
("Off_Throttle_2", "Throttle", 0),
("Signal4", "Throttle", 0),

("UNITS", "Dash_State2", 0),
("Brake_Pedal", "Brake_Pedal", 0),
]

checks += [
("Dash_State2", 1),
("Brake_Pedal", 50),
]

if CP.carFingerprint == CAR.FORESTER_PREGLOBAL:
Expand Down Expand Up @@ -194,6 +222,7 @@ def get_cam_can_parser(CP):
signals = [
("Cruise_Set_Speed", "ES_DashStatus", 0),
("Conventional_Cruise", "ES_DashStatus", 0),
("Cruise_State", "ES_DashStatus", 0),

("Counter", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
Expand Down
19 changes: 19 additions & 0 deletions selfdrive/car/subaru/subarucan.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,15 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_

return packer.make_can_msg("ES_LKAS_State", 0, values)

def create_throttle(packer, throttle_msg, throttle_cmd):

values = copy.copy(throttle_msg)
if throttle_cmd:
values["Throttle_Pedal"] = 5

return packer.make_can_msg("Throttle", 2, values)


# *** Subaru Pre-global ***

def subaru_preglobal_checksum(packer, values, addr):
Expand Down Expand Up @@ -88,3 +97,13 @@ def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance")

return packer.make_can_msg("ES_Distance", 0, values)

def create_preglobal_throttle(packer, throttle_msg, throttle_cmd):

values = copy.copy(throttle_msg)
if throttle_cmd:
values["Throttle_Pedal"] = 5

values["Checksum"] = subaru_preglobal_checksum(packer, values, "Throttle")

return packer.make_can_msg("Throttle", 2, values)

0 comments on commit c249cbd

Please sign in to comment.