sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Subaru: Global gen1 experimental longitudinal (#28872)

Browse Source 
* Add longitudinal support for Subaru Crosstrek and Impreza

* Update experimentalLongitudinalAvailable check

* Update supported cars list

* bump panda

* Remove/rename es_lkas_msg to es_lkas_state_msg

* Use stockAeb for AEB passthrough

* bump panda

* bump panda

* remove stockFcw from stockAeb

* Subaru: Add FCW_Cont_Beep to stockFcw signals

* bump panda

* bump panda

* update poetry deps: shellingham

* bump panda

* bump panda

* Revert "update poetry deps: shellingham"

This reverts commit 6e9b20964890c8a5c416a17b8aaad4cc16fddcfc.

* Merge fixes

* bump panda

* bump panda

* update supported cars list

* dont use counters for long control

* fix unittests

* submodules update

* only soft disable in long control

* use common functions and cleanup

* apply hystersis correctly

* move to comma repo

* use CanBus

* cleanup

* explicit copy

* behind a flag

* remove unrequired rpm checks

* add comment

* fix flag issue

* we still need to check rpm

* update docs

* enable long for a test route

* unit tests

* inactive throttle fix

* Update subarucan.py

* Update carcontroller.py

* Update carcontroller.py

* inactive throttle fix

* Delete settings

* fix rate limit

* bump submodules

* bump panda

* bump panda

* bump panda

* bump panda

* simplify initial implementation, remove AEB

* reduce initial complexity by not intercepting cruisecontrol or brake_status

* fix fwd hook test

* show pcb off warning

* cleanup and setup for tuning

* fix sumobuldes

* revert unrelated changes

* only whats required

* only whats required

* clean that up

* better comments

* behind the flag for now

* comments and minimize diff

* align stuff

* cleanup for PR

* apply review suggestions

---------

Co-authored-by: Martin Lillepuu <martin@mlp.ee>
x-golf3
Justin Newberry 2 years ago committed by GitHub
parent e8e03197ca
commit d6c682b401
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 144 additions and 10 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 38
      selfdrive/car/subaru/carcontroller.py
  2. 8
      selfdrive/car/subaru/carstate.py
  3. 12
      selfdrive/car/subaru/interface.py
  4. 73
      selfdrive/car/subaru/subarucan.py
  5. 23
      selfdrive/car/subaru/values.py

38
selfdrive/car/subaru/carcontroller.py
Unescape View File

@ -1,3 +1,4 @@
from common.numpy_fast import clip, interp
from opendbc.can.packer import CANPacker
from selfdrive.car import apply_driver_steer_torque_limits
from selfdrive.car.subaru import subarucan
@ -42,6 +43,21 @@ class CarController:
self.apply_steer_last = apply_steer
# *** longitudinal ***
if CC.longActive:
apply_throttle = int(round(interp(actuators.accel, CarControllerParams.THROTTLE_LOOKUP_BP, CarControllerParams.THROTTLE_LOOKUP_V)))
apply_rpm = int(round(interp(actuators.accel, CarControllerParams.RPM_LOOKUP_BP, CarControllerParams.RPM_LOOKUP_V)))
apply_brake = int(round(interp(actuators.accel, CarControllerParams.BRAKE_LOOKUP_BP, CarControllerParams.BRAKE_LOOKUP_V)))
# limit min and max values
cruise_throttle = clip(apply_throttle, CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX)
cruise_rpm = clip(apply_rpm, CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX)
cruise_brake = clip(apply_brake, CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX)
else:
cruise_throttle = CarControllerParams.THROTTLE_INACTIVE
cruise_rpm = CarControllerParams.RPM_INACTIVE
cruise_brake = CarControllerParams.BRAKE_MIN
# *** alerts and pcm cancel ***
if self.CP.carFingerprint in PREGLOBAL_CARS:
@ -64,13 +80,9 @@ class CarController:
can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
else:
if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))
self.last_cancel_frame = self.frame
if self.frame % 10 == 0:
can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg))
can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg, CC.enabled, self.CP.openpilotLongitudinalControl,
CC.longActive, hud_control.leadVisible))
can_sends.append(subarucan.create_es_lkas_state(self.packer, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
@ -79,6 +91,20 @@ class CarController:
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert))
if self.CP.openpilotLongitudinalControl:
if self.frame % 5 == 0:
can_sends.append(subarucan.create_es_status(self.packer, CS.es_status_msg, self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm))
can_sends.append(subarucan.create_es_brake(self.packer, CS.es_brake_msg, CC.enabled, cruise_brake))
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, 0, pcm_cancel_cmd,
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
else:
if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))
self.last_cancel_frame = self.frame
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last

8
selfdrive/car/subaru/carstate.py
Unescape View File

@ -91,6 +91,12 @@ class CarState(CarStateBase):
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
self.brake_status_msg = copy.copy(cp_brakes.vl["Brake_Status"])
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
@ -114,6 +120,8 @@ class CarState(CarStateBase):
messages = [
("ES_Brake", 20),
("ES_Distance", 20),
("ES_Status", 20),
("ES_Brake", 20),
]
return messages

12
selfdrive/car/subaru/interface.py
Unescape View File

@ -107,6 +107,18 @@ class CarInterface(CarInterfaceBase):
else:
raise ValueError(f"unknown car: {candidate}")
#ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
if ret.openpilotLongitudinalControl:
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
ret.stoppingControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
return ret
# returns a car.CarState

73
selfdrive/car/subaru/subarucan.py
Unescape View File

@ -16,8 +16,7 @@ def create_steering_control(packer, apply_steer, steer_req):
def create_steering_status(packer):
return packer.make_can_msg("ES_LKAS_State", 0, {})
def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
values = {s: es_distance_msg[s] for s in [
"CHECKSUM",
"COUNTER",
@ -41,9 +40,20 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
"Signal6",
]}
values["COUNTER"] = (values["COUNTER"] + 1) % 0x10
if long_enabled:
values["Cruise_Throttle"] = cruise_throttle
# Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long
values["Cruise_Soft_Disable"] = 0
if brake_cmd:
values["Cruise_Brake_Active"] = 1
if pcm_cancel_cmd:
values["Cruise_Cancel"] = 1
values["Cruise_Throttle"] = 1818 # inactive throttle
return packer.make_can_msg("ES_Distance", bus, values)
@ -106,8 +116,7 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_
return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
def create_es_dashstatus(packer, dashstatus_msg):
def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
values = {s: dashstatus_msg[s] for s in [
"CHECKSUM",
"COUNTER",
@ -138,12 +147,68 @@ def create_es_dashstatus(packer, dashstatus_msg):
"Cruise_State",
]}
if enabled and long_active:
values["Cruise_State"] = 0
values["Cruise_Activated"] = 1
values["Cruise_Disengaged"] = 0
values["Car_Follow"] = int(lead_visible)
if long_enabled:
values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
if values["LKAS_State_Msg"] in (2, 3):
values["LKAS_State_Msg"] = 0
return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
def create_es_brake(packer, es_brake_msg, enabled, brake_value):
values = {s: es_brake_msg[s] for s in [
"CHECKSUM",
"COUNTER",
"Signal1",
"Brake_Pressure",
"AEB_Status",
"Cruise_Brake_Lights",
"Cruise_Brake_Fault",
"Cruise_Brake_Active",
"Cruise_Activated",
"Signal3",
]}
if enabled:
values["Cruise_Activated"] = 1
values["Brake_Pressure"] = brake_value
if brake_value > 0:
values["Cruise_Brake_Active"] = 1
values["Cruise_Brake_Lights"] = 1 if brake_value >= 70 else 0
return packer.make_can_msg("ES_Brake", CanBus.main, values)
def create_es_status(packer, es_status_msg, long_enabled, long_active, cruise_rpm):
values = {s: es_status_msg[s] for s in [
"CHECKSUM",
"COUNTER",
"Signal1",
"Cruise_Fault",
"Cruise_RPM",
"Signal2",
"Cruise_Activated",
"Brake_Lights",
"Cruise_Hold",
"Signal3",
]}
if long_enabled:
values["Cruise_RPM"] = cruise_rpm
if long_active:
values["Cruise_Activated"] = 1
return packer.make_can_msg("ES_Status", CanBus.main, values)
def create_es_infotainment(packer, es_infotainment_msg, visual_alert):
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts

23
selfdrive/car/subaru/values.py
Unescape View File

@ -29,6 +29,29 @@ class CarControllerParams:
else:
self.STEER_MAX = 2047
THROTTLE_MIN = 808
THROTTLE_MAX = 3400
THROTTLE_INACTIVE = 1818 # corresponds to zero acceleration
THROTTLE_ENGINE_BRAKE = 808 # while braking, eyesight sets throttle to this, probably for engine braking
BRAKE_MIN = 0
BRAKE_MAX = 600 # about -3.5m/s2 from testing
RPM_MIN = 0
RPM_MAX = 2400
RPM_INACTIVE = 600 # a good base rpm for zero acceleration
THROTTLE_LOOKUP_BP = [0, 1]
THROTTLE_LOOKUP_V = [THROTTLE_INACTIVE, THROTTLE_MAX]
RPM_LOOKUP_BP = [0, 1]
RPM_LOOKUP_V = [RPM_INACTIVE, RPM_MAX]
BRAKE_LOOKUP_BP = [-1, 0]
BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN]
class SubaruFlags(IntFlag):
SEND_INFOTAINMENT = 1

Write Preview
Loading…
Cancel
Save