mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-21 08:03:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot beta3

Browse Source 
date: 2023-07-26T22:20:36
commit: c6d842c412052be1985b63d683c63be9dcb2b0eb
This commit is contained in:
dragonpilot
2023-07-26 22:14:57 +08:00
parent 67c2c03b43
commit 1abc7d7daa
536 changed files with 437552 additions and 24400 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/assets/images/triangle.svg
+3 -3
View File
@@ -21,12 +21,12 @@
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
</cc:Work>
</rdf:RDF>
</metadata>
<defs
id="defs6" />
id="defs6" />
<sodipodi:namedview
pagecolor="#ffffff"
bordercolor="#666666"
@@ -51,6 +51,6 @@
id="rect3338"
d="m 38.008927,0.00304862 c -1.672524,-0.07115 -3.24191,1.10742798 -4.103539,3.08128498 L 0.63797357,79.511649 c -1.85216397,4.240355 0.59099403,9.61933 3.95882353,9.642068 3.3678291,0.02287 28.1477799,-24.380023 33.9100889,-24.380023 5.762307,0 29.946434,24.380314 32.624742,24.380023 2.678305,-2.9e-4 3.249598,-1.351011 4.021616,-3.231338 0.772007,-1.880328 0.863286,-4.290554 -0.06281,-6.41073 L 58.456744,41.297993 41.823037,3.0843336 C 41.013805,1.2321186 39.577483,0.07190362 38.008927,0.00304862 Z"
inkscape:connector-curvature="0"
style="fill:#25DA6E;stroke-width:9.76952076"
style="fill:#cccccc;stroke-width:9.76952076"
sodipodi:nodetypes="ccczzzscccc" />
</svg>
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.1 KiB

After

Width:  |  Height:  |  Size: 2.1 KiB

selfdrive/assets/img_turn_left_icon.png
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.1 KiB

selfdrive/assets/img_turn_right_icon.png
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.1 KiB

selfdrive/assets/img_world_icon.png
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 3.6 KiB

selfdrive/assets/locales/ja-JP/LC_MESSAGES/events.mo
BIN
View File
Binary file not shown.
selfdrive/assets/locales/ko-KR/LC_MESSAGES/events.mo
BIN
View File
Binary file not shown.
selfdrive/assets/locales/zh-CN/LC_MESSAGES/events.mo
BIN
View File
Binary file not shown.
selfdrive/assets/locales/zh-TW/LC_MESSAGES/events.mo
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/rec_off.wav
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/rec_on.wav
BIN
View File
Binary file not shown.
selfdrive/athena/athenad.py
+42 -9
View File
@@ -42,6 +42,10 @@ from selfdrive.statsd import STATS_DIR
from system.swaglog import SWAGLOG_DIR, cloudlog
from system.version import get_commit, get_origin, get_short_branch, get_version
# missing in pysocket
TCP_USER_TIMEOUT = 18
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
LOCAL_PORT_WHITELIST = {8022}
@@ -137,10 +141,11 @@ class UploadQueueCache:
cloudlog.exception("athena.UploadQueueCache.cache.exception")
def handle_long_poll(ws: WebSocket) -> None:
def handle_long_poll(ws: WebSocket, exit_event: Optional[threading.Event]) -> None:
end_event = threading.Event()
threads = [
threading.Thread(target=ws_manage, args=(ws, end_event), name='ws_manage'),
threading.Thread(target=ws_recv, args=(ws, end_event), name='ws_recv'),
threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'),
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
@@ -154,8 +159,9 @@ def handle_long_poll(ws: WebSocket) -> None:
for thread in threads:
thread.start()
try:
while not end_event.is_set():
time.sleep(0.1)
while not end_event.wait(0.1):
if exit_event is not None and exit_event.is_set():
end_event.set()
except (KeyboardInterrupt, SystemExit):
end_event.set()
raise
@@ -754,11 +760,30 @@ def ws_send(ws: WebSocket, end_event: threading.Event) -> None:
end_event.set()
def ws_manage(ws: WebSocket, end_event: threading.Event) -> None:
params = Params()
onroad_prev = None
sock = ws.sock
while True:
onroad = params.get_bool("IsOnroad")
if onroad != onroad_prev:
onroad_prev = onroad
sock.setsockopt(socket.IPPROTO_TCP, TCP_USER_TIMEOUT, 16000 if onroad else 0)
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 7 if onroad else 30)
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 7 if onroad else 10)
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 2 if onroad else 3)
if end_event.wait(5):
break
def backoff(retries: int) -> int:
return random.randrange(0, min(128, int(2 ** retries)))
def main():
def main(exit_event: Optional[threading.Event] = None):
try:
set_core_affinity([0, 1, 2, 3])
except Exception:
@@ -771,26 +796,34 @@ def main():
ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
api = Api(dongle_id)
conn_start = None
conn_retries = 0
while 1:
while exit_event is None or not exit_event.is_set():
try:
cloudlog.event("athenad.main.connecting_ws", ws_uri=ws_uri)
if conn_start is None:
conn_start = time.monotonic()
cloudlog.event("athenad.main.connecting_ws", ws_uri=ws_uri, retries=conn_retries)
ws = create_connection(ws_uri,
cookie="jwt=" + api.get_token(),
enable_multithread=True,
timeout=30.0)
cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri)
cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri, retries=conn_retries,
duration=time.monotonic() - conn_start)
conn_start = None
conn_retries = 0
cur_upload_items.clear()
handle_long_poll(ws)
handle_long_poll(ws, exit_event)
except (KeyboardInterrupt, SystemExit):
break
except (ConnectionError, TimeoutError, WebSocketException):
conn_retries += 1
params.remove("LastAthenaPingTime")
except socket.timeout:
# TODO: socket.timeout and TimeoutError are now the same exception since python3.10
# Remove the socket.timeout case once we have fully moved to python3.11
except socket.timeout: # pylint: disable=duplicate-except
params.remove("LastAthenaPingTime")
except Exception:
cloudlog.exception("athenad.main.exception")
selfdrive/athena/registration.py
+1 -2
View File
@@ -31,8 +31,7 @@ def register(show_spinner=False) -> Optional[str]:
HardwareSerial = params.get("HardwareSerial", encoding='utf8')
dongle_id: Optional[str] = params.get("DongleId", encoding='utf8')
needs_registration = None in (IMEI, HardwareSerial, dongle_id)
return UNREGISTERED_DONGLE_ID
pubkey = Path(PERSIST+"/comma/id_rsa.pub")
if not pubkey.is_file():
dongle_id = UNREGISTERED_DONGLE_ID
selfdrive/boardd/boardd
BIN
View File
Binary file not shown.
selfdrive/boardd/boardd_api_impl.cpp
+6373
View File
File diff suppressed because it is too large Load Diff
selfdrive/boardd/boardd_api_impl.pyx
+8 -6
View File
@@ -4,13 +4,15 @@ from libcpp.vector cimport vector
from libcpp.string cimport string
from libcpp cimport bool
cdef struct can_frame:
long address
string dat
long busTime
long src
cdef extern from "panda.h":
cdef struct can_frame:
long address
string dat
long busTime
long src
cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid)
cdef extern from "can_list_to_can_capnp.cc":
void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid)
def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
cdef vector[can_frame] can_list
selfdrive/boardd/boardd_api_impl.so
BIN
View File
Binary file not shown.
selfdrive/boardd/panda.h
+1
View File
@@ -72,6 +72,7 @@ public:
std::optional<can_health_t> get_can_state(uint16_t can_number);
void set_loopback(bool loopback);
std::optional<std::vector<uint8_t>> get_firmware_version();
bool up_to_date();
std::optional<std::string> get_serial();
void set_power_saving(bool power_saving);
void enable_deepsleep();
selfdrive/boardd/panda_comms.h
+2 -2
View File
@@ -14,7 +14,7 @@
#define TIMEOUT 0
#define SPI_BUF_SIZE 1024
#define SPI_BUF_SIZE 2048
// comms base class
@@ -74,7 +74,7 @@ private:
uint8_t rx_buf[SPI_BUF_SIZE];
inline static std::recursive_mutex hw_lock;
int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout);
int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length);
int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout);
int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
selfdrive/boardd/pandad.py
+57 -4
View File
@@ -3,11 +3,12 @@
import os
import usb1
import time
import json
import subprocess
from typing import List, NoReturn
from functools import cmp_to_key
from panda import Panda, PandaDFU, FW_PATH
from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
from common.basedir import BASEDIR
from common.params import Params
from selfdrive.boardd.set_time import set_time
@@ -23,9 +24,56 @@ def get_expected_signature(panda: Panda) -> bytes:
cloudlog.exception("Error computing expected signature")
return b""
def read_panda_logs(panda: Panda) -> None:
"""
Forward panda logs to the cloud
"""
params = Params()
serial = panda.get_usb_serial()
log_state = {}
try:
l = json.loads(params.get("PandaLogState"))
for k, v in l.items():
if isinstance(k, str) and isinstance(v, int):
log_state[k] = v
except (TypeError, json.JSONDecodeError):
cloudlog.exception("failed to parse PandaLogState")
try:
if serial in log_state:
logs = panda.get_logs(last_id=log_state[serial])
else:
logs = panda.get_logs(get_all=True)
# truncate logs to 100 entries if needed
MAX_LOGS = 100
if len(logs) > MAX_LOGS:
cloudlog.warning(f"Panda {serial} has {len(logs)} logs, truncating to {MAX_LOGS}")
logs = logs[-MAX_LOGS:]
# update log state
if len(logs) > 0:
log_state[serial] = logs[-1]["id"]
for log in logs:
if log['timestamp'] is not None:
log['timestamp'] = log['timestamp'].isoformat()
cloudlog.event("panda_log", **log, serial=serial)
params.put("PandaLogState", json.dumps(log_state))
except Exception:
cloudlog.exception(f"Error getting logs for panda {serial}")
def flash_panda(panda_serial: str) -> Panda:
panda = Panda(panda_serial)
try:
panda = Panda(panda_serial)
except PandaProtocolMismatch:
cloudlog.warning("detected protocol mismatch, reflashing panda")
HARDWARE.recover_internal_panda()
raise
fw_signature = get_expected_signature(panda)
internal_panda = panda.is_internal()
@@ -45,7 +93,7 @@ def flash_panda(panda_serial: str) -> Panda:
if internal_panda:
HARDWARE.recover_internal_panda()
panda.recover(reset=(not internal_panda))
cloudlog.info("Done flashing bootloader")
cloudlog.info("Done flashing bootstub")
if panda.bootstub:
cloudlog.info("Panda still not booting, exiting")
@@ -121,7 +169,7 @@ def main() -> NoReturn:
# sort pandas to have deterministic order
pandas.sort(key=cmp_to_key(panda_sort_cmp))
panda_serials = list(map(lambda p: p.get_usb_serial(), pandas)) # type: ignore
panda_serials = list(map(lambda p: p.get_usb_serial(), pandas))
# log panda fw versions
params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas))
@@ -133,6 +181,8 @@ def main() -> NoReturn:
params.put_bool("PandaHeartbeatLost", True)
cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
read_panda_logs(panda)
if first_run:
if panda.is_internal():
# update time from RTC
@@ -152,6 +202,9 @@ def main() -> NoReturn:
# a panda was disconnected while setting everything up. let's try again
cloudlog.exception("Panda USB exception while setting up")
continue
except PandaProtocolMismatch:
cloudlog.exception("pandad.protocol_mismatch")
continue
except Exception:
cloudlog.exception("pandad.uncaught_exception")
continue
selfdrive/boardd/tests/test_boardd_loopback.py
+1 -1
View File
@@ -36,7 +36,7 @@ class TestBoardd(unittest.TestCase):
params = Params()
params.put_bool("IsOnroad", False)
with Timeout(60, "boardd didn't start"):
with Timeout(90, "boardd didn't start"):
sm = messaging.SubMaster(['pandaStates'])
while sm.rcv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
selfdrive/car/__init__.py
+1 -2
View File
@@ -1,5 +1,4 @@
# functions common among cars
import math
from collections import namedtuple
from typing import Dict, Optional
@@ -186,7 +185,7 @@ class CanBusBase:
def __init__(self, CP, fingerprint: Optional[Dict[int, Dict[int, int]]]) -> None:
if CP is None:
assert fingerprint is not None
num = math.ceil(max([k for k, v in fingerprint.items() if len(v)], default=1) / 4)
num = max([k for k, v in fingerprint.items() if len(v)], default=0) // 4 + 1
else:
num = len(CP.safetyConfigs)
self.offset = 4 * (num - 1)
selfdrive/car/body/carcontroller.py
+16 -8
View File
@@ -1,5 +1,6 @@
import numpy as np
from common.params import Params
from common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from selfdrive.car.body import bodycan
@@ -23,10 +24,14 @@ class CarController:
self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL)
self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL)
self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
self.torque_r_filtered = 0.
self.torque_l_filtered = 0.
params = Params()
self.wheeled_body = params.get("WheeledBody")
@staticmethod
def deadband_filter(torque, deadband):
if torque > 0:
@@ -45,19 +50,22 @@ class CarController:
# Read these from the joystick
# TODO: this isn't acceleration, okay?
speed_desired = CC.actuators.accel / 5.
speed_diff_desired = -CC.actuators.steer
speed_diff_desired = -CC.actuators.steer / 2.
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
speed_error = speed_desired - speed_measured
freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or
(speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator)
if self.wheeled_body is None:
freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or
(speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator)
# Clip angle error, this is enough to get up from stands
angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate)
# Clip angle error, this is enough to get up from stands
angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate)
else:
torque = self.wheeled_speed_pid.update(speed_error, freeze_integrator=False)
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
turn_error = speed_diff_measured - speed_diff_desired
selfdrive/car/car_helpers.py
+14 -63
View File
@@ -4,7 +4,7 @@ from typing import Dict, List
from cereal import car
from common.params import Params
from common.basedir import BASEDIR
# from system.version import is_comma_remote, is_tested_branch
from system.version import is_comma_remote, is_tested_branch
from selfdrive.car.interfaces import get_interface_attr
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
@@ -13,11 +13,6 @@ from system.swaglog import cloudlog
import cereal.messaging as messaging
from selfdrive.car import gen_empty_fingerprint
import threading
import requests
import time
import selfdrive.sentry as sentry
EventName = car.CarEvent.EventName
@@ -90,18 +85,18 @@ def fingerprint(logcan, sendcan, num_pandas):
dp_car_assigned = Params().get('dp_car_assigned', encoding='utf8')
if not fixed_fingerprint and dp_car_assigned is not None:
car_selected = dp_car_assigned.strip()
fixed_fingerprint = car_selected
fixed_fingerprint = dp_car_assigned.strip()
skip_fw_query = True
if not fixed_fingerprint and not skip_fw_query:
if not skip_fw_query:
# Vin query only reliably works through OBDII
bus = 1
cached_params = params.get("CarParamsCache")
if cached_params is not None:
cached_params = car.CarParams.from_bytes(cached_params)
if cached_params.carName == "mock":
cached_params = None
with car.CarParams.from_bytes(cached_params) as cached_params:
if cached_params.carName == "mock":
cached_params = None
if cached_params is not None and len(cached_params.carFw) > 0 and \
cached_params.carVin is not VIN_UNKNOWN and not disable_fw_cache:
@@ -190,41 +185,6 @@ def fingerprint(logcan, sendcan, num_pandas):
fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True)
return car_fingerprint, finger, vin, car_fw, source, exact_match
#dp
def is_connected_to_internet(timeout=5):
try:
requests.get("https://sentry.io", timeout=timeout)
return True
except Exception:
return False
def crash_log(candidate):
no_internet = 0
while True:
if is_connected_to_internet():
sentry.capture_warning("fingerprinted %s" % candidate)
break
else:
no_internet += 1
if no_internet >= 2:
break
time.sleep(600)
def crash_log2(fingerprints, fw):
no_internet = 0
while True:
if is_connected_to_internet():
sentry.capture_warning("car doesn't match any fingerprints: %s" % fingerprints)
sentry.capture_warning("car doesn't match any fw: %s" % fw)
break
else:
no_internet += 1
if no_internet >= 2:
break
time.sleep(600)
def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1):
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas)
@@ -233,20 +193,11 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1):
cloudlog.event("car doesn't match any fingerprints", fingerprints=fingerprints, error=True)
candidate = "mock"
y = threading.Thread(target=crash_log2, args=(fingerprints,car_fw,))
y.start()
CarInterface, CarController, CarState = interfaces[candidate]
CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False)
CP.carVin = vin
CP.carFw = car_fw
CP.fingerprintSource = source
CP.fuzzyFingerprint = not exact_match
x = threading.Thread(target=crash_log, args=(candidate,))
x.start()
try:
CarInterface, CarController, CarState = interfaces[candidate]
CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False)
CP.carVin = vin
CP.carFw = car_fw
CP.fingerprintSource = source
CP.fuzzyFingerprint = not exact_match
return CarInterface(CP, CarController, CarState), CP
except KeyError:
return None, None
return CarInterface(CP, CarController, CarState), CP
selfdrive/car/chrysler/interface.py
-3
View File
@@ -78,9 +78,6 @@ class CarInterface(CarInterfaceBase):
else:
raise ValueError(f"Unsupported car: {candidate}")
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
# TODO: allow these cars to steer down to 13 m/s if already engaged.
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
selfdrive/car/disable_ecu.py
+4 -4
View File
@@ -7,22 +7,22 @@ EXT_DIAG_RESPONSE = b'\x50\x03'
COM_CONT_RESPONSE = b''
def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False):
def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False):
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
The ECU will stay silent as long as openpilot keeps sending Tester Present.
This is used to disable the radar in some cars. Openpilot will emulate the radar.
WARNING: THIS DISABLES AEB!"""
cloudlog.warning(f"ecu disable {hex(addr)} ...")
cloudlog.warning(f"ecu disable {hex(addr), sub_addr} ...")
for i in range(retry):
try:
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
for _, _ in query.get_data(timeout).items():
cloudlog.warning("communication control disable tx/rx ...")
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [com_cont_req], [COM_CONT_RESPONSE], debug=debug)
query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], debug=debug)
query.get_data(0)
cloudlog.warning("ecu disabled")
selfdrive/car/docs_definitions.py
+1 -1
View File
@@ -149,7 +149,7 @@ class CarParts:
return copy.deepcopy(self)
@classmethod
def common(cls, add: List[EnumBase] = None, remove: List[EnumBase] = None):
def common(cls, add: Optional[List[EnumBase]] = None, remove: Optional[List[EnumBase]] = None):
p = [part for part in (add or []) + DEFAULT_CAR_PARTS if part not in (remove or [])]
return cls(p)
selfdrive/car/ford/carcontroller.py
+2 -2
View File
@@ -4,7 +4,7 @@ from opendbc.can.packer import CANPacker
from selfdrive.car import apply_std_steer_angle_limits
from selfdrive.car.ford.fordcan import CanBus, create_acc_msg, create_acc_ui_msg, create_button_msg, \
create_lat_ctl_msg, create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg
from selfdrive.car.ford.values import CANFD_CARS, CarControllerParams
from selfdrive.car.ford.values import CANFD_CAR, CarControllerParams
LongCtrlState = car.CarControl.Actuators.LongControlState
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -69,7 +69,7 @@ class CarController:
self.apply_curvature_last = apply_curvature
if self.CP.carFingerprint in CANFD_CARS:
if self.CP.carFingerprint in CANFD_CAR:
# TODO: extended mode
mode = 1 if CC.latActive else 0
counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF
selfdrive/car/ford/carstate.py
+27 -5
View File
@@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.ford.fordcan import CanBus
from selfdrive.car.ford.values import DBC, CarControllerParams
from selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
@@ -55,6 +55,10 @@ class CarState(CarStateBase):
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
# ret.espDisabled = False # TODO: find traction control signal
if self.CP.carFingerprint in CANFD_CAR:
# this signal is always 0 on non-CAN FD cars
ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
# cruise state
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
@@ -93,8 +97,9 @@ class CarState(CarStateBase):
# blindspot sensors
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CAR else cp
ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
# Stock steering buttons so that we can passthru blinkers etc.
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
@@ -181,12 +186,19 @@ class CarState(CarStateBase):
("Cluster_Info1_FD1", 10),
("SteeringPinion_Data", 100),
("EPAS_INFO", 50),
("Lane_Assist_Data3_FD1", 33),
("Steering_Data_FD1", 10),
("BodyInfo_3_FD1", 2),
("RCMStatusMessage2_FD1", 10),
]
if CP.carFingerprint in CANFD_CAR:
signals += [
("LatCtlSte_D_Stat", "Lane_Assist_Data3_FD1"), # PSCM lateral control status
]
checks += [
("Lane_Assist_Data3_FD1", 33),
]
if CP.transmissionType == TransmissionType.automatic:
signals += [
("TrnRng_D_RqGsm", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position
@@ -204,7 +216,7 @@ class CarState(CarStateBase):
("BCM_Lamp_Stat_FD1", 1),
]
if CP.enableBsm:
if CP.enableBsm and CP.carFingerprint not in CANFD_CAR:
signals += [
("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left
("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right
@@ -274,4 +286,14 @@ class CarState(CarStateBase):
("IPMA_Data", 1),
]
if CP.enableBsm and CP.carFingerprint in CANFD_CAR:
signals += [
("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left
("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right
]
checks += [
("Side_Detect_L_Stat", 5),
("Side_Detect_R_Stat", 5),
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).camera)
selfdrive/car/ford/interface.py
+11 -5
View File
@@ -4,7 +4,7 @@ from panda import Panda
from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.ford.fordcan import CanBus
from selfdrive.car.ford.values import CAR, Ecu
from selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu
from selfdrive.car.interfaces import CarInterfaceBase
TransmissionType = car.CarParams.TransmissionType
@@ -15,6 +15,7 @@ class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "ford"
ret.dashcamOnly = candidate in {CAR.F_150_MK14}
ret.radarUnavailable = True
ret.steerControlType = car.CarParams.SteerControlType.angle
@@ -32,6 +33,9 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL
ret.openpilotLongitudinalControl = True
if candidate in CANFD_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD
if candidate == CAR.BRONCO_SPORT_MK1:
ret.wheelbase = 2.67
ret.steerRatio = 17.7
@@ -47,6 +51,12 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 16.8
ret.mass = 2050 + STD_CARGO_KG
elif candidate == CAR.F_150_MK14:
# required trim only on SuperCrew
ret.wheelbase = 3.69
ret.steerRatio = 17.0
ret.mass = 2000 + STD_CARGO_KG
elif candidate == CAR.FOCUS_MK4:
ret.wheelbase = 2.7
ret.steerRatio = 15.0
@@ -60,9 +70,6 @@ class CarInterface(CarInterfaceBase):
else:
raise ValueError(f"Unsupported car: {candidate}")
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
# Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
found_ecus = [fw.ecu for fw in car_fw]
if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs:
@@ -86,7 +93,6 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam)
events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
events = self.dp_atl_warning(ret, events)
if not self.CS.vehicle_sensors_valid:
events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
if self.CS.hybrid_platform:
selfdrive/car/ford/values.py
+34 -5
View File
@@ -1,7 +1,7 @@
from collections import defaultdict
from dataclasses import dataclass, field
from enum import Enum
from typing import Dict, List, Set, Union
from typing import Dict, List, Union
from cereal import car
from selfdrive.car import AngleRateLimit, dbc_dict
@@ -44,11 +44,12 @@ class CAR:
BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN"
ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
EXPLORER_MK6 = "FORD EXPLORER 6TH GEN"
F_150_MK14 = "FORD F-150 14TH GEN"
FOCUS_MK4 = "FORD FOCUS 4TH GEN"
MAVERICK_MK1 = "FORD MAVERICK 1ST GEN"
CANFD_CARS: Set[str] = set()
CANFD_CAR = {CAR.F_150_MK14}
class RADAR:
@@ -58,6 +59,9 @@ class RADAR:
DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR))
# F-150 radar is not yet supported
DBC[CAR.F_150_MK14] = dbc_dict("ford_lincoln_base_pt", None)
class Footnote(Enum):
FOCUS = CarFootnote(
@@ -87,22 +91,28 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
FordCarInfo("Ford Explorer 2020-22"),
FordCarInfo("Lincoln Aviator 2020-21", "Co-Pilot360 Plus"),
],
CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"),
CAR.FOCUS_MK4: FordCarInfo("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS]),
CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022-23", "Co-Pilot360 Assist"),
}
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# CAN and CAN FD queries are combined.
# FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus
# TODO: properly handle auxiliary requests to separate queries and add back whitelists
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine],
# whitelist_ecus=[Ecu.engine],
auxiliary=True,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
# whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
bus=0,
whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
auxiliary=True,
),
],
extra_ecus=[
@@ -158,8 +168,8 @@ FW_VERSIONS = {
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-BEF\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-BEJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NX6A-14C204-BLE\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-CAB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NX6A-14C204-BLE\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.EXPLORER_MK6: {
@@ -168,6 +178,7 @@ FW_VERSIONS = {
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -192,9 +203,27 @@ FW_VERSIONS = {
b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MB5A-14C204-RC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NB5A-14C204-AZD\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.F_150_MK14: {
(Ecu.eps, 0x730, None): [
b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'PL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7E0, None): [
b'PL3A-14C204-BRB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FOCUS_MK4: {
(Ecu.eps, 0x730, None): [
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
selfdrive/car/gm/carcontroller.py
+6 -1
View File
@@ -85,6 +85,7 @@ class CarController:
if self.CP.openpilotLongitudinalControl:
# Gas/regen, brakes, and UI commands - all at 25Hz
if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping
if not CC.longActive:
# ASCM sends max regen when not enabled
self.apply_gas = self.params.INACTIVE_REGEN
@@ -92,6 +93,10 @@ class CarController:
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)))
# Don't allow any gas above inactive regen while stopping
# FIXME: brakes aren't applied immediately when enabling at a stop
if stopping:
self.apply_gas = self.params.INACTIVE_REGEN
idx = (self.frame // 4) % 4
@@ -101,7 +106,7 @@ class CarController:
# GM Camera exceptions
# TODO: can we always check the longControlState?
if self.CP.networkLocation == NetworkLocation.fwdCamera:
at_full_stop = at_full_stop and actuators.longControlState == LongCtrlState.stopping
at_full_stop = at_full_stop and stopping
friction_brake_bus = CanBus.POWERTRAIN
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
selfdrive/car/gm/interface.py
+15 -19
View File
@@ -19,6 +19,12 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
NON_LINEAR_TORQUE_PARAMS = {
CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772]
}
class CarInterface(CarInterfaceBase):
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
@@ -31,23 +37,14 @@ class CarInterface(CarInterfaceBase):
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.10006696 * sigmoid * (v_ego + 3.12485927)
@staticmethod
def get_steer_feedforward_acadia(desired_angle, v_ego):
desired_angle *= 0.09760208
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.04689655 * sigmoid * (v_ego + 10.028217)
def get_steer_feedforward_function(self):
if self.CP.carFingerprint == CAR.VOLT:
return self.get_steer_feedforward_volt
elif self.CP.carFingerprint == CAR.ACADIA:
return self.get_steer_feedforward_acadia
else:
return CarInterfaceBase.get_steer_feedforward_default
@staticmethod
def torque_from_lateral_accel_bolt(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
def torque_from_lateral_accel_siglin(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
def sig(val):
@@ -57,14 +54,15 @@ class CarInterface(CarInterfaceBase):
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
a, b, c, _ = [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178] # weights computed offline
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, _ = non_linear_torque_params
steer_torque = (sig(lateral_accel_value * a) * b) + (lateral_accel_value * c)
return float(steer_torque) + friction
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint == CAR.BOLT_EUV:
return self.torque_from_lateral_accel_bolt
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
return self.torque_from_lateral_accel_siglin
else:
return self.torque_from_lateral_accel_linear
@@ -119,9 +117,6 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.36]
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
@@ -172,7 +167,8 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.86
ret.steerRatio = 14.4 # end to end is 13.46
ret.centerToFront = ret.wheelbase * 0.4
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia()
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_LACROSSE:
ret.mass = 1712. + STD_CARGO_KG
selfdrive/car/honda/carcontroller.py
+3 -8
View File
@@ -6,7 +6,7 @@ from common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from selfdrive.car import create_gas_interceptor_command
from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams, CAR
from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
from selfdrive.controls.lib.drive_helpers import rate_limit
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -124,7 +124,7 @@ class CarController:
self.brake = 0.0
self.last_steer = 0.0
def update(self, CC, CS, now_nanos, dragonconf):
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric)
@@ -206,10 +206,7 @@ class CarController:
if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
if CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID_BSM) and CS.hud_lead == 1:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, CS.CP.carFingerprint))
else:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint))
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint))
else:
# Send gas and brake commands.
@@ -228,8 +225,6 @@ class CarController:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
if self.CP.carFingerprint == CAR.ODYSSEY_HYBRID:
pump_on = apply_brake > 0
pcm_override = True
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
selfdrive/car/honda/carstate.py
+9 -28
View File
@@ -42,9 +42,6 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
("CRUISE_SETTING", "SCM_BUTTONS"),
("ACC_STATUS", "POWERTRAIN_DATA"),
("MAIN_ON", main_on_sig_msg),
#dp
("ENGINE_RPM", "POWERTRAIN_DATA"),
("HUD_LEAD", "ACC_HUD"),
]
checks = [
@@ -60,7 +57,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
("STEER_MOTOR_TORQUE", 0), # TODO: not on every car
]
if CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.ODYSSEY_HYBRID):
if CP.carFingerprint == CAR.ODYSSEY_CHN:
checks += [
("SCM_FEEDBACK", 25),
("SCM_BUTTONS", 50),
@@ -71,7 +68,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
("SCM_BUTTONS", 25),
]
if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CRV_HYBRID_BSM):
if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E):
checks.append((gearbox_msg, 50))
else:
checks.append((gearbox_msg, 100))
@@ -80,7 +77,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
signals.append(("BRAKE_PRESSED", "BRAKE_MODULE"))
checks.append(("BRAKE_MODULE", 50))
if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.ODYSSEY_HYBRID}):
if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
signals.append(("EPB_STATE", "EPB_STATUS"))
checks.append(("EPB_STATUS", 50))
@@ -92,8 +89,6 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
("CRUISE_SPEED", "ACC_HUD"),
("ACCEL_COMMAND", "ACC_CONTROL"),
("AEB_STATUS", "ACC_CONTROL"),
#dp
("BRAKE_LIGHTS", "ACC_CONTROL"),
]
checks += [
("ACC_HUD", 10),
@@ -103,12 +98,12 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
signals += [("CRUISE_SPEED_PCM", "CRUISE"),
("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS")]
if CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.ODYSSEY_HYBRID):
if CP.carFingerprint == CAR.ODYSSEY_CHN:
checks.append(("CRUISE_PARAMS", 10))
else:
checks.append(("CRUISE_PARAMS", 50))
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G, CAR.CRV_HYBRID_BSM):
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK"))
elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS"))
@@ -157,8 +152,6 @@ class CarState(CarStateBase):
self.brake_switch_active = False
self.cruise_setting = 0
self.v_cruise_pcm_prev = 0
self.engineRpm = 0
self.hud_lead = 0
# When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster
# However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX)
@@ -185,7 +178,7 @@ class CarState(CarStateBase):
# panda checks if the signal is non-zero
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
# TODO: find a common signal across all cars
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G, CAR.CRV_HYBRID_BSM):
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
@@ -238,11 +231,9 @@ class CarState(CarStateBase):
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
#dp
self.engineRpm = cp.vl["POWERTRAIN_DATA"]['ENGINE_RPM']
# TODO: set for all cars
if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.ODYSSEY_HYBRID}):
if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
@@ -293,21 +284,11 @@ class CarState(CarStateBase):
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"])
# afa feature
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.RIDGELINE):
if ret.brake > 0.1:
ret.brakePressed = True
if self.CP.carFingerprint in HONDA_BOSCH and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
ret.brakeLightsDEPRECATED = bool(ret.brakePressed or ret.brake > 0.4 or ret.parkingBrake)
if not self.CP.openpilotLongitudinalControl:
ret.brakeLightsDEPRECATED = ret.brakeLightsDEPRECATED or cp.vl["ACC_CONTROL"]['BRAKE_LIGHTS'] != 0
else:
ret.brakeLightsDEPRECATED = bool(ret.brakePressed)
if self.CP.carFingerprint in HONDA_BOSCH:
# TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts
if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
@@ -324,7 +305,7 @@ class CarState(CarStateBase):
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
self.lkas_hud = cp_cam.vl["LKAS_HUD"]
if self.CP.enableBsm and self.CP.carFingerprint in (CAR.CRV_5G, CAR.CRV_HYBRID_BSM,):
if self.CP.enableBsm:
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
# more info here: https://github.com/commaai/openpilot/pull/1867
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
@@ -374,7 +355,7 @@ class CarState(CarStateBase):
@staticmethod
def get_body_can_parser(CP):
if CP.enableBsm and CP.carFingerprint in (CAR.CRV_5G, CAR.CRV_HYBRID_BSM,):
if CP.enableBsm:
signals = [("BSM_ALERT", "BSM_STATUS_RIGHT"),
("BSM_ALERT", "BSM_STATUS_LEFT")]
selfdrive/car/honda/hondacan.py
+1 -2
View File
@@ -130,11 +130,10 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud,
'IMPERIAL_UNIT': int(not is_metric),
'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0,
'SET_ME_X01_2': 1,
'ACC_ON': int(enabled),
}
if CP.carFingerprint in HONDA_BOSCH:
# acc_hud_values['ACC_ON'] = int(enabled)
acc_hud_values['ACC_ON'] = int(enabled)
acc_hud_values['FCM_OFF'] = 1
acc_hud_values['FCM_OFF_2'] = 1
else:
selfdrive/car/honda/interface.py
+6 -40
View File
@@ -7,7 +7,6 @@ from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, Honda
from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.disable_ecu import disable_ecu
from common.params import Params
ButtonType = car.CarState.ButtonEvent.Type
@@ -51,15 +50,7 @@ class CarInterface(CarInterfaceBase):
ret.pcmCruise = not ret.enableGasInterceptor
# dp - attempt to disable op long
params = Params()
if int(params.get("dp_atl").decode('utf-8')) == 1:
ret.openpilotLongitudinalControl = False
# update pcmCruise again
if candidate in HONDA_BOSCH:
ret.pcmCruise = True
if candidate in (CAR.CRV_5G, CAR.CRV_HYBRID_BSM):
if candidate == CAR.CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
# Detect Bosch cars with new HUD msgs
@@ -173,7 +164,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.677
ret.wheelSpeedFactor = 1.025
elif candidate in (CAR.CRV_HYBRID, CAR.CRV_HYBRID_BSM):
elif candidate == CAR.CRV_HYBRID:
ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg
ret.wheelbase = 2.66
ret.centerToFront = ret.wheelbase * 0.41
@@ -233,14 +224,14 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
tire_stiffness_factor = 0.677
elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.ODYSSEY_HYBRID):
elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
ret.mass = 1900. + STD_CARGO_KG
ret.wheelbase = 3.00
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 14.35 # as spec
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
if candidate in (CAR.ODYSSEY_CHN, CAR.ODYSSEY_HYBRID):
if candidate == CAR.ODYSSEY_CHN:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
@@ -301,7 +292,7 @@ class CarInterface(CarInterfaceBase):
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
# conflict with PCM acc
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY_HYBRID}) or ret.enableGasInterceptor
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
@@ -312,26 +303,6 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
params.put("dp_lateral_steer_rate_cost", "0.5")
if params.get_bool('dp_honda_eps_mod'):
if candidate == CAR.CIVIC:
# tuned by a-tao
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096, 8000], [0, 4096, 4096]]
elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] #2.5 default mod #Tuned by TMG
elif candidate in (CAR.ACCORD, CAR.ACCORDH):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
elif candidate == CAR.CRV_5G:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] #tuned by Titanminer (8000)
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
elif candidate in (CAR.CRV_HYBRID, CAR.CRV_HYBRID_BSM):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0x0, 0xB5, 0x161, 0x2D6, 0x4C0, 0x70D, 0xC42, 0x1058, 0x2C00], [0x0, 0x160, 0x1F0, 0x2E0, 0x378, 0x4A0, 0x5F0, 0x804, 0xF00]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] #still needs to finish tuning for the new car
ret.lateralTuning.pid.kf = 0.00004
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
return ret
@staticmethod
@@ -343,9 +314,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
#dp
ret.engineRpm = self.CS.engineRpm
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
@@ -358,8 +326,6 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret, pcm_enable=False)
#events = self.dp_atl_warning(ret, events)
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
@@ -385,4 +351,4 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos, self.dragonconf)
return self.CC.update(c, self.CS, now_nanos)
selfdrive/car/honda/values.py
+2 -9
View File
@@ -84,14 +84,12 @@ class CAR:
CRV_5G = "HONDA CR-V 2017"
CRV_EU = "HONDA CR-V EU 2016"
CRV_HYBRID = "HONDA CR-V HYBRID 2019"
CRV_HYBRID_BSM = "HONDA CR-V HYBRID 2019 w/ BSM"
FIT = "HONDA FIT 2018"
FREED = "HONDA FREED 2020"
HRV = "HONDA HRV 2019"
HRV_3G = "HONDA HR-V 2023"
ODYSSEY = "HONDA ODYSSEY 2018"
ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019"
ODYSSEY_HYBRID = "HONDA ODYSSEY HYBRID CHN 2022"
ACURA_RDX = "ACURA RDX 2018"
ACURA_RDX_3G = "ACURA RDX 2020"
PILOT = "HONDA PILOT 2017"
@@ -144,7 +142,6 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
CAR.HRV_3G: HondaCarInfo("Honda HR-V 2023", "All"),
CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"),
CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey
CAR.ODYSSEY_HYBRID: HondaCarInfo("Honda Odyssey hybrid china 2022", min_steer_speed=0. * CV.MPH_TO_MS),
CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
CAR.PILOT: [
@@ -201,8 +198,6 @@ FW_QUERY_CONFIG = FwQueryConfig(
)
FW_VERSIONS = {
CAR.CRV_HYBRID_BSM: {(Ecu.vsa, 0xfff, None): [b'\x00']},
CAR.ODYSSEY_HYBRID: {(Ecu.vsa, 0xfff, None): [b'\x00']},
CAR.ACCORD: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
b'37805-6A0-8720\x00\x00',
@@ -1574,14 +1569,12 @@ DBC = {
CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'),
CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.CRV_HYBRID: dbc_dict('honda_accord_2018_can_generated', None),
CAR.CRV_HYBRID_BSM: dbc_dict('honda_accord_2018_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'),
CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.HRV_3G: dbc_dict('honda_civic_ex_2022_can_generated', None),
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_HYBRID: dbc_dict('honda_odyssey_hybrid_2022_china_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None),
@@ -1597,8 +1590,8 @@ STEER_THRESHOLD = {
HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY}
HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
CAR.ODYSSEY_HYBRID, CAR.PILOT, CAR.RIDGELINE}
CAR.PILOT, CAR.RIDGELINE}
HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G, CAR.CRV_HYBRID_BSM}
CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G}
HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G, CAR.HRV_3G}
HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022, CAR.HRV_3G}
selfdrive/car/hyundai/carcontroller.py
+8 -4
View File
@@ -61,8 +61,12 @@ class CarController:
hud_control = CC.hudControl
# steering torque
# rick - from taco
self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
# rick - from taco
apply_steer = clip(apply_steer, -self.params.STEER_MAX, self.params.STEER_MAX)
if not CC.latActive:
apply_steer = 0
@@ -114,7 +118,7 @@ class CarController:
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.latActive, lat_active, apply_steer))
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, lat_active, apply_steer))
# disable LFA on HDA2
if self.frame % 5 == 0 and hda2:
@@ -122,7 +126,7 @@ class CarController:
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.latActive))
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
@@ -141,7 +145,7 @@ class CarController:
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CAN, CS.cruise_info))
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
@@ -159,7 +163,7 @@ class CarController:
self.last_button_frame = self.frame
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.latActive,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
selfdrive/car/hyundai/carstate.py
-8
View File
@@ -89,8 +89,6 @@ class CarState(CarStateBase):
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
#dp
ret.engineRpm = cp.vl["TCU_DCT13"]['Cluster_Engine_RPM']
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
@@ -112,8 +110,6 @@ class CarState(CarStateBase):
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
#dp
ret.brakeLightsDEPRECATED = bool(cp.vl["TCS13"]["BrakeLight"] or ret.brakePressed or ret.brakeHoldActive or ret.parkingBrake)
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
@@ -292,9 +288,6 @@ class CarState(CarStateBase):
("SAS_Angle", "SAS11"),
("SAS_Speed", "SAS11"),
#dp
("Cluster_Engine_RPM", "TCU_DCT13"),
("BrakeLight", "TCS13"),
]
checks = [
# address, frequency
@@ -309,7 +302,6 @@ class CarState(CarStateBase):
("CGW4", 5),
("WHL_SPD11", 50),
("SAS11", 100),
("TCU_DCT13", 100),
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
selfdrive/car/hyundai/hyundaican.py
+31 -2
View File
@@ -7,7 +7,23 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
torque_fault, lkas11, sys_warning, sys_state, enabled,
left_lane, right_lane,
left_lane_depart, right_lane_depart):
values = lkas11
values = {s: lkas11[s] for s in [
"CF_Lkas_LdwsActivemode",
"CF_Lkas_LdwsSysState",
"CF_Lkas_SysWarning",
"CF_Lkas_LdwsLHWarning",
"CF_Lkas_LdwsRHWarning",
"CF_Lkas_HbaLamp",
"CF_Lkas_FcwBasReq",
"CF_Lkas_HbaSysState",
"CF_Lkas_FcwOpt",
"CF_Lkas_HbaOpt",
"CF_Lkas_FcwSysState",
"CF_Lkas_FcwCollisionWarning",
"CF_Lkas_FusionState",
"CF_Lkas_FcwOpt_USM",
"CF_Lkas_LdwsOpt_USM",
]}
values["CF_Lkas_LdwsSysState"] = sys_state
values["CF_Lkas_SysWarning"] = 3 if sys_warning else 0
values["CF_Lkas_LdwsLHWarning"] = left_lane_depart
@@ -79,7 +95,20 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
def create_clu11(packer, frame, clu11, button, car_fingerprint):
values = clu11
values = {s: clu11[s] for s in [
"CF_Clu_CruiseSwState",
"CF_Clu_CruiseSwMain",
"CF_Clu_SldMainSW",
"CF_Clu_ParityBit1",
"CF_Clu_VanzDecimal",
"CF_Clu_Vanz",
"CF_Clu_SPEED_UNIT",
"CF_Clu_DetentOut",
"CF_Clu_RheostatLevel",
"CF_Clu_CluInfo",
"CF_Clu_AmpInfo",
"CF_Clu_AliveCnt1",
]}
values["CF_Clu_CruiseSwState"] = button
values["CF_Clu_AliveCnt1"] = frame % 0x10
# send buttons to camera on camera-scc based cars
selfdrive/car/hyundai/hyundaicanfd.py
+30 -7
View File
@@ -60,11 +60,11 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
return ret
def create_cam_0x2a4(packer, CAN, camera_values):
camera_values.update({
"BYTE7": 0,
})
return packer.make_can_msg("CAM_0x2a4", CAN.ACAN, camera_values)
def create_cam_0x2a4(packer, CAN, cam_0x2a4):
values = {f"BYTE{i}": cam_0x2a4[f"BYTE{i}"] for i in range(3, 24)}
values['COUNTER'] = cam_0x2a4['COUNTER']
values["BYTE7"] = 0
return packer.make_can_msg("CAM_0x2a4", CAN.ACAN, values)
def create_buttons(packer, CP, CAN, cnt, btn):
values = {
@@ -76,10 +76,33 @@ def create_buttons(packer, CP, CAN, cnt, btn):
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def create_acc_cancel(packer, CAN, cruise_info_copy):
values = cruise_info_copy
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# TODO: why do we copy different values here?
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"NEW_SIGNAL_1",
"MainMode_ACC",
"ACCMode",
"ZEROS_9",
"CRUISE_STANDSTILL",
"ZEROS_5",
"DISTANCE_SETTING",
"VSetDis",
]}
else:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"ACCMode",
"VSetDis",
"CRUISE_STANDSTILL",
]}
values.update({
"ACCMode": 4,
"aReqRaw": 0.0,
"aReqValue": 0.0,
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
selfdrive/car/hyundai/interface.py
+9 -14
View File
@@ -8,7 +8,6 @@ from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.disable_ecu import disable_ecu
from common.params import Params
Ecu = car.CarParams.Ecu
ButtonType = car.CarState.ButtonEvent.Type
@@ -27,7 +26,7 @@ class CarInterface(CarInterfaceBase):
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, }
ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.IONIQ_6}
hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
CAN = CanBus(None, hda2, fingerprint)
@@ -187,10 +186,10 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.9
ret.steerRatio = 16.
tire_stiffness_factor = 0.65
elif candidate == CAR.IONIQ_5:
ret.mass = 2012 + STD_CARGO_KG
ret.wheelbase = 3.0
ret.steerRatio = 16.
elif candidate in (CAR.IONIQ_5, CAR.IONIQ_6):
ret.mass = 1948 + STD_CARGO_KG
ret.wheelbase = 2.97
ret.steerRatio = 14.26
tire_stiffness_factor = 0.65
elif candidate == CAR.KIA_SPORTAGE_HYBRID_5TH_GEN:
ret.mass = 1767. + STD_CARGO_KG # SX Prestige trim support only
@@ -203,6 +202,10 @@ class CarInterface(CarInterfaceBase):
ret.mass = 3957 * CV.LB_TO_KG + STD_CARGO_KG
else:
ret.mass = 4537 * CV.LB_TO_KG + STD_CARGO_KG
elif candidate == CAR.KIA_CARNIVAL_4TH_GEN:
ret.mass = 2087. + STD_CARGO_KG
ret.wheelbase = 3.09
ret.steerRatio = 14.23
# Genesis
elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN:
@@ -245,9 +248,6 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
params = Params()
if int(params.get("dp_atl").decode('utf-8')) == 1:
ret.openpilotLongitudinalControl = False
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.stoppingControl = True
@@ -264,10 +264,6 @@ class CarInterface(CarInterfaceBase):
ret.enableBsm = 0x58b in fingerprint[0]
# *** panda safety config ***
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
# panda safety config
if candidate in CANFD_CAR:
cfgs = [get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd), ]
if CAN.ECAN >= 4:
@@ -307,7 +303,6 @@ class CarInterface(CarInterfaceBase):
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
params.put("dp_lateral_steer_rate_cost", "0.5")
return ret
@staticmethod
selfdrive/car/hyundai/values.py
+61 -15
View File
@@ -17,7 +17,7 @@ class CarControllerParams:
ACCEL_MIN = -3.5 # m/s
ACCEL_MAX = 2.0 # m/s
def __init__(self, CP):
def __init__(self, CP, vEgoRaw=100.):
self.STEER_DELTA_UP = 3
self.STEER_DELTA_DOWN = 7
self.STEER_DRIVER_ALLOWANCE = 50
@@ -27,12 +27,19 @@ class CarControllerParams:
self.STEER_STEP = 1 # 100 Hz
if CP.carFingerprint in CANFD_CAR:
self.STEER_MAX = 270
self.STEER_DRIVER_ALLOWANCE = 250
# self.STEER_MAX = 270
# self.STEER_DRIVER_ALLOWANCE = 250
# self.STEER_DRIVER_MULTIPLIER = 2
# self.STEER_THRESHOLD = 250
# self.STEER_DELTA_UP = 2
# self.STEER_DELTA_DOWN = 3
# rick - taco tune
self.STEER_MAX = 384 if vEgoRaw < 11. else 330
self.STEER_DRIVER_ALLOWANCE = 350
self.STEER_DRIVER_MULTIPLIER = 2
self.STEER_THRESHOLD = 250
self.STEER_DELTA_UP = 2
self.STEER_DELTA_DOWN = 3
self.STEER_THRESHOLD = 350
self.STEER_DELTA_UP = 10 if vEgoRaw < 11. else 2
self.STEER_DELTA_DOWN = 10 if vEgoRaw < 11. else 3
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
# If the max stock LKAS request is <384, add your car to this list.
@@ -92,6 +99,7 @@ class CAR:
VELOSTER = "HYUNDAI VELOSTER 2019"
SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021"
IONIQ_5 = "HYUNDAI IONIQ 5 2022"
IONIQ_6 = "HYUNDAI IONIQ 6 2023"
TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN"
TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN"
SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN"
@@ -118,6 +126,7 @@ class CAR:
KIA_STINGER_2022 = "KIA STINGER 2022"
KIA_CEED = "KIA CEED INTRO ED 2019"
KIA_EV6 = "KIA EV6 2022"
KIA_CARNIVAL_4TH_GEN = "KIA CARNIVAL 4TH GEN"
# Genesis
GENESIS_GV60_EV_1ST_GEN = "GENESIS GV60 ELECTRIC 1ST GEN"
@@ -183,12 +192,16 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
HyundaiCarInfo("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
],
CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e])),
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.IONIQ_5: [
HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])),
HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])),
HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
],
CAR.IONIQ_6: [
HyundaiCarInfo("Hyundai Ioniq 6 (without HDA II) 2023", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])), # TODO: unknown
HyundaiCarInfo("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])),
],
CAR.TUCSON_4TH_GEN: [
HyundaiCarInfo("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])),
HyundaiCarInfo("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
@@ -241,6 +254,10 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))
],
CAR.KIA_CARNIVAL_4TH_GEN: [
HyundaiCarInfo("Kia Carnival 2023", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarInfo("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k]))
],
# Genesis
CAR.GENESIS_GV60_EV_1ST_GEN: [
@@ -632,6 +649,7 @@ FW_VERSIONS = {
b'\xf1\x00DN8_ SCC F-CUP 1.00 1.02 99110-L1000 ',
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ',
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ',
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.02 99110-L1000 ',
],
(Ecu.abs, 0x7d1, None): [
b'\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100',
@@ -646,6 +664,7 @@ FW_VERSIONS = {
b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100',
b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
b'\xf1\x00DN ESC \x06 107 \x07\x03 58910-L1300',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x81HM6M1_0a0_F00',
@@ -660,6 +679,7 @@ FW_VERSIONS = {
b'\xf1\x87391162M003',
b'\xf1\x87391162M013',
b'\xf1\x87391162M023',
b'\xf1\x87391162M010',
b'HM6M1_0a0_F00',
b'HM6M1_0a0_G20',
b'HM6M2_0a0_BD0',
@@ -683,11 +703,13 @@ FW_VERSIONS = {
b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101',
b'\xf1\x8757700-L0000\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100',
b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101',
b'\xf1\x00DN8 MDPS R 1.00 1.02 57700-L1000 4DNDP105',
b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC102',
b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0200\x00 4DNAC102',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422',
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.04 99211-L1000 191016',
b'\xf1\x00DN8 MFC AT RUS LHD 1.00 1.03 99211-L1000 190705',
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.00 99211-L0000 190716',
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016',
@@ -700,6 +722,7 @@ FW_VERSIONS = {
b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1',
b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc',
b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16KB05\x95h%',
b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'\xf1\x00HT6TA260BLHT6TA810A1TDN8M25GS0\x00\x00\x00\x00\x00\x00\xaa\x8c\xd9p',
b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -1666,8 +1689,8 @@ FW_VERSIONS = {
},
CAR.SONATA_HYBRID: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\000DNhe SCC FHCUP 1.00 1.02 99110-L5000 ',
b'\xf1\x8799110L5000\xf1\000DNhe SCC FHCUP 1.00 1.02 99110-L5000 ',
b'\xf1\x00DNhe SCC FHCUP 1.00 1.02 99110-L5000 ',
b'\xf1\x8799110L5000\xf1\x00DNhe SCC FHCUP 1.00 1.02 99110-L5000 ',
b'\xf1\000DNhe SCC F-CUP 1.00 1.02 99110-L5000 ',
b'\xf1\x8799110L5000\xf1\000DNhe SCC F-CUP 1.00 1.02 99110-L5000 ',
],
@@ -1675,23 +1698,27 @@ FW_VERSIONS = {
b'\xf1\x8756310-L5500\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102',
b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5450 4DNHC102',
b'\xf1\x8756310-L5450\xf1\000DN8 MDPS C 1.00 1.03 56310-L5450 4DNHC103',
b'\xf1\x00DN8 MDPS C 1.00 1.03 56310L5450\x00 4DNHC104',
b'\xf1\x8756310L5450\x00\xf1\x00DN8 MDPS C 1.00 1.03 56310L5450\x00 4DNHC104',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.04 99211-L1000 191016',
b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.05 99211-L1000 201109',
b'\xf1\000DN8HMFC AT USA LHD 1.00 1.06 99211-L1000 210325',
b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.07 99211-L1000 211223',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\000PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW',
b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E09\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2323 E09\x00\x00\x00\x00\x00\x00\x00TDN2H20SA5\x97R\x88\x9e',
b'\xf1\000PSBG2323 E09\000\000\000\000\000\000\000TDN2H20SA5\x97R\x88\x9e',
b'\xf1\000PSBG2333 E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab',
b'\xf1\x87PCU\000\000\000\000\000\000\000\000\000\xf1\x81E16\000\000\000\000\000\000\000\xf1\000PSBG2333 E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab',
b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TDN2H20SA7\x1a3\xf9\xab',
b'\xf1\x87PCU\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TDN2H20SA7\x1a3\xf9\xab',
b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x87391162J012',
b'\xf1\x87391162J013',
b'\xf1\x87391162J014',
b'\xf1\x87391062J002',
],
},
@@ -1751,11 +1778,18 @@ FW_VERSIONS = {
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.03 99211-GI010 220401',
],
},
CAR.IONIQ_6: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00CE__ RDR ----- 1.00 1.01 99110-KL000 ',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00CE MFC AT USA LHD 1.00 1.04 99211-KL000 221213',
],
},
CAR.TUCSON_4TH_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9210 14G',
b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9240 14T',
b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW010 14X',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ',
@@ -1853,6 +1887,16 @@ FW_VERSIONS = {
b'\xf1\x00JX1_ SCC FHCUP 1.00 1.01 99110-T6100 ',
],
},
CAR.KIA_CARNIVAL_4TH_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00KA4 MFC AT USA LHD 1.00 1.06 99210-R0000 220221',
b'\xf1\x00KA4CMFC AT CHN LHD 1.00 1.01 99211-I4000 210525',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00KA4_ SCC FHCUP 1.00 1.03 99110-R0000 ',
b'\xf1\x00KA4c SCC FHCUP 1.00 1.01 99110-I4000 ',
],
},
}
CHECKSUM = {
@@ -1867,16 +1911,16 @@ CAN_GEARS = {
"use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022, CAR.KIA_K5_HEV_2020},
}
CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN, CAR.GENESIS_GV80}
CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN, CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN}
# The radar does SCC on these cars when HDA I, rather than the camera
CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80}
CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN}
# The camera does SCC on these cars, rather than the radar
CAMERA_SCC_CAR = {CAR.KONA_EV_2022, }
HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020, CAR.KIA_NIRO_HEV_2ND_GEN} # these cars use a different gas signal
EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.GENESIS_GV60_EV_1ST_GEN}
EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.GENESIS_GV60_EV_1ST_GEN}
# these cars require a special panda safety mode due to missing counters and checksums in the messages
LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER,
@@ -1931,6 +1975,7 @@ DBC = {
CAR.TUCSON_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.TUCSON_HYBRID_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.IONIQ_5: dbc_dict('hyundai_canfd', None),
CAR.IONIQ_6: dbc_dict('hyundai_canfd', None),
CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: dbc_dict('hyundai_canfd', None),
@@ -1941,4 +1986,5 @@ DBC = {
CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None),
CAR.KIA_CARNIVAL_4TH_GEN: dbc_dict('hyundai_canfd', None),
}
selfdrive/car/interfaces.py
+3 -52
View File
@@ -14,8 +14,6 @@ from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_ine
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
from selfdrive.controls.lib.events import Events
from selfdrive.controls.lib.vehicle_model import VehicleModel
from common.params import Params
from selfdrive.car.lat_controller_helper import configure_pid_tune, configure_lqr_tune
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
@@ -86,10 +84,6 @@ class CarInterfaceBase(ABC):
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP, self.VM)
# dp
self.dp_last_cruise_actual_enabled = False
self.dragonconf = None
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return ACCEL_MIN, ACCEL_MAX
@@ -137,8 +131,7 @@ class CarInterfaceBase(ABC):
def get_steer_feedforward_function(self):
return self.get_steer_feedforward_default
@staticmethod
def torque_from_lateral_accel_linear(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
def torque_from_lateral_accel_linear(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
# The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
@@ -198,53 +191,11 @@ class CarInterfaceBase(ABC):
tune.torque.latAccelOffset = 0.0
tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
@staticmethod
def configure_dp_tune(stock, collection):
try:
dp_lateral_tune = int(Params().get("dp_lateral_tune").decode('utf-8'))
except:
dp_lateral_tune = 0
stock_tune = 0
if stock.which() == 'pid':
stock_tune = 1
collection.pid = stock.pid
elif stock.which() == 'lqr':
stock_tune = 2
collection.lqr = stock.lqr
elif stock.which() == 'torque':
stock_tune = 3
collection.torque = stock.torque
elif stock.which() == 'indi':
stock_tune = 4
if dp_lateral_tune > 0 and dp_lateral_tune != stock_tune:
if dp_lateral_tune == 1 and collection.pid is not None:
stock.pid = collection.pid
elif dp_lateral_tune == 2 and collection.lqr is not None:
stock.lqr = collection.lqr
elif dp_lateral_tune == 3 and collection.torque is not None:
stock.torque = collection.torque
@staticmethod
def dp_lat_tune_collection(candidate, collection, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
for i in range(1, 4):
# pid - car specific
if i == 1:
configure_pid_tune(candidate, collection)
# lqr - all uses RAV4 one
elif i == 2:
configure_lqr_tune(candidate, collection)
# torque - car specific as per lookup table
elif i == 3:
CarInterfaceBase.configure_torque_tune(candidate, collection, steering_angle_deadzone_deg, use_steering_angle)
@abstractmethod
def _update(self, c: car.CarControl) -> car.CarState:
pass
def update(self, c: car.CarControl, can_strings: List[bytes], dragonconf) -> car.CarState:
self.dragonconf = dragonconf
def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
# parse can
for cp in self.can_parsers:
if cp is not None:
@@ -302,7 +253,7 @@ class CarInterfaceBase(ABC):
events.add(EventName.stockFcw)
if cs_out.stockAeb:
events.add(EventName.stockAeb)
if self.dragonconf.dpSpeedCheck and cs_out.vEgo > MAX_CTRL_SPEED:
if cs_out.vEgo > MAX_CTRL_SPEED:
events.add(EventName.speedTooHigh)
if cs_out.cruiseState.nonAdaptive:
events.add(EventName.wrongCruiseMode)
selfdrive/car/isotp_parallel_query.py
+7 -1
View File
@@ -115,7 +115,13 @@ class IsoTpParallelQuery:
addrs_responded.add(tx_addr)
response_timeouts[tx_addr] = time.monotonic() + timeout
if not dat:
if dat is None:
continue
# Log unexpected empty responses
if len(dat) == 0:
cloudlog.error(f"iso-tp query empty response: {tx_addr}")
request_done[tx_addr] = True
continue
counter = request_counter[tx_addr]
selfdrive/car/lat_controller_helper.py
-225
View File
@@ -1,225 +0,0 @@
'''
dp - we create a separate controller helper to restore PID/LQR steering tune.
'''
from selfdrive.car.tunes import set_lat_tune, LatTunes
from selfdrive.car.toyota.values import CAR as TOYOTA
from selfdrive.car.hyundai.values import CAR as HYUNDAI
from selfdrive.car.volkswagen.values import CAR as VW
from selfdrive.car.subaru.values import CAR as SUBARU
from common.params import Params
def configure_pid_tune(candidate, tune):
# toyota
if candidate == TOYOTA.PRIUS:
# indi only
pass
elif candidate == TOYOTA.PRIUS_V:
# lqr only
pass
elif candidate in (TOYOTA.RAV4, TOYOTA.RAV4H):
# lqr only
pass
elif candidate == TOYOTA.COROLLA:
set_lat_tune(tune, LatTunes.PID_A)
elif candidate in (TOYOTA.LEXUS_RX, TOYOTA.LEXUS_RXH, TOYOTA.LEXUS_RX_TSS2, TOYOTA.LEXUS_RXH_TSS2):
set_lat_tune(tune, LatTunes.PID_C)
elif candidate in (TOYOTA.CHR, TOYOTA.CHRH, TOYOTA.CHR_TSS2):
set_lat_tune(tune, LatTunes.PID_F)
elif candidate in (TOYOTA.CAMRY, TOYOTA.CAMRYH, TOYOTA.CAMRY_TSS2, TOYOTA.CAMRYH_TSS2):
if candidate not in (TOYOTA.CAMRY_TSS2, TOYOTA.CAMRYH_TSS2):
set_lat_tune(tune, LatTunes.PID_C)
elif candidate in (TOYOTA.HIGHLANDER, TOYOTA.HIGHLANDERH, TOYOTA.HIGHLANDER_TSS2, TOYOTA.HIGHLANDERH_TSS2):
set_lat_tune(tune, LatTunes.PID_G)
elif candidate in (TOYOTA.AVALON, TOYOTA.AVALON_2019, TOYOTA.AVALONH_2019, TOYOTA.AVALON_TSS2, TOYOTA.AVALONH_TSS2):
set_lat_tune(tune, LatTunes.PID_H)
elif candidate in (TOYOTA.RAV4_TSS2, TOYOTA.RAV4_TSS2_2022, TOYOTA.RAV4H_TSS2, TOYOTA.RAV4H_TSS2_2022):
# 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
if Params().get_bool("dp_toyota_rav4_tss2_tune"):
set_lat_tune(tune, LatTunes.PID_I)
else:
set_lat_tune(tune, LatTunes.PID_D)
elif candidate in (TOYOTA.COROLLA_TSS2, TOYOTA.COROLLAH_TSS2):
set_lat_tune(tune, LatTunes.PID_D)
elif candidate in (TOYOTA.LEXUS_ES_TSS2, TOYOTA.LEXUS_ESH_TSS2, TOYOTA.LEXUS_ESH):
set_lat_tune(tune, LatTunes.PID_D)
elif candidate == TOYOTA.SIENNA:
set_lat_tune(tune, LatTunes.PID_J)
elif candidate in (TOYOTA.LEXUS_IS, TOYOTA.LEXUS_RC):
set_lat_tune(tune, LatTunes.PID_L)
elif candidate == TOYOTA.LEXUS_CTH:
set_lat_tune(tune, LatTunes.PID_M)
elif candidate in (TOYOTA.LEXUS_NX, TOYOTA.LEXUS_NXH, TOYOTA.LEXUS_NX_TSS2, TOYOTA.LEXUS_NXH_TSS2):
set_lat_tune(tune, LatTunes.PID_C)
elif candidate == TOYOTA.PRIUS_TSS2:
set_lat_tune(tune, LatTunes.PID_N)
elif candidate == TOYOTA.MIRAI:
set_lat_tune(tune, LatTunes.PID_C)
elif candidate in (TOYOTA.ALPHARD_TSS2, TOYOTA.ALPHARDH_TSS2):
set_lat_tune(tune, LatTunes.PID_J)
# hyundai
elif candidate in (HYUNDAI.SANTA_FE, HYUNDAI.SANTA_FE_2022, HYUNDAI.SANTA_FE_HEV_2022, HYUNDAI.SANTA_FE_PHEV_2022):
set_lat_tune(tune, LatTunes.PID_HYUNDAI_D)
elif candidate in (HYUNDAI.SONATA, HYUNDAI.SONATA_HYBRID):
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.SONATA_LF:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.PALISADE:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_C)
elif candidate == HYUNDAI.ELANTRA:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_B)
elif candidate == HYUNDAI.ELANTRA_2021:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.ELANTRA_HEV_2021:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.HYUNDAI_GENESIS:
# indi only
pass
elif candidate in (HYUNDAI.KONA, HYUNDAI.KONA_EV, HYUNDAI.KONA_HEV):
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate in (HYUNDAI.IONIQ, HYUNDAI.IONIQ_EV_LTD, HYUNDAI.IONIQ_EV_2020, HYUNDAI.IONIQ_PHEV, HYUNDAI.IONIQ_HEV_2022):
set_lat_tune(tune, LatTunes.PID_HYUNDAI_B)
elif candidate == HYUNDAI.IONIQ_PHEV_2019:
# indi only
pass
elif candidate == HYUNDAI.VELOSTER:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
# Kia
elif candidate == HYUNDAI.KIA_SORENTO:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate in (HYUNDAI.KIA_NIRO_EV, HYUNDAI.KIA_NIRO_HEV_2021):
set_lat_tune(tune, LatTunes.PID_HYUNDAI_B)
elif candidate == HYUNDAI.KIA_SELTOS:
# indi only
pass
elif candidate == HYUNDAI.KIA_OPTIMA_H:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.KIA_STINGER:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.KIA_FORTE:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.KIA_CEED:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
elif candidate == HYUNDAI.KIA_K5_2021:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
# Genesis
elif candidate == HYUNDAI.GENESIS_G70:
# indi only
pass
elif candidate == HYUNDAI.GENESIS_G70_2020:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_E)
elif candidate == HYUNDAI.GENESIS_G80:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_F)
elif candidate == HYUNDAI.GENESIS_G90:
set_lat_tune(tune, LatTunes.PID_HYUNDAI_G)
# VW
elif candidate == VW.ARTEON_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.ATLAS_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.GOLF_MK7:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.JETTA_MK7:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.PASSAT_MK8:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.PASSAT_NMS:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.POLO_MK6:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SHARAN_MK2:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.TAOS_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.TCROSS_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.TIGUAN_MK2:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.TOURAN_MK2:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.TRANSPORTER_T61:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.TROC_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.AUDI_A3_MK3:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.AUDI_Q2_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.AUDI_Q3_MK2:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SEAT_ATECA_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SEAT_LEON_MK3:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SKODA_KAMIQ_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SKODA_KAROQ_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SKODA_KODIAQ_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SKODA_OCTAVIA_MK3:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SKODA_SCALA_MK1:
set_lat_tune(tune, LatTunes.PID_VW)
elif candidate == VW.SKODA_SUPERB_MK3:
set_lat_tune(tune, LatTunes.PID_VW)
# subaru
elif candidate == SUBARU.ASCENT:
set_lat_tune(tune, LatTunes.PID_SUBARU_A)
elif candidate == SUBARU.IMPREZA:
set_lat_tune(tune, LatTunes.PID_SUBARU_B)
elif candidate == SUBARU.IMPREZA_2020:
set_lat_tune(tune, LatTunes.PID_SUBARU_C)
elif candidate == SUBARU.FORESTER:
set_lat_tune(tune, LatTunes.PID_SUBARU_D)
elif candidate in (SUBARU.OUTBACK, SUBARU.LEGACY):
# torque only
pass
elif candidate in (SUBARU.FORESTER_PREGLOBAL, SUBARU.OUTBACK_PREGLOBAL_2018):
set_lat_tune(tune, LatTunes.PID_SUBARU_E)
elif candidate == SUBARU.LEGACY_PREGLOBAL:
set_lat_tune(tune, LatTunes.PID_SUBARU_F)
elif candidate == SUBARU.OUTBACK_PREGLOBAL:
set_lat_tune(tune, LatTunes.PID_SUBARU_E)
'''
from RAV4
'''
def configure_lqr_tune(candidate, tune):
tune.init('lqr')
tune.lqr.scale = 1500.0
tune.lqr.ki = 0.05
tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
tune.lqr.c = [1., 0.]
tune.lqr.k = [-110.73572306, 451.22718255]
tune.lqr.l = [0.3233671, 0.3185757]
tune.lqr.dcGain = 0.002237852961363602
# '''
# directly copy from CarInterface.configure_torque_tune
# '''
# def config_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
# try:
# params = get_torque_params(candidate)
#
# tune.init('torque')
# tune.torque.useSteeringAngle = use_steering_angle
# tune.torque.kp = 1.0
# tune.torque.kf = 1.0
# tune.torque.ki = 0.1
# tune.torque.friction = params['FRICTION']
# tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
# tune.torque.latAccelOffset = 0.0
# tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
# except:
# pass
selfdrive/car/mazda/interface.py
+2 -6
View File
@@ -4,7 +4,6 @@ from common.conversions import Conversions as CV
from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from common.params import Params
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
@@ -17,7 +16,7 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
ret.radarUnavailable = True
ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) and not Params().get_bool('dp_mazda_dashcam_bypass')
ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021)
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
@@ -45,9 +44,6 @@ class CarInterface(CarInterfaceBase):
if candidate not in (CAR.CX5_2022, ):
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
ret.centerToFront = ret.wheelbase * 0.41
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
@@ -66,7 +62,7 @@ class CarInterface(CarInterfaceBase):
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)
elif self.dragonconf.dpMazdaSteerAlert and self.CS.low_speed_alert:
elif self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()
selfdrive/car/mazda/values.py
+6
View File
@@ -88,6 +88,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX85-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SH54-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXFG-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
@@ -100,10 +101,12 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x706, None): [
b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-U\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SH51-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXDL-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
@@ -185,6 +188,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'PX23-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX24-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM4-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXN8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXN8-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -209,10 +213,12 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x706, None): [
b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TK80-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PXM4-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
selfdrive/car/nissan/interface.py
-3
View File
@@ -36,9 +36,6 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.824
ret.centerToFront = ret.wheelbase * 0.44
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
return ret
# returns a car.CarState
selfdrive/car/nissan/values.py
+11 -21
View File
@@ -79,35 +79,25 @@ FINGERPRINTS = {
]
}
NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL])
NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40])
NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0x81])
NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0x81])
NISSAN_VERSION_REQUEST_KWP = b'\x21\x83'
NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83'
NISSAN_RX_OFFSET = 0x20
# Try diagnostic sessions: default, standby, extended, Nissan-specific
NISSAN_DIAGNOSTIC_SESSION_TYPES = (0x81, 0x89, 0x92, 0xc0)
NISSAN_DEFAULT_DIAGNOSTIC_SESSION_TYPE = 0xc0
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
*[
Request(
[NISSAN_DIAGNOSTIC_REQUEST_KWP + bytes([subfunction]), NISSAN_VERSION_REQUEST_KWP],
[NISSAN_DIAGNOSTIC_RESPONSE_KWP + bytes([subfunction]), NISSAN_VERSION_RESPONSE_KWP],
logging=subfunction != NISSAN_DEFAULT_DIAGNOSTIC_SESSION_TYPE,
) for subfunction in NISSAN_DIAGNOSTIC_SESSION_TYPES
],
*[
Request(
[NISSAN_DIAGNOSTIC_REQUEST_KWP + bytes([subfunction]), NISSAN_VERSION_REQUEST_KWP],
[NISSAN_DIAGNOSTIC_RESPONSE_KWP + bytes([subfunction]), NISSAN_VERSION_RESPONSE_KWP],
rx_offset=NISSAN_RX_OFFSET,
logging=subfunction != NISSAN_DEFAULT_DIAGNOSTIC_SESSION_TYPE,
) for subfunction in NISSAN_DIAGNOSTIC_SESSION_TYPES
],
Request(
[NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
[NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
),
Request(
[NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
[NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
rx_offset=NISSAN_RX_OFFSET,
),
Request(
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
selfdrive/car/subaru/interface.py
-4
View File
@@ -4,7 +4,6 @@ from panda import Panda
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags
from common.params import Params
class CarInterface(CarInterfaceBase):
@@ -106,9 +105,6 @@ class CarInterface(CarInterfaceBase):
else:
raise ValueError(f"unknown car: {candidate}")
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
Params().put("dp_lateral_steer_rate_cost", "0.7")
return ret
# returns a car.CarState
selfdrive/car/subaru/values.py
+9 -2
View File
@@ -1,11 +1,11 @@
from dataclasses import dataclass, field
from enum import IntFlag
from enum import Enum, IntFlag
from typing import Dict, List, Union
from cereal import car
from panda.python import uds
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
Ecu = car.CarParams.Ecu
@@ -56,10 +56,17 @@ class CAR:
OUTBACK_PREGLOBAL_2018 = "SUBARU OUTBACK 2018 - 2019"
class Footnote(Enum):
GLOBAL = CarFootnote(
"In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance.",
Column.PACKAGE)
@dataclass
class SubaruCarInfo(CarInfo):
package: str = "EyeSight Driver Assistance"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.subaru_a]))
footnotes: List[Enum] = field(default_factory=lambda: [Footnote.GLOBAL])
CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
selfdrive/car/tests/test_car_interfaces.py
+39 -17
View File
@@ -7,30 +7,49 @@ import importlib
from parameterized import parameterized
from cereal import car
from common.realtime import DT_CTRL
from selfdrive.car import gen_empty_fingerprint
from selfdrive.car.car_helpers import interfaces
from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS, all_known_cars
from selfdrive.test.fuzzy_generation import FuzzyGenerator
from selfdrive.car.fingerprints import all_known_cars
from selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator
def get_fuzzy_car_interface_args(draw: DrawType) -> dict:
# Fuzzy CAN fingerprints and FW versions to test more states of the CarInterface
fingerprint_strategy = st.fixed_dictionaries({key: st.dictionaries(st.integers(min_value=0, max_value=0x800),
st.integers(min_value=0, max_value=64)) for key in
gen_empty_fingerprint()})
# just the most important fields
car_fw_strategy = st.lists(st.fixed_dictionaries({
'ecu': st.sampled_from(list(car.CarParams.Ecu.schema.enumerants.keys())),
# TODO: only use reasonable addrs for the paired ecu and brand/platform
'address': st.integers(min_value=0, max_value=0x800),
}))
params_strategy = st.fixed_dictionaries({
'fingerprints': fingerprint_strategy,
'car_fw': car_fw_strategy,
'experimental_long': st.booleans(),
})
params: dict = draw(params_strategy)
params['car_fw'] = [car.CarParams.CarFw(**fw) for fw in params['car_fw']]
return params
class TestCarInterfaces(unittest.TestCase):
@parameterized.expand([(car,) for car in all_known_cars()])
@parameterized.expand([(car,) for car in sorted(all_known_cars())])
@settings(max_examples=5)
@given(data=st.data())
def test_car_interfaces(self, car_name, data):
if car_name in FINGERPRINTS:
fingerprint = FINGERPRINTS[car_name][0]
else:
fingerprint = {}
CarInterface, CarController, CarState = interfaces[car_name]
fingerprints = gen_empty_fingerprint()
fingerprints.update({k: fingerprint for k in fingerprints.keys()})
car_fw = []
args = get_fuzzy_car_interface_args(data.draw)
car_params = CarInterface.get_params(car_name, fingerprints, car_fw, experimental_long=False, docs=False)
car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'],
experimental_long=args['experimental_long'], docs=False)
car_interface = CarInterface(car_params, CarController, CarState)
assert car_params
assert car_interface
@@ -61,20 +80,23 @@ class TestCarInterfaces(unittest.TestCase):
elif tune.which() == 'indi':
self.assertTrue(len(tune.indi.outerLoopGainV))
cc_msg=FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True)
cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True)
# Run car interface
now_nanos = 0
CC = car.CarControl.new_message(**cc_msg)
for _ in range(10):
car_interface.update(CC, [])
car_interface.apply(CC, 0)
car_interface.apply(CC, 0)
car_interface.apply(CC, now_nanos)
car_interface.apply(CC, now_nanos)
now_nanos += DT_CTRL * 1e9 # 10 ms
CC = car.CarControl.new_message(**cc_msg)
CC.enabled = True
for _ in range(10):
car_interface.update(CC, [])
car_interface.apply(CC, 0)
car_interface.apply(CC, 0)
car_interface.apply(CC, now_nanos)
car_interface.apply(CC, now_nanos)
now_nanos += DT_CTRL * 1e9 # 10ms
# Test radar interface
RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface
selfdrive/car/torque_data/override.yaml
+3
View File
@@ -19,6 +19,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan]
FORD BRONCO SPORT 1ST GEN: [.nan, 1.5, .nan]
FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan]
FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan]
FORD F-150 14TH GEN: [.nan, 1.5, .nan]
FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
FORD MAVERICK 1ST GEN: [.nan, 1.5, .nan]
###
@@ -47,6 +48,8 @@ KIA SORENTO 4TH GEN: [2.5, 2.5, 0.1]
KIA NIRO HYBRID 2ND GEN: [2.42, 2.5, 0.12]
KIA NIRO EV 2ND GEN: [2.05, 2.5, 0.14]
GENESIS GV80 2023: [2.5, 2.5, 0.1]
KIA CARNIVAL 4TH GEN: [1.75, 1.75, 0.15]
GMC ACADIA DENALI 2018: [1.6, 1.6, 0.2]
# Dashcam or fallback configured as ideal car
mock: [10.0, 10, 0.0]
selfdrive/car/torque_data/params.yaml
-1
View File
@@ -10,7 +10,6 @@ CHRYSLER PACIFICA HYBRID 2017: [1.79422, 1.06831764583744, 0.116237]
CHRYSLER PACIFICA HYBRID 2018: [2.08887, 1.2943025830995154, 0.114818]
CHRYSLER PACIFICA HYBRID 2019: [1.90120, 1.1958788168371808, 0.131520]
GENESIS G70 2018: [3.8520195946707947, 2.354697063349854, 0.06830285485626221]
GMC ACADIA DENALI 2018: [1.3181430320331884, 1.1853735340610179, 0.3450592280031644]
HONDA ACCORD 2018: [1.7135052593468778, 0.3461280068322071, 0.21579936052863807]
HONDA ACCORD HYBRID 2018: [1.6651615004829625, 0.30322180951193245, 0.2083000440586149]
HONDA CIVIC (BOSCH) 2019: [1.691708637466905, 0.40132900729454185, 0.25460295304024094]
selfdrive/car/torque_data/substitute.yaml
+1 -3
View File
@@ -34,6 +34,7 @@ HYUNDAI KONA ELECTRIC 2022: HYUNDAI KONA ELECTRIC 2019
HYUNDAI IONIQ HYBRID 2017-2019: HYUNDAI IONIQ PLUG-IN HYBRID 2019
HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019
HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019
HYUNDAI IONIQ 6 2023: HYUNDAI IONIQ 5 2022
HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019
HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020
HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019
@@ -50,15 +51,12 @@ HONDA CR-V EU 2016: HONDA CR-V 2016
HONDA CIVIC SEDAN 1.6 DIESEL 2019: HONDA CIVIC (BOSCH) 2019
HONDA E 2020: HONDA CIVIC (BOSCH) 2019
HONDA ODYSSEY CHN 2019: HONDA ODYSSEY 2018
HONDA ODYSSEY HYBRID CHN 2022: HONDA ODYSSEY 2018
HONDA CR-V HYBRID 2019 w/ BSM: HONDA CR-V HYBRID 2019
BUICK LACROSSE 2017: CHEVROLET VOLT PREMIER 2017
BUICK REGAL ESSENCE 2018: CHEVROLET VOLT PREMIER 2017
CADILLAC ESCALADE ESV 2016: CHEVROLET VOLT PREMIER 2017
CADILLAC ATS Premium Performance 2018: CHEVROLET VOLT PREMIER 2017
CHEVROLET MALIBU PREMIER 2017: CHEVROLET VOLT PREMIER 2017
CHEVROLET TRAILBLAZER 2022: CHEVROLET VOLT PREMIER 2017
HOLDEN ASTRA RS-V BK 2017: CHEVROLET VOLT PREMIER 2017
SKODA FABIA 4TH GEN: VOLKSWAGEN GOLF 7TH GEN
selfdrive/car/toyota/carcontroller.py
+83 -91
View File
@@ -26,7 +26,9 @@ MAX_USER_TORQUE = 500
# LTA limits
# EPS ignores commands above this angle and causes PCS to fault
MAX_STEER_ANGLE = 94.9461 # deg
MAX_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes
# rick - toyota auto lock / unlock
GearShifter = car.CarState.GearShifter
UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00'
LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00'
@@ -60,43 +62,31 @@ class CarController:
self.standstill_req = False
self.steer_rate_counter = 0
self.steer_rate_counter = 0
self.packer = CANPacker(dbc_name)
self.gas = 0
self.accel = 0
# dp
self.dp_toyota_sng = False
p = Params()
# dp - auto lock / unlock
self.dp_toyota_auto_lock = p.get_bool("dp_toyota_auto_lock")
self.dp_toyota_auto_unlock = p.get_bool("dp_toyota_auto_unlock")
self.dp_toyota_sng = p.get_bool("dp_toyota_sng")
self.dp_toyota_auto_lock_gear_prev = GearShifter.park
self.dp_toyota_auto_lock_once = False
self.dp_toyota_auto_lock = False
self.dp_toyota_auto_unlock = False
self.last_gear = GearShifter.park
self.lock_once = False
self.lat_controller_type = None
self.lat_controller_type_prev = None
self.blindspot_debug_enabled_left = False
self.blindspot_debug_enabled_right = False
self.blindspot_frame = 0
# dp - bsm
self.dp_toyota_enhanced_bsm = p.get_bool("dp_toyota_enhanced_bsm")
self._blindspot_debug_enabled_left = False
self._blindspot_debug_enabled_right = False
self._blindspot_frame = 0
if self.CP.carFingerprint in TSS2_CAR: # tss2 can do higher hz then tss1 and can be on at all speed/standstill
self.blindspot_rate = 2
self.blindspot_always_on = True
self._blindspot_rate = 2
self._blindspot_always_on = True
else:
self.blindspot_rate = 20
self.blindspot_always_on = False
self._blindspot_rate = 20
self._blindspot_always_on = False
def update(self, CC, CS, now_nanos, dragonconf):
if dragonconf is not None:
self.dp_toyota_sng = dragonconf.dpToyotaSng
self.dp_toyota_auto_lock = dragonconf.dpToyotaAutoLock
self.dp_toyota_auto_unlock = dragonconf.dpToyotaAutoUnlock
self.dp_toyota_debug_bsm = dragonconf.dpToyotaDebugBsm
self.lat_controller_type = CC.latController
if self.lat_controller_type != self.lat_controller_type_prev:
self.params.update(CC.latController)
self.lat_controller_type_prev = self.lat_controller_type
self.dp_toyota_change5speed = Params().get_bool("dp_toyota_change5speed")
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
@@ -105,6 +95,60 @@ class CarController:
# *** control msgs ***
can_sends = []
# dp - door auto lock / unlock logic
# thanks to AlexandreSato & cydia2020
# https://github.com/AlexandreSato/animalpilot/blob/personal/doors.py
if not CS.out.doorOpen:
gear = CS.out.gearShifter
if gear == GearShifter.park and self.dp_toyota_auto_lock_gear_prev != gear:
if self.dp_toyota_auto_lock:
can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0))
self.dp_toyota_auto_lock_once = False
elif gear == GearShifter.drive and not self.dp_toyota_auto_lock_once and CS.out.vEgo >= LOCK_AT_SPEED:
if self.dp_toyota_auto_unlock:
can_sends.append(make_can_msg(0x750, LOCK_CMD, 0))
self.dp_toyota_auto_lock_once = True
self.dp_toyota_auto_lock_gear_prev = gear
# Enable blindspot debug mode once (@arne182)
# let's keep all the commented out code for easy debug purpose for future.
if self.dp_toyota_enhanced_bsm:
if self.frame > 2000:
#left bsm
if not self._blindspot_debug_enabled_left:
if (self._blindspot_always_on or (CS.out.leftBlinker and CS.out.vEgo > 6)): # eagle eye camera will stop working if right bsm is switched on under 6m/s
can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, True))
self._blindspot_debug_enabled_left = True
# print("bsm debug left, on")
else:
if not self._blindspot_always_on and not CS.out.leftBlinker and self.frame - self._blindspot_frame > 500:
can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
self._blindspot_debug_enabled_left = False
# print("bsm debug left, off")
if self.frame % self._blindspot_rate == 0:
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
if CS.out.leftBlinker:
self._blindspot_frame = self.frame
# print(self._blindspot_frame)
# print("bsm poll left")
#right bsm
if not self._blindspot_debug_enabled_right:
if (self._blindspot_always_on or (CS.out.rightBlinker and CS.out.vEgo > 6)): # eagle eye camera will stop working if right bsm is switched on under 6m/s
can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
self._blindspot_debug_enabled_right = True
# print("bsm debug right, on")
else:
if not self._blindspot_always_on and not CS.out.rightBlinker and self.frame - self._blindspot_frame > 500:
can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self._blindspot_debug_enabled_right = False
# print("bsm debug right, off")
if self.frame % self._blindspot_rate == self._blindspot_rate/2:
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
if CS.out.rightBlinker:
self._blindspot_frame = self.frame
# print(self._blindspot_frame)
# print("bsm poll right")
# *** steer torque ***
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
@@ -135,7 +179,7 @@ class CarController:
# Angular rate limit based on speed
apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params)
if not CC.latActive:
if not lat_active:
apply_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
self.last_angle = clip(apply_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE)
@@ -147,8 +191,11 @@ class CarController:
# on consecutive messages
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req))
if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
lta_active = CC.latActive and self.CP.steerControlType == SteerControlType.angle
can_sends.append(create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2))
lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle
full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and
abs(CS.out.steeringTorque) < MAX_DRIVER_TORQUE_ALLOWANCE)
setme_x64 = 100 if lta_active and full_torque_condition else 0
can_sends.append(create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2, setme_x64))
# *** gas and brake ***
if self.CP.enableGasInterceptor and CC.longActive:
@@ -174,69 +221,14 @@ class CarController:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor):
if not self.dp_toyota_sng and CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor):
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.standstill_req = False if self.dp_toyota_sng else self.standstill_req
self.last_standstill = CS.out.standstill
# dp - door auto lock / unlock logic
# thanks to AlexandreSato & cydia2020
# https://github.com/AlexandreSato/animalpilot/blob/personal/doors.py
if self.dp_toyota_auto_lock or self.dp_toyota_auto_unlock:
gear = CS.out.gearShifter
if self.last_gear != gear and gear == GearShifter.park:
if self.dp_toyota_auto_unlock:
can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0))
if self.dp_toyota_auto_lock:
self.lock_once = False
elif self.dp_toyota_auto_lock and not CS.out.doorOpen and gear == GearShifter.drive and not self.lock_once and CS.out.vEgo >= LOCK_AT_SPEED:
can_sends.append(make_can_msg(0x750, LOCK_CMD, 0))
self.lock_once = True
self.last_gear = gear
# Enable blindspot debug mode once (@arne182)
# let's keep all the commented out code for easy debug purpose for future.
if self.dp_toyota_debug_bsm:
if self.frame > 2000:
#left bsm
if not self.blindspot_debug_enabled_left:
if (self.blindspot_always_on or (CS.out.leftBlinker and CS.out.vEgo > 6)): # eagle eye camera will stop working if right bsm is switched on under 6m/s
can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, True))
self.blindspot_debug_enabled_left = True
# print("bsm debug left, on")
else:
if not self.blindspot_always_on and not CS.out.leftBlinker and self.frame - self.blindspot_frame > 500:
can_sends.append(set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
self.blindspot_debug_enabled_left = False
# print("bsm debug left, off")
if self.frame % self.blindspot_rate == 0:
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
if CS.out.leftBlinker:
self.blindspot_frame = self.frame
# print(self.blindspot_frame)
# print("bsm poll left")
#right bsm
if not self.blindspot_debug_enabled_right:
if (self.blindspot_always_on or (CS.out.rightBlinker and CS.out.vEgo > 6)): # eagle eye camera will stop working if right bsm is switched on under 6m/s
can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
self.blindspot_debug_enabled_right = True
# print("bsm debug right, on")
else:
if not self.blindspot_always_on and not CS.out.rightBlinker and self.frame - self.blindspot_frame > 500:
can_sends.append(set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
# print("bsm debug right, off")
if self.frame % self.blindspot_rate == self.blindspot_rate/2:
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
if CS.out.rightBlinker:
self.blindspot_frame = self.frame
# print(self.blindspot_frame)
# print("bsm poll right")
# 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
@@ -245,10 +237,10 @@ class CarController:
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, CS.distance, self.dp_toyota_change5speed))
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
self.accel = pcm_accel_cmd
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance, self.dp_toyota_change5speed))
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
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.
@@ -276,7 +268,7 @@ class CarController:
if self.frame % 20 == 0 or send_ui:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, CC.latActive, CS.lkas_hud))
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
selfdrive/car/toyota/carstate.py
+60 -297
View File
@@ -9,22 +9,7 @@ from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
from common.params import Params, put_nonblocking
import time
from math import floor
# dp
DP_ACCEL_ECO = 0
DP_ACCEL_NORMAL = 1
DP_ACCEL_SPORT = 2
_TRAFFIC_SINGAL_MAP = {
1: "kph",
36: "mph",
65: "No overtake",
66: "No overtake"
}
from common.params import Params
SteerControlType = car.CarParams.SteerControlType
@@ -35,6 +20,9 @@ SteerControlType = car.CarParams.SteerControlType
# - initializing: LTA can report 0 as long as STEER_TORQUE_SENSOR->STEER_ANGLE_INITIALIZING is 1,
# and is a catch-all for LKA
TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
# - lka/lta msg drop out: 3 (recoverable)
# - prolonged high driver torque: 17 (permanent)
PERM_STEER_FAULTS = (3, 17)
class CarState(CarStateBase):
@@ -51,44 +39,24 @@ class CarState(CarStateBase):
# Need to apply an offset as soon as the steering angle measurements are both received
self.accurate_steer_angle_seen = False
self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False)
self._init_traffic_signals()
self.low_speed_lockout = False
self.acc_type = 1
self.lkas_hud = {}
#dp
self.frame = 0
self.dp_sig_check = False
self.dp_sig_sport_on_seen = True
self.dp_sig_econ_on_seen = True
self.dp_accel_profile = None
self.dp_accel_profile_prev = None
self.dp_accel_profile_init = False
self.dp_toyota_ap_btn_link = Params().get_bool('dp_toyota_ap_btn_link')
self.read_distance_lines = 0
self.read_distance_lines_init = False
self.distance = 0
self.dp_toyota_fp_btn_link = Params().get_bool('dp_toyota_fp_btn_link')
# zss
self.dp_toyota_zss = Params().get_bool('dp_toyota_zss')
self.dp_zss_compute = False
self.dp_zss_cruise_active_last = False
self.dp_zss_angle_offset = 0.
# bsm
self.dp_toyota_debug_bsm = Params().get_bool('dp_toyota_debug_bsm')
self.dp_toyota_enhanced_bsm = Params().get_bool('dp_toyota_enhanced_bsm')
self._left_blindspot = False
self._left_blindspot_d1 = 0
self._left_blindspot_d2 = 0
self._left_blindspot_counter = 0
self.left_blindspot = False
self.left_blindspot_d1 = 0
self.left_blindspot_d2 = 0
self.left_blindspot_counter = 0
self._right_blindspot = False
self._right_blindspot_d1 = 0
self._right_blindspot_d2 = 0
self._right_blindspot_counter = 0
self.right_blindspot = False
self.right_blindspot_d1 = 0
self.right_blindspot_d2 = 0
self.right_blindspot_counter = 0
self.frame = 0
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
@@ -99,8 +67,7 @@ class CarState(CarStateBase):
ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
#ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
ret.brakeLightsDEPRECATED = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0)
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
ret.gasPressed = ret.gas > 805
@@ -138,85 +105,10 @@ class CarState(CarStateBase):
ret.steeringAngleOffsetDeg = self.angle_offset.x
ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
# dp - toyota zss
if self.dp_toyota_zss:
zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
# only compute zss offset when acc is active
if bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) and not self.dp_zss_cruise_active_last:
self.dp_zss_compute = True # cruise was just activated, so allow offset to be recomputed
self.dp_zss_cruise_active_last = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
# compute zss offset
if self.dp_zss_compute:
if abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3:
self.dp_toyota_zss = False
self.dp_zss_angle_offset = zorro_steer - ret.steeringAngleDeg
# apply offset
ret.steeringAngleDeg = zorro_steer - self.dp_zss_angle_offset
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
#dp: Thank you Arne (acceleration)
if self.dp_toyota_ap_btn_link:
sport_on_sig = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.RAV4_TSS2, CAR.LEXUS_ES_TSS2, CAR.HIGHLANDER_TSS2) else 'SPORT_ON'
# check signal once
if not self.dp_sig_check:
self.dp_sig_check = True
# sport on
try:
sport_on = cp.vl["GEAR_PACKET"][sport_on_sig]
except KeyError:
sport_on = 0
self.dp_sig_sport_on_seen = False
# econ on
try:
econ_on = cp.vl["GEAR_PACKET"]['ECON_ON']
except KeyError:
econ_on = 0
self.dp_sig_econ_on_seen = False
else:
sport_on = cp.vl["GEAR_PACKET"][sport_on_sig] if self.dp_sig_sport_on_seen else 0
econ_on = cp.vl["GEAR_PACKET"]['ECON_ON'] if self.dp_sig_econ_on_seen else 0
if sport_on == 0 and econ_on == 0:
self.dp_accel_profile = DP_ACCEL_NORMAL
elif sport_on == 1:
self.dp_accel_profile = DP_ACCEL_SPORT
elif econ_on == 1:
self.dp_accel_profile = DP_ACCEL_ECO
# if init is false, we sync profile with whatever mode we have on car
if not self.dp_accel_profile_init or self.dp_accel_profile != self.dp_accel_profile_prev:
put_nonblocking('dp_accel_profile', str(self.dp_accel_profile))
put_nonblocking('dp_last_modified',str(floor(time.time())))
self.dp_accel_profile_init = True
self.dp_accel_profile_prev = self.dp_accel_profile
# distance button
#dp: Thank you Arne (distance button)
if self.dp_toyota_fp_btn_link:
if not self.read_distance_lines_init or self.read_distance_lines != cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']:
self.read_distance_lines_init = True
self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
put_nonblocking('dp_following_profile', str(int(max(self.read_distance_lines - 1, 0)))) # Skipping one profile toyota mid is weird.
put_nonblocking('dp_last_modified',str(floor(time.time())))
if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
# KRKeegan - Add support for toyota distance button
self.distance = 1 if cp_cam.vl["ACC_CONTROL"]["DISTANCE"] == 1 else 0
ret.distanceLines = cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"]
if self.CP.carFingerprint in RADAR_ACC_CAR:
# KRKeegan - Add support for toyota distance button these cars have the acc_control on car can
self.distance = 1 if cp.vl["ACC_CONTROL"]["DISTANCE"] == 1 else 0
ret.distanceLines = cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"]
#dp
ret.engineRpm = cp.vl["ENGINE_RPM"]['RPM']
ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
@@ -227,13 +119,11 @@ class CarState(CarStateBase):
# Check EPS LKA/LTA fault status
ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in TEMP_STEER_FAULTS
# 3 is a fault from the lka command message not being received by the EPS (recoverable)
# 17 is a fault from a prolonged high torque delta between cmd and user (permanent)
ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in (3, 17)
ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in PERM_STEER_FAULTS
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 (3,)
ret.steerFaultPermanent = ret.steerFaultPermanent or cp.vl["EPS_STATUS"]["LTA_STATE"] in PERM_STEER_FAULTS
if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
# TODO: find the bit likely in DSU_CRUISE that describes an ACC fault. one may also exist in CLUTCH
@@ -286,136 +176,54 @@ class CarState(CarStateBase):
# Enable blindspot debug mode once (@arne182)
# let's keep all the commented out code for easy debug purpose for future.
if self.dp_toyota_debug_bsm and self.frame > 1999: #self.CP.carFingerprint == CAR.PRIUS_TSS2: #not (self.CP.carFingerprint in TSS2_CAR or self.CP.carFingerprint == CAR.CAMRY or self.CP.carFingerprint == CAR.CAMRYH):
distance_1 = cp.vl["DEBUG"].get('BLINDSPOTD1')
distance_2 = cp.vl["DEBUG"].get('BLINDSPOTD2')
side = cp.vl["DEBUG"].get('BLINDSPOTSIDE')
if self.dp_toyota_enhanced_bsm and self.frame > 1999: #self.CP.carFingerprint == CAR.PRIUS_TSS2: #not (self.CP.carFingerprint in TSS2_CAR or self.CP.carFingerprint == CAR.CAMRY or self.CP.carFingerprint == CAR.CAMRYH):
distance_1 = cp.vl["DEBUG"].get('BLINDSPOTD1')
distance_2 = cp.vl["DEBUG"].get('BLINDSPOTD2')
side = cp.vl["DEBUG"].get('BLINDSPOTSIDE')
if distance_1 is not None and distance_2 is not None and side is not None:
if side == 65: # Left blind spot
if distance_1 != self.left_blindspot_d1:
self.left_blindspot_d1 = distance_1
self.left_blindspot_counter = 100
if distance_2 != self.left_blindspot_d2:
self.left_blindspot_d2 = distance_2
self.left_blindspot_counter = 100
if self.left_blindspot_d1 > 10 or self.left_blindspot_d2 > 10:
self.left_blindspot = True
elif side == 66: # Right blind spot
if distance_1 != self.right_blindspot_d1:
self.right_blindspot_d1 = distance_1
self.right_blindspot_counter = 100
if distance_2 != self.right_blindspot_d2:
self.right_blindspot_d2 = distance_2
self.right_blindspot_counter = 100
if self.right_blindspot_d1 > 10 or self.right_blindspot_d2 > 10:
self.right_blindspot = True
if distance_1 is not None and distance_2 is not None and side is not None:
if side == 65: # Left blind spot
if distance_1 != self._left_blindspot_d1:
self._left_blindspot_d1 = distance_1
self._left_blindspot_counter = 100
if distance_2 != self._left_blindspot_d2:
self._left_blindspot_d2 = distance_2
self._left_blindspot_counter = 100
if self._left_blindspot_d1 > 10 or self._left_blindspot_d2 > 10:
self._left_blindspot = True
elif side == 66: # Right blind spot
if distance_1 != self._right_blindspot_d1:
self._right_blindspot_d1 = distance_1
self._right_blindspot_counter = 100
if distance_2 != self._right_blindspot_d2:
self._right_blindspot_d2 = distance_2
self._right_blindspot_counter = 100
if self._right_blindspot_d1 > 10 or self._right_blindspot_d2 > 10:
self._right_blindspot = True
if self.left_blindspot_counter > 0:
self.left_blindspot_counter -= 2
else:
self.left_blindspot = False
self.left_blindspot_d1 = 0
self.left_blindspot_d2 = 0
if self._left_blindspot_counter > 0:
self._left_blindspot_counter -= 2
else:
self._left_blindspot = False
self._left_blindspot_d1 = 0
self._left_blindspot_d2 = 0
if self.right_blindspot_counter > 0:
self.right_blindspot_counter -= 2
else:
self.right_blindspot = False
self.right_blindspot_d1 = 0
self.right_blindspot_d2 = 0
ret.leftBlindspot = self.left_blindspot
ret.rightBlindspot = self.right_blindspot
if self._right_blindspot_counter > 0:
self._right_blindspot_counter -= 2
else:
self._right_blindspot = False
self._right_blindspot_d1 = 0
self._right_blindspot_d2 = 0
ret.leftBlindspot = self._left_blindspot
ret.rightBlindspot = self._right_blindspot
if self.CP.carFingerprint != CAR.PRIUS_V:
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
self._update_traffic_signals(cp_cam)
ret.cruiseState.speedLimit = self._calculate_speed_limit()
self.frame += 1
return ret
def _init_traffic_signals(self):
self._tsgn1 = None
self._spdval1 = None
self._splsgn1 = None
self._tsgn2 = None
self._splsgn2 = None
self._tsgn3 = None
self._splsgn3 = None
self._tsgn4 = None
self._splsgn4 = None
def _update_traffic_signals(self, cp_cam):
# Print out car signals for traffic signal detection
tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
has_changed = tsgn1 != self._tsgn1 \
or spdval1 != self._spdval1 \
or splsgn1 != self._splsgn1 \
or tsgn2 != self._tsgn2 \
or splsgn2 != self._splsgn2 \
or tsgn3 != self._tsgn3 \
or splsgn3 != self._splsgn3 \
or tsgn4 != self._tsgn4 \
or splsgn4 != self._splsgn4
self._tsgn1 = tsgn1
self._spdval1 = spdval1
self._splsgn1 = splsgn1
self._tsgn2 = tsgn2
self._splsgn2 = splsgn2
self._tsgn3 = tsgn3
self._splsgn3 = splsgn3
self._tsgn4 = tsgn4
self._splsgn4 = splsgn4
if not has_changed:
return
print('---- TRAFFIC SIGNAL UPDATE -----')
if tsgn1 is not None and tsgn1 != 0:
print(f'TSGN1: {self._traffic_signal_description(tsgn1)}')
if spdval1 is not None and spdval1 != 0:
print(f'SPDVAL1: {spdval1}')
if splsgn1 is not None and splsgn1 != 0:
print(f'SPLSGN1: {splsgn1}')
if tsgn2 is not None and tsgn2 != 0:
print(f'TSGN2: {self._traffic_signal_description(tsgn2)}')
if splsgn2 is not None and splsgn2 != 0:
print(f'SPLSGN2: {splsgn2}')
if tsgn3 is not None and tsgn3 != 0:
print(f'TSGN3: {self._traffic_signal_description(tsgn3)}')
if splsgn3 is not None and splsgn3 != 0:
print(f'SPLSGN3: {splsgn3}')
if tsgn4 is not None and tsgn4 != 0:
print(f'TSGN4: {self._traffic_signal_description(tsgn4)}')
if splsgn4 is not None and splsgn4 != 0:
print(f'SPLSGN4: {splsgn4}')
print('------------------------')
def _traffic_signal_description(self, tsgn):
desc = _TRAFFIC_SINGAL_MAP.get(int(tsgn))
return f'{tsgn}: {desc}' if desc is not None else f'{tsgn}'
def _calculate_speed_limit(self):
if self._tsgn1 == 1:
return self._spdval1 * CV.KPH_TO_MS
if self._tsgn1 == 36:
return self._spdval1 * CV.MPH_TO_MS
return 0
@staticmethod
def get_can_parser(CP):
signals = [
@@ -435,7 +243,7 @@ class CarState(CarStateBase):
("PARKING_BRAKE", "BODY_CONTROL_STATE"),
("UNITS", "BODY_CONTROL_STATE_2"),
("TC_DISABLED", "ESP_CONTROL"),
#("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"),
("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"),
("STEER_FRACTION", "STEER_ANGLE_SENSOR"),
("STEER_RATE", "STEER_ANGLE_SENSOR"),
("CRUISE_ACTIVE", "PCM_CRUISE"),
@@ -449,12 +257,6 @@ class CarState(CarStateBase):
("TURN_SIGNALS", "BLINKERS_STATE"),
("LKA_STATE", "EPS_STATUS"),
("AUTO_HIGH_BEAM", "LIGHT_STALK"),
#dp
("SPORT_ON", "GEAR_PACKET"),
("ECON_ON", "GEAR_PACKET"),
("RPM", "ENGINE_RPM"),
("BRAKE_LIGHTS_ACC", "ESP_CONTROL"),
("DISTANCE_LINES", "PCM_CRUISE_SM"),
]
# Check LTA state if using LTA angle control
@@ -475,8 +277,6 @@ class CarState(CarStateBase):
("PCM_CRUISE", 33),
("PCM_CRUISE_SM", 1),
("STEER_TORQUE_SENSOR", 50),
#dp
("ENGINE_RPM", 100),
]
if CP.flags & ToyotaFlags.HYBRID:
@@ -485,17 +285,6 @@ class CarState(CarStateBase):
else:
signals.append(("GAS_PEDAL", "GAS_PEDAL"))
checks.append(("GAS_PEDAL", 33))
#dp acceleration
if CP.carFingerprint in (CAR.RAV4_TSS2, CAR.LEXUS_ES_TSS2, CAR.HIGHLANDER_TSS2):
signals.append(("SPORT_ON_2", "GEAR_PACKET"))
if CP.carFingerprint in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.CHR_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.RAV4H_TSS2, CAR.MIRAI, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.CHRH):
signals.append(("SPORT_ON", "GEAR_PACKET"))
signals.append(("ECON_ON", "GEAR_PACKET"))
if CP.flags & ToyotaFlags.SMART_DSU:
signals.append(("FD_BUTTON", "SDSU"))
checks.append(("SDSU", 0))
if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
signals.append(("MAIN_ON", "DSU_CRUISE"))
@@ -516,8 +305,7 @@ class CarState(CarStateBase):
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR"))
checks.append(("GAS_SENSOR", 50))
dp_toyota_debug_bsm = Params().get_bool('dp_toyota_debug_bsm')
dp_toyota_enhanced_bsm = Params().get_bool('dp_toyota_enhanced_bsm')
if CP.enableBsm:
signals += [
@@ -528,13 +316,14 @@ class CarState(CarStateBase):
]
checks.append(("BSM", 1))
if dp_toyota_debug_bsm:
if dp_toyota_enhanced_bsm:
signals +=[
("BLINDSPOT", "DEBUG"),
("BLINDSPOTSIDE", "DEBUG"),
("BLINDSPOTD1", "DEBUG"),
("BLINDSPOTD2", "DEBUG"),
]
checks.append(("DEBUG", 65))
if CP.carFingerprint in RADAR_ACC_CAR:
if not CP.flags & ToyotaFlags.SMART_DSU.value:
@@ -560,34 +349,12 @@ class CarState(CarStateBase):
("PRE_COLLISION", 33),
]
# dp - add zss signal check
if Params().get_bool('dp_toyota_zss'):
signals += [("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0)]
checks += [("SECONDARY_STEER_ANGLE", 0)]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
# Include traffic signal, single
signals = [
("TSGN1", "RSA1", 0),
("SPDVAL1", "RSA1", 0),
("SPLSGN1", "RSA1", 0),
("TSGN2", "RSA1", 0),
("SPLSGN2", "RSA1", 0),
("TSGN3", "RSA2", 0),
("SPLSGN3", "RSA2", 0),
("TSGN4", "RSA2", 0),
("SPLSGN4", "RSA2", 0),
]
# use steering message to check if panda is connected to frc
checks = [
("RSA1", 0),
("RSA2", 0),
]
signals = []
checks = []
if CP.carFingerprint != CAR.PRIUS_V:
signals += [
@@ -607,15 +374,11 @@ class CarState(CarStateBase):
("FORCE", "PRE_COLLISION"),
("ACC_TYPE", "ACC_CONTROL"),
("FCW", "ACC_HUD"),
#dp
("DISTANCE_LINES", "PCM_CRUISE_SM"),
("DISTANCE", "ACC_CONTROL"),
]
checks += [
("PRE_COLLISION", 33),
("ACC_CONTROL", 33),
("ACC_HUD", 1),
("PCM_CRUISE_SM", 0),
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
selfdrive/car/toyota/interface.py
+12 -35
View File
@@ -3,10 +3,9 @@ from cereal import car
from common.conversions import Conversions as CV
from panda import Panda
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
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
from selfdrive.car.interfaces import CarInterfaceBase
from common.params import Params
EventName = car.CarEvent.EventName
SteerControlType = car.CarParams.SteerControlType
@@ -45,7 +44,6 @@ class CarInterface(CarInterfaceBase):
stop_and_go = False
params = Params()
if candidate == CAR.PRIUS:
stop_and_go = True
ret.wheelbase = 2.70
@@ -53,14 +51,10 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
# Only give steer angle deadzone to for bad angle sensor prius
if params.get_bool("dp_toyota_prius_bad_angle_tune"):
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
else:
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
elif candidate == CAR.PRIUS_V:
stop_and_go = True
@@ -206,9 +200,6 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.444
ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection, steering_angle_deadzone_deg = 0.0)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
ret.centerToFront = ret.wheelbase * 0.44
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
@@ -247,12 +238,6 @@ class CarInterface(CarInterfaceBase):
ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR)
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
if int(Params().get("dp_atl").decode('utf-8')) == 1:
ret.openpilotLongitudinalControl = False
if candidate == CAR.CHR_TSS2:
ret.enableBsm = True
if not ret.openpilotLongitudinalControl:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
@@ -271,26 +256,18 @@ class CarInterface(CarInterfaceBase):
if candidate in TSS2_CAR or ret.enableGasInterceptor:
tune.kpBP = [0., 5., 20.]
tune.kpV = [1.3, 1.0, 0.7]
#tune.kpBP = [0., 5., 20., 30.]
#tune.kpV = [1.3, 1.0, 0.7, 0.1]
tune.kiBP = [0., 1., 2., 3., 4., 5., 12., 20., 27., 40.]
tune.kiV = [.348, .3361, .3168, .2831, .2571, .226, .198, .17, .10, .01]
tune.kiBP = [0., 5., 12., 20., 27.]
tune.kiV = [.35, .23, .20, .17, .1]
if candidate in TSS2_CAR:
ret.vEgoStopping = 0.15 # car is near 0.1 to 0.2 when car starts requesting stopping accel
ret.vEgoStarting = 0.15 # needs to be > or == vEgoStopping
ret.stopAccel = -0.4 # Toyota requests -0.4 when stopped
ret.stoppingDecelRate = 0.05 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4
#ret.longitudinalActuatorDelayLowerBound = 0.2
#ret.longitudinalActuatorDelayUpperBound = 0.2
### stock ###
#ret.vEgoStopping = 0.25
#ret.vEgoStarting = 0.25
#ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
else:
tune.kpBP = [0., 5., 35.]
tune.kiBP = [0., 35.]
tune.kpV = [3.6, 2.4, 1.5]
tune.kiV = [0.54, 0.36]
return ret
# returns a car.CarState
@@ -326,4 +303,4 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos, self.dragonconf)
return self.CC.update(c, self.CS, now_nanos)
selfdrive/car/toyota/toyotacan.py
+6 -6
View File
@@ -9,7 +9,7 @@ def create_steer_command(packer, steer, steer_req):
return packer.make_can_msg("STEERING_LKA", 0, values)
def create_lta_steer_command(packer, steer_angle, steer_req, frame):
def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64):
"""Creates a CAN message for the Toyota LTA Steer Command."""
values = {
@@ -17,27 +17,27 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame):
"SETME_X1": 1,
"SETME_X3": 3,
"PERCENTAGE": 100,
"SETME_X64": 0,
"SETME_X64": setme_x64,
"ANGLE": 0,
"STEER_ANGLE_CMD": steer_angle,
"STEER_REQUEST": steer_req,
"STEER_REQUEST_2": steer_req,
"BIT": 0,
"CLEAR_HOLD_STEERING_ALERT": 0,
}
return packer.make_can_msg("STEERING_LTA", 0, values)
def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance, dp_toyota_change5speed):
def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type):
# TODO: find the exact canceling bit that does not create a chime
values = {
"ACCEL_CMD": accel,
"ACC_TYPE": acc_type,
"DISTANCE": distance,
"DISTANCE": 0,
"MINI_CAR": lead,
"PERMIT_BRAKING": 1,
"RELEASE_STANDSTILL": not standstill_req,
"CANCEL_REQ": pcm_cancel,
"ALLOW_LONG_PRESS": 2 if dp_toyota_change5speed else 1,
"ALLOW_LONG_PRESS": 1,
}
return packer.make_can_msg("ACC_CONTROL", 0, values)
selfdrive/car/toyota/tunes.py
-129
View File
@@ -1,129 +0,0 @@
#!/usr/bin/env python3
from enum import Enum
class LongTunes(Enum):
TSS2 = 0
TSS = 1
class LatTunes(Enum):
INDI_PRIUS = 0
LQR_RAV4 = 1
PID_A = 2
PID_B = 3
PID_C = 4
PID_D = 5
PID_E = 6
PID_F = 7
PID_G = 8
PID_I = 9
PID_H = 10
PID_J = 11
PID_K = 12
PID_L = 13
PID_M = 14
PID_N = 15
INDI_PRIUS_TSS2 = 16
###### LONG ######
def set_long_tune(tune, name):
# Improved longitudinal tune
if name == LongTunes.TSS2:
tune.deadzoneBP = [0., 8.05]
tune.deadzoneV = [.0, .14]
tune.kpBP = [0., 5., 20., 30.]
tune.kpV = [1.3, 1.0, 0.7, 0.1]
#really smooth (make it toggleable)
#tune.kiBP = [0., 0.07, 5, 8, 11., 18., 20., 24., 33.]
#tune.kiV = [.001, .01, .1, .18, .21, .22, .23, .22, .001]
#okay ish
#tune.kiBP = [0., 11., 17., 20., 24., 30., 33., 40.]
#tune.kiV = [.001, .21, .22, .23, .22, .1, .001, .0001]
tune.kiBP = [0., 6., 8., 11., 30., 33., 40.]
tune.kiV = [.001, .07, .15, .2, .2, .01, .0001]
# Default longitudinal tune
elif name == LongTunes.TSS:
tune.deadzoneBP = [0., 9.]
tune.deadzoneV = [.0, .15]
tune.kpBP = [0., 5., 35.]
tune.kiBP = [0., 35.]
tune.kpV = [3.6, 2.4, 1.5]
tune.kiV = [0.54, 0.36]
else:
raise NotImplementedError('This longitudinal tune does not exist')
###### LAT ######
def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
if name == LatTunes.INDI_PRIUS_TSS2:
tune.init('indi')
#tune.indi.innerLoopGainBP = [20, 24, 30]
#tune.indi.innerLoopGainV = [7.25, 7.5, 9]
#tune.indi.outerLoopGainBP = [20, 24, 30]
#tune.indi.outerLoopGainV = [6, 7.25, 6]
#tune.indi.timeConstantBP = [20, 24]
#tune.indi.timeConstantV = [2.0, 2.2]
#tune.indi.actuatorEffectivenessBP = [20, 24]
#tune.indi.actuatorEffectivenessV = [2, 3]
tune.indi.innerLoopGainBP = [0.]
tune.indi.innerLoopGainV = [15]
tune.indi.outerLoopGainBP = [0.]
tune.indi.outerLoopGainV = [17]
tune.indi.timeConstantBP = [0.]
tune.indi.timeConstantV = [4.5]
tune.indi.actuatorEffectivenessBP = [0.]
tune.indi.actuatorEffectivenessV = [15]
elif 'PID' in str(name):
tune.init('pid')
tune.pid.kiBP = [0.0]
tune.pid.kpBP = [0.0]
if name == LatTunes.PID_A:
tune.pid.kpV = [0.2]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00003
elif name == LatTunes.PID_C:
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_D:
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1]
tune.pid.kf = 0.00007818594
elif name == LatTunes.PID_F:
tune.pid.kpV = [0.723]
tune.pid.kiV = [0.0428]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_G:
tune.pid.kpV = [0.18]
tune.pid.kiV = [0.015]
tune.pid.kf = 0.00012
elif name == LatTunes.PID_H:
tune.pid.kpV = [0.17]
tune.pid.kiV = [0.03]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_I:
tune.pid.kpV = [0.15]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00004
elif name == LatTunes.PID_J:
tune.pid.kpV = [0.19]
tune.pid.kiV = [0.02]
tune.pid.kf = 0.00007818594
elif name == LatTunes.PID_L:
tune.pid.kpV = [0.3]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_M:
tune.pid.kpV = [0.3]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00007
elif name == LatTunes.PID_N:
tune.pid.kpV = [0.35]
tune.pid.kiV = [0.15]
tune.pid.kf = 0.00007818594
else:
raise NotImplementedError('This PID tune does not exist')
else:
raise NotImplementedError('This lateral tune does not exist')
selfdrive/car/toyota/values.py
+24 -14
View File
@@ -24,17 +24,14 @@ class CarControllerParams:
# Lane Tracing Assist (LTA) control limits
# Assuming a steering ratio of 13.7:
# Limit to ~2.5 m/s^3 up (9 deg/s), ~3.6 m/s^3 down (13 deg/s) at 75 mph
# Worst case, the low speed limits will allow 4.9 m/s^3 up and down (18 deg/s) at 75 mph,
# Limit to ~2.0 m/s^3 up (7.5 deg/s), ~3.5 m/s^3 down (13 deg/s) at 75 mph
# Worst case, the low speed limits will allow ~4.0 m/s^3 up (15 deg/s) and ~4.9 m/s^3 down (18 deg/s) at 75 mph,
# however the EPS has its own internal limits at all speeds which are less than that
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.18])
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.3, 0.15])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26])
def __init__(self, CP):
self.update(CP.lateralTuning.which)
def update(self, tune):
if tune == 'torque':
if CP.lateralTuning.which == 'torque':
self.STEER_DELTA_UP = 15 # 1.0s time to peak torque
self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
else:
@@ -146,7 +143,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"),
ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5),
ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5),
ToyotaCarInfo("Lexus UX Hybrid 2019-22"),
ToyotaCarInfo("Lexus UX Hybrid 2019-23"),
],
CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"),
CAR.HIGHLANDER_TSS2: ToyotaCarInfo("Toyota Highlander 2020-23"),
@@ -200,7 +197,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
ToyotaCarInfo("Lexus RX Hybrid 2017-19"),
],
CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"),
CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"),
CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-22"),
}
# (addr, cars, bus, 1/freq*100, vl)
@@ -275,9 +272,8 @@ FW_QUERY_CONFIG = FwQueryConfig(
# Responds to KWP (0x1a8881):
# - Body Control Module ((0x750, 0x40))
# Hybrid control computer can be on one of two addresses
# Hybrid control computer can be on 0x7e2 (KWP) or 0x7d2 (UDS) depending on platform
(Ecu.hybrid, 0x7e2, None), # Hybrid Control Assembly & Computer
(Ecu.hybrid, 0x7d2, None), # Hybrid Control Assembly & Computer
# TODO: if these duplicate ECUs always exist together, remove one
(Ecu.srs, 0x780, None), # SRS Airbag
(Ecu.srs, 0x784, None), # SRS Airbag 2
@@ -989,6 +985,7 @@ FW_VERSIONS = {
b'8965B16170\x00\x00\x00\x00\x00\x00',
b'8965B76012\x00\x00\x00\x00\x00\x00',
b'8965B76050\x00\x00\x00\x00\x00\x00',
b'8965B76091\x00\x00\x00\x00\x00\x00',
b'\x018965B12350\x00\x00\x00\x00\x00\x00',
b'\x018965B12470\x00\x00\x00\x00\x00\x00',
b'\x018965B12490\x00\x00\x00\x00\x00\x00',
@@ -1019,12 +1016,14 @@ FW_VERSIONS = {
b'F152676293\x00\x00\x00\x00\x00\x00',
b'F152676303\x00\x00\x00\x00\x00\x00',
b'F152676304\x00\x00\x00\x00\x00\x00',
b'F152676371\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
b'\x018821F3301200\x00\x00\x00\x00',
b'\x018821F3301300\x00\x00\x00\x00',
b'\x018821F3301400\x00\x00\x00\x00',
b'\x018821F6201400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
@@ -1041,6 +1040,7 @@ FW_VERSIONS = {
b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F7603100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F7603200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F7605100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.HIGHLANDER: {
@@ -1642,12 +1642,20 @@ FW_VERSIONS = {
CAR.RAV4H_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15264283200\x00\x00\x00\x00',
b'\x01F15264283300\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
b'8965B42371\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AE1001\x00\x00\x00\x00',
b'\x01896634AF0000\x00\x00\x00\x00',
],
(Ecu.hybrid, 0x7d2, None): [
b'\x02899830R41000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00',
b'\x028998342C0000\x00\x00\x00\x00899854224000\x00\x00\x00\x00',
b'\x02899830R39000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R03100\x00\x00\x00\x00',
@@ -2121,15 +2129,16 @@ FW_VERSIONS = {
b'F152648811\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B48271\x00\x00\x00\x00\x00\x00',
b'8965B48261\x00\x00\x00\x00\x00\x00',
b'8965B48271\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.PRIUS_TSS2: {
@@ -2137,6 +2146,7 @@ FW_VERSIONS = {
b'\x028966347B1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347C4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347C6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347C7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347C8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x038966347C0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00',
b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00',
@@ -2288,4 +2298,4 @@ EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2
CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2}
# no resume button press required
NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH, CAR.AVALONH_2019}
NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH}
selfdrive/car/tunes.py
-181
View File
@@ -1,181 +0,0 @@
#!/usr/bin/env python3
from enum import Enum
class LatTunes(Enum):
#TOYOTA
INDI_PRIUS = 0
LQR_RAV4 = 1
PID_A = 2
PID_B = 3
PID_C = 4
PID_D = 5
PID_E = 6
PID_F = 7
PID_G = 8
PID_I = 9
PID_H = 10
PID_J = 11
PID_K = 12
PID_L = 13
PID_M = 14
PID_N = 15
INDI_PRIUS_TSS2 = 16
#HKG
PID_HYUNDAI_A = 17
PID_HYUNDAI_B = 18
PID_HYUNDAI_C = 19
PID_HYUNDAI_D = 20
PID_HYUNDAI_E = 21
PID_HYUNDAI_F = 22
PID_HYUNDAI_G = 23
#VW
PID_VW = 24
#SUBARU
PID_SUBARU_A = 25
PID_SUBARU_B = 26
PID_SUBARU_C = 27
PID_SUBARU_D = 28
PID_SUBARU_E = 29
PID_SUBARU_F = 30
###### LAT ######
def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
#TODO: add toggle for sepcial prius_tss2 indi
if name == LatTunes.INDI_PRIUS_TSS2:
tune.init('indi')
#tune.indi.innerLoopGainBP = [20, 24, 30]
#tune.indi.innerLoopGainV = [7.25, 7.5, 9]
#tune.indi.outerLoopGainBP = [20, 24, 30]
#tune.indi.outerLoopGainV = [6, 7.25, 6]
#tune.indi.timeConstantBP = [20, 24]
#tune.indi.timeConstantV = [2.0, 2.2]
#tune.indi.actuatorEffectivenessBP = [20, 24]
#tune.indi.actuatorEffectivenessV = [2, 3]
tune.indi.innerLoopGainBP = [0.]
tune.indi.innerLoopGainV = [15]
tune.indi.outerLoopGainBP = [0.]
tune.indi.outerLoopGainV = [17]
tune.indi.timeConstantBP = [0.]
tune.indi.timeConstantV = [4.5]
tune.indi.actuatorEffectivenessBP = [0.]
tune.indi.actuatorEffectivenessV = [15]
elif 'PID' in str(name):
tune.init('pid')
tune.pid.kiBP = [0.0]
tune.pid.kpBP = [0.0]
if name == LatTunes.PID_A:
tune.pid.kpV = [0.2]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00003
elif name == LatTunes.PID_C:
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_D:
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1]
tune.pid.kf = 0.00007818594
elif name == LatTunes.PID_F:
tune.pid.kpV = [0.723]
tune.pid.kiV = [0.0428]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_G:
tune.pid.kpV = [0.18]
tune.pid.kiV = [0.015]
tune.pid.kf = 0.00012
elif name == LatTunes.PID_H:
tune.pid.kpV = [0.17]
tune.pid.kiV = [0.03]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_I:
tune.pid.kpV = [0.15]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00004
elif name == LatTunes.PID_J:
tune.pid.kpV = [0.19]
tune.pid.kiV = [0.02]
tune.pid.kf = 0.00007818594
elif name == LatTunes.PID_L:
tune.pid.kpV = [0.3]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_M:
tune.pid.kpV = [0.3]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00007
elif name == LatTunes.PID_N:
tune.pid.kpV = [0.35]
tune.pid.kiV = [0.15]
tune.pid.kf = 0.00007818594
# hyundai
elif name == LatTunes.PID_HYUNDAI_A:
tune.pid.kf = 0.00005
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
tune.pid.kpV, tune.pid.kiV = [[0.25], [0.05]]
elif name == LatTunes.PID_HYUNDAI_B:
tune.pid.kf = 0.00006
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
tune.pid.kpV, tune.pid.kiV = [[0.25], [0.05]]
elif name == LatTunes.PID_HYUNDAI_C:
tune.pid.kf = 0.00005
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
tune.pid.kpV, tune.pid.kiV = [[0.3], [0.05]]
elif name == LatTunes.PID_HYUNDAI_D:
tune.pid.kf = 0.00005
tune.pid.kiBP, tune.pid.kpBP = [[9., 22.], [9., 22.]]
tune.pid.kpV, tune.pid.kiV = [[0.2, 0.35], [0.05, 0.09]]
elif name == LatTunes.PID_HYUNDAI_E:
tune.pid.kf = 0.
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
tune.pid.kpV, tune.pid.kiV = [[0.112], [0.004]]
elif name == LatTunes.PID_HYUNDAI_F:
tune.pid.kf = 0.00005
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
tune.pid.kpV, tune.pid.kiV = [[0.16], [0.01]]
elif name == LatTunes.PID_HYUNDAI_G:
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
tune.pid.kpV, tune.pid.kiV = [[0.16], [0.01]]
# vw
elif name == LatTunes.PID_VW:
tune.pid.kpBP = [0.]
tune.pid.kiBP = [0.]
tune.pid.kf = 0.00006
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.2]
# subaru
elif name == LatTunes.PID_SUBARU_A:
tune.pid.kf = 0.00003
tune.pid.kiBP, tune.pid.kpBP = [[0., 20.], [0., 20.]]
tune.pid.kpV, tune.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
elif name == LatTunes.PID_SUBARU_B:
tune.pid.kf = 0.00005
tune.pid.kiBP, tune.pid.kpBP = [[0., 20.], [0., 20.]]
tune.pid.kpV, tune.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
elif name == LatTunes.PID_SUBARU_C:
tune.pid.kf = 0.00005
tune.pid.kiBP, tune.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
tune.pid.kpV, tune.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
elif name == LatTunes.PID_SUBARU_D:
tune.pid.kf = 0.000038
tune.pid.kiBP, tune.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
tune.pid.kpV, tune.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
elif name == LatTunes.PID_SUBARU_E:
tune.pid.kf = 0.000039
tune.pid.kiBP, tune.pid.kpBP = [[0., 10., 20.], [0., 10., 20.]]
tune.pid.kpV, tune.pid.kiV = [[0.01, 0.05, 0.2], [0.003, 0.018, 0.025]]
elif name == LatTunes.PID_SUBARU_F:
tune.pid.kf = 0.00005
tune.pid.kiBP, tune.pid.kpBP = [[0., 20.], [0., 20.]]
tune.pid.kpV, tune.pid.kiV = [[0.1, 0.2], [0.01, 0.02]]
else:
raise NotImplementedError('This PID tune does not exist')
else:
raise NotImplementedError('This lateral tune does not exist')
selfdrive/car/volkswagen/carcontroller.py
+1 -1
View File
@@ -81,7 +81,7 @@ class CarController:
hud_alert = 0
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.latActive,
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled,
CS.out.steeringPressed, hud_alert, hud_control))
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
selfdrive/car/volkswagen/carstate.py
-4
View File
@@ -67,8 +67,6 @@ class CarState(CarStateBase):
brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
ret.brakePressed = brake_pedal_pressed or brake_pressure_detected
ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well
#dp
ret.brakeLightsDEPRECATED = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'] or ret.brakePressed or ret.parkingBrake)
# Update gear and/or clutch position data.
if trans_type == TransmissionType.automatic:
@@ -301,8 +299,6 @@ class CarState(CarStateBase):
("GRA_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type
("GRA_ButtonTypeInfo", "GRA_ACC_01"), # unknown related to stalk type
("COUNTER", "GRA_ACC_01"), # GRA_ACC_01 CAN message counter
#dp
("ESP_Status_Bremsdruck", "ESP_05"), # Brakes applied
]
checks = [
selfdrive/car/volkswagen/interface.py
+1 -9
View File
@@ -4,7 +4,6 @@ from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
from common.params import Params
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
@@ -81,12 +80,8 @@ class CarInterface(CarInterfaceBase):
# Global longitudinal tuning defaults, can be overridden per-vehicle
dp_atl = int(Params().get("dp_atl").decode('utf-8'))
if dp_atl == 1:
ret.openpilotLongitudinalControl = False
ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or docs
if experimental_long and dp_atl != 1:
if experimental_long:
# Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required.
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
@@ -224,9 +219,6 @@ class CarInterface(CarInterfaceBase):
else:
raise ValueError(f"unsupported car {candidate}")
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
ret.autoResumeSng = ret.minEnableSpeed == -1
ret.centerToFront = ret.wheelbase * 0.45
return ret
selfdrive/car/volkswagen/values.py
+22 -5
View File
@@ -189,9 +189,9 @@ class VWCarInfo(CarInfo):
CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
CAR.ARTEON_MK1: [
VWCarInfo("Volkswagen Arteon 2018-22", video_link="https://youtu.be/FAomFKPFlDA"),
VWCarInfo("Volkswagen Arteon R 2020-22", video_link="https://youtu.be/FAomFKPFlDA"),
VWCarInfo("Volkswagen Arteon eHybrid 2020-22", video_link="https://youtu.be/FAomFKPFlDA"),
VWCarInfo("Volkswagen Arteon 2018-23", video_link="https://youtu.be/FAomFKPFlDA"),
VWCarInfo("Volkswagen Arteon R 2020-23", video_link="https://youtu.be/FAomFKPFlDA"),
VWCarInfo("Volkswagen Arteon eHybrid 2020-23", video_link="https://youtu.be/FAomFKPFlDA"),
VWCarInfo("Volkswagen CC 2018-22", video_link="https://youtu.be/FAomFKPFlDA"),
],
CAR.ATLAS_MK1: [
@@ -262,7 +262,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]),
CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21"),
CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2017-23"),
CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020", footnotes=[Footnote.VW_MQB_A0]),
CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020-23", footnotes=[Footnote.VW_MQB_A0]),
CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"),
CAR.SKODA_OCTAVIA_MK3: [
VWCarInfo("Škoda Octavia 2015, 2018-19"),
@@ -307,6 +307,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
FW_VERSIONS = {
CAR.ARTEON_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x873G0906259AH\xf1\x890001',
b'\xf1\x873G0906259F \xf1\x890004',
b'\xf1\x873G0906259G \xf1\x890004',
b'\xf1\x873G0906259G \xf1\x890005',
@@ -320,6 +321,7 @@ FW_VERSIONS = {
b'\xf1\x870DL300014C \xf1\x893704',
b'\xf1\x870GC300011L \xf1\x891401',
b'\xf1\x870GC300014M \xf1\x892802',
b'\xf1\x870GC300019G \xf1\x892804',
b'\xf1\x870GC300040P \xf1\x891401',
],
(Ecu.srs, 0x715, None): [
@@ -327,7 +329,7 @@ FW_VERSIONS = {
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121177161113772900',
b'\xf1\x873Q0959655CK\xf1\x890711\xf1\x82\x0e1712141712141105121122052900',
b'\xf1\x873Q0959655DA\xf1\x890720\xf1\x82\x0e1712141712141105121122052900',
b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00',
b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\x0e1812141812171105141123052J00',
b'\xf1\x875QF959655AP\xf1\x890755\xf1\x82\x1311110011111311111100110200--1611125F49',
],
(Ecu.eps, 0x712, None): [
@@ -336,6 +338,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020800',
b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002MB4092M7N',
b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002NB4202N7N',
b'\xf1\x875WA907145Q \xf1\x891063\xf1\x82\x002KB4092KOM',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572AA\xf1\x890396',
@@ -358,11 +361,13 @@ FW_VERSIONS = {
b'\xf1\x8703H906026S \xf1\x896693',
b'\xf1\x8703H906026S \xf1\x899970',
b'\xf1\x873CN906259 \xf1\x890005',
b'\xf1\x873CN906259F \xf1\x890002',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927158A \xf1\x893387',
b'\xf1\x8709G927158DR\xf1\x893536',
b'\xf1\x8709G927158DR\xf1\x893742',
b'\xf1\x8709G927158EN\xf1\x893691',
b'\xf1\x8709G927158F \xf1\x893489',
b'\xf1\x8709G927158FT\xf1\x893835',
b'\xf1\x8709G927158GL\xf1\x893939',
@@ -392,17 +397,21 @@ FW_VERSIONS = {
CAR.CRAFTER_MK2: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704L906056EK\xf1\x896391',
b'\xf1\x8705L906023BC\xf1\x892688',
],
# Only current upstreamed vehicle has a manual transmission
#(Ecu.transmission, 0x7e1, None): [
#],
(Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655BG\xf1\x890703\xf1\x82\x0e16120016130012051G1313052900',
b'\xf1\x875QF959655AS\xf1\x890755\xf1\x82\x1315140015150011111100050200--1311120749',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x872N0909143E \xf1\x897021\xf1\x82\x05163AZ306A2',
b'\xf1\x872N0909144K \xf1\x897045\xf1\x82\x05233AZ810A2',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572AA\xf1\x890396',
b'\xf1\x872Q0907572M \xf1\x890233',
],
},
@@ -488,6 +497,7 @@ FW_VERSIONS = {
b'\xf1\x870D9300041P \xf1\x894507',
b'\xf1\x870DD300045K \xf1\x891120',
b'\xf1\x870DD300046F \xf1\x891601',
b'\xf1\x870GC300012A \xf1\x891401',
b'\xf1\x870GC300012A \xf1\x891403',
b'\xf1\x870GC300014B \xf1\x892401',
b'\xf1\x870GC300014B \xf1\x892405',
@@ -504,6 +514,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120043114417121411149113',
b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120053114317121C111C9113',
b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\x13141500111233003142114A2131219333313100',
b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333423100',
b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333463100',
b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100',
b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2251229333463100',
@@ -577,6 +588,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\x0101',
b'\xf1\x875Q0907572G \xf1\x890571',
b'\xf1\x875Q0907572H \xf1\x890620',
b'\xf1\x875Q0907572J \xf1\x890653',
b'\xf1\x875Q0907572J \xf1\x890654',
b'\xf1\x875Q0907572P \xf1\x890682',
b'\xf1\x875Q0907572R \xf1\x890771',
@@ -1270,19 +1282,24 @@ FW_VERSIONS = {
CAR.SKODA_SCALA_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704C906025AK\xf1\x897053',
b'\xf1\x8705C906032M \xf1\x892365',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x870CW300020 \xf1\x891907',
b'\xf1\x870CW300050 \xf1\x891709',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1211110411110411--04040404131111112H14',
b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\022111104111104112104040404111111112H14',
b'\xf1\x872Q0959655AS\xf1\x890411\xf1\x82\x1311150411110411210404040417151215391413',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x872Q1909144M \xf1\x896041',
b'\xf1\x872Q1909144AB\xf1\x896050',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572R \xf1\x890372',
b'\xf1\x872Q0907572AA\xf1\x890396',
],
},
CAR.SKODA_SUPERB_MK3: {
selfdrive/controls/controlsd.py
+28 -151
View File
@@ -7,7 +7,7 @@ from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL
from common.profiler import Profiler
from common.params import Params, put_nonblocking
from common.params import Params, put_nonblocking, put_bool_nonblocking
import cereal.messaging as messaging
from cereal.visionipc import VisionIpcClient, VisionStreamType
from common.conversions import Conversions as CV
@@ -17,13 +17,12 @@ from system.version import is_release_branch, get_short_branch
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature, V_CRUISE_UNSET
from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
from selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -37,7 +36,7 @@ REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd", "mapd", "gpxd", "gpxd_uploader"}
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd", "mapd", "otisserv", "fileserv"}
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
@@ -63,14 +62,6 @@ class Controls:
# Ensure the current branch is cached, otherwise the first iteration of controlsd lags
self.branch = get_short_branch("")
# dp
self.params = Params()
self.dp_jetson = self.params.get_bool('dp_jetson')
try:
self.dp_lat_version = int(self.params.get('dp_lateral_version').decode('utf8'))
except:
self.dp_lat_version = 0
# Setup sockets
self.pm = pm
if self.pm is None:
@@ -78,8 +69,6 @@ class Controls:
'carControl', 'carEvents', 'carParams'])
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
if self.dp_jetson:
self.camera_packets = ["roadCameraState", "wideRoadCameraState"]
self.can_sock = can_sock
if can_sock is None:
@@ -88,18 +77,18 @@ class Controls:
self.log_sock = messaging.sub_sock('androidLog')
# self.params = Params()
self.params = Params()
self.dp_alka = self.params.get_bool("dp_alka")
self.dp_device_disable_temp_check = self.params.get_bool("dp_device_disable_temp_check")
self.sm = sm
if self.sm is None:
ignore = ['testJoystick']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
if self.dp_jetson:
ignore += ['driverCameraState', 'driverMonitoringState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick', 'dragonConf'] + self.camera_packets,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick', 'longitudinalPlan', 'dragonConf'])
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'])
if CI is None:
# wait for one pandaState and one CAN packet
@@ -120,22 +109,8 @@ class Controls:
if not self.disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
# dp
self.sm['dragonConf'].dpAtl = int(self.params.get('dp_atl', encoding='utf8'))
if self.sm['dragonConf'].dpAtl:
if self.dp_alka:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALKA
self.dp_temp_check = self.params.get_bool('dp_temp_check')
self.dp_lateral_road_edge_detected = False
# alt lat ctrl
self.sm['dragonConf'].dpLateralAlt = False
self.sm['dragonConf'].dpLateralAltCtrl = 0
self.sm['dragonConf'].dpLateralAltSpeed = 80
self.dp_lateral_alt_v_cruise_kph = 0
self.dp_lateral_alt_v_cruise_kph_prev = 0
self.dp_lateral_alt_active = False
self.local_trip_min_total = float(self.params.get("local_trip_min_total", encoding='utf8'))
self.local_trip_meter_total = float(self.params.get("local_trip_meter_total", encoding='utf8'))
self.local_trip_count_added = False
# read params
self.is_metric = self.params.get_bool("IsMetric")
@@ -182,11 +157,6 @@ class Controls:
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'torque':
self.LaC = LatControlTorque(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP, self.CI)
# dp, keep the original LaC for alt lac ctrl
self.LaC_default = self.LaC
self.initialized = False
self.state = State.disabled
@@ -237,49 +207,12 @@ class Controls:
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def dp_update_lat_controller(self):
if self.sm['dragonConf'].dpLateralAlt:
self.dp_lateral_alt_v_cruise_kph = self.v_cruise_helper.v_cruise_kph
# when cruise set speed changed
if self.dp_lateral_alt_v_cruise_kph != self.dp_lateral_alt_v_cruise_kph_prev:
# when set speed below config speed or set speed equal unset speed, fallback to default
if self.dp_lateral_alt_v_cruise_kph == V_CRUISE_UNSET or self.dp_lateral_alt_v_cruise_kph < self.sm['dragonConf'].dpLateralAltSpeed:
self.dp_lateral_alt_active = False
if type(self.LaC) != type(self.LaC_default):
# set lateralTuning back
if isinstance(self.LaC_default, LatControlPID):
self.CP.lateralTuning.pid = self.CP.latTuneCollection.pid
elif isinstance(self.LaC_default, LatControlLQR):
self.CP.lateralTuning.lqr = self.CP.latTuneCollection.lqr
elif isinstance(self.LaC_default, LatControlTorque):
self.CP.lateralTuning.torque = self.CP.latTuneCollection.torque
# set LaC back
self.LaC = self.LaC_default
self.LaC.reset()
# when set speed >= config speed
else:
# save current status
if type(self.LaC) == type(self.LaC_default):
self.LaC_default = self.LaC
self.dp_lateral_alt_active = True
if getattr(self.CP.latTuneCollection.pid, 'kpV') and self.sm['dragonConf'].dpLateralAltCtrl == 1 and not isinstance(self.LaC, LatControlPID):
self.CP.lateralTuning.pid = self.CP.latTuneCollection.pid
self.LaC = LatControlPID(self.CP, self.CI)
elif self.sm['dragonConf'].dpLateralAltCtrl == 2 and not isinstance(self.LaC, LatControlLQR):
self.CP.lateralTuning.lqr = self.CP.latTuneCollection.lqr
self.LaC = LatControlLQR(self.CP, self.CI)
elif self.sm['dragonConf'].dpLateralAltCtrl == 3 and not isinstance(self.LaC, LatControlTorque):
self.CP.lateralTuning.torque = self.CP.latTuneCollection.torque
self.LaC = LatControlTorque(self.CP, self.CI)
self.LaC.reset()
self.dp_lateral_alt_v_cruise_kph_prev = self.dp_lateral_alt_v_cruise_kph
def set_initial_state(self):
if REPLAY:
controls_state = Params().get("ReplayControlsState")
if controls_state is not None:
controls_state = log.ControlsState.from_bytes(controls_state)
self.v_cruise_helper.v_cruise_kph = controls_state.vCruise
with log.ControlsState.from_bytes(controls_state) as controls_state:
self.v_cruise_helper.v_cruise_kph = controls_state.vCruise
if any(ps.controlsAllowed for ps in self.sm['pandaStates']):
self.state = State.enabled
@@ -320,16 +253,15 @@ class Controls:
if CS.gasPressed:
self.events.add(EventName.gasPressedOverride)
if not self.CP.notCar and not self.dp_jetson:
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
self.events.add_from_msg(self.sm['longitudinalPlan'].eventsDEPRECATED)
# Add car events, ignore if CAN isn't valid
if CS.canValid:
self.events.add_from_msg(CS.events)
# Create events for temperature, disk space, and memory
if self.dp_temp_check and self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
if not self.dp_device_disable_temp_check and self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
# under 7% of space free no enable allowed
@@ -363,28 +295,13 @@ class Controls:
self.events.add(EventName.calibrationRecalibrating)
else:
self.events.add(EventName.calibrationInvalid)
direction = self.sm['lateralPlan'].laneChangeDirection
# Handle lane change
if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
self.dp_lateral_road_edge_detected = self.sm['dragonConf'].dpLateralRoadEdgeDetected
#dp - moved it up L364
#direction = self.sm['lateralPlan'].laneChangeDirection
direction = self.sm['lateralPlan'].laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
#dp
elif self.dp_lateral_road_edge_detected:
md = self.sm['modelV2']
left_road_edge = -md.roadEdges[0].y[0]
right_road_edge = md.roadEdges[1].y[0]
if (((left_road_edge < 3.5) and direction == LaneChangeDirection.left) or \
((right_road_edge < 3.5) and direction == LaneChangeDirection.right)):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
@@ -392,11 +309,7 @@ class Controls:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['lateralPlan'].laneChangeState in (LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing):
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
self.events.add(EventName.laneChange)
self.events.add(EventName.laneChange)
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
@@ -465,7 +378,7 @@ class Controls:
else:
self.logged_comm_issue = None
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and not SIMULATION:
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['lateralPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
@@ -503,7 +416,7 @@ class Controls:
pass
# TODO: fix simulator
if not SIMULATION:
if not SIMULATION or REPLAY:
if not NOSENSOR:
if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000):
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
@@ -519,7 +432,7 @@ class Controls:
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs, self.sm['dragonConf'])
CS = self.CI.update(self.CC, can_strs)
if len(can_strs) and REPLAY:
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
@@ -528,7 +441,7 @@ class Controls:
if not self.initialized:
all_valid = CS.canValid and self.sm.all_checks()
timed_out = self.sm.frame * DT_CTRL > (6. if REPLAY else 3.5)
if all_valid or timed_out or SIMULATION:
if all_valid or timed_out or (SIMULATION and not REPLAY):
available_streams = VisionIpcClient.available_streams("camerad", block=False)
if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
self.sm.ignore_alive.append('roadCameraState')
@@ -540,7 +453,7 @@ class Controls:
self.initialized = True
self.set_initial_state()
Params().put_bool("ControlsReady", True)
put_bool_nonblocking("ControlsReady", True)
# Check for CAN timeout
if not can_strs:
@@ -562,26 +475,13 @@ class Controls:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
# dp - local trip log
self.local_trip_meter_total += CS.vEgo * DT_CTRL
if not self.local_trip_count_added:
if self.local_trip_meter_total > 0:
put_nonblocking("local_trip_count_total", str(float(self.params.get("local_trip_count_total").decode('utf-8')) + 1))
self.local_trip_count_added = True
# every 30 secs
if self.local_trip_count_added and self.sm.frame % int(30. / DT_CTRL) == 0:
put_nonblocking("local_trip_meter_total", str(round(self.local_trip_meter_total, 2)))
put_nonblocking("local_trip_min_total", str(self.local_trip_min_total + 0.5))
return CS
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
# dp - toyota speed override here
# dp - @todo may apply to other makes in the future?
dp_override_speed = self.sm['dragonConf'].dpToyotaCruiseOverrideSpeed if self.sm['dragonConf'].dpToyotaCruiseOverride else False
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, dp_override_speed)
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric)
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
@@ -673,7 +573,6 @@ class Controls:
sr = max(lp.steerRatio, 0.1)
self.VM.update_params(x, sr)
self.dp_update_lat_controller()
# Update Torque Params
if self.CP.lateralTuning.which() == 'torque':
torque_params = self.sm['liveTorqueParameters']
@@ -686,20 +585,16 @@ class Controls:
CC = car.CarControl.new_message()
CC.enabled = self.enabled
# dp - keep the current lat controller type
CC.latController = self.CP.lateralTuning.which()
# Check which actuators can be enabled
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
if not standstill and CS.cruiseState.available and self.sm['dragonConf'].dpAtl > 0:
if self.dp_alka and not standstill and CS.cruiseState.available:
if self.sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated:
pass
elif CS.steerFaultTemporary:
pass
elif CS.steerFaultPermanent:
elif CS.steerFaultTemporary or CS.steerFaultPermanent:
pass
elif CS.gearShifter == car.CarState.GearShifter.reverse:
pass
@@ -716,18 +611,7 @@ class Controls:
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# dp - manual lane change
if self.sm['dragonConf'].dpLateralLcManual:
speed = CS.vEgo * CV.MS_TO_MPH
if self.sm['dragonConf'].dpLateralMode == 1 and speed >= self.sm['dragonConf'].dpLcMinMph:
pass
# we use "or" here in case dpLcAutoMinMph is smaller than dpLcMinMph
elif self.sm['dragonConf'].dpLateralMode == 2 and (speed >= self.sm['dragonConf'].dpLcMinMph or speed >= self.sm['dragonConf'].dpLcAutoMinMph):
pass
else:
if CC.latActive:
self.events.add(EventName.manualSteeringRequiredBlinkersOn)
CC.latActive = False
# State specific actions
if not CC.latActive:
@@ -745,7 +629,7 @@ class Controls:
self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates, self.dp_lat_version)
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.last_actuators, self.steer_limited, self.desired_curvature,
self.desired_curvature_rate, self.sm['liveLocationKalman'])
@@ -771,7 +655,7 @@ class Controls:
recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
# Send a "steering required alert" if saturation count has reached the limit
if lac_log.active and not recent_steer_pressed:
if lac_log.active and not recent_steer_pressed and not self.CP.notCar:
if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
turning = abs(lac_log.desiredLateralAccel) > 1.0
@@ -930,8 +814,6 @@ class Controls:
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
elif lat_tuning == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif lat_tuning == 'pid':
@@ -941,7 +823,6 @@ class Controls:
elif lat_tuning == 'indi':
controlsState.lateralControlState.indiState = lac_log
controlsState.dpLateralAltActive = self.dp_lateral_alt_active
self.pm.send('controlsState', dat)
# carState
@@ -979,11 +860,7 @@ class Controls:
self.prof.checkpoint("Ratekeeper", ignore=True)
self.is_metric = self.params.get_bool("IsMetric")
if self.CP.openpilotLongitudinalControl:
if self.sm['dragonConf'].dpE2EConditional:
self.experimental_mode = self.sm['longitudinalPlan'].dpE2EIsBlended
else:
self.experimental_mode = self.params.get_bool("ExperimentalMode")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
# Sample data from sockets and get a carState
CS = self.data_sample()
selfdrive/controls/lib/accel_controller.py
+48
View File
@@ -0,0 +1,48 @@
from common.numpy_fast import interp
from common.params import Params
DP_ACCEL_STOCK = 0
DP_ACCEL_ECO = 1
DP_ACCEL_NORMAL = 2
DP_ACCEL_SPORT = 3
# accel profile by @arne182 modified by cgw
_DP_CRUISE_MIN_V = [-0.765, -0.765, -0.80, -0.80, -0.75, -0.70]
_DP_CRUISE_MIN_V_ECO = [-0.760, -0.760, -0.76, -0.76, -0.70, -0.65]
_DP_CRUISE_MIN_V_SPORT = [-0.770, -0.770, -0.90, -1.00, -0.90, -0.80]
_DP_CRUISE_MIN_BP = [0., 15.66, 17.88, 20., 30., 55.]
#DP_CRUISE_MIN_BP in mph=[0., 18, 35, 40, 45, 67, 123]
_DP_CRUISE_MAX_V = [3.4, 2.8, 1.8, 1.4, 1.06, .88, .68, .46, .35, .13]
_DP_CRUISE_MAX_V_ECO = [3.2, 2.6, 1.6, 1.2, .76, .62, .48, .36, .28, .09]
_DP_CRUISE_MAX_V_SPORT = [3.5, 3.0, 2.4, 2.9, 2.1, 1.7, 1.3, .9, .7, .5]
_DP_CRUISE_MAX_BP = [0., 3, 6., 8., 11., 15., 20., 25., 30., 55.]
#DP_CRUISE_MAX_BP in mph=[0., 6.7, 13, 18, 25, 33, 45, 56, 67, 123]
class AccelController:
def __init__(self):
self._params = Params()
self._dp_long_accel_profile = DP_ACCEL_STOCK
def read_params(self):
try:
self._dp_long_accel_profile = int(self._params.get("dp_long_accel_profile", encoding='utf-8'))
except (KeyError, TypeError, ValueError):
self._dp_long_accel_profile = DP_ACCEL_STOCK
def _dp_calc_cruise_accel_limits(self, v_ego):
if self._dp_long_accel_profile == DP_ACCEL_ECO:
a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, _DP_CRUISE_MIN_V_ECO)
a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, _DP_CRUISE_MAX_V_ECO)
elif self._dp_long_accel_profile == DP_ACCEL_SPORT:
a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, _DP_CRUISE_MIN_V_SPORT)
a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, _DP_CRUISE_MAX_V_SPORT)
else:
a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, _DP_CRUISE_MIN_V)
a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, _DP_CRUISE_MAX_V)
return a_cruise_min, a_cruise_max
def get_accel_limits(self, v_ego, accel_limits):
return accel_limits if self._dp_long_accel_profile == DP_ACCEL_STOCK else self._dp_calc_cruise_accel_limits(v_ego)
selfdrive/controls/lib/desire_helper.py
+19 -75
View File
@@ -1,6 +1,6 @@
from cereal import log
from common.conversions import Conversions as CV
from common.realtime import DT_MDL, sec_since_boot
from common.realtime import DT_MDL
LaneChangeState = log.LateralPlan.LaneChangeState
LaneChangeDirection = log.LateralPlan.LaneChangeDirection
@@ -40,43 +40,20 @@ class DesireHelper:
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
# dp
self.dp_lc_auto_done = False
self.dp_lc_auto_delay_start_sec = None
self.dp_lateral_mode = 1 # 0 = blinker mode (should we remove?), 1 = assist lane change, 2 = auto lane change
self.dp_lc_min_mph = LANE_CHANGE_SPEED_MIN
self.dp_lc_auto_min_mph = LANE_CHANGE_SPEED_MIN + 10
self.dp_lc_auto_delay = 3 # secs
self.dp_lateral_road_edge_detected = False
def update(self, carstate, lateral_active, lane_change_prob, dragonconf, md):
# dp - sync with dragonConf
self.dp_lateral_mode = dragonconf.dpLateralMode
self.dp_lc_min_mph = dragonconf.dpLcMinMph * CV.MPH_TO_MS
self.dp_lc_auto_min_mph = dragonconf.dpLcAutoMinMph * CV.MPH_TO_MS
self.dp_lc_auto_min_mph = self.dp_lc_min_mph if self.dp_lc_auto_min_mph < self.dp_lc_min_mph else self.dp_lc_auto_min_mph
self.dp_lc_auto_delay = dragonconf.dpLcAutoDelay
self.dp_lateral_road_edge_detected = dragonconf.dpLateralRoadEdgeDetected
def update(self, carstate, lateral_active, lane_change_prob):
v_ego = carstate.vEgo
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_lane_change_speed = v_ego < self.dp_lc_min_mph
below_alc_speed = v_ego < self.dp_lc_auto_min_mph
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
# LaneChangeState.off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
self.dp_lc_auto_done = False
self.dp_lc_auto_delay_start_sec = None
# LaneChangeState.preLaneChange
elif self.lane_change_state == LaneChangeState.preLaneChange:
# Set lane change direction
@@ -87,68 +64,35 @@ class DesireHelper:
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
if self.dp_lateral_mode == 2:
if self.dp_lc_auto_delay_start_sec is None:
self.dp_lc_auto_delay_start_sec = sec_since_boot()
else:
if one_blinker and not below_alc_speed and (not self.dp_lc_auto_done) and \
(sec_since_boot() - self.dp_lc_auto_delay_start_sec >= self.dp_lc_auto_delay):
torque_applied = True
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
#dp
if self.dp_lateral_road_edge_detected:
left_road_edge = -md.roadEdges[0].y[0]
right_road_edge = md.roadEdges[1].y[0]
road_edge_detected = (((left_road_edge < 3.5) and self.lane_change_direction == LaneChangeDirection.left) or
((right_road_edge < 3.5) and self.lane_change_direction == LaneChangeDirection.right))
else:
road_edge_detected = False
if blindspot_detected:
self.dp_lc_auto_done = False
self.dp_lc_auto_delay_start_sec = None
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
elif torque_applied and not blindspot_detected and not road_edge_detected:
elif torque_applied and not blindspot_detected:
self.lane_change_state = LaneChangeState.laneChangeStarting
# LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
if blindspot_detected:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
self.dp_lc_auto_done = False
self.dp_lc_auto_delay_start_sec = None
else:
# fade out over .5s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# fade out over .5s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# LaneChangeState.laneChangeFinishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
if blindspot_detected:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
self.dp_lc_auto_done = False
self.dp_lc_auto_delay_start_sec = None
else:
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if self.lane_change_ll_prob > 0.99:
self.lane_change_direction = LaneChangeDirection.none
if one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
else:
self.lane_change_state = LaneChangeState.off
self.dp_lc_auto_done = True
if self.lane_change_ll_prob > 0.99:
self.lane_change_direction = LaneChangeDirection.none
if one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
else:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
self.lane_change_timer = 0.0
selfdrive/controls/lib/drive_helpers.py
+9 -109
View File
@@ -20,10 +20,6 @@ MIN_SPEED = 1.0
CONTROL_N = 17
CAR_ROTATION_RADIUS = 0.0
# dp - needed for 0813/0816 controller
LAT_MPC_N = 16
LON_MPC_N = 32
# EU guidelines
MAX_LATERAL_JERK = 5.0
@@ -41,20 +37,15 @@ CRUISE_INTERVAL_SIGN = {
ButtonType.decelCruise: -1,
}
# mapd
# Constants for Limit controllers.
LIMIT_ADAPT_ACC = -0.8 # (closer to zero ealier it decel) m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
LIMIT_MIN_ACC = -1.4 # m/s^2 Maximum deceleration allowed for limit controllers to provide.
LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acelration allowed for limit controllers to provide while active.
LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide.
LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acelration allowed for limit controllers to provide while active.
LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers.
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
# dp - used in some lateral planners
class MPC_COST_LAT:
PATH = 1.0
HEADING = 1.0
STEER_RATE = 1.0
class VCruiseHelper:
def __init__(self, CP):
self.CP = CP
@@ -63,15 +54,12 @@ class VCruiseHelper:
self.v_cruise_kph_last = 0
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
self.dp_override_v_cruise_kph = V_CRUISE_UNSET
self.dp_override_cruise_speed_last = V_CRUISE_UNSET
self.dp_override_enabled_last = False
@property
def v_cruise_initialized(self):
return self.v_cruise_kph != V_CRUISE_UNSET
def update_v_cruise(self, CS, enabled, is_metric, dp_override_speed):
def update_v_cruise(self, CS, enabled, is_metric):
self.v_cruise_kph_last = self.v_cruise_kph
if CS.cruiseState.available:
@@ -81,25 +69,9 @@ class VCruiseHelper:
self.v_cruise_cluster_kph = self.v_cruise_kph
self.update_button_timers(CS, enabled)
else:
if enabled and dp_override_speed and CS.cruiseState.speed * CV.MS_TO_KPH < dp_override_speed:
if self.dp_override_v_cruise_kph == V_CRUISE_UNSET:
self.dp_override_v_cruise_kph = max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_MIN)
else:
self.dp_override_v_cruise_kph = V_CRUISE_UNSET
# when we have an override_speed, use it
if self.dp_override_v_cruise_kph != V_CRUISE_UNSET:
self.v_cruise_kph = self.dp_override_v_cruise_kph
self.v_cruise_cluster_kph = self.dp_override_v_cruise_kph
else:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
self.dp_override_cruise_speed_last = CS.cruiseState.speed
self.dp_override_enabled_last = enabled
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
else:
self.dp_override_v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_UNSET
@@ -199,71 +171,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, dp_lat_version):
if dp_lat_version == 1: # 0813
return get_0813_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates)
elif dp_lat_version == 2: # 0816
return get_0816_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates)
if len(psis) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
curvature_rates = [0.0]*CONTROL_N
v_ego = max(MIN_SPEED, v_ego)
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
current_curvature_desired = curvatures[0]
psi = interp(delay, T_IDXS[:CONTROL_N], psis)
average_curvature_desired = psi / (v_ego * delay)
desired_curvature = 2 * average_curvature_desired - current_curvature_desired
# This is the "desired rate of the setpoint" not an actual desired rate
desired_curvature_rate = curvature_rates[0]
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
safe_desired_curvature_rate = clip(desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
safe_desired_curvature = clip(desired_curvature,
current_curvature_desired - max_curvature_rate * DT_MDL,
current_curvature_desired + max_curvature_rate * DT_MDL)
return safe_desired_curvature, safe_desired_curvature_rate
def get_0813_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
if len(psis) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
curvature_rates = [0.0]*CONTROL_N
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
current_curvature = curvatures[0]
psi = interp(delay, T_IDXS[:CONTROL_N], psis)
desired_curvature_rate = curvature_rates[0]
# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature
desired_curvature = current_curvature + 2 * curvature_diff_from_psi
v_ego = max(v_ego, 0.1)
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2)
safe_desired_curvature_rate = clip(desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
safe_desired_curvature = clip(desired_curvature,
current_curvature - max_curvature_rate * DT_MDL,
current_curvature + max_curvature_rate * DT_MDL)
return safe_desired_curvature, safe_desired_curvature_rate
def get_0816_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
if len(psis) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
@@ -310,11 +218,3 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
return float(vel_err)
return 0.0
def get_lane_laneless_mode(lll_prob, rll_prob, mode):
if lll_prob < 0.3 and rll_prob < 0.3:
mode = False
elif lll_prob > 0.5 or rll_prob > 0.5:
mode = True
return mode
selfdrive/controls/lib/dynamic_endtoend_controller.py
+123
View File
@@ -0,0 +1,123 @@
from common.numpy_fast import interp
from common.params import Params
# d-e2e, from modeldata.h
TRAJECTORY_SIZE = 33
_DP_E2E_LEAD_COUNT = 5
_DP_E2E_STOP_BP = [0., 10., 20., 30., 40., 50., 55.]
_DP_E2E_STOP_DIST = [10, 30., 50., 70., 80., 90., 120.]
_DP_E2E_STOP_COUNT = 3
_DP_E2E_SNG_COUNT = 3
_DP_E2E_SNG_ACC_COUNT = 5
_DP_E2E_SWAP_COUNT = 10
_DP_E2E_TF_COUNT = 5
class DynamicEndtoEndController:
def __init__(self):
self._params = Params()
self._dp_long_de2e = False
self._mode = 'blended'
# conditional e2e
self.dp_e2e_has_lead = False
self.dp_e2e_lead_last = False
self.dp_e2e_lead_count = 0
self.dp_e2e_sng = False
self.dp_e2e_sng_count = 0
self.dp_e2e_standstill_last = False
self.dp_e2e_swap_count = 0
self.dp_e2e_stop_count = 0
self.dp_e2e_tf_count = 0
pass
def read_params(self):
self._dp_long_de2e = self._params.get_bool('dp_long_de2e')
pass
def _set_dp_e2e_mode(self, mode, force=False):
if force:
self.dp_e2e_swap_count = 0
self._mode = mode
return
else:
# prevent switching in a short period of time.
if self._mode == mode:
self.dp_e2e_swap_count = 0
else:
self.dp_e2e_swap_count += 1
if self.dp_e2e_swap_count >= _DP_E2E_SWAP_COUNT:
self._mode = mode
def _process_conditional_e2e(self, radar_unavailable, car_state, lead_one, md):
v_ego_kph = car_state.vEgo * 3.6
# make sure it see lead enough time
if lead_one.status != self.dp_e2e_lead_last:
self.dp_e2e_lead_count = 0
else:
self.dp_e2e_lead_count += 1
if self.dp_e2e_lead_count >= _DP_E2E_LEAD_COUNT:
self.dp_e2e_has_lead = lead_one.status
self.dp_e2e_lead_last = lead_one.status
# when standstill, always e2e
if car_state.standstill:
self.dp_e2e_sng_count = 0
self.dp_e2e_sng = False
return self._set_dp_e2e_mode('blended')
if self.dp_e2e_standstill_last and not car_state.standstill:
self.dp_e2e_sng = True
# when sng, we e2e for 0.5 secs
if self.dp_e2e_sng:
self.dp_e2e_sng_count += 1
if self.dp_e2e_sng_count > _DP_E2E_SNG_COUNT:
if self.dp_e2e_sng_count > _DP_E2E_SNG_ACC_COUNT:
self.dp_e2e_sng = False
return self._set_dp_e2e_mode('acc', True)
return self._set_dp_e2e_mode('blended')
# when we see a lead
# voacc cars only
if radar_unavailable and self.dp_e2e_has_lead:
ttc = lead_one.dRel / lead_one.vRel
if ttc <= interp(car_state.vEgo, [0., 22.2, 25.], [.85, 1., 1.22]):
self.dp_e2e_tf_count += 1
else:
self.dp_e2e_tf_count = 0
if self.dp_e2e_tf_count > _DP_E2E_TF_COUNT:
return self._set_dp_e2e_mode('blended', True)
# stop sign detection
if abs(car_state.steeringAngleDeg) <= 60 and len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE:
if md.position.x[TRAJECTORY_SIZE - 1] < interp(v_ego_kph, _DP_E2E_STOP_BP, _DP_E2E_STOP_DIST):
self.dp_e2e_stop_count += 1
else:
self.dp_e2e_stop_count = 0
else:
self.dp_e2e_stop_count = 0
if self.dp_e2e_stop_count >= _DP_E2E_STOP_COUNT:
return self._set_dp_e2e_mode('blended', True)
return self._set_dp_e2e_mode('acc')
def set_mpc_mode(self, mode, radar_unavailable, car_state, lead_one, md):
if not self._dp_long_de2e:
return 'blended'
self._mode = mode
self._process_conditional_e2e(radar_unavailable, car_state, lead_one, md)
return self._mode
selfdrive/controls/lib/events.py
Regular → Executable
+63 -59
View File
@@ -235,12 +235,12 @@ def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
return StartupAlert(_("WARNING: This branch is not tested"), branch, alert_status=AlertStatus.userPrompt)
def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage")
return NoEntryAlert(_("Drive above {speed} to engage").format(speed=get_display_speed(CP.minEnableSpeed, metric)))
def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return Alert(
_("Steer Unavailable Below %s") % get_display_speed(CP.minSteerSpeed, metric),
_("Steer Unavailable Below {speed}").format(speed=get_display_speed(CP.minSteerSpeed, metric)),
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
@@ -249,8 +249,8 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
first_word = _('Recalibration') if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else _('Calibration')
return Alert(
f"{first_word} in Progress: {sm['liveCalibration'].calPerc:.0f}%",
f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}",
_("{word} in Progress: {perc}%").format(word=first_word, perc=sm['liveCalibration'].calPerc),
_("Drive Above {speed}").format(speed=get_display_speed(MIN_SPEED_FILTER, metric)),
AlertStatus.normal, AlertSize.mid,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
@@ -266,13 +266,13 @@ def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, m
def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
full_perc = round(100. - sm['deviceState'].freeSpacePercent)
return NormalPermanentAlert(_("Out of Storage"), _("%s%% full") % full_perc)
return NormalPermanentAlert(_("Out of Storage"), _("{full_perc}% full").format(full_perc=full_perc))
def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan
err = CS.vEgo - mdl
msg = f"Speed Error: {err:.1f} m/s"
msg = _("Speed Error: {err:.1f} m/s").format(err=err)
return NoEntryAlert(msg, alert_text_1=_("Posenet Speed Invalid"))
@@ -298,7 +298,7 @@ def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging
rpy = sm['liveCalibration'].rpyCalib
yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan)
pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan)
angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)"
angles = _("Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)").format(pitch=pitch, yaw=yaw)
return NormalPermanentAlert(_("Calibration Invalid"), angles)
@@ -310,16 +310,16 @@ def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster,
def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NormalPermanentAlert(_("Low Memory"), f"{sm['deviceState'].memoryUsagePercent}% used")
return NormalPermanentAlert(_("Low Memory"), _("{memory_usage_percent}% used").format(memory_usage_percent=sm['deviceState'].memoryUsagePercent))
def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
x = max(sm['deviceState'].cpuUsagePercent, default=0.)
return NormalPermanentAlert(_("High CPU Usage"), _("%s%% used") % x)
return NormalPermanentAlert(_("High CPU Usage"), _("{x}% used").format(x=x))
def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NormalPermanentAlert(_("Driving Model Lagging"), f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped")
return NormalPermanentAlert(_("Driving Model Lagging"), _("{frame_drop_perc:.1f}% frames dropped").format(frame_drop_perc=sm['modelV2'].frameDropPerc))
def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
@@ -332,18 +332,9 @@ def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
axes = sm['testJoystick'].axes
gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
vals = _("Gas: {gas_percent}%, Steer: {steer_percent}%").format(gas_percent=round(gb * 100.), steer_percent=round(steer * 100.))
return NormalPermanentAlert(_("Joystick Mode"), vals)
def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
speedLimit = sm['longitudinalPlan'].speedLimit
speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
message = _("Adjusting to %(speed)s %(unit)s") % ({"speed": speed, "unit": (_("km/h") if metric else _("mph"))})
return Alert(
message,
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
@@ -398,7 +389,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.cruiseMismatch: {
#ET.PERMANENT: ImmediateDisableAlert(_("openpilot failed to cancel cruise")),
#ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"),
},
# openpilot doesn't recognize the car. This switches openpilot into a
@@ -451,7 +442,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.steerTempUnavailableSilent: {
ET.WARNING: Alert(
ET.PERMANENT: Alert(
_("Steering Temporarily Unavailable"),
"",
AlertStatus.userPrompt, AlertSize.small,
@@ -544,10 +535,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.laneChangeBlocked: {
ET.PERMANENT: Alert(
_("Car Detected in Blindspot or RoadEdge"),
_("Car Detected in Blindspot"),
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .2),
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
},
EventName.laneChange: {
@@ -593,19 +584,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# current GPS position. This alert is thrown when the localizer is reset
# more often than expected.
EventName.localizerMalfunction: {
# ET.PERMANENT: NormalPermanentAlert(_("Sensor Malfunction"), _("Hardware Malfunction")),
},
EventName.speedLimitActive: {
ET.WARNING: Alert(
"Cruise set to speed limit",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 2.),
},
EventName.speedLimitValueChange: {
ET.WARNING: speed_limit_adjust_alert,
# ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Hardware Malfunction"),
},
# ********** events that affect controls state transitions **********
@@ -624,7 +603,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.buttonCancel: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"),
ET.NO_ENTRY: NoEntryAlert(_("Cancel Pressed")),
},
EventName.brakeHold: {
@@ -673,7 +652,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.resumeBlocked: {
ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"),
ET.NO_ENTRY: NoEntryAlert(_("Press Set to Engage")),
},
EventName.wrongCruiseMode: {
@@ -687,8 +666,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.steerTimeLimit: {
ET.SOFT_DISABLE: soft_disable_alert("Vehicle Steering Time Limit"),
ET.NO_ENTRY: NoEntryAlert("Vehicle Steering Time Limit"),
ET.SOFT_DISABLE: soft_disable_alert(_("Vehicle Steering Time Limit")),
ET.NO_ENTRY: NoEntryAlert(_("Vehicle Steering Time Limit")),
},
EventName.outOfSpace: {
@@ -730,7 +709,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.wrongGear: {
# ET.SOFT_DISABLE: user_soft_disable_alert(_("Gear not D")),
ET.SOFT_DISABLE: user_soft_disable_alert(_("Gear not D")),
ET.NO_ENTRY: NoEntryAlert(_("Gear not D")),
},
@@ -747,14 +726,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.calibrationIncomplete: {
ET.PERMANENT: calibration_incomplete_alert,
ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"),
ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
ET.SOFT_DISABLE: soft_disable_alert(_("Calibration Incomplete")),
ET.NO_ENTRY: NoEntryAlert(_("Calibration in Progress")),
},
EventName.calibrationRecalibrating: {
ET.PERMANENT: calibration_incomplete_alert,
ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"),
ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"),
ET.SOFT_DISABLE: soft_disable_alert(_("Device Remount Detected: Recalibrating")),
ET.NO_ENTRY: NoEntryAlert(_("Remount Detected: Recalibrating")),
},
EventName.doorOpen: {
@@ -839,8 +818,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.highCpuUsage: {
#ET.SOFT_DISABLE: soft_disable_alert(_("System Malfunction: Reboot Your Device")),
#ET.PERMANENT: NormalPermanentAlert(_("System Malfunction"), _("Reboot your Device")),
#ET.SOFT_DISABLE: soft_disable_alert("System Malfunction: Reboot Your Device"),
#ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"),
ET.NO_ENTRY: high_cpu_usage_alert,
},
@@ -915,9 +894,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.PERMANENT: Alert(
_("Reverse\nGear"),
"",
AlertStatus.normal, AlertSize.none,
AlertStatus.normal, AlertSize.full,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
# ET.USER_DISABLE: ImmediateDisableAlert(_("Reverse Gear")),
ET.USER_DISABLE: ImmediateDisableAlert(_("Reverse Gear")),
ET.NO_ENTRY: NoEntryAlert(_("Reverse Gear")),
},
@@ -976,15 +955,40 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.vehicleSensorsInvalid: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Vehicle Sensors Invalid")),
ET.PERMANENT: NormalPermanentAlert(_("Vehicle Sensors Calibrating"), _("Drive to Calibrate")),
ET.NO_ENTRY: NoEntryAlert(_("Vehicle Sensors Calibrating"), _("Drive to Calibrate")),
ET.NO_ENTRY: NoEntryAlert(_("Vehicle Sensors Calibrating")),
},
# dp - use for manual lane change
EventName.manualSteeringRequiredBlinkersOn: {
ET.PERMANENT: Alert(
_("STEERING REQUIRED: Blinkers ON"),
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, alert_rate=0.25),
},
}
if __name__ == '__main__':
# print all alerts by type and priority
from cereal.services import service_list
from collections import defaultdict, OrderedDict
event_names = {v: k for k, v in EventName.schema.enumerants.items()}
alerts_by_type: Dict[str, Dict[int, List[str]]] = defaultdict(lambda: defaultdict(list))
CP = car.CarParams.new_message()
CS = car.CarState.new_message()
sm = messaging.SubMaster(list(service_list.keys()))
for i, alerts in EVENTS.items():
for et, alert in alerts.items():
if callable(alert):
alert = alert(CP, CS, sm, False, 1)
priority = alert.priority
alerts_by_type[et][priority].append(event_names[i])
all_alerts = {}
for et, priority_alerts in alerts_by_type.items():
all_alerts[et] = OrderedDict([
(str(priority), l)
for priority, l in sorted(priority_alerts.items(), key=lambda x: -int(x[0]))
])
for status, evs in sorted(all_alerts.items(), key=lambda x: x[0]):
print(f"**** {status} ****")
for p, alert_list in evs.items():
print(f" {p}:")
print(" ", ', '.join(alert_list), "\n")
selfdrive/controls/lib/lane_planner.py
+25 -20
View File
@@ -3,7 +3,7 @@ from cereal import log
from common.filter_simple import FirstOrderFilter
from common.numpy_fast import interp
from common.realtime import DT_MDL
from selfdrive.hardware import TICI#, EON
from selfdrive.hardware import TICI
from system.swaglog import cloudlog
@@ -11,8 +11,13 @@ TRAJECTORY_SIZE = 33
# camera offset is meters from center car to camera
# model path is in the frame of EON's camera. TICI is 0.1 m away,
# however the average measured path difference is 0.04 m
PATH_OFFSET = 0.00
CAMERA_OFFSET = 0.04
if TICI:
CAMERA_OFFSET = 0.04
PATH_OFFSET = 0.04
# PC
else:
CAMERA_OFFSET = 0.0
PATH_OFFSET = 0.0
class LanePlanner:
@@ -35,25 +40,25 @@ class LanePlanner:
self.l_lane_change_prob = 0.
self.r_lane_change_prob = 0.
self.camera_offset = -CAMERA_OFFSET
self.path_offset = -PATH_OFFSET
self.camera_offset = CAMERA_OFFSET
self.path_offset = PATH_OFFSET
self.dp_camera_offset = None
self.dp_path_offset = None
# self.dp_camera_offset = None
# self.dp_path_offset = None
def update_dp_camera_offsets(self, camera_offset, path_offset):
if self.dp_camera_offset != camera_offset:
self.dp_camera_offset = camera_offset
camera_offset = -camera_offset
# from 0.04 to -0.04, difference is -0.08
# so we can assume the distance between C3's 2 cameras is 8 cm
self.camera_offset = camera_offset * 0.01
if self.dp_path_offset != path_offset:
self.dp_path_offset = path_offset
path_offset = -path_offset
# from 0.04 to -0.04, difference is -0.08
# so we can assume the distance between C3's 2 cameras is 8 cm
self.path_offset = path_offset * 0.01
# def update_dp_camera_offsets(self, camera_offset, path_offset):
# if self.dp_camera_offset != camera_offset:
# self.dp_camera_offset = camera_offset
# camera_offset = -camera_offset
# # from 0.04 to -0.04, difference is -0.08
# # so we can assume the distance between C3's 2 cameras is 8 cm
# self.camera_offset = camera_offset * 0.01
# if self.dp_path_offset != path_offset:
# self.dp_path_offset = path_offset
# path_offset = -path_offset
# # from 0.04 to -0.04, difference is -0.08
# # so we can assume the distance between C3's 2 cameras is 8 cm
# self.path_offset = path_offset * 0.01
def parse_model(self, md):
lane_lines = md.laneLines
selfdrive/controls/lib/latcontrol_lqr.py
-85
View File
@@ -1,85 +0,0 @@
import math
import numpy as np
from common.numpy_fast import clip
from common.realtime import DT_CTRL
from cereal import log
from selfdrive.controls.lib.latcontrol import LatControl
class LatControlLQR(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
self.dc_gain = CP.lateralTuning.lqr.dcGain
self.x_hat = np.array([[0], [0]])
self.i_unwind_rate = 0.3 * DT_CTRL
self.i_rate = 1.0 * DT_CTRL
self.reset()
def reset(self):
super().reset()
self.i_lqr = 0.0
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
# def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
lqr_log = log.ControlsState.LateralLQRState.new_message()
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
desired_angle += instant_offset # Only add offset that originates from vehicle model errors
lqr_log.steeringAngleDesiredDeg = desired_angle
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle_no_offset - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
if not active:
lqr_log.active = False
lqr_output = 0.
output_steer = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = desired_angle - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
(error <= 0 and (control >= -self.steer_max or i > 0.0)):
self.i_lqr = i
output_steer = lqr_output + self.i_lqr
output_steer = clip(output_steer, -self.steer_max, self.steer_max)
lqr_log.steeringAngleDeg = angle_steers_k
lqr_log.i = self.i_lqr
lqr_log.output = output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
return output_steer, desired_angle, lqr_log
selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
+1 -1
View File
@@ -336,7 +336,7 @@
"zu": [],
"zu_e": []
},
"cython_include_dirs": "/usr/local/pyenv/versions/3.8.10/lib/python3.8/site-packages/numpy/core/include",
"cython_include_dirs": "/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"dims": {
"N": 32,
"nbu": 0,
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/Makefile
+1 -1
View File
@@ -152,7 +152,7 @@ ocp_cython_o: ocp_cython_c
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /usr/local/pyenv/versions/3.8.10/lib/python3.8/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.c
+40397
View File
File diff suppressed because it is too large Load Diff
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+278
View File
@@ -0,0 +1,278 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_sim_solver_lat.h"
// ** solver data **
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int lat_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = LAT_NX;
const int nu = LAT_NU;
const int nz = LAT_NZ;
const int np = LAT_NP;
bool tmp_bool;
double Tsim = 0.009765625;
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = ERK;
// create correct config based on plan
sim_config * lat_sim_config = sim_config_create(plan);
capsule->acados_sim_config = lat_sim_config;
// sim dims
void *lat_sim_dims = sim_dims_create(lat_sim_config);
capsule->acados_sim_dims = lat_sim_dims;
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
// sim opts
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
tmp_int = 4;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
tmp_int = 1;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
tmp_bool = 0;
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
// sim in / out
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_in = lat_sim_in;
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_out = lat_sim_out;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "T", &Tsim);
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
// sim solver
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
lat_sim_dims, lat_sim_opts);
capsule->acados_sim_solver = lat_sim_solver;
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
lat_acados_sim_update_params(capsule, p, np);
free(p);
/* initialize input */
// x
double x0[4];
for (int ii = 0; ii < 4; ii++)
x0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "x", x0);
// u
double u0[1];
for (int ii = 0; ii < 1; ii++)
u0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "u", u0);
// S_forw
double S_forw[20];
for (int ii = 0; ii < 20; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < 4; ii++)
S_forw[ii + ii * 4 ] = 1.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "S_forw", S_forw);
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
return status;
}
int lat_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in lat_acados_sim_solve()! Exiting.\n");
return status;
}
int lat_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
}
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = LAT_NP;
if (casadi_np != np) {
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
}
/* getters pointers to C objects*/
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.c
+960
View File
@@ -0,0 +1,960 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
// acados
// #include "acados/utils/print.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "lat_cost/lat_cost_y_fun.h"
#include "lat_cost/lat_cost_y_0_fun.h"
#include "lat_cost/lat_cost_y_e_fun.h"
#include "acados_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
#define NBX LAT_NBX
#define NBX0 LAT_NBX0
#define NBU LAT_NBU
#define NSBX LAT_NSBX
#define NSBU LAT_NSBU
#define NSH LAT_NSH
#define NSG LAT_NSG
#define NSPHI LAT_NSPHI
#define NSHN LAT_NSHN
#define NSGN LAT_NSGN
#define NSPHIN LAT_NSPHIN
#define NSBXN LAT_NSBXN
#define NS LAT_NS
#define NSN LAT_NSN
#define NG LAT_NG
#define NBXN LAT_NBXN
#define NGN LAT_NGN
#define NY0 LAT_NY0
#define NY LAT_NY
#define NYN LAT_NYN
// #define N LAT_N
#define NH LAT_NH
#define NPHI LAT_NPHI
#define NHN LAT_NHN
#define NPHIN LAT_NPHIN
#define NR LAT_NR
// ** solver data **
lat_solver_capsule * lat_acados_create_capsule(void)
{
void* capsule_mem = malloc(sizeof(lat_solver_capsule));
lat_solver_capsule *capsule = (lat_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_free_capsule(lat_solver_capsule *capsule)
{
free(capsule);
return 0;
}
int lat_acados_create(lat_solver_capsule* capsule)
{
int N_shooting_intervals = LAT_N;
double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps
return lat_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps);
}
int lat_acados_update_time_steps(lat_solver_capsule* capsule, int N, double* new_time_steps)
{
if (N != capsule->nlp_solver_plan->N) {
fprintf(stderr, "lat_acados_update_time_steps: given number of time steps (= %d) " \
"differs from the currently allocated number of " \
"time steps (= %d)!\n" \
"Please recreate with new discretization and provide a new vector of time_stamps!\n",
N, capsule->nlp_solver_plan->N);
return 1;
}
ocp_nlp_config * nlp_config = capsule->nlp_config;
ocp_nlp_dims * nlp_dims = capsule->nlp_dims;
ocp_nlp_in * nlp_in = capsule->nlp_in;
for (int i = 0; i < N; i++)
{
ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]);
}
return 0;
}
/**
* Internal function for lat_acados_create: step 1
*/
void lat_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N)
{
assert(N == nlp_solver_plan->N);
/************************************************
* plan
************************************************/
nlp_solver_plan->nlp_solver = SQP_RTI;
nlp_solver_plan->ocp_qp_solver_plan.qp_solver = PARTIAL_CONDENSING_HPIPM;
nlp_solver_plan->nlp_cost[0] = NONLINEAR_LS;
for (int i = 1; i < N; i++)
nlp_solver_plan->nlp_cost[i] = NONLINEAR_LS;
nlp_solver_plan->nlp_cost[N] = NONLINEAR_LS;
for (int i = 0; i < N; i++)
{
nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL;
nlp_solver_plan->sim_solver_plan[i].sim_solver = ERK;
}
for (int i = 0; i < N; i++)
{nlp_solver_plan->nlp_constraints[i] = BGH;
}
nlp_solver_plan->nlp_constraints[N] = BGH;
}
/**
* Internal function for lat_acados_create: step 2
*/
ocp_nlp_dims* lat_acados_create_2_create_and_set_dimensions(lat_solver_capsule* capsule)
{
ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan;
const int N = nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
/************************************************
* dimensions
************************************************/
#define NINTNP1MEMS 17
int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS );
int* nx = intNp1mem + (N+1)*0;
int* nu = intNp1mem + (N+1)*1;
int* nbx = intNp1mem + (N+1)*2;
int* nbu = intNp1mem + (N+1)*3;
int* nsbx = intNp1mem + (N+1)*4;
int* nsbu = intNp1mem + (N+1)*5;
int* nsg = intNp1mem + (N+1)*6;
int* nsh = intNp1mem + (N+1)*7;
int* nsphi = intNp1mem + (N+1)*8;
int* ns = intNp1mem + (N+1)*9;
int* ng = intNp1mem + (N+1)*10;
int* nh = intNp1mem + (N+1)*11;
int* nphi = intNp1mem + (N+1)*12;
int* nz = intNp1mem + (N+1)*13;
int* ny = intNp1mem + (N+1)*14;
int* nr = intNp1mem + (N+1)*15;
int* nbxe = intNp1mem + (N+1)*16;
for (int i = 0; i < N+1; i++)
{
// common
nx[i] = NX;
nu[i] = NU;
nz[i] = NZ;
ns[i] = NS;
// cost
ny[i] = NY;
// constraints
nbx[i] = NBX;
nbu[i] = NBU;
nsbx[i] = NSBX;
nsbu[i] = NSBU;
nsg[i] = NSG;
nsh[i] = NSH;
nsphi[i] = NSPHI;
ng[i] = NG;
nh[i] = NH;
nphi[i] = NPHI;
nr[i] = NR;
nbxe[i] = 0;
}
// for initial state
nbx[0] = NBX0;
nsbx[0] = 0;
ns[0] = NS - NSBX;
nbxe[0] = 4;
ny[0] = NY0;
// terminal - common
nu[N] = 0;
nz[N] = 0;
ns[N] = NSN;
// cost
ny[N] = NYN;
// constraint
nbx[N] = NBXN;
nbu[N] = 0;
ng[N] = NGN;
nh[N] = NHN;
nphi[N] = NPHIN;
nr[N] = 0;
nsbx[N] = NSBXN;
nsbu[N] = 0;
nsg[N] = NSGN;
nsh[N] = NSHN;
nsphi[N] = NSPHIN;
/* create and set ocp_nlp_dims */
ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config);
ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx);
ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu);
ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz);
ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns);
for (int i = 0; i <= N; i++)
{
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]);
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]);
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]);
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]);
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]);
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]);
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]);
}
ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]);
for (int i = 1; i < N; i++)
ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]);
for (int i = 0; i < N; i++)
{
}
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]);
ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]);
ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]);
free(intNp1mem);
return nlp_dims;
}
/**
* Internal function for lat_acados_create: step 3
*/
void lat_acados_create_3_create_and_set_functions(lat_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
/************************************************
* external functions
************************************************/
#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \
capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\
capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \
capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \
capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \
capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \
capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \
external_function_param_casadi_create(&capsule->__CAPSULE_FNC__ , 2); \
}while(false)
// explicit ode
capsule->forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
for (int i = 0; i < N; i++) {
MAP_CASADI_FNC(forw_vde_casadi[i], lat_expl_vde_forw);
}
capsule->expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
for (int i = 0; i < N; i++) {
MAP_CASADI_FNC(expl_ode_fun[i], lat_expl_ode_fun);
}
// nonlinear least squares function
MAP_CASADI_FNC(cost_y_0_fun, lat_cost_y_0_fun);
MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, lat_cost_y_0_fun_jac_ut_xt);
MAP_CASADI_FNC(cost_y_0_hess, lat_cost_y_0_hess);
// nonlinear least squares cost
capsule->cost_y_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
for (int i = 0; i < N-1; i++)
{
MAP_CASADI_FNC(cost_y_fun[i], lat_cost_y_fun);
}
capsule->cost_y_fun_jac_ut_xt = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
for (int i = 0; i < N-1; i++)
{
MAP_CASADI_FNC(cost_y_fun_jac_ut_xt[i], lat_cost_y_fun_jac_ut_xt);
}
capsule->cost_y_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
for (int i = 0; i < N-1; i++)
{
MAP_CASADI_FNC(cost_y_hess[i], lat_cost_y_hess);
}
// nonlinear least square function
MAP_CASADI_FNC(cost_y_e_fun, lat_cost_y_e_fun);
MAP_CASADI_FNC(cost_y_e_fun_jac_ut_xt, lat_cost_y_e_fun_jac_ut_xt);
MAP_CASADI_FNC(cost_y_e_hess, lat_cost_y_e_hess);
#undef MAP_CASADI_FNC
}
/**
* Internal function for lat_acados_create: step 4
*/
void lat_acados_create_4_set_default_parameters(lat_solver_capsule* capsule) {
const int N = capsule->nlp_solver_plan->N;
// initialize parameters to nominal value
double* p = calloc(NP, sizeof(double));
for (int i = 0; i <= N; i++) {
lat_acados_update_params(capsule, i, p, NP);
}
free(p);
}
/**
* Internal function for lat_acados_create: step 5
*/
void lat_acados_create_5_set_nlp_in(lat_solver_capsule* capsule, const int N, double* new_time_steps)
{
assert(N == capsule->nlp_solver_plan->N);
ocp_nlp_config* nlp_config = capsule->nlp_config;
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
/************************************************
* nlp_in
************************************************/
// ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims);
// capsule->nlp_in = nlp_in;
ocp_nlp_in * nlp_in = capsule->nlp_in;
// set up time_steps
if (new_time_steps) {
lat_acados_update_time_steps(capsule, N, new_time_steps);
} else {// time_steps are different
double* time_steps = malloc(N*sizeof(double));
time_steps[0] = 0.009765625;
time_steps[1] = 0.029296875;
time_steps[2] = 0.048828125;
time_steps[3] = 0.068359375;
time_steps[4] = 0.087890625;
time_steps[5] = 0.107421875;
time_steps[6] = 0.126953125;
time_steps[7] = 0.146484375;
time_steps[8] = 0.166015625;
time_steps[9] = 0.185546875;
time_steps[10] = 0.205078125;
time_steps[11] = 0.224609375;
time_steps[12] = 0.244140625;
time_steps[13] = 0.263671875;
time_steps[14] = 0.283203125;
time_steps[15] = 0.302734375;
time_steps[16] = 0.322265625;
time_steps[17] = 0.341796875;
time_steps[18] = 0.361328125;
time_steps[19] = 0.380859375;
time_steps[20] = 0.400390625;
time_steps[21] = 0.419921875;
time_steps[22] = 0.439453125;
time_steps[23] = 0.458984375;
time_steps[24] = 0.478515625;
time_steps[25] = 0.498046875;
time_steps[26] = 0.517578125;
time_steps[27] = 0.537109375;
time_steps[28] = 0.556640625;
time_steps[29] = 0.576171875;
time_steps[30] = 0.595703125;
time_steps[31] = 0.615234375;
lat_acados_update_time_steps(capsule, N, time_steps);
free(time_steps);
}
/**** Dynamics ****/
for (int i = 0; i < N; i++)
{
ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &capsule->forw_vde_casadi[i]);
ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &capsule->expl_ode_fun[i]);
}
/**** Cost ****/
double* W_0 = calloc(NY0*NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
free(W_0);
double* yref_0 = calloc(NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
free(yref_0);
double* W = calloc(NY*NY, sizeof(double));
// change only the non-zero elements:
double* yref = calloc(NY, sizeof(double));
// change only the non-zero elements:
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref);
}
free(W);
free(yref);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess);
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun", &capsule->cost_y_fun[i-1]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]);
}
// terminal cost
double* yref_e = calloc(NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
free(yref_e);
double* W_e = calloc(NYN*NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
free(W_e);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess);
/**** Constraints ****/
// bounds for initial stage
// x0
int* idxbx0 = malloc(NBX0 * sizeof(int));
idxbx0[0] = 0;
idxbx0[1] = 1;
idxbx0[2] = 2;
idxbx0[3] = 3;
double* lubx0 = calloc(2*NBX0, sizeof(double));
double* lbx0 = lubx0;
double* ubx0 = lubx0 + NBX0;
// change only the non-zero elements:
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
free(idxbx0);
free(lubx0);
// idxbxe_0
int* idxbxe_0 = malloc(4 * sizeof(int));
idxbxe_0[0] = 0;
idxbxe_0[1] = 1;
idxbxe_0[2] = 2;
idxbxe_0[3] = 3;
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0);
free(idxbxe_0);
/* constraints that are the same for initial and intermediate */
// x
int* idxbx = malloc(NBX * sizeof(int));
idxbx[0] = 2;
idxbx[1] = 3;
double* lubx = calloc(2*NBX, sizeof(double));
double* lbx = lubx;
double* ubx = lubx + NBX;
lbx[0] = -1.5707963267948966;
ubx[0] = 1.5707963267948966;
lbx[1] = -0.8726646259971648;
ubx[1] = 0.8726646259971648;
for (int i = 1; i < N; i++)
{
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx);
}
free(idxbx);
free(lubx);
/* terminal constraints */
}
/**
* Internal function for lat_acados_create: step 6
*/
void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
void *nlp_opts = capsule->nlp_opts;
/************************************************
* opts
************************************************/
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "fixed_step");int full_step_dual = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "full_step_dual", &full_step_dual);
// set collocation type (relevant for implicit integrators)
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type);
// set up sim_method_num_steps
// all sim_method_num_steps are identical
int sim_method_num_steps = 1;
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps);
// set up sim_method_num_stages
// all sim_method_num_stages are identical
int sim_method_num_stages = 4;
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages);
int newton_iter_val = 3;
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val);
// set up sim_method_jac_reuse
bool tmp_bool = (bool) 0;
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
double nlp_solver_step_length = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length);
double levenberg_marquardt = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
/* options QP solver */
int qp_solver_cond_N;
const int qp_solver_cond_N_ori = 1;
qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
// set HPIPM mode: should be done before setting other QP solver options
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE");
int qp_solver_iter_max = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max);
int print_level = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
int ext_cost_num_hess = 0;
}
/**
* Internal function for lat_acados_create: step 7
*/
void lat_acados_create_7_set_nlp_out(lat_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
ocp_nlp_out* nlp_out = capsule->nlp_out;
// initialize primal solution
double* xu0 = calloc(NX+NU, sizeof(double));
double* x0 = xu0;
// initialize with x0
double* u0 = xu0 + NX;
for (int i = 0; i < N; i++)
{
// x0
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0);
// u0
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
}
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0);
free(xu0);
}
/**
* Internal function for lat_acados_create: step 8
*/
//void lat_acados_create_8_create_solver(lat_solver_capsule* capsule)
//{
// capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
//}
/**
* Internal function for lat_acados_create: step 9
*/
int lat_acados_create_9_precompute(lat_solver_capsule* capsule) {
int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
if (status != ACADOS_SUCCESS) {
printf("\nocp_nlp_precompute failed!\n\n");
exit(1);
}
return status;
}
int lat_acados_create_with_discretization(lat_solver_capsule* capsule, int N, double* new_time_steps)
{
// If N does not match the number of shooting intervals used for code generation, new_time_steps must be given.
if (N != LAT_N && !new_time_steps) {
fprintf(stderr, "lat_acados_create_with_discretization: new_time_steps is NULL " \
"but the number of shooting intervals (= %d) differs from the number of " \
"shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \
N, LAT_N);
return 1;
}
// number of expected runtime parameters
capsule->nlp_np = NP;
// 1) create and set nlp_solver_plan; create nlp_config
capsule->nlp_solver_plan = ocp_nlp_plan_create(N);
lat_acados_create_1_set_plan(capsule->nlp_solver_plan, N);
capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan);
// 3) create and set dimensions
capsule->nlp_dims = lat_acados_create_2_create_and_set_dimensions(capsule);
lat_acados_create_3_create_and_set_functions(capsule);
// 4) set default parameters in functions
lat_acados_create_4_set_default_parameters(capsule);
// 5) create and set nlp_in
capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims);
lat_acados_create_5_set_nlp_in(capsule, N, new_time_steps);
// 6) create and set nlp_opts
capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims);
lat_acados_create_6_set_opts(capsule);
// 7) create and set nlp_out
// 7.1) nlp_out
capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims);
// 7.2) sens_out
capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims);
lat_acados_create_7_set_nlp_out(capsule);
// 8) create solver
capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
//lat_acados_create_8_create_solver(capsule);
// 9) do precomputations
int status = lat_acados_create_9_precompute(capsule);
return status;
}
/**
* This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export.
*/
int lat_acados_update_qp_solver_cond_N(lat_solver_capsule* capsule, int qp_solver_cond_N)
{
// 1) destroy solver
ocp_nlp_solver_destroy(capsule->nlp_solver);
// 2) set new value for "qp_cond_N"
const int N = capsule->nlp_solver_plan->N;
if(qp_solver_cond_N > N)
printf("Warning: qp_solver_cond_N = %d > N = %d\n", qp_solver_cond_N, N);
ocp_nlp_solver_opts_set(capsule->nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N);
// 3) continue with the remaining steps from lat_acados_create_with_discretization(...):
// -> 8) create solver
capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
// -> 9) do precomputations
int status = lat_acados_create_9_precompute(capsule);
return status;
}
int lat_acados_reset(lat_solver_capsule* capsule)
{
// set initialization to all zeros
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
ocp_nlp_out* nlp_out = capsule->nlp_out;
ocp_nlp_in* nlp_in = capsule->nlp_in;
ocp_nlp_solver* nlp_solver = capsule->nlp_solver;
int nx, nu, nv, ns, nz, ni, dim;
double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double));
for(int i=0; i<N+1; i++)
{
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", buffer);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", buffer);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "sl", buffer);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "su", buffer);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "lam", buffer);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "t", buffer);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "z", buffer);
if (i<N)
{
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "pi", buffer);
}
}
// get qp_status: if NaN -> reset memory
int qp_status;
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status);
if (qp_status == 3)
{
// printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status);
ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out);
}
free(buffer);
return 0;
}
int lat_acados_update_params(lat_solver_capsule* capsule, int stage, double *p, int np)
{
int solver_status = 0;
int casadi_np = 2;
if (casadi_np != np) {
printf("acados_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
capsule->forw_vde_casadi[stage].set_param(capsule->forw_vde_casadi+stage, p);
capsule->expl_ode_fun[stage].set_param(capsule->expl_ode_fun+stage, p);
// constraints
// cost
if (stage == 0)
{
capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p);
capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p);
capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p);
}
else // 0 < stage < N
{
capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p);
capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p);
capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p);
}
}
else // stage == N
{
// terminal shooting node has no dynamics
// cost
capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p);
capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p);
capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p);
// constraints
}
return solver_status;
}
int lat_acados_solve(lat_solver_capsule* capsule)
{
// solve NLP
int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
return solver_status;
}
int lat_acados_free(lat_solver_capsule* capsule)
{
// before destroying, keep some info
const int N = capsule->nlp_solver_plan->N;
// free memory
ocp_nlp_solver_opts_destroy(capsule->nlp_opts);
ocp_nlp_in_destroy(capsule->nlp_in);
ocp_nlp_out_destroy(capsule->nlp_out);
ocp_nlp_out_destroy(capsule->sens_out);
ocp_nlp_solver_destroy(capsule->nlp_solver);
ocp_nlp_dims_destroy(capsule->nlp_dims);
ocp_nlp_config_destroy(capsule->nlp_config);
ocp_nlp_plan_destroy(capsule->nlp_solver_plan);
/* free external function */
// dynamics
for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->forw_vde_casadi[i]);
external_function_param_casadi_free(&capsule->expl_ode_fun[i]);
}
free(capsule->forw_vde_casadi);
free(capsule->expl_ode_fun);
// cost
external_function_param_casadi_free(&capsule->cost_y_0_fun);
external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt);
external_function_param_casadi_free(&capsule->cost_y_0_hess);
for (int i = 0; i < N - 1; i++)
{
external_function_param_casadi_free(&capsule->cost_y_fun[i]);
external_function_param_casadi_free(&capsule->cost_y_fun_jac_ut_xt[i]);
external_function_param_casadi_free(&capsule->cost_y_hess[i]);
}
free(capsule->cost_y_fun);
free(capsule->cost_y_fun_jac_ut_xt);
free(capsule->cost_y_hess);
external_function_param_casadi_free(&capsule->cost_y_e_fun);
external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt);
external_function_param_casadi_free(&capsule->cost_y_e_hess);
// constraints
return 0;
}
ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule* capsule) { return capsule->nlp_config; }
void *lat_acados_get_nlp_opts(lat_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
void lat_acados_print_stats(lat_solver_capsule* capsule)
{
int sqp_iter, stat_m, stat_n, tmp_int;
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter);
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_n", &stat_n);
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_m", &stat_m);
double stat[1200];
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m;
printf("iter\tqp_stat\tqp_iter\n");
for (int i = 0; i < nrow; i++)
{
for (int j = 0; j < stat_n + 1; j++)
{
tmp_int = (int) stat[i + j * nrow];
printf("%d\t", tmp_int);
}
printf("\n");
}
}
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
+264
View File
@@ -0,0 +1,264 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (32+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 5);
// y_ref
ssSetInputPortVectorDimension(S, 4, 155);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 3);
// lbx
ssSetInputPortVectorDimension(S, 6, 62);
// ubx
ssSetInputPortVectorDimension(S, 7, 62);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[5];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 5; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 5; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 32, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun.c
+163
View File
@@ -0,0 +1,163 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_0_fun_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* lat_cost_y_0_fun:(i0[4],i1,i2[2])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][1]=a2;
a2=arg[0]? arg[0][3] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][2]=a2;
a2=arg[1]? arg[1][0] : 0;
a1=(a1*a2);
if (res[0]!=0) res[0][3]=a1;
a1=1.0000000000000001e-01;
a0=(a0+a1);
a2=(a2/a0);
if (res[0]!=0) res[0][4]=a2;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c
+174
View File
@@ -0,0 +1,174 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_0_fun_jac_ut_xt_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
/* lat_cost_y_0_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][1]=a2;
a2=arg[0]? arg[0][3] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][2]=a2;
a2=arg[1]? arg[1][0] : 0;
a3=(a1*a2);
if (res[0]!=0) res[0][3]=a3;
a3=1.0000000000000001e-01;
a0=(a0+a3);
a2=(a2/a0);
if (res[0]!=0) res[0][4]=a2;
a2=1.;
if (res[1]!=0) res[1][0]=a2;
if (res[1]!=0) res[1][1]=a1;
if (res[1]!=0) res[1][2]=a1;
if (res[1]!=0) res[1][3]=a1;
a0=(1./a0);
if (res[1]!=0) res[1][4]=a0;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
case 1: return "o1";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_hess.c
+148
View File
@@ -0,0 +1,148 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_0_hess_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
/* lat_cost_y_0_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_hess_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun.c
+156
View File
@@ -0,0 +1,156 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_e_fun_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
/* lat_cost_y_e_fun:(i0[4],i1[],i2[2])->(o0[3]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a1=10.;
a0=(a0+a1);
a1=arg[0]? arg[0][2] : 0;
a1=(a0*a1);
if (res[0]!=0) res[0][1]=a1;
a1=arg[0]? arg[0][3] : 0;
a0=(a0*a1);
if (res[0]!=0) res[0][2]=a0;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c
+164
View File
@@ -0,0 +1,164 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_e_fun_jac_ut_xt_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s4[9] = {4, 3, 0, 1, 2, 3, 1, 2, 3};
/* lat_cost_y_e_fun_jac_ut_xt:(i0[4],i1[],i2[2])->(o0[3],o1[4x3,3nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a1=10.;
a0=(a0+a1);
a1=arg[0]? arg[0][2] : 0;
a1=(a0*a1);
if (res[0]!=0) res[0][1]=a1;
a1=arg[0]? arg[0][3] : 0;
a1=(a0*a1);
if (res[0]!=0) res[0][2]=a1;
a1=1.;
if (res[1]!=0) res[1][0]=a1;
if (res[1]!=0) res[1][1]=a0;
if (res[1]!=0) res[1][2]=a0;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
case 1: return "o1";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_hess.c
+148
View File
@@ -0,0 +1,148 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_e_hess_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[7] = {4, 4, 0, 0, 0, 0, 0};
/* lat_cost_y_e_hess:(i0[4],i1[],i2[3],i3[2])->(o0[4x4,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_hess_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun.c
+163
View File
@@ -0,0 +1,163 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_fun_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* lat_cost_y_fun:(i0[4],i1,i2[2])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][1]=a2;
a2=arg[0]? arg[0][3] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][2]=a2;
a2=arg[1]? arg[1][0] : 0;
a1=(a1*a2);
if (res[0]!=0) res[0][3]=a1;
a1=1.0000000000000001e-01;
a0=(a0+a1);
a2=(a2/a0);
if (res[0]!=0) res[0][4]=a2;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun_jac_ut_xt.c
+174
View File
@@ -0,0 +1,174 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_fun_jac_ut_xt_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
/* lat_cost_y_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][1]=a2;
a2=arg[0]? arg[0][3] : 0;
a2=(a1*a2);
if (res[0]!=0) res[0][2]=a2;
a2=arg[1]? arg[1][0] : 0;
a3=(a1*a2);
if (res[0]!=0) res[0][3]=a3;
a3=1.0000000000000001e-01;
a0=(a0+a3);
a2=(a2/a0);
if (res[0]!=0) res[0][4]=a2;
a2=1.;
if (res[1]!=0) res[1][0]=a2;
if (res[1]!=0) res[1][1]=a1;
if (res[1]!=0) res[1][2]=a1;
if (res[1]!=0) res[1][3]=a1;
a0=(1./a0);
if (res[1]!=0) res[1][4]=a0;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
case 1: return "o1";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_hess.c
+148
View File
@@ -0,0 +1,148 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_cost_y_hess_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
/* lat_cost_y_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_hess_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_expl_ode_fun.c
+164
View File
@@ -0,0 +1,164 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_expl_ode_fun_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
/* lat_expl_ode_fun:(i0[4],i1,i2[2])->(o0[4]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4, a5;
a0=arg[2]? arg[2][0] : 0;
a1=arg[0]? arg[0][2] : 0;
a2=cos(a1);
a2=(a0*a2);
a3=arg[2]? arg[2][1] : 0;
a4=sin(a1);
a4=(a3*a4);
a5=arg[0]? arg[0][3] : 0;
a4=(a4*a5);
a2=(a2-a4);
if (res[0]!=0) res[0][0]=a2;
a2=sin(a1);
a0=(a0*a2);
a1=cos(a1);
a3=(a3*a1);
a3=(a3*a5);
a0=(a0+a3);
if (res[0]!=0) res[0][1]=a0;
if (res[0]!=0) res[0][2]=a5;
a5=arg[1]? arg[1][0] : 0;
if (res[0]!=0) res[0][3]=a5;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_expl_ode_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_expl_ode_fun_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_expl_ode_fun_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_expl_ode_fun_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_expl_ode_fun_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_ode_fun_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_ode_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_expl_vde_adj.c
+186
View File
@@ -0,0 +1,186 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_expl_vde_adj_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* lat_expl_vde_adj:(i0[4],i1[4],i2,i3[2])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4, a5, a6, a7;
a0=0.;
if (res[0]!=0) res[0][0]=a0;
if (res[0]!=0) res[0][1]=a0;
a0=arg[0]? arg[0][2] : 0;
a1=cos(a0);
a2=arg[3]? arg[3][0] : 0;
a3=arg[1]? arg[1][1] : 0;
a4=(a2*a3);
a1=(a1*a4);
a4=sin(a0);
a5=arg[3]? arg[3][1] : 0;
a6=arg[0]? arg[0][3] : 0;
a7=(a6*a3);
a7=(a5*a7);
a4=(a4*a7);
a1=(a1-a4);
a4=cos(a0);
a7=arg[1]? arg[1][0] : 0;
a6=(a6*a7);
a6=(a5*a6);
a4=(a4*a6);
a1=(a1-a4);
a4=sin(a0);
a2=(a2*a7);
a4=(a4*a2);
a1=(a1-a4);
if (res[0]!=0) res[0][2]=a1;
a1=arg[1]? arg[1][2] : 0;
a4=cos(a0);
a4=(a5*a4);
a4=(a4*a3);
a1=(a1+a4);
a0=sin(a0);
a5=(a5*a0);
a5=(a5*a7);
a1=(a1-a5);
if (res[0]!=0) res[0][3]=a1;
a1=arg[1]? arg[1][3] : 0;
if (res[0]!=0) res[0][4]=a1;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_adj_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_adj_n_out(void) { return 1;}
CASADI_SYMBOL_EXPORT casadi_real lat_expl_vde_adj_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_adj_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_adj_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_adj_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s0;
case 2: return casadi_s1;
case 3: return casadi_s2;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_adj_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_expl_vde_forw.c
+299
View File
@@ -0,0 +1,299 @@
/* This file was automatically generated by CasADi 3.6.3.
* It consists of:
* 1) content generated by CasADi runtime: not copyrighted
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
* 3) user code: owned by the user
*
*/
#ifdef __cplusplus
extern "C" {
#endif
/* How to prefix internal symbols */
#ifdef CASADI_CODEGEN_PREFIX
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
#else
#define CASADI_PREFIX(ID) lat_expl_vde_forw_ ## ID
#endif
#include <math.h>
#ifndef casadi_real
#define casadi_real double
#endif
#ifndef casadi_int
#define casadi_int int
#endif
/* Add prefix to internal symbols */
#define casadi_f0 CASADI_PREFIX(f0)
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
#if defined(STATIC_LINKED)
#define CASADI_SYMBOL_EXPORT
#else
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
#endif
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
#else
#define CASADI_SYMBOL_EXPORT
#endif
#endif
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[23] = {4, 4, 0, 4, 8, 12, 16, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3};
static const casadi_int casadi_s2[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
/* lat_expl_vde_forw:(i0[4],i1[4x4],i2[4],i3,i4[2])->(o0[4],o1[4x4],o2[4]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a2, a3, a4, a5, a6, a7, a8, a9;
a0=arg[4]? arg[4][0] : 0;
a1=arg[0]? arg[0][2] : 0;
a2=cos(a1);
a2=(a0*a2);
a3=arg[4]? arg[4][1] : 0;
a4=sin(a1);
a4=(a3*a4);
a5=arg[0]? arg[0][3] : 0;
a6=(a4*a5);
a2=(a2-a6);
if (res[0]!=0) res[0][0]=a2;
a2=sin(a1);
a2=(a0*a2);
a6=cos(a1);
a6=(a3*a6);
a7=(a6*a5);
a2=(a2+a7);
if (res[0]!=0) res[0][1]=a2;
if (res[0]!=0) res[0][2]=a5;
a2=arg[3]? arg[3][0] : 0;
if (res[0]!=0) res[0][3]=a2;
a2=sin(a1);
a7=arg[1]? arg[1][2] : 0;
a8=(a2*a7);
a8=(a0*a8);
a9=cos(a1);
a10=(a9*a7);
a10=(a3*a10);
a10=(a5*a10);
a11=arg[1]? arg[1][3] : 0;
a12=(a4*a11);
a10=(a10+a12);
a8=(a8+a10);
a8=(-a8);
if (res[1]!=0) res[1][0]=a8;
a8=cos(a1);
a10=(a8*a7);
a10=(a0*a10);
a12=(a6*a11);
a13=sin(a1);
a7=(a13*a7);
a7=(a3*a7);
a7=(a5*a7);
a12=(a12-a7);
a10=(a10+a12);
if (res[1]!=0) res[1][1]=a10;
if (res[1]!=0) res[1][2]=a11;
a11=0.;
if (res[1]!=0) res[1][3]=a11;
a10=arg[1]? arg[1][6] : 0;
a12=(a2*a10);
a12=(a0*a12);
a7=(a9*a10);
a7=(a3*a7);
a7=(a5*a7);
a14=arg[1]? arg[1][7] : 0;
a15=(a4*a14);
a7=(a7+a15);
a12=(a12+a7);
a12=(-a12);
if (res[1]!=0) res[1][4]=a12;
a12=(a8*a10);
a12=(a0*a12);
a7=(a6*a14);
a10=(a13*a10);
a10=(a3*a10);
a10=(a5*a10);
a7=(a7-a10);
a12=(a12+a7);
if (res[1]!=0) res[1][5]=a12;
if (res[1]!=0) res[1][6]=a14;
if (res[1]!=0) res[1][7]=a11;
a14=arg[1]? arg[1][10] : 0;
a12=(a2*a14);
a12=(a0*a12);
a7=(a9*a14);
a7=(a3*a7);
a7=(a5*a7);
a10=arg[1]? arg[1][11] : 0;
a15=(a4*a10);
a7=(a7+a15);
a12=(a12+a7);
a12=(-a12);
if (res[1]!=0) res[1][8]=a12;
a12=(a8*a14);
a12=(a0*a12);
a7=(a6*a10);
a14=(a13*a14);
a14=(a3*a14);
a14=(a5*a14);
a7=(a7-a14);
a12=(a12+a7);
if (res[1]!=0) res[1][9]=a12;
if (res[1]!=0) res[1][10]=a10;
if (res[1]!=0) res[1][11]=a11;
a10=arg[1]? arg[1][14] : 0;
a2=(a2*a10);
a2=(a0*a2);
a9=(a9*a10);
a9=(a3*a9);
a9=(a5*a9);
a12=arg[1]? arg[1][15] : 0;
a7=(a4*a12);
a9=(a9+a7);
a2=(a2+a9);
a2=(-a2);
if (res[1]!=0) res[1][12]=a2;
a8=(a8*a10);
a8=(a0*a8);
a2=(a6*a12);
a13=(a13*a10);
a13=(a3*a13);
a13=(a5*a13);
a2=(a2-a13);
a8=(a8+a2);
if (res[1]!=0) res[1][13]=a8;
if (res[1]!=0) res[1][14]=a12;
if (res[1]!=0) res[1][15]=a11;
a11=sin(a1);
a12=arg[2]? arg[2][2] : 0;
a11=(a11*a12);
a11=(a0*a11);
a8=cos(a1);
a8=(a8*a12);
a8=(a3*a8);
a8=(a5*a8);
a2=arg[2]? arg[2][3] : 0;
a4=(a4*a2);
a8=(a8+a4);
a11=(a11+a8);
a11=(-a11);
if (res[2]!=0) res[2][0]=a11;
a11=cos(a1);
a11=(a11*a12);
a0=(a0*a11);
a6=(a6*a2);
a1=sin(a1);
a1=(a1*a12);
a3=(a3*a1);
a5=(a5*a3);
a6=(a6-a5);
a0=(a0+a6);
if (res[2]!=0) res[2][1]=a0;
if (res[2]!=0) res[2][2]=a2;
a2=1.;
if (res[2]!=0) res[2][3]=a2;
return 0;
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
return casadi_f0(arg, res, iw, w, mem);
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_alloc_mem(void) {
return 0;
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_init_mem(int mem) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_free_mem(int mem) {
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_checkout(void) {
return 0;
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_release(int mem) {
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_incref(void) {
}
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_forw_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_forw_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real lat_expl_vde_forw_default_in(casadi_int i) {
switch (i) {
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_forw_name_in(casadi_int i) {
switch (i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_forw_name_out(casadi_int i) {
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_forw_sparsity_in(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s0;
case 3: return casadi_s2;
case 4: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_forw_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s0;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
}
#ifdef __cplusplus
} /* extern "C" */
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_lat.c
+216
View File
@@ -0,0 +1,216 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
#define NBX LAT_NBX
#define NBX0 LAT_NBX0
#define NBU LAT_NBU
#define NSBX LAT_NSBX
#define NSBU LAT_NSBU
#define NSH LAT_NSH
#define NSG LAT_NSG
#define NSPHI LAT_NSPHI
#define NSHN LAT_NSHN
#define NSGN LAT_NSGN
#define NSPHIN LAT_NSPHIN
#define NSBXN LAT_NSBXN
#define NS LAT_NS
#define NSN LAT_NSN
#define NG LAT_NG
#define NBXN LAT_NBXN
#define NGN LAT_NGN
#define NY0 LAT_NY0
#define NY LAT_NY
#define NYN LAT_NYN
#define NH LAT_NH
#define NPHI LAT_NPHI
#define NHN LAT_NHN
#define NPHIN LAT_NPHIN
#define NR LAT_NR
int main()
{
lat_solver_capsule *acados_ocp_capsule = lat_acados_create_capsule();
// there is an opportunity to change the number of shooting intervals in C without new code generation
int N = LAT_N;
// allocate the array and fill it accordingly
double* new_time_steps = NULL;
int status = lat_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
if (status)
{
printf("lat_acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(acados_ocp_capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(acados_ocp_capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(acados_ocp_capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(acados_ocp_capsule);
ocp_nlp_solver *nlp_solver = lat_acados_get_nlp_solver(acados_ocp_capsule);
void *nlp_opts = lat_acados_get_nlp_opts(acados_ocp_capsule);
// initial condition
int idxbx0[NBX0];
idxbx0[0] = 0;
idxbx0[1] = 1;
idxbx0[2] = 2;
idxbx0[3] = 3;
double lbx0[NBX0];
double ubx0[NBX0];
lbx0[0] = 0;
ubx0[0] = 0;
lbx0[1] = 0;
ubx0[1] = 0;
lbx0[2] = 0;
ubx0[2] = 0;
lbx0[3] = 0;
ubx0[3] = 0;
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
// initialization for state values
double x_init[NX];
x_init[0] = 0.0;
x_init[1] = 0.0;
x_init[2] = 0.0;
x_init[3] = 0.0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
for (int ii = 0; ii <= N; ii++)
{
lat_acados_update_params(acados_ocp_capsule, ii, p, NP);
}
// prepare evaluation
int NTIMINGS = 1;
double min_time = 1e12;
double kkt_norm_inf;
double elapsed_time;
int sqp_iter;
double xtraj[NX * (N+1)];
double utraj[NU * N];
// solve ocp in loop
int rti_phase = 0;
for (int ii = 0; ii < NTIMINGS; ii++)
{
// initialize solution
for (int i = 0; i < N; i++)
{
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
}
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
status = lat_acados_solve(acados_ocp_capsule);
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
min_time = MIN(elapsed_time, min_time);
}
/* print solution and statistics */
for (int ii = 0; ii <= nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
for (int ii = 0; ii < nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
printf("\n--- xtraj ---\n");
d_print_exp_tran_mat( NX, N+1, xtraj, NX);
printf("\n--- utraj ---\n");
d_print_exp_tran_mat( NU, N, utraj, NU );
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
if (status == ACADOS_SUCCESS)
{
printf("lat_acados_solve(): SUCCESS!\n");
}
else
{
printf("lat_acados_solve() failed with status %d.\n", status);
}
// get solution
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
lat_acados_print_stats(acados_ocp_capsule);
printf("\nSolver info:\n");
printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
// free solver
status = lat_acados_free(acados_ocp_capsule);
if (status) {
printf("lat_acados_free() returned status %d. \n", status);
}
// free solver capsule
status = lat_acados_free_capsule(acados_ocp_capsule);
if (status) {
printf("lat_acados_free_capsule() returned status %d. \n", status);
}
return status;
}
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+128
View File
@@ -0,0 +1,128 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
status = lat_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
lat_acados_sim_update_params(capsule, p, NP);
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = lat_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = lat_acados_sim_free(capsule);
if (status) {
printf("lat_acados_sim_free() returned status %d. \n", status);
}
lat_acados_sim_solver_free_capsule(capsule);
return status;
}
selfdrive/controls/lib/lateral_planner.py
+53 -51
View File
@@ -4,12 +4,11 @@ from common.numpy_fast import interp
from system.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_lane_laneless_mode, get_speed_error
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
from selfdrive.controls.lib.desire_helper import DesireHelper
import cereal.messaging as messaging
from cereal import log
from selfdrive.controls.lib.lane_planner import LanePlanner
from selfdrive.hardware import TICI
from common.params import Params
TRAJECTORY_SIZE = 33
@@ -30,13 +29,15 @@ STEERING_RATE_COST = 700.0
class LateralPlanner:
def __init__(self, CP, debug=False):
self.DH = DesireHelper()
self.LP = LanePlanner()
# dp - laneline mode
self.dp_lanelines_enable = False
self.dp_lanelines_active = False
self.dp_camera_offset = 4 if TICI else -6
self.dp_path_offset = 4 if TICI else 0
# dp - lanefull
params = Params()
self._dp_lat_lane_priority_mode = params.get_bool("dp_lat_lane_priority_mode")
self._dp_lat_lane_priority_mode_active = False
self._dp_lat_lane_priority_mode_active_prev = False
self.LP = LanePlanner()
# dp // mapd - for vision turn controller
self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
# Vehicle model parameters used to calculate lateral movement of car
self.factor1 = CP.wheelbase - CP.centerToFront
@@ -54,7 +55,6 @@ class LateralPlanner:
self.v_ego = 0.0
self.l_lane_change_prob = 0.0
self.r_lane_change_prob = 0.0
self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.debug_mode = debug
@@ -69,14 +69,6 @@ class LateralPlanner:
# clip speed , lateral planning is not possible at 0 speed
measured_curvature = sm['controlsState'].curvature
v_ego_car = sm['carState'].vEgo
if sm.updated['dragonConf']:
self.dp_lanelines_enable = sm['dragonConf'].dpLateralLanelines
self.dp_camera_offset = sm['dragonConf'].dpLateralCameraOffset
self.dp_path_offset = sm['dragonConf'].dpLateralPathOffset
if sm['controlsState'].dpLateralAltActive and sm['dragonConf'].dpLateralAltLanelines:
self.dp_lanelines_enable = True
self.dp_camera_offset = sm['dragonConf'].dpLateralAltCameraOffset
self.dp_path_offset = sm['dragonConf'].dpLateralAltPathOffset
# Parse model predictions
md = sm['modelV2']
@@ -90,27 +82,28 @@ class LateralPlanner:
self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
self.v_ego = self.v_plan[0]
if self.dp_lanelines_enable:
# dp - when laneline mode enabled, we use old logic (including lane changing)
d_path_xyz = self.lanelines_mode(md, sm['carState'], sm['carControl'].latActive, sm['dragonConf'])
# Lane change logic
desire_state = md.meta.desireState
if len(desire_state):
self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
if self._dp_lat_lane_priority_mode:
self.LP.parse_model(md)
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
else:
self.dp_lanelines_active = False
# dp -- tab spacing begin (stock logic) --
# Lane change logic
desire_state = md.meta.desireState
if len(desire_state):
self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['dragonConf'], md)
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
path_xyz = self._get_laneless_laneline_d_path_xyz() if self._dp_lat_lane_priority_mode else self.path_xyz
self.d_path_w_lines_xyz = path_xyz
d_path_xyz = self.path_xyz
# dp -- tab spacing end (stock logic) --
self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
STEERING_RATE_COST)
y_pts = d_path_xyz[:LAT_MPC_N+1, 1]
y_pts = path_xyz[:LAT_MPC_N+1, 1]
heading_pts = self.plan_yaw[:LAT_MPC_N+1]
yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1]
self.y_pts = y_pts
@@ -168,35 +161,44 @@ class LateralPlanner:
lateralPlan.solverState.u = self.lat_mpc.u_sol.flatten().tolist()
lateralPlan.desire = self.DH.desire
lateralPlan.useLaneLines = self.dp_lanelines_active
lateralPlan.useLaneLines = self._dp_lat_lane_priority_mode and self._dp_lat_lane_priority_mode_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
plan_send.lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
plan_send.lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
pm.send('lateralPlan', plan_send)
def lanelines_mode(self, md, car_state, lat_active, dragon_conf):
# update camera/path offset to lane planner
self.LP.update_dp_camera_offsets(self.dp_camera_offset, self.dp_path_offset)
# Parse model predictions
self.LP.parse_model(md)
# dp - extension
plan_ext_send = messaging.new_message('lateralPlanExt')
# Lane change logic
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
self.DH.update(car_state, lat_active, lane_change_prob, dragon_conf, md)
lateralPlanExt = plan_ext_send.lateralPlanExt
lateralPlanExt.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
lateralPlanExt.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
# Turn off lanes during lane change
if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.DH.lane_change_ll_prob
self.LP.rll_prob *= self.DH.lane_change_ll_prob
pm.send('lateralPlanExt', plan_ext_send)
# dynamic laneline/laneless logic
self.dp_lanelines_active = get_lane_laneless_mode(self.LP.lll_prob, self.LP.rll_prob, self.dp_lanelines_active)
def _get_laneless_laneline_d_path_xyz(self):
if self._dp_lat_lane_priority_mode and self.LP is not None:
# Turn off lanes during lane change
if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.DH.lane_change_ll_prob
self.LP.rll_prob *= self.DH.lane_change_ll_prob
# Calculate final driving path and set MPC costs
if self.dp_lanelines_active:
# decide what mode should we use
if (self.LP.lll_prob + self.LP.rll_prob)/2 < 0.3:
self._dp_lat_lane_priority_mode_active = False
if (self.LP.lll_prob + self.LP.rll_prob)/2 > 0.5:
self._dp_lat_lane_priority_mode_active = True
# perform reset mpc
if self._dp_lat_lane_priority_mode_active != self._dp_lat_lane_priority_mode_active_prev:
self.reset_mpc()
self._dp_lat_lane_priority_mode_active_prev = self._dp_lat_lane_priority_mode_active
# use default path if not active
if not self._dp_lat_lane_priority_mode_active:
return self.path_xyz
# use lane planner path
return self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz)
else:
return self.path_xyz
selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json
+1 -1
View File
@@ -431,7 +431,7 @@
],
"zu_e": []
},
"cython_include_dirs": "/usr/local/pyenv/versions/3.8.10/lib/python3.8/site-packages/numpy/core/include",
"cython_include_dirs": "/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"dims": {
"N": 12,
"nbu": 0,
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile
+1 -1
View File
@@ -154,7 +154,7 @@ ocp_cython_o: ocp_cython_c
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /usr/local/pyenv/versions/3.8.10/lib/python3.8/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.c
+40397
View File
File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More