Skip to content

Commit

Permalink
update for prius alpha
Browse files Browse the repository at this point in the history
  • Loading branch information
Edison-CBS committed Jul 28, 2023
1 parent f911e5f commit ba778e2
Show file tree
Hide file tree
Showing 53 changed files with 2,940 additions and 6,555 deletions.
14 changes: 7 additions & 7 deletions .gitmodules
Original file line number Diff line number Diff line change
@@ -1,21 +1,21 @@
[submodule "panda"]
path = panda
url = ../../commaai/panda.git
url = ../../Edison-CBS/panda.git
[submodule "opendbc"]
path = opendbc
url = ../../commaai/opendbc.git
url = ../../Edison-CBS/opendbc.git
[submodule "laika_repo"]
path = laika_repo
url = ../../commaai/laika.git
url = ../../Edison-CBS/laika.git
[submodule "cereal"]
path = cereal
url = ../../commaai/cereal.git
url = ../../Edison-CBS/cereal.git
[submodule "rednose_repo"]
path = rednose_repo
url = ../../commaai/rednose.git
url = ../../Edison-CBS/rednose.git
[submodule "body"]
path = body
url = ../../commaai/body.git
url = ../../Edison-CBS/body.git
[submodule "tinygrad"]
path = tinygrad_repo
url = https://github.com/geohot/tinygrad.git
url = ../../Edison-CBS/tinygrad.git
8 changes: 8 additions & 0 deletions common/params.cc
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"CarParamsCache", CLEAR_ON_MANAGER_START},
{"CarParamsPersistent", PERSISTENT},
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"Compass", PERSISTENT},
{"CompletedTrainingVersion", PERSISTENT},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"CurrentBootlog", PERSISTENT},
Expand All @@ -112,6 +113,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"DoReboot", CLEAR_ON_MANAGER_START},
{"DoShutdown", CLEAR_ON_MANAGER_START},
{"DoUninstall", CLEAR_ON_MANAGER_START},
{"DrivingPersonalitiesUIWheel", PERSISTENT},
{"ExperimentalLongitudinalEnabled", PERSISTENT},
{"ExperimentalMode", PERSISTENT},
{"ExperimentalModeConfirmed", PERSISTENT},
Expand Down Expand Up @@ -206,6 +208,12 @@ std::unordered_map<std::string, uint32_t> keys = {
{"Version", PERSISTENT},
{"VisionRadarToggle", PERSISTENT},
{"WheeledBody", PERSISTENT},
// edison params
{"CarBrightnessControl", PERSISTENT},
{"LQR", PERSISTENT},
{"DisableOpenpilotSounds", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
{"ScreenOffTimer", PERSISTENT},
{"CruiseSpeedRewrite", PERSISTENT},
};

} // namespace
Expand Down
7 changes: 7 additions & 0 deletions common/params.h
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,10 @@ class Params {
inline bool getBool(const std::string &key, bool block = false) {
return get(key, block) == "1";
}
inline int getInt(const std::string &key, bool block = false) {
std::string value = get(key, block);
return value.empty() ? 0 : std::stoi(value);
}
std::map<std::string, std::string> readAll();

// helpers for writing values
Expand All @@ -42,6 +46,9 @@ class Params {
inline int putBool(const std::string &key, bool val) {
return put(key.c_str(), val ? "1" : "0", 1);
}
inline int putInt(const std::string &key, int val) {
return put(key.c_str(), std::to_string(val).c_str(), std::to_string(val).size());
}

private:
std::string params_path;
Expand Down
3 changes: 2 additions & 1 deletion common/params.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking # pylint: disable=no-name-in-module, import-error
from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking, put_int_nonblocking # pylint: disable=no-name-in-module, import-error
assert Params
assert ParamKeyType
assert UnknownKeyName
assert put_nonblocking
assert put_bool_nonblocking
assert put_int_nonblocking

if __name__ == "__main__":
import sys
Expand Down
17 changes: 17 additions & 0 deletions common/params_pyx.pyx
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,11 @@ cdef extern from "common/params.h":
c_Params(string) nogil
string get(string, bool) nogil
bool getBool(string, bool) nogil
int getInt(string, bool) nogil
int remove(string) nogil
int put(string, string) nogil
int putBool(string, bool) nogil
int putInt(string, int) nogil
bool checkKey(string) nogil
string getParamPath(string) nogil
void clearAll(ParamKeyType)
Expand Down Expand Up @@ -75,6 +77,13 @@ cdef class Params:
r = self.p.getBool(k, block)
return r

def get_int(self, key, bool block=False):
cdef string k = self.check_key(key)
cdef int r
with nogil:
r = self.p.getInt(k, block)
return r

def put(self, key, dat):
"""
Warning: This function blocks until the param is written to disk!
Expand All @@ -92,6 +101,11 @@ cdef class Params:
with nogil:
self.p.putBool(k, val)

def put_int(self, key, int val):
cdef string k = self.check_key(key)
with nogil:
self.p.putInt(k, val)

def remove(self, key):
cdef string k = self.check_key(key)
with nogil:
Expand All @@ -109,3 +123,6 @@ def put_nonblocking(key, val, d=""):

def put_bool_nonblocking(key, bool val, d=""):
threading.Thread(target=lambda: Params(d).put_bool(key, val)).start()

def put_int_nonblocking(key, int val, d=""):
threading.Thread(target=lambda: Params(d).put_int(key, val)).start()
1 change: 1 addition & 0 deletions release/files_common
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,7 @@ selfdrive/controls/lib/desire_helper.py
selfdrive/controls/lib/drive_helpers.py
selfdrive/controls/lib/events.py
selfdrive/controls/lib/latcontrol_angle.py
selfdrive/controls/lib/latcontrol_lqr.py
selfdrive/controls/lib/latcontrol_torque.py
selfdrive/controls/lib/latcontrol_pid.py
selfdrive/controls/lib/latcontrol.py
Expand Down
Binary file added selfdrive/assets/aggressive.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/images/compass_inner.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/images/compass_outer.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/offroad/icon_brightness.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/offroad/icon_compass.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/offroad/icon_display_off.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/offroad/icon_distance.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/offroad/icon_speaker.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/relaxed.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added selfdrive/assets/standard.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
3 changes: 3 additions & 0 deletions selfdrive/car/tests/test_car_interfaces.py
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,9 @@ def test_car_interfaces(self, car_name, data):
self.assertTrue(len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP))
self.assertTrue(len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP))

elif tune.which() == 'lqr':
self.assertTrue(len(tune.lqr.a))

elif tune.which() == 'torque':
self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0)
self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0)
Expand Down
2 changes: 2 additions & 0 deletions selfdrive/car/tests/test_models.py
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,8 @@ def test_car_params(self):
tuning = self.CP.lateralTuning.which()
if tuning == 'pid':
self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
elif tuning == 'lqr':
self.assertTrue(len(self.CP.lateralTuning.lqr.a))
elif tuning == 'torque':
self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
elif tuning == 'indi':
Expand Down
1 change: 1 addition & 0 deletions selfdrive/car/torque_data/params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@ TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600]
TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993]
TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
TOYOTA PRIUS 2017: [1.60, 1.5023147650693636, 0.151515]
TOYOTA PRIUS v 2017 : [2.3, 1.9, 0.038]
TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968]
TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975]
TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822]
Expand Down
1 change: 0 additions & 1 deletion selfdrive/car/torque_data/substitute.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@ MAZDA CX-9: MAZDA CX-9 2021

TOYOTA ALPHARD HYBRID 2021 : TOYOTA SIENNA 2018
TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018
TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017
TOYOTA RAV4 2022: TOYOTA RAV4 HYBRID 2022
TOYOTA C-HR HYBRID 2018: TOYOTA C-HR 2018
TOYOTA C-HR HYBRID 2022: TOYOTA C-HR 2021
Expand Down
13 changes: 8 additions & 5 deletions selfdrive/car/toyota/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -132,18 +132,24 @@ def update(self, CC, CS, now_nanos):

self.last_standstill = CS.out.standstill

# handle UI messages
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
fcw_alert_acc = (hud_control.visualAlert == VisualAlert.fcw) and not self.CP.enableDsu
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)

# we can spam can to cancel the system even if we are using lat only control
if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
adjust_distance = CS.distance_btn == 1

# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
can_sends.append(create_acc_cancel_command(self.packer))
elif self.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert_acc))
self.accel = pcm_accel_cmd
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False))

if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
Expand All @@ -156,9 +162,6 @@ def update(self, CC, CS, now_nanos):
# ui mesg is at 1Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)

send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
Expand Down
49 changes: 49 additions & 0 deletions selfdrive/car/toyota/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
from common.conversions import Conversions as CV
from common.numpy_fast import mean
from common.filter_simple import FirstOrderFilter
from common.params import Params, put_int_nonblocking
from common.realtime import DT_CTRL
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
Expand Down Expand Up @@ -43,6 +44,14 @@ def __init__(self, CP):
self.acc_type = 1
self.lkas_hud = {}

# KRKeegan - Add support for toyota distance button
# FrogPilot variables
self.params = Params()
self.driving_personalities_via_wheel = self.params.get_bool("DrivingPersonalitiesUIWheel")
self.distance_btn = 0
self.distance_lines = 0
self.previous_distance_lines = 0

def update(self, cp, cp_cam):
ret = car.CarState.new_message()

Expand Down Expand Up @@ -106,6 +115,11 @@ def update(self, cp, cp_cam):
ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in TEMP_STEER_FAULTS
ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in PERM_STEER_FAULTS

# copy from cydia2020's stuff
ret.lightingSystem.parkingLightON = cp.vl["LIGHT_STALK"]['PARKING_LIGHT'] == 1
ret.lightingSystem.headlightON = cp.vl["LIGHT_STALK"]['LOW_BEAM'] == 1
ret.lightingSystem.meterDimmed = cp.vl["BODY_CONTROL_STATE"]['METER_DIMMED'] == 1
ret.lightingSystem.meterLowBrightness = cp.vl["BODY_CONTROL_STATE_2"]["METER_SLIDER_LOW_BRIGHTNESS"] == 1
if self.CP.steerControlType == SteerControlType.angle:
ret.steerFaultTemporary = ret.steerFaultTemporary or cp.vl["EPS_STATUS"]["LTA_STATE"] in TEMP_STEER_FAULTS
ret.steerFaultPermanent = ret.steerFaultPermanent or cp.vl["EPS_STATUS"]["LTA_STATE"] in PERM_STEER_FAULTS
Expand Down Expand Up @@ -162,6 +176,24 @@ def update(self, cp, cp_cam):
if self.CP.carFingerprint != CAR.PRIUS_V:
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])

# Driving personalities function
if self.driving_personalities_via_wheel:
# KRKeegan - Add support for toyota distance button
# order must be: RADAR_ACC_CAR --> TSS2_CAR --> smartDsu
# cp_acc dynamic according to car carFingerprint
if self.CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR):
if not (self.CP.flags & ToyotaFlags.SMART_DSU.value):
self.distance_btn = 1 if cp_acc.vl["ACC_CONTROL"]["DISTANCE"] == 1 else 0
# Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2"
self.distance_lines = max(cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1, 0)
elif bool(self.CP.flags & ToyotaFlags.SMART_DSU):
self.distance_btn = 1 if cp_acc.vl["SDSU"]["FD_BUTTON"] == 1 else 0
self.distance_lines = max(cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1, 0)

if self.distance_lines != self.previous_distance_lines:
put_int_nonblocking('LongitudinalPersonality', self.distance_lines)
self.previous_distance_lines = self.distance_lines

return ret

@staticmethod
Expand Down Expand Up @@ -197,6 +229,11 @@ def get_can_parser(CP):
("TURN_SIGNALS", "BLINKERS_STATE"),
("LKA_STATE", "EPS_STATUS"),
("AUTO_HIGH_BEAM", "LIGHT_STALK"),
# copy from cydia2020's
("PARKING_LIGHT", "LIGHT_STALK"),
("LOW_BEAM", "LIGHT_STALK"),
("METER_DIMMED", "BODY_CONTROL_STATE"),
("METER_SLIDER_LOW_BRIGHTNESS", "BODY_CONTROL_STATE_2"),
]

# Check LTA state if using LTA angle control
Expand Down Expand Up @@ -254,6 +291,11 @@ def get_can_parser(CP):
]
checks.append(("BSM", 1))

# KRKeegan - Add support for toyota distance button
if bool(CP.flags & ToyotaFlags.SMART_DSU):
signals.append(("FD_BUTTON", "SDSU"))
checks.append(("SDSU", 0))

if CP.carFingerprint in RADAR_ACC_CAR:
if not CP.flags & ToyotaFlags.SMART_DSU.value:
signals += [
Expand All @@ -263,9 +305,11 @@ def get_can_parser(CP):
("ACC_CONTROL", 33),
]
signals += [
("DISTANCE", 'ACC_CONTROL'),
("FCW", "ACC_HUD"),
]
checks += [
("ACC_CONTROL", 0),
("ACC_HUD", 1),
]

Expand All @@ -278,6 +322,10 @@ def get_can_parser(CP):
("PRE_COLLISION", 33),
]

if CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR | NO_STOP_TIMER_CAR):
signals.append(("DISTANCE_LINES", "PCM_CRUISE_SM"))
checks.append(("PCM_CRUISE_SM", 0))

return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)

@staticmethod
Expand All @@ -302,6 +350,7 @@ def get_cam_can_parser(CP):
("PRECOLLISION_ACTIVE", "PRE_COLLISION"),
("FORCE", "PRE_COLLISION"),
("ACC_TYPE", "ACC_CONTROL"),
("DISTANCE", 'ACC_CONTROL'),
("FCW", "ACC_HUD"),
]
checks += [
Expand Down
27 changes: 26 additions & 1 deletion selfdrive/car/toyota/interface.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
#!/usr/bin/env python3
from cereal import car
from common.conversions import Conversions as CV
from common.params import Params
from panda import Panda
from selfdrive.car.toyota.tunes import LatTunes, set_lat_tune
from selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
Expand All @@ -12,6 +14,11 @@


class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)

self.override_speed = 0.

@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
Expand Down Expand Up @@ -59,9 +66,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
elif candidate == CAR.PRIUS_V:
stop_and_go = True
ret.wheelbase = 2.78
ret.steerRatio = 17.4
ret.steerRatio = 16.8
tire_stiffness_factor = 0.5533
ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
if Params().get_bool("LQR"):
set_lat_tune(ret.lateralTuning, LatTunes.LQR_PV)
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)

elif candidate in (CAR.RAV4, CAR.RAV4H):
stop_and_go = True if (candidate in CAR.RAV4H) else False
Expand Down Expand Up @@ -273,6 +284,20 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
params = Params()

# low speed re-write
if ret.cruiseState.enabled and params.get_bool("CruiseSpeedRewrite") and \
self.CP.openpilotLongitudinalControl and ret.cruiseState.speed < 45. * CV.KPH_TO_MS:
if params.get_bool("CruiseSpeedRewrite"):
if self.override_speed == 0.:
ret.cruiseState.speed = ret.cruiseState.speedCluster = self.override_speed = max(24. * CV.KPH_TO_MS, ret.vEgo)
else:
ret.cruiseState.speed = ret.cruiseState.speedCluster = self.override_speed
else:
ret.cruiseState.speed = ret.cruiseState.speedCluster = 24. * CV.KPH_TO_MS
else:
self.override_speed = 0.

# events
events = self.create_common_events(ret)
Expand Down
Loading

0 comments on commit ba778e2

Please sign in to comment.