Merge remote-tracking branch 'upstream/master' into cleanup-honda-cc
Shane Smiskol
commit
69412bf824
|
|
@ -9,7 +9,7 @@ assignees: ''
|
|||
**Checklist**
|
||||
|
||||
- [ ] added to README
|
||||
- [ ] test route added to [test_routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/test/test_models.py)
|
||||
- [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py)
|
||||
- [ ] route with openpilot:
|
||||
- [ ] route with stock system:
|
||||
- [ ] car harness used (if comma doesn't sell it, put N/A):
|
||||
|
|
|
|||
|
|
@ -22,8 +22,8 @@ Route: [a route with the bug fix]
|
|||
<!--- ***** Template: Car port *****
|
||||
|
||||
**Checklist**
|
||||
- [ ] added to README
|
||||
- [ ] test route added to [test_routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/test/test_routes.py)
|
||||
- [ ] added to docs/CARS.md
|
||||
- [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py)
|
||||
- [ ] route with openpilot:
|
||||
- [ ] route with stock system:
|
||||
|
||||
|
|
|
|||
|
|
@ -245,7 +245,6 @@ jobs:
|
|||
- name: Run unit tests
|
||||
run: |
|
||||
${{ env.RUN }} "scons -j$(nproc) --test && \
|
||||
coverage run selfdrive/test/test_fingerprints.py && \
|
||||
$UNIT_TEST common && \
|
||||
$UNIT_TEST opendbc/can && \
|
||||
$UNIT_TEST selfdrive/boardd && \
|
||||
|
|
@ -384,7 +383,7 @@ jobs:
|
|||
uses: actions/cache@v2
|
||||
with:
|
||||
path: /tmp/comma_download_cache
|
||||
key: car_models-${{ hashFiles('selfdrive/test/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }}
|
||||
key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/car/tests/routes.py') }}-${{ matrix.job }}
|
||||
- name: Cache scons
|
||||
id: scons-cache
|
||||
# TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged.
|
||||
|
|
@ -402,7 +401,7 @@ jobs:
|
|||
- name: Test car models
|
||||
run: |
|
||||
${{ env.RUN }} "scons -j$(nproc) --test && \
|
||||
FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \
|
||||
FILEREADER_CACHE=1 coverage run -m pytest selfdrive/car/tests/test_models.py && \
|
||||
coverage xml && \
|
||||
chmod -R 777 /tmp/comma_download_cache"
|
||||
env:
|
||||
|
|
|
|||
11
SConstruct
11
SConstruct
|
|
@ -89,7 +89,6 @@ if arch == "aarch64" or arch == "larch64":
|
|||
"/usr/local/lib",
|
||||
"/usr/lib",
|
||||
"/system/vendor/lib64",
|
||||
"/system/comma/usr/lib",
|
||||
f"#third_party/acados/{arch}/lib",
|
||||
]
|
||||
|
||||
|
|
@ -172,8 +171,8 @@ if arch != "Darwin":
|
|||
ldflags += ["-Wl,--as-needed", "-Wl,--no-undefined"]
|
||||
|
||||
# Enable swaglog include in submodules
|
||||
cflags += ["-DSWAGLOG"]
|
||||
cxxflags += ["-DSWAGLOG"]
|
||||
cflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
|
||||
cxxflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
|
||||
|
||||
env = Environment(
|
||||
ENV=lenv,
|
||||
|
|
@ -306,11 +305,11 @@ if arch == "Darwin":
|
|||
qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"]
|
||||
qt_env.AppendENVPath('PATH', os.path.join(qt_env['QTDIR'], "bin"))
|
||||
elif arch == "aarch64":
|
||||
qt_env['QTDIR'] = "/system/comma/usr"
|
||||
qt_env['QTDIR'] = "/usr"
|
||||
qt_dirs = [
|
||||
f"/system/comma/usr/include/qt",
|
||||
f"/usr/include/qt",
|
||||
]
|
||||
qt_dirs += [f"/system/comma/usr/include/qt/Qt{m}" for m in qt_modules]
|
||||
qt_dirs += [f"/usr/include/qt/Qt{m}" for m in qt_modules]
|
||||
|
||||
qt_libs = [f"Qt5{m}" for m in qt_modules]
|
||||
qt_libs += ['EGL', 'GLESv3', 'c++_shared']
|
||||
|
|
|
|||
2
cereal
2
cereal
|
|
@ -1 +1 @@
|
|||
Subproject commit 28d458a9af49b38bd0a9052f09fbe927324320fb
|
||||
Subproject commit 447d8b8a7a9c75e5f3478ac08622d6f1d2121528
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
import numpy as np
|
||||
|
||||
class Conversions:
|
||||
# Speed
|
||||
MPH_TO_KPH = 1.609344
|
||||
KPH_TO_MPH = 1. / MPH_TO_KPH
|
||||
MS_TO_KPH = 3.6
|
||||
KPH_TO_MS = 1. / MS_TO_KPH
|
||||
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
|
||||
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
|
||||
MS_TO_KNOTS = 1.9438
|
||||
KNOTS_TO_MS = 1. / MS_TO_KNOTS
|
||||
|
||||
# Angle
|
||||
DEG_TO_RAD = np.pi / 180.
|
||||
RAD_TO_DEG = 1. / DEG_TO_RAD
|
||||
|
||||
# Mass
|
||||
LB_TO_KG = 0.453592
|
||||
391
docs/CARS.md
391
docs/CARS.md
|
|
@ -1,185 +1,220 @@
|
|||
# Supported Cars
|
||||
|
||||
## comma.ai supported cars
|
||||
A supported vehicle is one that just works when you install openpilot on a compatible device. Every car performs differently with openpilot, but we aim for all supported cars to provide a solid highway experience in the US market.
|
||||
|
||||
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
|
||||
| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
|
||||
| Acura | ILX 2016-19 | AcuraWatch Plus | openpilot | 25mph<sup>1</sup> | 25mph |
|
||||
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Acura | RDX 2019-21 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Accord 2018-21 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Accord Hybrid 2018-21 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Civic Hatchback 2017-21 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | Civic Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph |
|
||||
| Honda | Civic Coupe 2019-20 | All | Stock | 0mph | 2mph<sup>2</sup> |
|
||||
| Honda | Civic Sedan 2016-18 | Honda Sensing | openpilot | 0mph | 12mph |
|
||||
| Honda | Civic Sedan 2019-20 | All | Stock | 0mph | 2mph<sup>2</sup> |
|
||||
| Honda | CR-V 2015-16 | Touring | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | CR-V 2017-21 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph |
|
||||
| Honda | e 2020 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | Freed 2020 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | HR-V 2019-20 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | Insight 2019-21 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Inspire 2018 | All | Stock | 0mph | 3mph |
|
||||
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 0mph |
|
||||
| Honda | Passport 2019-21 | All | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | Pilot 2016-21 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | Ridgeline 2017-21 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Hyundai | Palisade 2020-21 | All | Stock | 0mph | 0mph |
|
||||
| Hyundai | Sonata 2020-22 | All | Stock | 0mph | 0mph |
|
||||
| Lexus | CT Hybrid 2017-18 | LSS | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Lexus | ES 2019-21 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | ES Hybrid 2017-18 | LSS | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Lexus | ES Hybrid 2019-21 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph |
|
||||
| Lexus | NX 2018-2019 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Lexus | NX 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | NX Hybrid 2018-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Lexus | RC 2020 | All | Stock | 22mph | 0mph |
|
||||
| Lexus | RX 2016-18 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Lexus | RX 2020-21 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2020-21 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | UX Hybrid 2019-21 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Alphard 2019-20 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Avalon 2016-21 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
|
||||
| Toyota | Avalon 2022 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Avalon Hybrid 2019-21 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
|
||||
| Toyota | Camry 2018-20 | All | Stock | 0mph<sup>4</sup> | 0mph |
|
||||
| Toyota | Camry 2021-22 | All | openpilot | 0mph<sup>4</sup> | 0mph |
|
||||
| Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph<sup>4</sup> | 0mph |
|
||||
| Toyota | Camry Hybrid 2021-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | C-HR 2017-21 | All | Stock | 0mph | 0mph |
|
||||
| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017-19 | All | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
|
||||
| Toyota | Corolla 2020-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Corolla Hatchback 2019-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Corolla Hybrid 2020-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Highlander 2017-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander 2020-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2020-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Mirai 2021 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Prius 2016-20 | TSS-P | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2021-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Prius v 2017 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
|
||||
| Toyota | Prius Prime 2017-20 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2021-22 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Rav4 2016-18 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
|
||||
| Toyota | Rav4 2019-21 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2016-18 | TSS-P | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2019-21 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Sienna 2018-20 | All | Stock<sup>3</sup>| 0mph | 0mph |
|
||||
Cars are organized into three tiers:
|
||||
|
||||
<sup>1</sup>[Comma Pedal](https://github.com/commaai/openpilot/wiki/comma-pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).*** <br />
|
||||
- Gold - The best openpilot experience. Great highway driving with continual updates.
|
||||
- Silver - A solid highway experience, but is limited by stock longitudinal.
|
||||
- Bronze - A solid highway experience, but will have limited performance in stop-and-go. May have ACC and ALC speed limitations.
|
||||
|
||||
How We Rate The Cars
|
||||
---
|
||||
|
||||
### openpilot Adaptive Cruise Control (ACC)
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a> - openpilot is able to control the gas and brakes.
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a> - openpilot is able to control the gas and brakes with some restrictions.
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a> - The gas and brakes are controlled by the car's stock Adaptive Cruise Control (ACC) system.
|
||||
|
||||
### Stop and Go
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a> - Adaptive Cruise Control (ACC) operates down to 0 mph.
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a> - Adaptive Cruise Control (ACC) available only above certain speeds. See your car's manual for the minimum speed.
|
||||
|
||||
### Steer to 0
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a> - openpilot can control the steering wheel down to 0 mph.
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a> - No steering control below certain speeds.
|
||||
|
||||
### Steering Torque
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a> - Car has enough steering torque for comfortable highway driving.
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a> - Limited ability to make turns.
|
||||
|
||||
### Actively Maintained
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a> - Mainline software support, harness hardware sold by comma, lots of users, primary development target.
|
||||
- <a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a> - Low user count, community maintained, harness hardware not sold by comma.
|
||||
|
||||
**All supported cars can move between the tiers as support changes.**
|
||||
|
||||
## Gold Cars
|
||||
|
||||
|Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|
|
||||
|Genesis|G70 2020|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Palisade 2020-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Santa Fe 2019-20|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Sonata 2020-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Sonata Hybrid 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Niro Electric 2019-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Telluride 2020|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|ES 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|ES Hybrid 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|NX 2020|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|RX 2020-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|RX Hybrid 2020-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|UX Hybrid 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Alphard 2019-20|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Avalon 2022|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Camry 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>[<sup>4</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Camry Hybrid 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Corolla 2020-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Corolla Hatchback 2019-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Corolla Hybrid 2020-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Highlander 2020-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Highlander Hybrid 2020-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Mirai 2021|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Prius 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Prius Prime 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|RAV4 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|RAV4 Hybrid 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|
||||
## Silver Cars
|
||||
|
||||
|Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|
|
||||
|Audi|A3 2014-19|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Audi|Q2 2018|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Audi|Q3 2020-21|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Audi|S3 2015-17|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Genesis|G70 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Genesis|G80 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Elantra 2021-22|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Elantra Hybrid 2021|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Ioniq Electric 2020|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Ioniq Hybrid 2020-22|SCC + LFA|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Ioniq Plug-In Hybrid 2020-21|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Kona 2020|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Kona Electric 2018-19|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Kona Hybrid 2020|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Santa Fe 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Santa Fe Hybrid 2022|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Santa Fe Plug-In Hybrid 2022|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Sonata 2018-19|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Ceed 2019|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Forte 2018-21|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|K5 2021-22|SCC + LFA|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Niro Hybrid 2021-22|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Optima 2019|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Seltos 2021|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Sorento 2018-19|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Stinger 2018|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|CT Hybrid 2017-18|LSS|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|ES Hybrid 2017-18|LSS|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|NX 2018-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|NX Hybrid 2018-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|RX 2016-18|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|RX Hybrid 2016-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Mazda|CX-5 2022|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|SEAT|Ateca 2018|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|SEAT|Leon 2014-20|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Avalon 2019-21|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Avalon Hybrid 2019-21|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|C-HR 2017-21|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|C-HR Hybrid 2017-19|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Camry 2018-20|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>[<sup>4</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Camry Hybrid 2018-20|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>[<sup>4</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Highlander 2017-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Highlander Hybrid 2017-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|RAV4 Hybrid 2016-18|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Sienna 2018-20|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Arteon 2018, 2021[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Atlas 2018-19, 2022[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Golf 2015-20|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Golf Alltrack 2017-18|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Golf GTE 2016|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Golf GTI 2018-20|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Golf R 2016-19|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Golf SportWagen 2015|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Golf SportsVan 2016|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Jetta 2018-21|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Jetta GLI 2021|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Passat 2016-18[<sup>7</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Polo 2020|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|T-Cross 2021[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|T-Roc 2021[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Taos 2022[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Tiguan 2020-22[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Touran 2017|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|e-Golf 2014, 2019-20|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Škoda|Kamiq 2021[<sup>6</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Škoda|Karoq 2019|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Škoda|Kodiaq 2018-19|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Škoda|Octavia 2015, 2018-19|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Škoda|Octavia RS 2016|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Škoda|Scala 2020|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Škoda|Superb 2015-18|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|
||||
## Bronze Cars
|
||||
|
||||
|Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|
|
||||
|Acura|ILX 2016-19|AcuraWatch Plus|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Acura|RDX 2016-18|AcuraWatch Plus|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Acura|RDX 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Cadillac|Escalade ESV 2016[<sup>1</sup>](#Footnotes)|ACC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Chevrolet|Volt 2017-18[<sup>1</sup>](#Footnotes)|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Chrysler|Pacifica 2017-18|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Chrysler|Pacifica 2020|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Chrysler|Pacifica Hybrid 2019-21|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|GMC|Acadia 2018[<sup>1</sup>](#Footnotes)|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Genesis|G90 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|
|
||||
|Honda|Accord 2018-21|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Accord Hybrid 2018-21|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|CR-V 2015-16|Touring|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|CR-V 2017-21|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|CR-V Hybrid 2017-19|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Civic 2016-18|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Civic 2019-20|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>[<sup>2</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Civic Hatchback 2017-21|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Fit 2018-19|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Freed 2020|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|HR-V 2019-20|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Insight 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Inspire 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Odyssey 2018-20|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Passport 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Pilot 2016-21|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|Ridgeline 2017-21|Honda Sensing|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Honda|e 2020|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Elantra 2017-19|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Genesis 2015-16|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Ioniq Electric 2019|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Ioniq Hybrid 2017-19|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Hyundai|Veloster 2019-20|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Jeep|Grand Cherokee 2019-20|Adaptive Cruise|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Niro Plug-In Hybrid 2019|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Kia|Optima 2017|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|IS 2017-19|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Lexus|RC 2020|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Mazda|CX-9 2021|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Nissan|Altima 2019-20|ProPILOT|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Nissan|Leaf 2018-22|ProPILOT|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Nissan|Rogue 2018-20|ProPILOT|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Nissan|X-Trail 2017|ProPILOT|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Subaru|Ascent 2019|EyeSight|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Subaru|Crosstrek 2018-20|EyeSight|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Subaru|Forester 2019-21|EyeSight|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Subaru|Impreza 2017-19|EyeSight|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Avalon 2016-18|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Corolla 2017-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Prius 2016-20|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>5</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Prius Prime 2017-20|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>5</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Prius v 2017|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>5</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|RAV4 2016-18|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|California 2021[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Volkswagen|Caravelle 2020[<sup>8</sup>](#Footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|
||||
|
||||
## Footnotes
|
||||
<sup>1</sup>Requires an [OBD-II](https://comma.ai/shop/products/comma-car-harness) car harness and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB). <br />
|
||||
<sup>2</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
|
||||
<sup>3</sup>When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).*** <br />
|
||||
<sup>3</sup>When disconnecting the Driver Support Unit (DSU), openpilot Adaptive Cruise Control (ACC) will replace stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB). <br />
|
||||
<sup>4</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
|
||||
<sup>5</sup>An inaccurate steering wheel angle sensor makes precise control difficult. <br />
|
||||
<sup>6</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
|
||||
<sup>7</sup>Not including the USA/China market Passat, which is based on the (currently) unsupported PQ35/NMS platform. <br />
|
||||
<sup>8</sup>Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). For the newer design, in the interim, choose "VW J533 Development" from the vehicle drop-down for a harness that integrates at the CAN gateway inside the dashboard. <br />
|
||||
|
||||
## Community Maintained Cars and Features
|
||||
|
||||
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
|
||||
| ----------| --------------------------------| ------------------| -----------------| -------------------| -------------|
|
||||
| Audi | A3 2014-19 | ACC + Lane Assist | Stock | 0mph | 0mph |
|
||||
| Audi | A3 Sportback e-tron 2017-18 | ACC + Lane Assist | Stock | 0mph | 0mph |
|
||||
| Audi | Q2 2018 | ACC + Lane Assist | Stock | 0mph | 0mph |
|
||||
| Audi | Q3 2020-21 | ACC + Lane Assist | Stock | 0mph | 0mph |
|
||||
| Audi | S3 2015 | ACC + Lane Assist | Stock | 0mph | 0mph |
|
||||
| Cadillac | Escalade ESV 2016<sup>1</sup> | ACC + LKAS | openpilot | 0mph | 7mph |
|
||||
| Chevrolet | Volt 2017-18<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph |
|
||||
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica Hybrid 2019-21 | Adaptive Cruise | Stock | 0mph | 39mph |
|
||||
| Genesis | G70 2018 | All | Stock | 0mph | 0mph |
|
||||
| Genesis | G70 2020 | All | Stock | 0mph | 0mph |
|
||||
| Genesis | G80 2018 | All | Stock | 0mph | 0mph |
|
||||
| Genesis | G90 2018 | All | Stock | 0mph | 0mph |
|
||||
| GMC | Acadia 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
|
||||
| Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph |
|
||||
| Hyundai | Elantra 2021-22 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Elantra Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph |
|
||||
| Hyundai | Ioniq Electric 2019 | SCC + LKAS | Stock | 0mph | 32mph |
|
||||
| Hyundai | Ioniq Electric 2020 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Ioniq Hybrid 2017-19 | SCC + LKAS | Stock | 0mph | 32mph |
|
||||
| Hyundai | Ioniq Hybrid 2020-22 | SCC + LFA | Stock | 0mph | 0mph |
|
||||
| Hyundai | Ioniq PHEV 2020-21 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Kona EV 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Kona Hybrid 2020 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph |
|
||||
| Hyundai | Santa Fe 2021-22 | All | Stock | 0mph | 0mph |
|
||||
| Hyundai | Santa Fe Hybrid 2022 | All | Stock | 0mph | 0mph |
|
||||
| Hyundai | Santa Fe Plug-in Hybrid 2022 | All | Stock | 0mph | 0mph |
|
||||
| Hyundai | Sonata 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph |
|
||||
| Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph |
|
||||
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph |
|
||||
| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
|
||||
| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph |
|
||||
| Kia | Niro EV 2019-22 | All | Stock | 0mph | 0mph |
|
||||
| Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph |
|
||||
| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph |
|
||||
| Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Seltos 2021 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Kia | Telluride 2020 | SCC + LKAS | Stock | 0mph | 0mph |
|
||||
| Mazda | CX-5 2022 | All | Stock | 0mph | 0mph |
|
||||
| Mazda | CX-9 2021 | All | Stock | 0mph | 28mph |
|
||||
| Nissan | Altima 2019-20 | ProPILOT | Stock | 0mph | 0mph |
|
||||
| Nissan | Leaf 2018-22 | ProPILOT | Stock | 0mph | 0mph |
|
||||
| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph |
|
||||
| Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph |
|
||||
| SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| SEAT | Leon 2014-2020 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Škoda | Kamiq 2021<sup>2</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Škoda | Karoq 2019 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Škoda | Kodiaq 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Škoda | Octavia 2015, 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Škoda | Octavia RS 2016 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Arteon 2018, 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Atlas 2018-19, 2022<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Caravelle 2020<sup>4</sup> | Driver Assistance | Stock | 0mph | 32mph |
|
||||
| Volkswagen| California 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 32mph |
|
||||
| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf 2015-20 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf Alltrack 2017-18 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf GTE 2016 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf GTI 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf R 2016-19 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf SportsVan 2016 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf SportWagen 2015 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Passat 2016-18<sup>3</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Polo 2020 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| T-Cross 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| T-Roc 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Taos 2022<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Touran 2017 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
|
||||
<sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
|
||||
<sup>2</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
|
||||
<sup>3</sup>Not including the USA/China market Passat, which is based on the (currently) unsupported PQ35/NMS platform. <br />
|
||||
<sup>4</sup>Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering,
|
||||
remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). For the newer design,
|
||||
in the interim, choose "VW J533 Development" from the vehicle drop-down for a harness that integrates at the CAN gateway inside the dashboard.<br />
|
||||
Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them.
|
||||
|
||||
To promote a car from community maintained, it must meet a few requirements. We must own one from the brand, we must sell the harness for it, has full ISO26262 in both panda and openpilot, there must be a path forward for longitudinal control, it must have AEB still enabled, and it must support fingerprinting 2.0
|
||||
|
||||
Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
|
||||
## Community Maintained Cars
|
||||
Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
<svg width="24" height="24" viewBox="0 0 24 24" fill="none" xmlns="http://www.w3.org/2000/svg">
|
||||
<path d="M12.2584 16.8419L12 16.686L11.7416 16.8419L6.57613 19.9596L7.94693 14.0836L8.01549 13.7897L7.78739 13.5921L3.22308 9.63803L9.23227 9.12821L9.5327 9.10272L9.65036 8.82511L12 3.2813L14.3496 8.82511L14.4673 9.10272L14.7677 9.12821L20.7769 9.63803L16.2126 13.5921L15.9845 13.7897L16.0531 14.0836L17.4239 19.9596L12.2584 16.8419Z" stroke="#B7B7B7"/>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 457 B |
|
|
@ -0,0 +1,3 @@
|
|||
<svg width="24" height="24" viewBox="0 0 24 24" fill="none" xmlns="http://www.w3.org/2000/svg">
|
||||
<path d="M12.2584 16.8419L12 16.686L11.7416 16.8419L6.57613 19.9596L7.94693 14.0836L8.01549 13.7897L7.78739 13.5921L3.22308 9.63803L9.23227 9.12821L9.5327 9.10272L9.65036 8.82511L12 3.2813L14.3496 8.82511L14.4673 9.10272L14.7677 9.12821L20.7769 9.63803L16.2126 13.5921L15.9845 13.7897L16.0531 14.0836L17.4239 19.9596L12.2584 16.8419Z" fill="#F5C543" stroke="#F0A43B"/>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 472 B |
|
|
@ -0,0 +1,5 @@
|
|||
<svg width="24" height="24" viewBox="0 0 24 24" fill="none" xmlns="http://www.w3.org/2000/svg">
|
||||
<path d="M12 2V17.27L5.82 21L7.46 13.97L2 9.24L9.19 8.63L12 2Z" fill="#F5C543"/>
|
||||
<path d="M12.2584 16.8419L12 16.686L11.7416 16.8419L6.57613 19.9596L7.94693 14.0836L8.01549 13.7897L7.78739 13.5921L3.22308 9.63803L9.23227 9.12821L9.5327 9.10272L9.65036 8.82511L12 3.2813L14.3496 8.82511L14.4673 9.10272L14.7677 9.12821L20.7769 9.63803L16.2126 13.5921L15.9845 13.7897L16.0531 14.0836L17.4239 19.9596L12.2584 16.8419Z" stroke="#B7B7B7"/>
|
||||
<path d="M7.33226 18.9192L11 16.7055L12 16.1V17.27L5.82 21L7.46 13.97L2 9.24L9.19 8.63L12 2V4.6L11 6.92203L9.8754 9.57544L4.44617 10.0361L8.57098 13.6094L7.33226 18.9192Z" fill="#F0A43B"/>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 727 B |
|
|
@ -1 +1 @@
|
|||
Subproject commit dec99a0f77328f7a9f104020d98d7227345d1288
|
||||
Subproject commit 94066cb2b4ad5f2bcb8e33ce02fe15a73a00aace
|
||||
BIN
models/big_supercombo.dlc (Stored with Git LFS)
BIN
models/big_supercombo.dlc (Stored with Git LFS)
Binary file not shown.
BIN
models/big_supercombo.onnx (Stored with Git LFS)
BIN
models/big_supercombo.onnx (Stored with Git LFS)
Binary file not shown.
|
|
@ -1,2 +1,2 @@
|
|||
4e19be90-bd5b-485d-b79a-2462f7f1b49e
|
||||
08f7ec37b78228cd1cb750b6ddb9c6ba1769e911
|
||||
0ff292a8-3a9c-47e7-9134-2c3b0f69a1fe
|
||||
d7c2883ee58d7b757e588bdf13ec45891caf397c
|
||||
BIN
models/dmonitoring_model.onnx (Stored with Git LFS)
BIN
models/dmonitoring_model.onnx (Stored with Git LFS)
Binary file not shown.
BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
Binary file not shown.
BIN
models/supercombo.dlc (Stored with Git LFS)
BIN
models/supercombo.dlc (Stored with Git LFS)
Binary file not shown.
BIN
models/supercombo.onnx (Stored with Git LFS)
BIN
models/supercombo.onnx (Stored with Git LFS)
Binary file not shown.
2
opendbc
2
opendbc
|
|
@ -1 +1 @@
|
|||
Subproject commit f475c355a65eb8050730fe8a1af5ac4b319e1d7f
|
||||
Subproject commit eab58f6f8f41f255920365ab1fd9c75e312a6869
|
||||
2
panda
2
panda
|
|
@ -1 +1 @@
|
|||
Subproject commit 51ccb9fbd266796e1bf6ffda8b93c4119ab09ff4
|
||||
Subproject commit 499906f3244712c1edd7f1cc8e92112d11f2b505
|
||||
|
|
@ -5,6 +5,9 @@
|
|||
"acados_include_path": [
|
||||
"str"
|
||||
],
|
||||
"cython_include_dirs": [
|
||||
"str"
|
||||
],
|
||||
"model": {
|
||||
"name" : [
|
||||
"str"
|
||||
|
|
@ -23,7 +26,15 @@
|
|||
],
|
||||
"dyn_disc_fun" : [
|
||||
"str"
|
||||
]
|
||||
],
|
||||
"gnsf" : {
|
||||
"nontrivial_f_LO": [
|
||||
"int"
|
||||
],
|
||||
"purely_linear": [
|
||||
"int"
|
||||
]
|
||||
}
|
||||
},
|
||||
"parameter_values": [
|
||||
"ndarray",
|
||||
|
|
@ -693,6 +704,18 @@
|
|||
"alpha_reduction": [
|
||||
"float"
|
||||
],
|
||||
"line_search_use_sufficient_descent": [
|
||||
"int"
|
||||
],
|
||||
"globalization_use_SOC": [
|
||||
"int"
|
||||
],
|
||||
"full_step_dual": [
|
||||
"int"
|
||||
],
|
||||
"eps_sufficient_descent": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_num_stages": [
|
||||
"ndarray",
|
||||
[
|
||||
|
|
|
|||
|
|
@ -78,6 +78,13 @@ class AcadosModel():
|
|||
self.dyn_disc_fun_jac = None #: name of function discrete dyanamics + jacobian; Default: :code:`None`
|
||||
self.dyn_disc_fun = None #: name of function discrete dyanamics; Default: :code:`None`
|
||||
|
||||
# for GNSF models
|
||||
self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0}
|
||||
"""
|
||||
dictionary containing information on GNSF structure needed when rendering templates.
|
||||
Contains integers `nontrivial_f_LO`, `purely_linear`.
|
||||
"""
|
||||
|
||||
## for OCP
|
||||
# constraints
|
||||
self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
|
||||
|
|
|
|||
|
|
@ -270,28 +270,28 @@ class AcadosOcpDims:
|
|||
|
||||
@nx.setter
|
||||
def nx(self, nx):
|
||||
if type(nx) == int and nx > 0:
|
||||
if isinstance(nx, int) and nx > 0:
|
||||
self.__nx = nx
|
||||
else:
|
||||
raise Exception('Invalid nx value, expected positive integer. Exiting.')
|
||||
|
||||
@nz.setter
|
||||
def nz(self, nz):
|
||||
if type(nz) == int and nz > -1:
|
||||
if isinstance(nz, int) and nz > -1:
|
||||
self.__nz = nz
|
||||
else:
|
||||
raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nu.setter
|
||||
def nu(self, nu):
|
||||
if type(nu) == int and nu > -1:
|
||||
if isinstance(nu, int) and nu > -1:
|
||||
self.__nu = nu
|
||||
else:
|
||||
raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@np.setter
|
||||
def np(self, np):
|
||||
if type(np) == int and np > -1:
|
||||
if isinstance(np, int) and np > -1:
|
||||
self.__np = np
|
||||
else:
|
||||
raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
|
||||
|
|
@ -312,49 +312,49 @@ class AcadosOcpDims:
|
|||
|
||||
@ny_e.setter
|
||||
def ny_e(self, ny_e):
|
||||
if type(ny_e) == int and ny_e > -1:
|
||||
if isinstance(ny_e, int) and ny_e > -1:
|
||||
self.__ny_e = ny_e
|
||||
else:
|
||||
raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nr.setter
|
||||
def nr(self, nr):
|
||||
if type(nr) == int and nr > -1:
|
||||
if isinstance(nr, int) and nr > -1:
|
||||
self.__nr = nr
|
||||
else:
|
||||
raise Exception('Invalid nr value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nr_e.setter
|
||||
def nr_e(self, nr_e):
|
||||
if type(nr_e) == int and nr_e > -1:
|
||||
if isinstance(nr_e, int) and nr_e > -1:
|
||||
self.__nr_e = nr_e
|
||||
else:
|
||||
raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nh.setter
|
||||
def nh(self, nh):
|
||||
if type(nh) == int and nh > -1:
|
||||
if isinstance(nh, int) and nh > -1:
|
||||
self.__nh = nh
|
||||
else:
|
||||
raise Exception('Invalid nh value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nh_e.setter
|
||||
def nh_e(self, nh_e):
|
||||
if type(nh_e) == int and nh_e > -1:
|
||||
if isinstance(nh_e, int) and nh_e > -1:
|
||||
self.__nh_e = nh_e
|
||||
else:
|
||||
raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nphi.setter
|
||||
def nphi(self, nphi):
|
||||
if type(nphi) == int and nphi > -1:
|
||||
if isinstance(nphi, int) and nphi > -1:
|
||||
self.__nphi = nphi
|
||||
else:
|
||||
raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nphi_e.setter
|
||||
def nphi_e(self, nphi_e):
|
||||
if type(nphi_e) == int and nphi_e > -1:
|
||||
if isinstance(nphi_e, int) and nphi_e > -1:
|
||||
self.__nphi_e = nphi_e
|
||||
else:
|
||||
raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.')
|
||||
|
|
@ -375,42 +375,42 @@ class AcadosOcpDims:
|
|||
|
||||
@nbx_0.setter
|
||||
def nbx_0(self, nbx_0):
|
||||
if type(nbx_0) == int and nbx_0 > -1:
|
||||
if isinstance(nbx_0, int) and nbx_0 > -1:
|
||||
self.__nbx_0 = nbx_0
|
||||
else:
|
||||
raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nbx_e.setter
|
||||
def nbx_e(self, nbx_e):
|
||||
if type(nbx_e) == int and nbx_e > -1:
|
||||
if isinstance(nbx_e, int) and nbx_e > -1:
|
||||
self.__nbx_e = nbx_e
|
||||
else:
|
||||
raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nbu.setter
|
||||
def nbu(self, nbu):
|
||||
if type(nbu) == int and nbu > -1:
|
||||
if isinstance(nbu, int) and nbu > -1:
|
||||
self.__nbu = nbu
|
||||
else:
|
||||
raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nsbx.setter
|
||||
def nsbx(self, nsbx):
|
||||
if type(nsbx) == int and nsbx > -1:
|
||||
if isinstance(nsbx, int) and nsbx > -1:
|
||||
self.__nsbx = nsbx
|
||||
else:
|
||||
raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nsbx_e.setter
|
||||
def nsbx_e(self, nsbx_e):
|
||||
if type(nsbx_e) == int and nsbx_e > -1:
|
||||
if isinstance(nsbx_e, int) and nsbx_e > -1:
|
||||
self.__nsbx_e = nsbx_e
|
||||
else:
|
||||
raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nsbu.setter
|
||||
def nsbu(self, nsbu):
|
||||
if type(nsbu) == int and nsbu > -1:
|
||||
if isinstance(nsbu, int) and nsbu > -1:
|
||||
self.__nsbu = nsbu
|
||||
else:
|
||||
raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.')
|
||||
|
|
@ -1592,14 +1592,14 @@ class AcadosOcpConstraints:
|
|||
# initial x
|
||||
@lbx_0.setter
|
||||
def lbx_0(self, lbx_0):
|
||||
if type(lbx_0) == np.ndarray:
|
||||
if isinstance(lbx_0, np.ndarray):
|
||||
self.__lbx_0 = lbx_0
|
||||
else:
|
||||
raise Exception('Invalid lbx_0 value. Exiting.')
|
||||
|
||||
@ubx_0.setter
|
||||
def ubx_0(self, ubx_0):
|
||||
if type(ubx_0) == np.ndarray:
|
||||
if isinstance(ubx_0, np.ndarray):
|
||||
self.__ubx_0 = ubx_0
|
||||
else:
|
||||
raise Exception('Invalid ubx_0 value. Exiting.')
|
||||
|
|
@ -1613,7 +1613,7 @@ class AcadosOcpConstraints:
|
|||
|
||||
@Jbx_0.setter
|
||||
def Jbx_0(self, Jbx_0):
|
||||
if type(Jbx_0) == np.ndarray:
|
||||
if isinstance(Jbx_0, np.ndarray):
|
||||
self.__idxbx_0 = J_to_idx(Jbx_0)
|
||||
else:
|
||||
raise Exception('Invalid Jbx_0 value. Exiting.')
|
||||
|
|
@ -1639,28 +1639,28 @@ class AcadosOcpConstraints:
|
|||
# bounds on x
|
||||
@lbx.setter
|
||||
def lbx(self, lbx):
|
||||
if type(lbx) == np.ndarray:
|
||||
if isinstance(lbx, np.ndarray):
|
||||
self.__lbx = lbx
|
||||
else:
|
||||
raise Exception('Invalid lbx value. Exiting.')
|
||||
|
||||
@ubx.setter
|
||||
def ubx(self, ubx):
|
||||
if type(ubx) == np.ndarray:
|
||||
if isinstance(ubx, np.ndarray):
|
||||
self.__ubx = ubx
|
||||
else:
|
||||
raise Exception('Invalid ubx value. Exiting.')
|
||||
|
||||
@idxbx.setter
|
||||
def idxbx(self, idxbx):
|
||||
if type(idxbx) == np.ndarray:
|
||||
if isinstance(idxbx, np.ndarray):
|
||||
self.__idxbx = idxbx
|
||||
else:
|
||||
raise Exception('Invalid idxbx value. Exiting.')
|
||||
|
||||
@Jbx.setter
|
||||
def Jbx(self, Jbx):
|
||||
if type(Jbx) == np.ndarray:
|
||||
if isinstance(Jbx, np.ndarray):
|
||||
self.__idxbx = J_to_idx(Jbx)
|
||||
else:
|
||||
raise Exception('Invalid Jbx value. Exiting.')
|
||||
|
|
@ -1668,28 +1668,28 @@ class AcadosOcpConstraints:
|
|||
# bounds on u
|
||||
@lbu.setter
|
||||
def lbu(self, lbu):
|
||||
if type(lbu) == np.ndarray:
|
||||
if isinstance(lbu, np.ndarray):
|
||||
self.__lbu = lbu
|
||||
else:
|
||||
raise Exception('Invalid lbu value. Exiting.')
|
||||
|
||||
@ubu.setter
|
||||
def ubu(self, ubu):
|
||||
if type(ubu) == np.ndarray:
|
||||
if isinstance(ubu, np.ndarray):
|
||||
self.__ubu = ubu
|
||||
else:
|
||||
raise Exception('Invalid ubu value. Exiting.')
|
||||
|
||||
@idxbu.setter
|
||||
def idxbu(self, idxbu):
|
||||
if type(idxbu) == np.ndarray:
|
||||
if isinstance(idxbu, np.ndarray):
|
||||
self.__idxbu = idxbu
|
||||
else:
|
||||
raise Exception('Invalid idxbu value. Exiting.')
|
||||
|
||||
@Jbu.setter
|
||||
def Jbu(self, Jbu):
|
||||
if type(Jbu) == np.ndarray:
|
||||
if isinstance(Jbu, np.ndarray):
|
||||
self.__idxbu = J_to_idx(Jbu)
|
||||
else:
|
||||
raise Exception('Invalid Jbu value. Exiting.')
|
||||
|
|
@ -1697,28 +1697,28 @@ class AcadosOcpConstraints:
|
|||
# bounds on x at shooting node N
|
||||
@lbx_e.setter
|
||||
def lbx_e(self, lbx_e):
|
||||
if type(lbx_e) == np.ndarray:
|
||||
if isinstance(lbx_e, np.ndarray):
|
||||
self.__lbx_e = lbx_e
|
||||
else:
|
||||
raise Exception('Invalid lbx_e value. Exiting.')
|
||||
|
||||
@ubx_e.setter
|
||||
def ubx_e(self, ubx_e):
|
||||
if type(ubx_e) == np.ndarray:
|
||||
if isinstance(ubx_e, np.ndarray):
|
||||
self.__ubx_e = ubx_e
|
||||
else:
|
||||
raise Exception('Invalid ubx_e value. Exiting.')
|
||||
|
||||
@idxbx_e.setter
|
||||
def idxbx_e(self, idxbx_e):
|
||||
if type(idxbx_e) == np.ndarray:
|
||||
if isinstance(idxbx_e, np.ndarray):
|
||||
self.__idxbx_e = idxbx_e
|
||||
else:
|
||||
raise Exception('Invalid idxbx_e value. Exiting.')
|
||||
|
||||
@Jbx_e.setter
|
||||
def Jbx_e(self, Jbx_e):
|
||||
if type(Jbx_e) == np.ndarray:
|
||||
if isinstance(Jbx_e, np.ndarray):
|
||||
self.__idxbx_e = J_to_idx(Jbx_e)
|
||||
else:
|
||||
raise Exception('Invalid Jbx_e value. Exiting.')
|
||||
|
|
@ -1742,14 +1742,14 @@ class AcadosOcpConstraints:
|
|||
|
||||
@lg.setter
|
||||
def lg(self, lg):
|
||||
if type(lg) == np.ndarray:
|
||||
if isinstance(lg, np.ndarray):
|
||||
self.__lg = lg
|
||||
else:
|
||||
raise Exception('Invalid lg value. Exiting.')
|
||||
|
||||
@ug.setter
|
||||
def ug(self, ug):
|
||||
if type(ug) == np.ndarray:
|
||||
if isinstance(ug, np.ndarray):
|
||||
self.__ug = ug
|
||||
else:
|
||||
raise Exception('Invalid ug value. Exiting.')
|
||||
|
|
@ -1765,14 +1765,14 @@ class AcadosOcpConstraints:
|
|||
|
||||
@lg_e.setter
|
||||
def lg_e(self, lg_e):
|
||||
if type(lg_e) == np.ndarray:
|
||||
if isinstance(lg_e, np.ndarray):
|
||||
self.__lg_e = lg_e
|
||||
else:
|
||||
raise Exception('Invalid lg_e value. Exiting.')
|
||||
|
||||
@ug_e.setter
|
||||
def ug_e(self, ug_e):
|
||||
if type(ug_e) == np.ndarray:
|
||||
if isinstance(ug_e, np.ndarray):
|
||||
self.__ug_e = ug_e
|
||||
else:
|
||||
raise Exception('Invalid ug_e value. Exiting.')
|
||||
|
|
@ -1780,14 +1780,14 @@ class AcadosOcpConstraints:
|
|||
# nonlinear constraints
|
||||
@lh.setter
|
||||
def lh(self, lh):
|
||||
if type(lh) == np.ndarray:
|
||||
if isinstance(lh, np.ndarray):
|
||||
self.__lh = lh
|
||||
else:
|
||||
raise Exception('Invalid lh value. Exiting.')
|
||||
|
||||
@uh.setter
|
||||
def uh(self, uh):
|
||||
if type(uh) == np.ndarray:
|
||||
if isinstance(uh, np.ndarray):
|
||||
self.__uh = uh
|
||||
else:
|
||||
raise Exception('Invalid uh value. Exiting.')
|
||||
|
|
@ -1795,14 +1795,14 @@ class AcadosOcpConstraints:
|
|||
# convex-over-nonlinear constraints
|
||||
@lphi.setter
|
||||
def lphi(self, lphi):
|
||||
if type(lphi) == np.ndarray:
|
||||
if isinstance(lphi, np.ndarray):
|
||||
self.__lphi = lphi
|
||||
else:
|
||||
raise Exception('Invalid lphi value. Exiting.')
|
||||
|
||||
@uphi.setter
|
||||
def uphi(self, uphi):
|
||||
if type(uphi) == np.ndarray:
|
||||
if isinstance(uphi, np.ndarray):
|
||||
self.__uphi = uphi
|
||||
else:
|
||||
raise Exception('Invalid uphi value. Exiting.')
|
||||
|
|
@ -1810,14 +1810,14 @@ class AcadosOcpConstraints:
|
|||
# nonlinear constraints at shooting node N
|
||||
@lh_e.setter
|
||||
def lh_e(self, lh_e):
|
||||
if type(lh_e) == np.ndarray:
|
||||
if isinstance(lh_e, np.ndarray):
|
||||
self.__lh_e = lh_e
|
||||
else:
|
||||
raise Exception('Invalid lh_e value. Exiting.')
|
||||
|
||||
@uh_e.setter
|
||||
def uh_e(self, uh_e):
|
||||
if type(uh_e) == np.ndarray:
|
||||
if isinstance(uh_e, np.ndarray):
|
||||
self.__uh_e = uh_e
|
||||
else:
|
||||
raise Exception('Invalid uh_e value. Exiting.')
|
||||
|
|
@ -1825,14 +1825,14 @@ class AcadosOcpConstraints:
|
|||
# convex-over-nonlinear constraints at shooting node N
|
||||
@lphi_e.setter
|
||||
def lphi_e(self, lphi_e):
|
||||
if type(lphi_e) == np.ndarray:
|
||||
if isinstance(lphi_e, np.ndarray):
|
||||
self.__lphi_e = lphi_e
|
||||
else:
|
||||
raise Exception('Invalid lphi_e value. Exiting.')
|
||||
|
||||
@uphi_e.setter
|
||||
def uphi_e(self, uphi_e):
|
||||
if type(uphi_e) == np.ndarray:
|
||||
if isinstance(uphi_e, np.ndarray):
|
||||
self.__uphi_e = uphi_e
|
||||
else:
|
||||
raise Exception('Invalid uphi_e value. Exiting.')
|
||||
|
|
@ -1841,21 +1841,21 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on x
|
||||
@lsbx.setter
|
||||
def lsbx(self, lsbx):
|
||||
if type(lsbx) == np.ndarray:
|
||||
if isinstance(lsbx, np.ndarray):
|
||||
self.__lsbx = lsbx
|
||||
else:
|
||||
raise Exception('Invalid lsbx value. Exiting.')
|
||||
|
||||
@usbx.setter
|
||||
def usbx(self, usbx):
|
||||
if type(usbx) == np.ndarray:
|
||||
if isinstance(usbx, np.ndarray):
|
||||
self.__usbx = usbx
|
||||
else:
|
||||
raise Exception('Invalid usbx value. Exiting.')
|
||||
|
||||
@idxsbx.setter
|
||||
def idxsbx(self, idxsbx):
|
||||
if type(idxsbx) == np.ndarray:
|
||||
if isinstance(idxsbx, np.ndarray):
|
||||
self.__idxsbx = idxsbx
|
||||
else:
|
||||
raise Exception('Invalid idxsbx value. Exiting.')
|
||||
|
|
@ -1870,28 +1870,28 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on u
|
||||
@lsbu.setter
|
||||
def lsbu(self, lsbu):
|
||||
if type(lsbu) == np.ndarray:
|
||||
if isinstance(lsbu, np.ndarray):
|
||||
self.__lsbu = lsbu
|
||||
else:
|
||||
raise Exception('Invalid lsbu value. Exiting.')
|
||||
|
||||
@usbu.setter
|
||||
def usbu(self, usbu):
|
||||
if type(usbu) == np.ndarray:
|
||||
if isinstance(usbu, np.ndarray):
|
||||
self.__usbu = usbu
|
||||
else:
|
||||
raise Exception('Invalid usbu value. Exiting.')
|
||||
|
||||
@idxsbu.setter
|
||||
def idxsbu(self, idxsbu):
|
||||
if type(idxsbu) == np.ndarray:
|
||||
if isinstance(idxsbu, np.ndarray):
|
||||
self.__idxsbu = idxsbu
|
||||
else:
|
||||
raise Exception('Invalid idxsbu value. Exiting.')
|
||||
|
||||
@Jsbu.setter
|
||||
def Jsbu(self, Jsbu):
|
||||
if type(Jsbu) == np.ndarray:
|
||||
if isinstance(Jsbu, np.ndarray):
|
||||
self.__idxsbu = J_to_idx_slack(Jsbu)
|
||||
else:
|
||||
raise Exception('Invalid Jsbu value. Exiting.')
|
||||
|
|
@ -1899,28 +1899,28 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on x at shooting node N
|
||||
@lsbx_e.setter
|
||||
def lsbx_e(self, lsbx_e):
|
||||
if type(lsbx_e) == np.ndarray:
|
||||
if isinstance(lsbx_e, np.ndarray):
|
||||
self.__lsbx_e = lsbx_e
|
||||
else:
|
||||
raise Exception('Invalid lsbx_e value. Exiting.')
|
||||
|
||||
@usbx_e.setter
|
||||
def usbx_e(self, usbx_e):
|
||||
if type(usbx_e) == np.ndarray:
|
||||
if isinstance(usbx_e, np.ndarray):
|
||||
self.__usbx_e = usbx_e
|
||||
else:
|
||||
raise Exception('Invalid usbx_e value. Exiting.')
|
||||
|
||||
@idxsbx_e.setter
|
||||
def idxsbx_e(self, idxsbx_e):
|
||||
if type(idxsbx_e) == np.ndarray:
|
||||
if isinstance(idxsbx_e, np.ndarray):
|
||||
self.__idxsbx_e = idxsbx_e
|
||||
else:
|
||||
raise Exception('Invalid idxsbx_e value. Exiting.')
|
||||
|
||||
@Jsbx_e.setter
|
||||
def Jsbx_e(self, Jsbx_e):
|
||||
if type(Jsbx_e) == np.ndarray:
|
||||
if isinstance(Jsbx_e, np.ndarray):
|
||||
self.__idxsbx_e = J_to_idx_slack(Jsbx_e)
|
||||
else:
|
||||
raise Exception('Invalid Jsbx_e value. Exiting.')
|
||||
|
|
@ -1959,21 +1959,21 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on nonlinear constraints
|
||||
@lsh.setter
|
||||
def lsh(self, lsh):
|
||||
if type(lsh) == np.ndarray:
|
||||
if isinstance(lsh, np.ndarray):
|
||||
self.__lsh = lsh
|
||||
else:
|
||||
raise Exception('Invalid lsh value. Exiting.')
|
||||
|
||||
@ush.setter
|
||||
def ush(self, ush):
|
||||
if type(ush) == np.ndarray:
|
||||
if isinstance(ush, np.ndarray):
|
||||
self.__ush = ush
|
||||
else:
|
||||
raise Exception('Invalid ush value. Exiting.')
|
||||
|
||||
@idxsh.setter
|
||||
def idxsh(self, idxsh):
|
||||
if type(idxsh) == np.ndarray:
|
||||
if isinstance(idxsh, np.ndarray):
|
||||
self.__idxsh = idxsh
|
||||
else:
|
||||
raise Exception('Invalid idxsh value. Exiting.')
|
||||
|
|
@ -1989,21 +1989,21 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on convex-over-nonlinear constraints
|
||||
@lsphi.setter
|
||||
def lsphi(self, lsphi):
|
||||
if type(lsphi) == np.ndarray:
|
||||
if isinstance(lsphi, np.ndarray):
|
||||
self.__lsphi = lsphi
|
||||
else:
|
||||
raise Exception('Invalid lsphi value. Exiting.')
|
||||
|
||||
@usphi.setter
|
||||
def usphi(self, usphi):
|
||||
if type(usphi) == np.ndarray:
|
||||
if isinstance(usphi, np.ndarray):
|
||||
self.__usphi = usphi
|
||||
else:
|
||||
raise Exception('Invalid usphi value. Exiting.')
|
||||
|
||||
@idxsphi.setter
|
||||
def idxsphi(self, idxsphi):
|
||||
if type(idxsphi) == np.ndarray:
|
||||
if isinstance(idxsphi, np.ndarray):
|
||||
self.__idxsphi = idxsphi
|
||||
else:
|
||||
raise Exception('Invalid idxsphi value. Exiting.')
|
||||
|
|
@ -2151,6 +2151,10 @@ class AcadosOcpOptions:
|
|||
self.__ext_cost_num_hess = 0
|
||||
self.__alpha_min = 0.05
|
||||
self.__alpha_reduction = 0.7
|
||||
self.__line_search_use_sufficient_descent = 0
|
||||
self.__globalization_use_SOC = 0
|
||||
self.__full_step_dual = 0
|
||||
self.__eps_sufficient_descent = 1e-4
|
||||
|
||||
|
||||
@property
|
||||
|
|
@ -2367,6 +2371,43 @@ class AcadosOcpOptions:
|
|||
"""Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7."""
|
||||
return self.__alpha_reduction
|
||||
|
||||
@property
|
||||
def line_search_use_sufficient_descent(self):
|
||||
"""
|
||||
Determines if sufficient descent (Armijo) condition is used in line search.
|
||||
Type: int; 0 or 1;
|
||||
default: 0.
|
||||
"""
|
||||
return self.__line_search_use_sufficient_descent
|
||||
|
||||
@property
|
||||
def eps_sufficient_descent(self):
|
||||
"""
|
||||
Factor for sufficient descent (Armijo) conditon, see line_search_use_sufficient_descent.
|
||||
Type: float,
|
||||
default: 1e-4.
|
||||
"""
|
||||
return self.__eps_sufficient_descent
|
||||
|
||||
@property
|
||||
def globalization_use_SOC(self):
|
||||
"""
|
||||
Determines if second order correction (SOC) is done when using MERIT_BACKTRACKING.
|
||||
SOC is done if preliminary line search does not return full step.
|
||||
Type: int; 0 or 1;
|
||||
default: 0.
|
||||
"""
|
||||
return self.__globalization_use_SOC
|
||||
|
||||
@property
|
||||
def full_step_dual(self):
|
||||
"""
|
||||
Determines if dual variables are updated with full steps (alpha=1.0) when primal variables are updated with smaller step.
|
||||
Type: int; 0 or 1;
|
||||
default: 0.
|
||||
"""
|
||||
return self.__full_step_dual
|
||||
|
||||
@property
|
||||
def nlp_solver_tol_ineq(self):
|
||||
"""NLP solver inequality tolerance"""
|
||||
|
|
@ -2524,12 +2565,23 @@ class AcadosOcpOptions:
|
|||
|
||||
@time_steps.setter
|
||||
def time_steps(self, time_steps):
|
||||
self.__time_steps = time_steps
|
||||
if isinstance(time_steps, np.ndarray):
|
||||
if len(time_steps.shape) == 1:
|
||||
self.__time_steps = time_steps
|
||||
else:
|
||||
raise Exception('Invalid time_steps, expected np.ndarray of shape (N,).')
|
||||
else:
|
||||
raise Exception('Invalid time_steps, expected np.ndarray.')
|
||||
|
||||
@shooting_nodes.setter
|
||||
def shooting_nodes(self, shooting_nodes):
|
||||
self.__shooting_nodes = shooting_nodes
|
||||
|
||||
if isinstance(shooting_nodes, np.ndarray):
|
||||
if len(shooting_nodes.shape) == 1:
|
||||
self.__shooting_nodes = shooting_nodes
|
||||
else:
|
||||
raise Exception('Invalid shooting_nodes, expected np.ndarray of shape (N+1,).')
|
||||
else:
|
||||
raise Exception('Invalid shooting_nodes, expected np.ndarray.')
|
||||
|
||||
@Tsim.setter
|
||||
def Tsim(self, Tsim):
|
||||
|
|
@ -2537,7 +2589,12 @@ class AcadosOcpOptions:
|
|||
|
||||
@globalization.setter
|
||||
def globalization(self, globalization):
|
||||
self.__globalization = globalization
|
||||
globalization_types = ('MERIT_BACKTRACKING', 'FIXED_STEP')
|
||||
if globalization in globalization_types:
|
||||
self.__globalization = globalization
|
||||
else:
|
||||
raise Exception('Invalid globalization value. Possible values are:\n\n' \
|
||||
+ ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.')
|
||||
|
||||
@alpha_min.setter
|
||||
def alpha_min(self, alpha_min):
|
||||
|
|
@ -2547,10 +2604,38 @@ class AcadosOcpOptions:
|
|||
def alpha_reduction(self, alpha_reduction):
|
||||
self.__alpha_reduction = alpha_reduction
|
||||
|
||||
@line_search_use_sufficient_descent.setter
|
||||
def line_search_use_sufficient_descent(self, line_search_use_sufficient_descent):
|
||||
if line_search_use_sufficient_descent in [0, 1]:
|
||||
self.__line_search_use_sufficient_descent = line_search_use_sufficient_descent
|
||||
else:
|
||||
raise Exception(f'Invalid value for line_search_use_sufficient_descent. Possible values are 0, 1, got {line_search_use_sufficient_descent}')
|
||||
|
||||
@globalization_use_SOC.setter
|
||||
def globalization_use_SOC(self, globalization_use_SOC):
|
||||
if globalization_use_SOC in [0, 1]:
|
||||
self.__globalization_use_SOC = globalization_use_SOC
|
||||
else:
|
||||
raise Exception(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}')
|
||||
|
||||
@full_step_dual.setter
|
||||
def full_step_dual(self, full_step_dual):
|
||||
if full_step_dual in [0, 1]:
|
||||
self.__full_step_dual = full_step_dual
|
||||
else:
|
||||
raise Exception(f'Invalid value for full_step_dual. Possible values are 0, 1, got {full_step_dual}')
|
||||
|
||||
@eps_sufficient_descent.setter
|
||||
def eps_sufficient_descent(self, eps_sufficient_descent):
|
||||
if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0:
|
||||
self.__eps_sufficient_descent = eps_sufficient_descent
|
||||
else:
|
||||
raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting')
|
||||
|
||||
@sim_method_num_stages.setter
|
||||
def sim_method_num_stages(self, sim_method_num_stages):
|
||||
|
||||
# if type(sim_method_num_stages) == int:
|
||||
# if isinstance(sim_method_num_stages, int):
|
||||
# self.__sim_method_num_stages = sim_method_num_stages
|
||||
# else:
|
||||
# raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.')
|
||||
|
|
@ -2560,7 +2645,7 @@ class AcadosOcpOptions:
|
|||
@sim_method_num_steps.setter
|
||||
def sim_method_num_steps(self, sim_method_num_steps):
|
||||
|
||||
# if type(sim_method_num_steps) == int:
|
||||
# if isinstance(sim_method_num_steps, int):
|
||||
# self.__sim_method_num_steps = sim_method_num_steps
|
||||
# else:
|
||||
# raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.')
|
||||
|
|
@ -2570,7 +2655,7 @@ class AcadosOcpOptions:
|
|||
@sim_method_newton_iter.setter
|
||||
def sim_method_newton_iter(self, sim_method_newton_iter):
|
||||
|
||||
if type(sim_method_newton_iter) == int:
|
||||
if isinstance(sim_method_newton_iter, int):
|
||||
self.__sim_method_newton_iter = sim_method_newton_iter
|
||||
else:
|
||||
raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.')
|
||||
|
|
@ -2593,7 +2678,7 @@ class AcadosOcpOptions:
|
|||
|
||||
@nlp_solver_step_length.setter
|
||||
def nlp_solver_step_length(self, nlp_solver_step_length):
|
||||
if type(nlp_solver_step_length) == float and nlp_solver_step_length > 0:
|
||||
if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0:
|
||||
self.__nlp_solver_step_length = nlp_solver_step_length
|
||||
else:
|
||||
raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting')
|
||||
|
|
@ -2614,7 +2699,7 @@ class AcadosOcpOptions:
|
|||
|
||||
@qp_solver_cond_N.setter
|
||||
def qp_solver_cond_N(self, qp_solver_cond_N):
|
||||
if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N > 0:
|
||||
if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0:
|
||||
self.__qp_solver_cond_N = qp_solver_cond_N
|
||||
else:
|
||||
raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting')
|
||||
|
|
@ -2705,21 +2790,21 @@ class AcadosOcpOptions:
|
|||
@nlp_solver_max_iter.setter
|
||||
def nlp_solver_max_iter(self, nlp_solver_max_iter):
|
||||
|
||||
if type(nlp_solver_max_iter) == int and nlp_solver_max_iter > 0:
|
||||
if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0:
|
||||
self.__nlp_solver_max_iter = nlp_solver_max_iter
|
||||
else:
|
||||
raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting')
|
||||
|
||||
@print_level.setter
|
||||
def print_level(self, print_level):
|
||||
if type(print_level) == int and print_level >= 0:
|
||||
if isinstance(print_level, int) and print_level >= 0:
|
||||
self.__print_level = print_level
|
||||
else:
|
||||
raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting')
|
||||
|
||||
@model_external_shared_lib_dir.setter
|
||||
def model_external_shared_lib_dir(self, model_external_shared_lib_dir):
|
||||
if type(model_external_shared_lib_dir) == str :
|
||||
if isinstance(model_external_shared_lib_dir, str) :
|
||||
self.__model_external_shared_lib_dir = model_external_shared_lib_dir
|
||||
else:
|
||||
raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \
|
||||
|
|
@ -2727,7 +2812,7 @@ class AcadosOcpOptions:
|
|||
|
||||
@model_external_shared_lib_name.setter
|
||||
def model_external_shared_lib_name(self, model_external_shared_lib_name):
|
||||
if type(model_external_shared_lib_name) == str :
|
||||
if isinstance(model_external_shared_lib_name, str) :
|
||||
if model_external_shared_lib_name[-3:] == '.so' :
|
||||
raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \
|
||||
+ '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
|
||||
|
|
@ -2810,6 +2895,9 @@ class AcadosOcp:
|
|||
self.acados_lib_path = f'{acados_path}/lib'
|
||||
"""Path to where acados library is located, type: string"""
|
||||
|
||||
import numpy
|
||||
self.cython_include_dirs = numpy.get_include()
|
||||
|
||||
self.__parameter_values = np.array([])
|
||||
self.__problem_class = 'OCP'
|
||||
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ import os
|
|||
import json
|
||||
import numpy as np
|
||||
from datetime import datetime
|
||||
import ctypes
|
||||
import importlib
|
||||
from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref
|
||||
|
||||
from copy import deepcopy
|
||||
|
|
@ -51,9 +51,9 @@ from .generate_c_code_nls_cost import generate_c_code_nls_cost
|
|||
from .generate_c_code_external_cost import generate_c_code_external_cost
|
||||
from .acados_ocp import AcadosOcp
|
||||
from .acados_model import acados_model_strip_casadi_symbolics
|
||||
from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
|
||||
from .utils import is_column, is_empty, casadi_length, render_template,\
|
||||
format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
|
||||
set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path
|
||||
set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path
|
||||
|
||||
|
||||
def make_ocp_dims_consistent(acados_ocp):
|
||||
|
|
@ -90,7 +90,7 @@ def make_ocp_dims_consistent(acados_ocp):
|
|||
raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
|
||||
f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n')
|
||||
|
||||
# cost
|
||||
## cost
|
||||
# initial stage - if not set, copy fields from path constraints
|
||||
if cost.cost_type_0 is None:
|
||||
cost.cost_type_0 = cost.cost_type
|
||||
|
|
@ -434,18 +434,14 @@ def make_ocp_dims_consistent(acados_ocp):
|
|||
if np.shape(opts.shooting_nodes)[0] != dims.N+1:
|
||||
raise Exception('inconsistent dimension N, regarding shooting_nodes.')
|
||||
|
||||
# time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
|
||||
# # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
|
||||
# delta_time_steps = time_steps[1:] - time_steps[0:-1]
|
||||
# avg_time_steps = np.average(time_steps)
|
||||
# # criterion for constant time-step detection: the min/max difference in values normalized by the average
|
||||
# check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
|
||||
# # if the criterion is small, we have a constant time-step
|
||||
# if check_const_time_step < 1e-9:
|
||||
# time_steps[:] = avg_time_steps # if we have a constant time-step: apply the average time-step
|
||||
time_steps = np.zeros((dims.N,))
|
||||
for i in range(dims.N):
|
||||
time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above
|
||||
time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
|
||||
# identify constant time_steps: due to numerical reasons the content of time_steps might vary a bit
|
||||
avg_time_steps = np.average(time_steps)
|
||||
# criterion for constant time step detection: the min/max difference in values normalized by the average
|
||||
check_const_time_step = (np.max(time_steps)-np.min(time_steps)) / avg_time_steps
|
||||
# if the criterion is small, we have a constant time_step
|
||||
if check_const_time_step < 1e-9:
|
||||
time_steps[:] = avg_time_steps # if we have a constant time_step: apply the average time_step
|
||||
|
||||
opts.time_steps = time_steps
|
||||
|
||||
|
|
@ -525,8 +521,7 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n
|
|||
|
||||
# strip shooting_nodes
|
||||
ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)
|
||||
|
||||
dims_dict = acados_class2dict(acados_ocp.dims)
|
||||
dims_dict = format_class_dict(acados_ocp.dims.__dict__)
|
||||
|
||||
ocp_check_against_layout(ocp_nlp_dict, dims_dict)
|
||||
|
||||
|
|
@ -782,8 +777,15 @@ class AcadosOcpSolver:
|
|||
dlclose.argtypes = [c_void_p]
|
||||
|
||||
@classmethod
|
||||
def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True):
|
||||
def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None):
|
||||
"""
|
||||
Generates the code for an acados OCP solver, given the description in acados_ocp.
|
||||
:param acados_ocp: type AcadosOcp - description of the OCP for acados
|
||||
:param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
|
||||
:param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
|
||||
"""
|
||||
model = acados_ocp.model
|
||||
acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory)
|
||||
|
||||
if simulink_opts is None:
|
||||
simulink_opts = get_simulink_default_opts()
|
||||
|
|
@ -807,24 +809,91 @@ class AcadosOcpSolver:
|
|||
# dump to json
|
||||
ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)
|
||||
|
||||
code_export_dir = acados_ocp.code_export_directory
|
||||
# render templates
|
||||
ocp_render_templates(acados_ocp, json_file)
|
||||
acados_ocp.json_file = json_file
|
||||
|
||||
if build:
|
||||
## Compile solver
|
||||
cwd=os.getcwd()
|
||||
os.chdir(code_export_dir)
|
||||
os.system('make clean_ocp_shared_lib')
|
||||
os.system('make ocp_shared_lib')
|
||||
os.chdir(cwd)
|
||||
|
||||
def __init__(self, model_name, N, code_export_dir):
|
||||
self.model_name = model_name
|
||||
self.N = N
|
||||
@classmethod
|
||||
def build(cls, code_export_dir, with_cython=False):
|
||||
"""
|
||||
Builds the code for an acados OCP solver, that has been generated in code_export_dir
|
||||
:param code_export_dir: directory in which acados OCP solver has been generated, see generate()
|
||||
:param with_cython: option indicating if the cython interface is build, default: False.
|
||||
"""
|
||||
cwd=os.getcwd()
|
||||
os.chdir(code_export_dir)
|
||||
if with_cython:
|
||||
os.system('make clean_ocp_cython')
|
||||
os.system('make ocp_cython')
|
||||
else:
|
||||
os.system('make clean_ocp_shared_lib')
|
||||
os.system('make ocp_shared_lib')
|
||||
os.chdir(cwd)
|
||||
|
||||
|
||||
@classmethod
|
||||
def create_cython_solver(cls, json_file):
|
||||
"""
|
||||
Returns an `AcadosOcpSolverCython` object.
|
||||
|
||||
This is an alternative Cython based Python wrapper to the acados OCP solver in C.
|
||||
This offers faster interaction with the solver, because getter and setter calls, which include shape checking are done in compiled C code.
|
||||
|
||||
The default wrapper `AcadosOcpSolver` is based on ctypes.
|
||||
"""
|
||||
with open(json_file, 'r') as f:
|
||||
acados_ocp_json = json.load(f)
|
||||
code_export_directory = acados_ocp_json['code_export_directory']
|
||||
|
||||
importlib.invalidate_caches()
|
||||
rel_code_export_directory = os.path.relpath(code_export_directory)
|
||||
acados_ocp_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_ocp_solver_pyx')
|
||||
|
||||
AcadosOcpSolverCython = getattr(acados_ocp_solver_pyx, 'AcadosOcpSolverCython')
|
||||
return AcadosOcpSolverCython(acados_ocp_json['model']['name'],
|
||||
acados_ocp_json['solver_options']['nlp_solver_type'],
|
||||
acados_ocp_json['dims']['N'])
|
||||
|
||||
|
||||
def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True):
|
||||
|
||||
self.solver_created = False
|
||||
self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so'
|
||||
if generate:
|
||||
self.generate(acados_ocp, json_file=json_file, simulink_opts=simulink_opts)
|
||||
|
||||
# load json, store options in object
|
||||
with open(json_file, 'r') as f:
|
||||
acados_ocp_json = json.load(f)
|
||||
self.N = acados_ocp_json['dims']['N']
|
||||
self.model_name = acados_ocp_json['model']['name']
|
||||
self.solver_options = acados_ocp_json['solver_options']
|
||||
|
||||
acados_lib_path = acados_ocp_json['acados_lib_path']
|
||||
code_export_directory = acados_ocp_json['code_export_directory']
|
||||
|
||||
if build:
|
||||
self.build(code_export_directory, with_cython=False)
|
||||
|
||||
# Load acados library to avoid unloading the library.
|
||||
# This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
|
||||
# Unloading a library which uses OpenMP results in a segfault (on any platform?).
|
||||
# see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
|
||||
# or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
|
||||
libacados_name = 'libacados.so'
|
||||
libacados_filepath = os.path.join(acados_lib_path, libacados_name)
|
||||
self.__acados_lib = CDLL(libacados_filepath)
|
||||
# find out if acados was compiled with OpenMP
|
||||
try:
|
||||
self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
|
||||
except AttributeError as e:
|
||||
self.__acados_lib_uses_omp = False
|
||||
if self.__acados_lib_uses_omp:
|
||||
print('acados was compiled with OpenMP.')
|
||||
else:
|
||||
print('acados was compiled without OpenMP.')
|
||||
|
||||
self.shared_lib_name = f'{code_export_directory}/libacados_ocp_solver_{self.model_name}.so'
|
||||
|
||||
# get shared_lib
|
||||
self.shared_lib = CDLL(self.shared_lib_name)
|
||||
|
|
@ -842,6 +911,8 @@ class AcadosOcpSolver:
|
|||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
self.status = 0
|
||||
|
||||
|
||||
def __get_pointers_solver(self):
|
||||
"""
|
||||
|
|
@ -864,6 +935,10 @@ class AcadosOcpSolver:
|
|||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p
|
||||
self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").restype = c_void_p
|
||||
self.sens_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p
|
||||
self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule)
|
||||
|
|
@ -872,46 +947,26 @@ class AcadosOcpSolver:
|
|||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
|
||||
self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
|
||||
|
||||
# treat parameters separately
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
|
||||
self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")
|
||||
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
|
||||
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
|
||||
|
||||
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the ocp with current input.
|
||||
"""
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int
|
||||
status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
|
||||
return status
|
||||
self.status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
|
||||
|
||||
return self.status
|
||||
|
||||
|
||||
def set_new_time_steps(self, new_time_steps):
|
||||
"""
|
||||
Set new time steps before solving. Only reload library without code generation but with new time steps.
|
||||
Set new time steps.
|
||||
Recreates the solver if N changes.
|
||||
|
||||
:param new_time_steps: vector of new time steps for the solver
|
||||
:param new_time_steps: 1 dimensional np array of new time steps for the solver
|
||||
|
||||
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
|
||||
.. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of
|
||||
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
|
||||
do not require a new code export and compilation.
|
||||
"""
|
||||
|
|
@ -921,15 +976,14 @@ class AcadosOcpSolver:
|
|||
raise Exception('Solver was not yet created!')
|
||||
|
||||
# check if time steps really changed in value
|
||||
if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
|
||||
if np.array_equal(self.solver_options['time_steps'], new_time_steps):
|
||||
return
|
||||
|
||||
N = new_time_steps.size
|
||||
model = self.acados_ocp.model
|
||||
new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))
|
||||
|
||||
# check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
|
||||
if self.acados_ocp.solver_options.time_steps.size == N:
|
||||
if len(self.solver_options['time_steps']) == N:
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
|
||||
assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
|
||||
|
|
@ -941,11 +995,6 @@ class AcadosOcpSolver:
|
|||
getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)
|
||||
|
||||
# store N and new time steps
|
||||
self.N = self.acados_ocp.dims.N = N
|
||||
self.acados_ocp.solver_options.time_steps = new_time_steps
|
||||
self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]
|
||||
|
||||
# create solver with new time steps
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
|
||||
|
|
@ -956,6 +1005,75 @@ class AcadosOcpSolver:
|
|||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
# store time_steps, N
|
||||
self.solver_options['time_steps'] = new_time_steps
|
||||
self.N = N
|
||||
self.solver_options['Tsim'] = self.solver_options['time_steps'][0]
|
||||
|
||||
|
||||
def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
|
||||
"""
|
||||
Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
|
||||
This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
|
||||
the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
|
||||
:param qp_solver_cond_N: new number of condensing stages for the solver
|
||||
|
||||
.. note:: This function can only be used in combination with a partial condensing QP solver.
|
||||
|
||||
.. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
|
||||
necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
|
||||
`qp_solver_cond_N < N`.
|
||||
"""
|
||||
# unlikely but still possible
|
||||
if not self.solver_created:
|
||||
raise Exception('Solver was not yet created!')
|
||||
if self.N < qp_solver_cond_N:
|
||||
raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
|
||||
if self.solver_options['qp_solver_cond_N'] != qp_solver_cond_N:
|
||||
self.solver_created = False
|
||||
|
||||
# recreate the solver
|
||||
fun_name = f'{self.model_name}_acados_update_qp_solver_cond_N'
|
||||
getattr(self.shared_lib, fun_name).argtypes = [c_void_p, c_int]
|
||||
getattr(self.shared_lib, fun_name).restype = c_int
|
||||
assert getattr(self.shared_lib, fun_name)(self.capsule, qp_solver_cond_N) == 0
|
||||
|
||||
# store the new value
|
||||
self.solver_options['qp_solver_cond_N'] = qp_solver_cond_N
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
|
||||
def eval_param_sens(self, index, stage=0, field="ex"):
|
||||
"""
|
||||
Calculate the sensitivity of the curent solution with respect to the initial state component of index
|
||||
|
||||
:param index: integer corresponding to initial state index in range(nx)
|
||||
"""
|
||||
|
||||
field_ = field
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# checks
|
||||
if not isinstance(index, int):
|
||||
raise Exception('AcadosOcpSolver.eval_param_sens(): index must be Integer.')
|
||||
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
nx = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
|
||||
|
||||
if index < 0 or index > nx:
|
||||
raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
|
||||
|
||||
# actual eval_param
|
||||
self.shared_lib.ocp_nlp_eval_param_sens.argtypes = [c_void_p, c_char_p, c_int, c_int, c_void_p]
|
||||
self.shared_lib.ocp_nlp_eval_param_sens.restype = None
|
||||
self.shared_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
|
||||
|
||||
return
|
||||
|
||||
|
||||
def get(self, stage_, field_):
|
||||
"""
|
||||
|
|
@ -978,23 +1096,30 @@ class AcadosOcpSolver:
|
|||
|
||||
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
|
||||
# mem_fields = ['sl', 'su']
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
sens_fields = ['sens_u', "sens_x"]
|
||||
all_fields = out_fields + sens_fields
|
||||
|
||||
if (field_ not in out_fields):
|
||||
field = field_
|
||||
|
||||
if (field_ not in all_fields):
|
||||
raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(field_, out_fields))
|
||||
\n Possible values are {}. Exiting.'.format(field_, all_fields))
|
||||
|
||||
if not isinstance(stage_, int):
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
|
||||
|
||||
if stage_ < 0 or stage_ > self.N:
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_))
|
||||
|
||||
if stage_ == self.N and field_ == 'pi':
|
||||
raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
|
||||
.format(field_, stage_))
|
||||
|
||||
if field_ in sens_fields:
|
||||
field = field_.replace('sens_', '')
|
||||
|
||||
field = field.encode('utf-8')
|
||||
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
|
|
@ -1015,6 +1140,11 @@ class AcadosOcpSolver:
|
|||
# [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
# self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
|
||||
# self.nlp_dims, self.nlp_solver, stage_, field, out_data)
|
||||
elif field_ in sens_fields:
|
||||
self.shared_lib.ocp_nlp_out_get.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_get(self.nlp_config, \
|
||||
self.nlp_dims, self.sens_out, stage_, field, out_data)
|
||||
|
||||
return out
|
||||
|
||||
|
|
@ -1029,6 +1159,7 @@ class AcadosOcpSolver:
|
|||
- res_comp: residual wrt complementarity conditions
|
||||
- qp_stat: status of QP solver
|
||||
- qp_iter: number of QP iterations
|
||||
- alpha: SQP step size
|
||||
- qp_res_stat: stationarity residual of the last QP solution
|
||||
- qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
|
||||
- qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution
|
||||
|
|
@ -1036,19 +1167,18 @@ class AcadosOcpSolver:
|
|||
"""
|
||||
stat = self.get_stats("statistics")
|
||||
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
|
||||
print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
|
||||
if stat.shape[0]>7:
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP':
|
||||
print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha')
|
||||
if stat.shape[0]>8:
|
||||
print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
|
||||
for jj in range(stat.shape[1]):
|
||||
print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \
|
||||
int(stat[0][jj]), stat[1][jj], stat[2][jj], \
|
||||
stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
|
||||
if stat.shape[0]>7:
|
||||
print(f'{int(stat[0][jj]):d}\t{stat[1][jj]:e}\t{stat[2][jj]:e}\t{stat[3][jj]:e}\t' +
|
||||
f'{stat[4][jj]:e}\t{int(stat[5][jj]):d}\t{int(stat[6][jj]):d}\t{stat[7][jj]:e}\t')
|
||||
if stat.shape[0]>8:
|
||||
print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
|
||||
stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
|
||||
stat[8][jj], stat[9][jj], stat[10][jj], stat[11][jj]))
|
||||
print('\n')
|
||||
elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
print('\niter\tqp_stat\tqp_iter')
|
||||
if stat.shape[0]>3:
|
||||
print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
|
||||
|
|
@ -1108,6 +1238,7 @@ class AcadosOcpSolver:
|
|||
with open(filename, 'r') as f:
|
||||
solution = json.load(f)
|
||||
|
||||
print(f"loading iterate {filename}")
|
||||
for key in solution.keys():
|
||||
(field, stage) = key.split('_')
|
||||
self.set(int(stage), field, np.array(solution[key]))
|
||||
|
|
@ -1117,62 +1248,99 @@ class AcadosOcpSolver:
|
|||
"""
|
||||
Get the information of the last solver call.
|
||||
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter', 'residuals', 'qp_iter', 'alpha']
|
||||
|
||||
Available fileds:
|
||||
- time_tot: total CPU time previous call
|
||||
- time_lin: CPU time for linearization
|
||||
- time_sim: CPU time for integrator
|
||||
- time_sim_ad: CPU time for integrator contribution of external function calls
|
||||
- time_sim_la: CPU time for integrator contribution of linear algebra
|
||||
- time_qp: CPU time qp solution
|
||||
- time_qp_solver_call: CPU time inside qp solver (without converting the QP)
|
||||
- time_qp_xcond: time_glob: CPU time globalization
|
||||
- time_solution_sensitivities: CPU time for previous call to eval_param_sens
|
||||
- time_reg: CPU time regularization
|
||||
- sqp_iter: number of SQP iterations
|
||||
- qp_iter: vector of QP iterations for last SQP call
|
||||
- statistics: table with info about last iteration
|
||||
- stat_m: number of rows in statistics matrix
|
||||
- stat_n: number of columns in statistics matrix
|
||||
- residuals: residuals of last iterate
|
||||
- alpha: step sizes of SQP iterations
|
||||
"""
|
||||
|
||||
fields = ['time_tot', # total cpu time previous call
|
||||
'time_lin', # cpu time for linearization
|
||||
'time_sim', # cpu time for integrator
|
||||
'time_sim_ad', # cpu time for integrator contribution of external function calls
|
||||
'time_sim_la', # cpu time for integrator contribution of linear algebra
|
||||
'time_qp', # cpu time qp solution
|
||||
'time_qp_solver_call', # cpu time inside qp solver (without converting the QP)
|
||||
double_fields = ['time_tot',
|
||||
'time_lin',
|
||||
'time_sim',
|
||||
'time_sim_ad',
|
||||
'time_sim_la',
|
||||
'time_qp',
|
||||
'time_qp_solver_call',
|
||||
'time_qp_xcond',
|
||||
'time_glob', # cpu time globalization
|
||||
'time_reg', # cpu time regularization
|
||||
'sqp_iter', # number of SQP iterations
|
||||
'qp_iter', # vector of QP iterations for last SQP call
|
||||
'statistics', # table with info about last iteration
|
||||
'time_glob',
|
||||
'time_solution_sensitivities',
|
||||
'time_reg'
|
||||
]
|
||||
fields = double_fields + [
|
||||
'sqp_iter',
|
||||
'qp_iter',
|
||||
'statistics',
|
||||
'stat_m',
|
||||
'stat_n',]
|
||||
'stat_n',
|
||||
'residuals',
|
||||
'alpha',
|
||||
]
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
if (field_ not in fields):
|
||||
raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(fields, fields))
|
||||
|
||||
if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
|
||||
out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_int64))
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
return out
|
||||
|
||||
# TODO: just return double instead of np.
|
||||
elif field_ in double_fields:
|
||||
out = np.zeros((1,))
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
return out
|
||||
|
||||
elif field_ == 'statistics':
|
||||
sqp_iter = self.get_stats("sqp_iter")
|
||||
stat_m = self.get_stats("stat_m")
|
||||
stat_n = self.get_stats("stat_n")
|
||||
|
||||
min_size = min([stat_m, sqp_iter+1])
|
||||
|
||||
out = np.ascontiguousarray(
|
||||
np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
return out
|
||||
|
||||
elif field_ == 'qp_iter':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
|
||||
out = full_stats[6, :]
|
||||
elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
out = full_stats[2, :]
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP':
|
||||
return full_stats[6, :]
|
||||
elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
return full_stats[2, :]
|
||||
|
||||
elif field_ == 'alpha':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP':
|
||||
return full_stats[7, :]
|
||||
else: # self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
raise Exception("alpha values are not available for SQP_RTI")
|
||||
|
||||
elif field_ == 'residuals':
|
||||
return self.get_residuals()
|
||||
|
||||
else:
|
||||
out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
if not field_ == 'qp_iter':
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
|
||||
return out
|
||||
raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.'
|
||||
+ f'\n Possible values are {fields}.')
|
||||
|
||||
|
||||
def get_cost(self):
|
||||
|
|
@ -1201,7 +1369,7 @@ class AcadosOcpSolver:
|
|||
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
|
||||
"""
|
||||
# compute residuals if RTI
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
|
|
@ -1230,9 +1398,7 @@ class AcadosOcpSolver:
|
|||
|
||||
|
||||
# Note: this function should not be used anymore, better use cost_set, constraints_set
|
||||
|
||||
def set(self, stage_, field_, value_):
|
||||
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
|
|
@ -1253,6 +1419,7 @@ class AcadosOcpSolver:
|
|||
cost_fields = ['y_ref', 'yref']
|
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
|
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
|
||||
mem_fields = ['xdot_guess']
|
||||
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
|
|
@ -1294,18 +1461,25 @@ class AcadosOcpSolver:
|
|||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
if field_ in constraints_fields:
|
||||
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in cost_fields:
|
||||
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in out_fields:
|
||||
self.shared_lib.ocp_nlp_out_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage, field, value_data_p)
|
||||
# elif field_ in mem_fields:
|
||||
# self.shared_lib.ocp_nlp_set(self.nlp_config, \
|
||||
# self.nlp_solver, stage, field, value_data_p)
|
||||
|
||||
elif field_ in mem_fields:
|
||||
self.shared_lib.ocp_nlp_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, value_data_p)
|
||||
return
|
||||
|
||||
|
||||
|
|
@ -1364,9 +1538,8 @@ class AcadosOcpSolver:
|
|||
raise Exception("Unknown api: '{}'".format(api))
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
|
||||
' for field "{}" with dimension {} (you have {})'.format( \
|
||||
field_, tuple(dims), value_shape))
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
|
@ -1433,8 +1606,8 @@ class AcadosOcpSolver:
|
|||
raise Exception("Unknown api: '{}'".format(api))
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
|
||||
' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
|
||||
raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
|
@ -1490,11 +1663,11 @@ class AcadosOcpSolver:
|
|||
"""
|
||||
Set options of the solver.
|
||||
|
||||
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
|
||||
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC'
|
||||
:param value: of type int, float
|
||||
"""
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
|
||||
string_fields = ['globalization']
|
||||
|
||||
# check field availability and type
|
||||
|
|
@ -1522,10 +1695,10 @@ class AcadosOcpSolver:
|
|||
|
||||
if field_ == 'rti_phase':
|
||||
if value_ < 0 or value_ > 2:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
|
||||
'take only values 0, 1, 2 for SQP-RTI-type solvers')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
if self.solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
|
||||
'take only value 0 for SQP-type solvers')
|
||||
|
||||
# encode
|
||||
|
|
|
|||
|
|
@ -1,402 +0,0 @@
|
|||
import sys
|
||||
import os
|
||||
import json
|
||||
import numpy as np
|
||||
from datetime import datetime
|
||||
|
||||
from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p
|
||||
|
||||
from copy import deepcopy
|
||||
|
||||
from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
|
||||
from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
|
||||
from .generate_c_code_gnsf import generate_c_code_gnsf
|
||||
from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
|
||||
from .generate_c_code_constraint import generate_c_code_constraint
|
||||
from .generate_c_code_nls_cost import generate_c_code_nls_cost
|
||||
from .generate_c_code_external_cost import generate_c_code_external_cost
|
||||
from .acados_ocp import AcadosOcp
|
||||
from .acados_model import acados_model_strip_casadi_symbolics
|
||||
from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
|
||||
format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
|
||||
set_up_imported_gnsf_model, get_acados_path
|
||||
|
||||
|
||||
class AcadosOcpSolverFast:
|
||||
dlclose = CDLL(None).dlclose
|
||||
dlclose.argtypes = [c_void_p]
|
||||
|
||||
def __init__(self, model_name, N, code_export_dir):
|
||||
|
||||
self.solver_created = False
|
||||
self.N = N
|
||||
self.model_name = model_name
|
||||
|
||||
self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so'
|
||||
|
||||
# get shared_lib
|
||||
self.shared_lib = CDLL(self.shared_lib_name)
|
||||
|
||||
# create capsule
|
||||
getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p
|
||||
self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")()
|
||||
|
||||
# create solver
|
||||
getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int
|
||||
assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p
|
||||
self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p
|
||||
self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p
|
||||
self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p
|
||||
self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p
|
||||
self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p
|
||||
self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule)
|
||||
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the ocp with current input.
|
||||
"""
|
||||
model_name = self.model_name
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int
|
||||
status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule)
|
||||
return status
|
||||
|
||||
def cost_set(self, start_stage_, field_, value_, api='warn'):
|
||||
self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
|
||||
return
|
||||
|
||||
def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
|
||||
"""
|
||||
Set numerical data in the cost module of the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
|
||||
:param value: of appropriate size
|
||||
"""
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
dim = np.product(value_.shape[1:])
|
||||
|
||||
start_stage = c_int(start_stage_)
|
||||
end_stage = c_int(end_stage_)
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
|
||||
|
||||
value_shape = value_.shape
|
||||
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
|
||||
if len(value_shape) == 2:
|
||||
value_shape = (value_shape[0], value_shape[1], 0)
|
||||
|
||||
elif len(value_shape) == 3:
|
||||
if api=='old':
|
||||
pass
|
||||
elif api=='warn':
|
||||
if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
|
||||
raise Exception("Ambiguity in API detected.\n"
|
||||
"Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
|
||||
"Are you seeing this error suddenly in previously running code? Read on.\n"
|
||||
" You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
|
||||
" acados_template now correctly passes on any matrices to acados in column major format.\n" +
|
||||
" Two options to fix this error: \n" +
|
||||
" * Add api='old' to cost_set to restore old incorrect behaviour\n" +
|
||||
" * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
|
||||
"such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
|
||||
"If there is no such manipulation, then you have probably been getting an incorrect solution before.")
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
elif api=='new':
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
else:
|
||||
raise Exception("Unknown api: '{}'".format(api))
|
||||
|
||||
if value_shape != expected_shape:
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension',
|
||||
' for field "{}" with dimension {} (you have {})'.format(
|
||||
field_, expected_shape, value_shape))
|
||||
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
|
||||
self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
|
||||
return
|
||||
|
||||
def constraints_set(self, start_stage_, field_, value_, api='warn'):
|
||||
self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
|
||||
return
|
||||
|
||||
def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
|
||||
"""
|
||||
Set numerical data in the constraint module of the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
|
||||
:param value: of appropriate size
|
||||
"""
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
value_ = value_.astype(float)
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
dim = np.product(value_.shape[1:])
|
||||
|
||||
start_stage = c_int(start_stage_)
|
||||
end_stage = c_int(end_stage_)
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
|
||||
|
||||
value_shape = value_.shape
|
||||
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
|
||||
if len(value_shape) == 2:
|
||||
value_shape = (value_shape[0], value_shape[1], 0)
|
||||
elif len(value_shape) == 3:
|
||||
if api=='old':
|
||||
pass
|
||||
elif api=='warn':
|
||||
if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
|
||||
raise Exception("Ambiguity in API detected.\n"
|
||||
"Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
|
||||
"Are you seeing this error suddenly in previously running code? Read on.\n"
|
||||
" You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
|
||||
" acados_template now correctly passes on any matrices to acados in column major format.\n" +
|
||||
" Two options to fix this error: \n" +
|
||||
" * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
|
||||
" * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
|
||||
"such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
|
||||
"If there is no such manipulation, then you have probably been getting an incorrect solution before.")
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
elif api=='new':
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
else:
|
||||
raise Exception("Unknown api: '{}'".format(api))
|
||||
if value_shape != expected_shape:
|
||||
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
|
||||
' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape))
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
|
||||
self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
|
||||
return
|
||||
|
||||
# Note: this function should not be used anymore, better use cost_set, constraints_set
|
||||
def set(self, stage_, field_, value_):
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
|
||||
|
||||
.. note:: regarding lam, t: \n
|
||||
the inequalities are internally organized in the following order: \n
|
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
|
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n
|
||||
lam: multipliers for inequalities \n
|
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
|
||||
sl: slack variables of soft lower inequality constraints \n
|
||||
su: slack variables of soft upper inequality constraints \n
|
||||
"""
|
||||
cost_fields = ['y_ref', 'yref']
|
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
|
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
|
||||
mem_fields = ['sl', 'su']
|
||||
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
value_ = value_.astype(float)
|
||||
|
||||
model_name = self.model_name
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
|
||||
stage = c_int(stage_)
|
||||
|
||||
# treat parameters separately
|
||||
if field_ == 'p':
|
||||
getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
|
||||
assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
|
||||
else:
|
||||
if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
|
||||
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
|
||||
\nPossible values are {}. Exiting.".format(field, \
|
||||
constraints_fields + cost_fields + out_fields + ['p']))
|
||||
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage_, field)
|
||||
|
||||
if value_.shape[0] != dims:
|
||||
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
|
||||
msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
|
||||
raise Exception(msg)
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
if field_ in constraints_fields:
|
||||
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in cost_fields:
|
||||
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in out_fields:
|
||||
self.shared_lib.ocp_nlp_out_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage, field, value_data_p)
|
||||
elif field_ in mem_fields:
|
||||
self.shared_lib.ocp_nlp_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, value_data_p)
|
||||
return
|
||||
|
||||
|
||||
def get_slice(self, start_stage_, end_stage_, field_):
|
||||
"""
|
||||
Get the last solution of the solver:
|
||||
|
||||
:param start_stage: integer corresponding to shooting node that indicates start of slice
|
||||
:param end_stage: integer corresponding to shooting node that indicates end of slice
|
||||
:param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
|
||||
|
||||
.. note:: regarding lam, t: \n
|
||||
the inequalities are internally organized in the following order: \n
|
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
|
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n
|
||||
lam: multipliers for inequalities \n
|
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
|
||||
sl: slack variables of soft lower inequality constraints \n
|
||||
su: slack variables of soft upper inequality constraints \n
|
||||
"""
|
||||
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
|
||||
mem_fields = ['sl', 'su']
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
|
||||
if (field_ not in out_fields + mem_fields):
|
||||
raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(field_, out_fields))
|
||||
|
||||
if not isinstance(start_stage_, int):
|
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
|
||||
|
||||
if not isinstance(end_stage_, int):
|
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
|
||||
|
||||
if start_stage_ >= end_stage_:
|
||||
raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
|
||||
|
||||
if start_stage_ < 0 or end_stage_ > self.N + 1:
|
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, start_stage_, field)
|
||||
|
||||
out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
if (field_ in out_fields):
|
||||
self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
|
||||
elif field_ in mem_fields:
|
||||
self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
|
||||
|
||||
return out
|
||||
|
||||
def get_cost(self):
|
||||
"""
|
||||
Returns the cost value of the current solution.
|
||||
"""
|
||||
# compute cost internally
|
||||
self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
# create output array
|
||||
out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
# call getter
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
|
||||
field = "cost_value".encode('utf-8')
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
|
||||
return out[0]
|
||||
|
|
@ -39,22 +39,19 @@ cimport cython
|
|||
from libc cimport string
|
||||
|
||||
cimport acados_solver_common
|
||||
# TODO: make this import more clear? it is not a general solver, but problem specific.
|
||||
cimport acados_solver
|
||||
|
||||
cimport numpy as cnp
|
||||
|
||||
import os
|
||||
import json
|
||||
from datetime import datetime
|
||||
import numpy as np
|
||||
|
||||
|
||||
cdef class AcadosOcpSolverFast:
|
||||
cdef class AcadosOcpSolverCython:
|
||||
"""
|
||||
Class to interact with the acados ocp solver C object.
|
||||
|
||||
:param model_name:
|
||||
:param N:
|
||||
"""
|
||||
|
||||
cdef acados_solver.nlp_solver_capsule *capsule
|
||||
|
|
@ -62,19 +59,26 @@ cdef class AcadosOcpSolverFast:
|
|||
cdef acados_solver_common.ocp_nlp_dims *nlp_dims
|
||||
cdef acados_solver_common.ocp_nlp_config *nlp_config
|
||||
cdef acados_solver_common.ocp_nlp_out *nlp_out
|
||||
cdef acados_solver_common.ocp_nlp_out *sens_out
|
||||
cdef acados_solver_common.ocp_nlp_in *nlp_in
|
||||
cdef acados_solver_common.ocp_nlp_solver *nlp_solver
|
||||
|
||||
cdef str model_name
|
||||
cdef int N
|
||||
cdef int status
|
||||
cdef bint solver_created
|
||||
|
||||
def __cinit__(self, str model_name, int N):
|
||||
self.model_name = model_name
|
||||
self.N = N
|
||||
cdef str model_name
|
||||
cdef int N
|
||||
|
||||
cdef str nlp_solver_type
|
||||
|
||||
def __cinit__(self, model_name, nlp_solver_type, N):
|
||||
|
||||
self.solver_created = False
|
||||
|
||||
self.N = N
|
||||
self.model_name = model_name
|
||||
self.nlp_solver_type = nlp_solver_type
|
||||
|
||||
# create capsule
|
||||
self.capsule = acados_solver.acados_create_capsule()
|
||||
|
||||
|
|
@ -82,11 +86,21 @@ cdef class AcadosOcpSolverFast:
|
|||
assert acados_solver.acados_create(self.capsule) == 0
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
self.status = 0
|
||||
|
||||
|
||||
def __get_pointers_solver(self):
|
||||
"""
|
||||
Private function to get the pointers for solver
|
||||
"""
|
||||
# get pointers solver
|
||||
self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
|
||||
self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
|
||||
self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
|
||||
self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
|
||||
self.sens_out = acados_solver.acados_get_sens_out(self.capsule)
|
||||
self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
|
||||
self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
|
||||
|
||||
|
|
@ -100,15 +114,112 @@ cdef class AcadosOcpSolverFast:
|
|||
|
||||
def set_new_time_steps(self, new_time_steps):
|
||||
"""
|
||||
Set new time steps before solving. Only reload library without code generation but with new time steps.
|
||||
Set new time steps.
|
||||
Recreates the solver if N changes.
|
||||
|
||||
:param new_time_steps: vector of new time steps for the solver
|
||||
:param new_time_steps: 1 dimensional np array of new time steps for the solver
|
||||
|
||||
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
|
||||
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
|
||||
do not require a new code export and compilation.
|
||||
"""
|
||||
raise NotImplementedError()
|
||||
|
||||
raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature")
|
||||
# # unlikely but still possible
|
||||
# if not self.solver_created:
|
||||
# raise Exception('Solver was not yet created!')
|
||||
|
||||
# ## check if time steps really changed in value
|
||||
# # get time steps
|
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64)
|
||||
# assert acados_solver.acados_get_time_steps(self.capsule, self.N, <double *> old_time_steps.data)
|
||||
|
||||
# if np.array_equal(old_time_steps, new_time_steps):
|
||||
# return
|
||||
|
||||
# N = new_time_steps.size
|
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64)
|
||||
|
||||
# # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
|
||||
# if len(old_time_steps) == N:
|
||||
# assert acados_solver.acados_update_time_steps(self.capsule, N, <double *> value.data) == 0
|
||||
|
||||
# else: # recreate the solver with the new time steps
|
||||
# self.solver_created = False
|
||||
|
||||
# # delete old memory (analog to __del__)
|
||||
# acados_solver.acados_free(self.capsule)
|
||||
|
||||
# # create solver with new time steps
|
||||
# assert acados_solver.acados_create_with_discretization(self.capsule, N, <double *> value.data) == 0
|
||||
|
||||
# self.solver_created = True
|
||||
|
||||
# # get pointers solver
|
||||
# self.__get_pointers_solver()
|
||||
|
||||
# # store time_steps, N
|
||||
# self.time_steps = new_time_steps
|
||||
# self.N = N
|
||||
|
||||
|
||||
def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
|
||||
"""
|
||||
Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
|
||||
This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
|
||||
the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
|
||||
:param qp_solver_cond_N: new number of condensing stages for the solver
|
||||
|
||||
.. note:: This function can only be used in combination with a partial condensing QP solver.
|
||||
|
||||
.. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
|
||||
necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
|
||||
`qp_solver_cond_N < N`.
|
||||
"""
|
||||
raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature")
|
||||
|
||||
# # unlikely but still possible
|
||||
# if not self.solver_created:
|
||||
# raise Exception('Solver was not yet created!')
|
||||
# if self.N < qp_solver_cond_N:
|
||||
# raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
|
||||
# if self.qp_solver_cond_N != qp_solver_cond_N:
|
||||
# self.solver_created = False
|
||||
|
||||
# # recreate the solver
|
||||
# acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N)
|
||||
|
||||
# # store the new value
|
||||
# self.qp_solver_cond_N = qp_solver_cond_N
|
||||
# self.solver_created = True
|
||||
|
||||
# # get pointers solver
|
||||
# self.__get_pointers_solver()
|
||||
|
||||
|
||||
def eval_param_sens(self, index, stage=0, field="ex"):
|
||||
"""
|
||||
Calculate the sensitivity of the curent solution with respect to the initial state component of index
|
||||
|
||||
:param index: integer corresponding to initial state index in range(nx)
|
||||
"""
|
||||
|
||||
field_ = field
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# checks
|
||||
if not isinstance(index, int):
|
||||
raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.')
|
||||
|
||||
cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
|
||||
|
||||
if index < 0 or index > nx:
|
||||
raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
|
||||
|
||||
# actual eval_param
|
||||
acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
|
||||
|
||||
return
|
||||
|
||||
|
||||
def get(self, int stage, str field_):
|
||||
|
|
@ -134,14 +245,14 @@ cdef class AcadosOcpSolverFast:
|
|||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ not in out_fields:
|
||||
raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
|
||||
raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(field_, out_fields))
|
||||
|
||||
if stage < 0 or stage > self.N:
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
|
||||
if stage == self.N and field_ == 'pi':
|
||||
raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
|
||||
raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\
|
||||
.format(field_, stage))
|
||||
|
||||
cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
|
||||
|
|
@ -179,6 +290,7 @@ cdef class AcadosOcpSolverFast:
|
|||
:param filename: if not set, use model_name + timestamp + '.json'
|
||||
:param overwrite: if false and filename exists add timestamp to filename
|
||||
"""
|
||||
import json
|
||||
if filename == '':
|
||||
filename += self.model_name + '_' + 'iterate' + '.json'
|
||||
|
||||
|
|
@ -212,6 +324,7 @@ cdef class AcadosOcpSolverFast:
|
|||
"""
|
||||
Loads the iterate stored in json file with filename into the ocp solver.
|
||||
"""
|
||||
import json
|
||||
if not os.path.isfile(filename):
|
||||
raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
|
||||
|
||||
|
|
@ -228,34 +341,53 @@ cdef class AcadosOcpSolverFast:
|
|||
Get the information of the last solver call.
|
||||
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
|
||||
Available fileds:
|
||||
- time_tot: total CPU time previous call
|
||||
- time_lin: CPU time for linearization
|
||||
- time_sim: CPU time for integrator
|
||||
- time_sim_ad: CPU time for integrator contribution of external function calls
|
||||
- time_sim_la: CPU time for integrator contribution of linear algebra
|
||||
- time_qp: CPU time qp solution
|
||||
- time_qp_solver_call: CPU time inside qp solver (without converting the QP)
|
||||
- time_qp_xcond: time_glob: CPU time globalization
|
||||
- time_solution_sensitivities: CPU time for previous call to eval_param_sens
|
||||
- time_reg: CPU time regularization
|
||||
- sqp_iter: number of SQP iterations
|
||||
- qp_iter: vector of QP iterations for last SQP call
|
||||
- statistics: table with info about last iteration
|
||||
- stat_m: number of rows in statistics matrix
|
||||
- stat_n: number of columns in statistics matrix
|
||||
- residuals: residuals of last iterate
|
||||
- alpha: step sizes of SQP iterations
|
||||
"""
|
||||
fields = ['time_tot', # total cpu time previous call
|
||||
'time_lin', # cpu time for linearization
|
||||
'time_sim', # cpu time for integrator
|
||||
'time_sim_ad', # cpu time for integrator contribution of external function calls
|
||||
'time_sim_la', # cpu time for integrator contribution of linear algebra
|
||||
'time_qp', # cpu time qp solution
|
||||
'time_qp_solver_call', # cpu time inside qp solver (without converting the QP)
|
||||
|
||||
double_fields = ['time_tot',
|
||||
'time_lin',
|
||||
'time_sim',
|
||||
'time_sim_ad',
|
||||
'time_sim_la',
|
||||
'time_qp',
|
||||
'time_qp_solver_call',
|
||||
'time_qp_xcond',
|
||||
'time_glob', # cpu time globalization
|
||||
'time_reg', # cpu time regularization
|
||||
'sqp_iter', # number of SQP iterations
|
||||
'qp_iter', # vector of QP iterations for last SQP call
|
||||
'statistics', # table with info about last iteration
|
||||
'time_glob',
|
||||
'time_solution_sensitivities',
|
||||
'time_reg'
|
||||
]
|
||||
fields = double_fields + [
|
||||
'sqp_iter',
|
||||
'qp_iter',
|
||||
'statistics',
|
||||
'stat_m',
|
||||
'stat_n',]
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
|
||||
if (field_ not in fields):
|
||||
raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(fields, fields))
|
||||
'stat_n',
|
||||
'residuals',
|
||||
'alpha',
|
||||
]
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
|
||||
return self.__get_stat_int(field)
|
||||
|
||||
elif field_.startswith('time'):
|
||||
elif field_ in double_fields:
|
||||
return self.__get_stat_double(field)
|
||||
|
||||
elif field_ == 'statistics':
|
||||
|
|
@ -266,14 +398,24 @@ cdef class AcadosOcpSolverFast:
|
|||
return self.__get_stat_matrix(field, stat_n+1, min_size)
|
||||
|
||||
elif field_ == 'qp_iter':
|
||||
NotImplementedError("TODO! Cython not aware if SQP or SQP_RTI")
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
|
||||
out = full_stats[6, :]
|
||||
elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
out = full_stats[2, :]
|
||||
if self.nlp_solver_type == 'SQP':
|
||||
return full_stats[6, :]
|
||||
elif self.nlp_solver_type == 'SQP_RTI':
|
||||
return full_stats[2, :]
|
||||
|
||||
elif field_ == 'alpha':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.nlp_solver_type == 'SQP':
|
||||
return full_stats[7, :]
|
||||
else: # self.nlp_solver_type == 'SQP_RTI':
|
||||
raise Exception("alpha values are not available for SQP_RTI")
|
||||
|
||||
elif field_ == 'residuals':
|
||||
return self.get_residuals()
|
||||
|
||||
else:
|
||||
NotImplementedError("TODO!")
|
||||
raise NotImplementedError("TODO!")
|
||||
|
||||
|
||||
def __get_stat_int(self, field):
|
||||
|
|
@ -312,10 +454,9 @@ cdef class AcadosOcpSolverFast:
|
|||
"""
|
||||
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
|
||||
"""
|
||||
# TODO: check if RTI, only eval then
|
||||
# if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
# compute residuals if RTI
|
||||
acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
if self.nlp_solver_type == 'SQP_RTI':
|
||||
acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
# create output array
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
|
||||
|
|
@ -363,6 +504,7 @@ cdef class AcadosOcpSolverFast:
|
|||
cost_fields = ['y_ref', 'yref']
|
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
|
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
|
||||
mem_fields = ['xdot_guess']
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
|
|
@ -373,7 +515,7 @@ cdef class AcadosOcpSolverFast:
|
|||
assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
|
||||
else:
|
||||
if field_ not in constraints_fields + cost_fields + out_fields:
|
||||
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
|
||||
raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
|
||||
\nPossible values are {}. Exiting.".format(field, \
|
||||
constraints_fields + cost_fields + out_fields + ['p']))
|
||||
|
||||
|
|
@ -381,7 +523,7 @@ cdef class AcadosOcpSolverFast:
|
|||
self.nlp_dims, self.nlp_out, stage, field)
|
||||
|
||||
if value_.shape[0] != dims:
|
||||
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
|
||||
msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_)
|
||||
msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
|
||||
raise Exception(msg)
|
||||
|
||||
|
|
@ -394,6 +536,9 @@ cdef class AcadosOcpSolverFast:
|
|||
elif field_ in out_fields:
|
||||
acados_solver_common.ocp_nlp_out_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
|
||||
elif field_ in mem_fields:
|
||||
acados_solver_common.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, <void *> value.data)
|
||||
|
||||
|
||||
def cost_set(self, int stage, str field_, value_):
|
||||
|
|
@ -422,9 +567,8 @@ cdef class AcadosOcpSolverFast:
|
|||
value = np.asfortranarray(value_)
|
||||
|
||||
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
|
||||
' for field "{}" with dimension {} (you have {})'.format( \
|
||||
field_, tuple(dims), value_shape))
|
||||
raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
|
||||
|
|
@ -456,8 +600,8 @@ cdef class AcadosOcpSolverFast:
|
|||
value = np.asfortranarray(value_)
|
||||
|
||||
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
|
||||
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
|
||||
' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
|
||||
raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
|
||||
|
|
@ -494,8 +638,8 @@ cdef class AcadosOcpSolverFast:
|
|||
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
|
||||
:param value: of type int, float
|
||||
"""
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
|
||||
string_fields = ['globalization']
|
||||
|
||||
# encode
|
||||
|
|
@ -512,10 +656,10 @@ cdef class AcadosOcpSolverFast:
|
|||
|
||||
if field_ == 'rti_phase':
|
||||
if value_ < 0 or value_ > 2:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
|
||||
'take only values 0, 1, 2 for SQP-RTI-type solvers')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
|
||||
'take only value 0 for SQP-type solvers')
|
||||
|
||||
int_value = value_
|
||||
|
|
@ -536,7 +680,7 @@ cdef class AcadosOcpSolverFast:
|
|||
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
|
||||
|
||||
else:
|
||||
raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
|
||||
raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\
|
||||
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -70,28 +70,28 @@ class AcadosSimDims:
|
|||
|
||||
@nx.setter
|
||||
def nx(self, nx):
|
||||
if type(nx) == int and nx > 0:
|
||||
if isinstance(nx, int) and nx > 0:
|
||||
self.__nx = nx
|
||||
else:
|
||||
raise Exception('Invalid nx value, expected positive integer. Exiting.')
|
||||
|
||||
@nz.setter
|
||||
def nz(self, nz):
|
||||
if type(nz) == int and nz > -1:
|
||||
if isinstance(nz, int) and nz > -1:
|
||||
self.__nz = nz
|
||||
else:
|
||||
raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nu.setter
|
||||
def nu(self, nu):
|
||||
if type(nu) == int and nu > -1:
|
||||
if isinstance(nu, int) and nu > -1:
|
||||
self.__nu = nu
|
||||
else:
|
||||
raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@np.setter
|
||||
def np(self, np):
|
||||
if type(np) == int and np > -1:
|
||||
if isinstance(np, int) and np > -1:
|
||||
self.__np = np
|
||||
else:
|
||||
raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
|
||||
|
|
@ -302,6 +302,7 @@ class AcadosSim:
|
|||
self.code_export_directory = 'c_generated_code'
|
||||
"""Path to where code will be exported. Default: `c_generated_code`."""
|
||||
|
||||
self.cython_include_dirs = ''
|
||||
self.__parameter_values = np.array([])
|
||||
|
||||
@property
|
||||
|
|
|
|||
|
|
@ -215,6 +215,24 @@ class AcadosSimSolver:
|
|||
model_name = self.sim_struct.model.name
|
||||
self.model_name = model_name
|
||||
|
||||
# Load acados library to avoid unloading the library.
|
||||
# This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
|
||||
# Unloading a library which uses OpenMP results in a segfault (on any platform?).
|
||||
# see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
|
||||
# or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
|
||||
libacados_name = 'libacados.so'
|
||||
libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name)
|
||||
self.__acados_lib = CDLL(libacados_filepath)
|
||||
# find out if acados was compiled with OpenMP
|
||||
try:
|
||||
self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
|
||||
except AttributeError as e:
|
||||
self.__acados_lib_uses_omp = False
|
||||
if self.__acados_lib_uses_omp:
|
||||
print('acados was compiled with OpenMP.')
|
||||
else:
|
||||
print('acados was compiled without OpenMP.')
|
||||
|
||||
# Ctypes
|
||||
shared_lib = f'{code_export_dir}/libacados_sim_solver_{model_name}.so'
|
||||
self.shared_lib = CDLL(shared_lib)
|
||||
|
|
|
|||
|
|
@ -95,6 +95,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h":
|
|||
|
||||
# solver
|
||||
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
|
||||
void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out)
|
||||
void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
|
||||
|
||||
# get/set
|
||||
|
|
|
|||
|
|
@ -125,134 +125,134 @@
|
|||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{# acados flags #}
|
||||
ACADOS_FLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
ACADOS_FLAGS += -DACADOS_WITH_QPOASES
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
|
||||
ACADOS_FLAGS += -DACADOS_WITH_OSQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
|
||||
ACADOS_FLAGS += -DACADOS_WITH_QPDUNES
|
||||
{%- endif %}
|
||||
# # Debugging
|
||||
# ACADOS_FLAGS += -g3
|
||||
# define sources and use make's implicit rules to generate object files (*.o)
|
||||
|
||||
MODEL_OBJ=
|
||||
# model
|
||||
MODEL_SRC=
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o
|
||||
{%- endif %}
|
||||
{%- else %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
|
||||
{%- endif %}
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- else %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
MODEL_OBJ := $(MODEL_SRC:.c=.o)
|
||||
|
||||
|
||||
OCP_OBJ=
|
||||
# optimal control problem - mostly CasADi exports
|
||||
OCP_SRC=
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.o
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.o
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.o
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o
|
||||
{%- endif %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.o
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o
|
||||
{%- endif %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
{%- elif cost_type_0 == "EXTERNAL" %}
|
||||
{% if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{% else %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{% endif %}
|
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{%- else %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
{%- elif cost_type == "EXTERNAL" %}
|
||||
{% if cost.cost_ext_fun_type == "casadi" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{% elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{% endif %}
|
||||
{%- if cost.cost_ext_fun_type == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
{%- elif cost_type_e == "EXTERNAL" %}
|
||||
{% if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{% elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{% endif %}
|
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
OCP_OBJ+= acados_solver_{{ model.name }}.o
|
||||
OCP_SRC+= acados_solver_{{ model.name }}.c
|
||||
OCP_OBJ := $(OCP_SRC:.c=.o)
|
||||
|
||||
# for sim solver
|
||||
SIM_SRC= acados_sim_solver_{{ model.name }}.c
|
||||
SIM_OBJ := $(SIM_SRC:.c=.o)
|
||||
|
||||
SIM_OBJ=
|
||||
SIM_OBJ+= acados_sim_solver_{{ model.name }}.o
|
||||
# for target example
|
||||
EX_SRC= main_{{ model.name }}.c
|
||||
EX_OBJ := $(EX_SRC:.c=.o)
|
||||
EX_EXE := $(EX_SRC:.c=)
|
||||
|
||||
EX_OBJ=
|
||||
EX_OBJ+= main_{{ model.name }}.o
|
||||
|
||||
EX_SIM_OBJ=
|
||||
EX_SIM_OBJ+= main_sim_{{ model.name }}.o
|
||||
# for target example_sim
|
||||
EX_SIM_SRC= main_sim_{{ model.name }}.c
|
||||
EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
|
||||
EX_SIM_EXE := $(EX_SIM_SRC:.c=)
|
||||
|
||||
# combine model, sim and ocp object files
|
||||
OBJ=
|
||||
OBJ+= $(MODEL_OBJ)
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
|
|
@ -271,233 +271,103 @@ EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
|
|||
INCLUDE_PATH = {{ acados_include_path }}
|
||||
LIB_PATH = {{ acados_lib_path }}
|
||||
|
||||
{%- if solver_options.integrator_type == "DISCRETE" %}
|
||||
all: clean casadi_fun example
|
||||
# preprocessor flags for make's implicit rules
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPOASES
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
|
||||
CPPFLAGS += -DACADOS_WITH_OSQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPDUNES
|
||||
{%- endif %}
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/acados
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
{# c-compiler flags #}
|
||||
# define the c-compiler flags for make's implicit rules
|
||||
CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
|
||||
# # Debugging
|
||||
# CFLAGS += -g3
|
||||
|
||||
# linker flags
|
||||
LDFLAGS+= -L$(LIB_PATH)
|
||||
|
||||
# link to libraries
|
||||
LDLIBS+= -lacados
|
||||
LDLIBS+= -lhpipm
|
||||
LDLIBS+= -lblasfeo
|
||||
LDLIBS+= -lm
|
||||
LDLIBS+= {{ link_libs }}
|
||||
|
||||
# libraries
|
||||
LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so
|
||||
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
|
||||
LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
|
||||
|
||||
# virtual targets
|
||||
.PHONY : all clean
|
||||
|
||||
#all: clean example_sim example shared_lib
|
||||
{% if solver_options.integrator_type == "DISCRETE" -%}
|
||||
all: clean example
|
||||
shared_lib: ocp_shared_lib
|
||||
{%- else %}
|
||||
all: clean casadi_fun example_sim example
|
||||
all: clean example_sim example
|
||||
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
|
||||
{%- endif %}
|
||||
|
||||
CASADI_MODEL_SOURCE=
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
|
||||
# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" and model.dyn_ext_fun_type == "casadi" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
CASADI_CON_PHI_SOURCE=
|
||||
CASADI_CON_PHI_SOURCE+= {{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
CASADI_CON_PHI_E_SOURCE=
|
||||
CASADI_CON_PHI_E_SOURCE+= {{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
CASADI_CON_H_SOURCE=
|
||||
CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
# some linker targets
|
||||
example: $(EX_OBJ) $(OBJ)
|
||||
$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
|
||||
$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{% if solver_options.integrator_type != "DISCRETE" -%}
|
||||
bundled_shared_lib: $(OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims_nh_e > 0 %}
|
||||
CASADI_CON_H_E_SOURCE=
|
||||
CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
|
||||
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
CASADI_COST_Y_SOURCE=
|
||||
CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun.c
|
||||
CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_hess.c
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
CASADI_COST_Y_E_SOURCE=
|
||||
CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun.c
|
||||
CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_hess.c
|
||||
{%- endif %}
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
CASADI_COST_Y_0_SOURCE=
|
||||
CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun.c
|
||||
CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_hess.c
|
||||
{%- endif %}
|
||||
|
||||
casadi_fun:
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
( cd {{ model.name }}_model {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_E_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_E_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_E_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_0_SOURCE))
|
||||
{%- endif %}
|
||||
|
||||
main:
|
||||
gcc $(ACADOS_FLAGS) -c main_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
main_sim:
|
||||
gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/
|
||||
|
||||
ocp_solver:
|
||||
gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
sim_solver:
|
||||
gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
example: ocp_solver main
|
||||
gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \
|
||||
-lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
|
||||
example_sim: sim_solver main_sim
|
||||
gcc $(ACADOS_FLAGS) -o main_sim_{{ model.name }} $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \
|
||||
-lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/acados/ \
|
||||
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
|
||||
bundled_shared_lib: casadi_fun ocp_solver sim_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-L $(LIB_PATH) \
|
||||
-lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
ocp_shared_lib: casadi_fun ocp_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
{%- else %}
|
||||
|
||||
ocp_shared_lib: casadi_fun ocp_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
{%- endif %}
|
||||
|
||||
# Cython targets
|
||||
ocp_cython_c: ocp_shared_lib
|
||||
cython \
|
||||
-o acados_ocp_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
|
||||
-I {{ code_export_directory }} \
|
||||
|
||||
ocp_cython_o: ocp_cython_c
|
||||
clang $(ACADOS_FLAGS) -c -O2 \
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_ocp_solver_pyx.o \
|
||||
-I /usr/include/python3.8 \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-I {{ cython_include_dirs }} \
|
||||
acados_ocp_solver_pyx.c \
|
||||
|
||||
ocp_cython: ocp_cython_o
|
||||
clang $(ACADOS_FLAGS) -shared \
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_ocp_solver_pyx.so \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_ocp_solver_pyx.o \
|
||||
$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo -lqpOASES_e \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
sim_shared_lib: casadi_fun sim_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{%- if os and os == "pc" %}
|
||||
|
||||
|
|
@ -510,15 +380,27 @@ clean_ocp_shared_lib:
|
|||
del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
|
||||
clean_ocp_cython:
|
||||
del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
del \Q acados_ocp_solver_pyx.so 2>nul
|
||||
del \Q acados_ocp_solver_pyx.o 2>nul
|
||||
|
||||
{%- else %}
|
||||
|
||||
clean:
|
||||
rm -f *.o
|
||||
rm -f *.so
|
||||
rm -f main_{{ model.name }}
|
||||
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
|
||||
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
|
||||
$(RM) $(EX_EXE) $(EX_SIM_EXE)
|
||||
|
||||
clean_ocp_shared_lib:
|
||||
rm -f libacados_ocp_solver_{{ model.name }}.so
|
||||
rm -f acados_solver_{{ model.name }}.o
|
||||
$(RM) $(LIBACADOS_OCP_SOLVER)
|
||||
$(RM) $(OCP_OBJ)
|
||||
|
||||
clean_ocp_cython:
|
||||
$(RM) libacados_ocp_solver_{{ model.name }}.so
|
||||
$(RM) acados_solver_{{ model.name }}.o
|
||||
$(RM) acados_ocp_solver_pyx.so
|
||||
$(RM) acados_ocp_solver_pyx.o
|
||||
|
||||
{%- endif %}
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
mexPrintf("{{ model.name }}_acados_create() -> success!\n");
|
||||
|
||||
// get pointers to nlp solver related objects
|
||||
ocp_nlp_plan *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
|
||||
ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
|
||||
|
|
@ -238,14 +238,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
l_ptr = mxGetData(gnsf_phi_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
|
||||
l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
|
||||
l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
|
||||
{% elif solver_options.integrator_type == "DISCRETE" %}
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
// plan
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
|
||||
ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
|
||||
ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
|
||||
// config
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
|
||||
ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
|
||||
|
|
@ -404,7 +404,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
}
|
||||
else if (!strcmp(field, "init_z"))
|
||||
{
|
||||
sim_solver_plan sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
|
|
@ -426,7 +426,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
}
|
||||
else if (!strcmp(field, "init_xdot"))
|
||||
{
|
||||
sim_solver_plan sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
|
|
@ -448,7 +448,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
}
|
||||
else if (!strcmp(field, "init_gnsf_phi"))
|
||||
{
|
||||
sim_solver_plan sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == GNSF)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -164,12 +164,17 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
{%- endif %}
|
||||
|
||||
{% elif solver_options.integrator_type == "GNSF" -%}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
|
||||
|
|
@ -194,6 +199,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
|
||||
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
|
||||
|
|
@ -201,6 +207,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
|
||||
|
|
@ -212,7 +220,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
{% endif %}
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan plan;
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = {{ solver_options.integrator_type }};
|
||||
|
||||
// create correct config based on plan
|
||||
|
|
@ -307,14 +315,18 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
"expl_ode_hess", capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun", capsule->sim_gnsf_phi_fun);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
|
||||
{%- endif %}
|
||||
|
|
@ -409,10 +421,14 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
|
|||
external_function_param_casadi_free(capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_fun);
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
|
||||
{% endif %}
|
||||
|
||||
|
|
@ -445,10 +461,14 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl
|
|||
capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
|
||||
capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
|
||||
capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
|
||||
{% endif %}
|
||||
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@
|
|||
#define MDL_START
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -78,9 +78,10 @@ typedef struct {{ model.name }}_solver_capsule
|
|||
// acados objects
|
||||
ocp_nlp_in *nlp_in;
|
||||
ocp_nlp_out *nlp_out;
|
||||
ocp_nlp_out *sens_out;
|
||||
ocp_nlp_solver *nlp_solver;
|
||||
void *nlp_opts;
|
||||
ocp_nlp_plan *nlp_solver_plan;
|
||||
ocp_nlp_plan_t *nlp_solver_plan;
|
||||
ocp_nlp_config *nlp_config;
|
||||
ocp_nlp_dims *nlp_dims;
|
||||
|
||||
|
|
@ -186,6 +187,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
|
|||
* nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
|
||||
*/
|
||||
int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
|
||||
/**
|
||||
* This function is used for updating an already initialized solver with a different number of qp_cond_N.
|
||||
*/
|
||||
int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
|
||||
int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
|
||||
int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
|
||||
int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
|
||||
|
|
@ -193,11 +198,12 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsu
|
|||
|
||||
ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
|
||||
void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
|
|
|
|||
|
|
@ -1,3 +1,36 @@
|
|||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
cimport acados_solver_common
|
||||
|
||||
cdef extern from "acados_solver_{{ model.name }}.h":
|
||||
|
|
@ -8,6 +41,11 @@ cdef extern from "acados_solver_{{ model.name }}.h":
|
|||
int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
|
||||
|
||||
int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
|
||||
|
||||
int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
|
||||
int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
|
||||
int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
|
||||
|
||||
int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
|
||||
int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
|
||||
int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
|
||||
|
|
@ -15,6 +53,7 @@ cdef extern from "acados_solver_{{ model.name }}.h":
|
|||
|
||||
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
|
||||
void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@
|
|||
#define MDL_START
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -156,11 +156,12 @@ int main()
|
|||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize solution
|
||||
for (int i = 0; i <= nlp_dims->N; i++)
|
||||
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 = {{ model.name }}_acados_solve(acados_ocp_capsule);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
|
|
|
|||
|
|
@ -46,10 +46,14 @@ SOURCES = { ...
|
|||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...
|
||||
|
|
|
|||
|
|
@ -47,10 +47,14 @@ SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
|
|||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c '
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c '
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c '
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c '
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c '
|
||||
{%- endif %}
|
||||
];
|
||||
|
|
|
|||
|
|
@ -125,15 +125,15 @@ classdef {{ model.name }}_mex_solver < handle
|
|||
if strcmp(field, 'stat')
|
||||
stat = obj.get('stat');
|
||||
{%- if solver_options.nlp_solver_type == "SQP" %}
|
||||
fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter');
|
||||
if size(stat,2)>7
|
||||
fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
|
||||
if size(stat,2)>8
|
||||
fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
|
||||
end
|
||||
fprintf('\n');
|
||||
for jj=1:size(stat,1)
|
||||
fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
|
||||
if size(stat,2)>7
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11));
|
||||
fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8));
|
||||
if size(stat,2)>8
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12));
|
||||
end
|
||||
fprintf('\n');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -90,14 +90,7 @@ int {{ model.name }}_impl_dae_hess_n_out(void);
|
|||
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
/* GNSF Functions */
|
||||
// used to import model matrices
|
||||
int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
|
||||
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
// phi_fun
|
||||
int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
|
||||
|
|
@ -121,7 +114,7 @@ const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
|
|||
const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
|
||||
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
// f_lo_fun_jac_x1k1uz
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
|
||||
|
|
@ -129,6 +122,15 @@ const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
|
|||
const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
// used to import model matrices
|
||||
int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
|
||||
|
||||
{% elif solver_options.integrator_type == "ERK" %}
|
||||
/* explicit ODE */
|
||||
|
|
|
|||
|
|
@ -254,22 +254,6 @@ def format_class_dict(d):
|
|||
return out
|
||||
|
||||
|
||||
def acados_class2dict(class_instance):
|
||||
"""
|
||||
removes the __ artifact from class to dict conversion
|
||||
"""
|
||||
|
||||
d = dict(class_instance.__dict__)
|
||||
out = {}
|
||||
for k, v in d.items():
|
||||
if isinstance(v, dict):
|
||||
v = format_class_dict(v)
|
||||
|
||||
out_key = k.split('__', 1)[-1]
|
||||
out[k.replace(k, out_key)] = v
|
||||
return out
|
||||
|
||||
|
||||
def get_ocp_nlp_layout():
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'acados_layout.json')
|
||||
|
|
@ -433,6 +417,13 @@ def set_up_imported_gnsf_model(acados_formulation):
|
|||
acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
|
||||
acados_formulation.model.get_matrices_fun = get_matrices_fun
|
||||
|
||||
# get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
|
||||
# {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
|
||||
# nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
|
||||
get_matrices_out = get_matrices_fun(0)
|
||||
acados_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
|
||||
acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13])
|
||||
|
||||
if "f_lo_fun_jac_x1k1uz" in gnsf:
|
||||
f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
|
||||
acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit 5b526a8e00bdc1c3922be470af1602cf9dc72dde
|
||||
Subproject commit 7663289f1e68860f53dc34337ef080dde69a2586
|
||||
|
|
@ -17,6 +17,7 @@ site_scons/site_tools/cython.py
|
|||
|
||||
common/.gitignore
|
||||
common/__init__.py
|
||||
common/conversions.py
|
||||
common/gpio.py
|
||||
common/realtime.py
|
||||
common/clock.pyx
|
||||
|
|
@ -58,7 +59,6 @@ common/transformations/transformations.pyx
|
|||
common/api/__init__.py
|
||||
|
||||
models/supercombo.dlc
|
||||
models/big_supercombo.dlc
|
||||
models/dmonitoring_model_q.dlc
|
||||
|
||||
release/*
|
||||
|
|
@ -70,12 +70,10 @@ installer/updater/updater
|
|||
selfdrive/version.py
|
||||
|
||||
selfdrive/__init__.py
|
||||
selfdrive/config.py
|
||||
selfdrive/sentry.py
|
||||
selfdrive/swaglog.py
|
||||
selfdrive/logmessaged.py
|
||||
selfdrive/tombstoned.py
|
||||
selfdrive/pandad.py
|
||||
selfdrive/updated.py
|
||||
selfdrive/rtshield.py
|
||||
selfdrive/statsd.py
|
||||
|
|
@ -99,8 +97,10 @@ selfdrive/boardd/panda.h
|
|||
selfdrive/boardd/pigeon.cc
|
||||
selfdrive/boardd/pigeon.h
|
||||
selfdrive/boardd/set_time.py
|
||||
selfdrive/boardd/pandad.py
|
||||
|
||||
selfdrive/car/__init__.py
|
||||
selfdrive/car/docs_definitions.py
|
||||
selfdrive/car/car_helpers.py
|
||||
selfdrive/car/fingerprints.py
|
||||
selfdrive/car/interfaces.py
|
||||
|
|
@ -337,11 +337,11 @@ selfdrive/sensord/sensord
|
|||
|
||||
selfdrive/thermald/thermald.py
|
||||
selfdrive/thermald/power_monitoring.py
|
||||
selfdrive/thermald/fan_controller.py
|
||||
|
||||
selfdrive/test/__init__.py
|
||||
selfdrive/test/helpers.py
|
||||
selfdrive/test/setup_device_ci.sh
|
||||
selfdrive/test/test_fingerprints.py
|
||||
selfdrive/test/test_onroad.py
|
||||
|
||||
selfdrive/ui/.gitignore
|
||||
|
|
@ -424,6 +424,7 @@ selfdrive/modeld/transforms/transform.cc
|
|||
selfdrive/modeld/transforms/transform.h
|
||||
selfdrive/modeld/transforms/transform.cl
|
||||
|
||||
selfdrive/modeld/thneed/*.py
|
||||
selfdrive/modeld/thneed/thneed.*
|
||||
selfdrive/modeld/thneed/serialize.cc
|
||||
selfdrive/modeld/thneed/compile.cc
|
||||
|
|
|
|||
|
|
@ -163,8 +163,6 @@ def upload_handler(end_event: threading.Event) -> None:
|
|||
sm = messaging.SubMaster(['deviceState'])
|
||||
tid = threading.get_ident()
|
||||
|
||||
cellular_unmetered = Params().get_bool("CellularUnmetered")
|
||||
|
||||
while not end_event.is_set():
|
||||
cur_upload_items[tid] = None
|
||||
|
||||
|
|
@ -181,46 +179,45 @@ def upload_handler(end_event: threading.Event) -> None:
|
|||
cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True)
|
||||
continue
|
||||
|
||||
# Check if uploading over cell is allowed
|
||||
# Check if uploading over metered connection is allowed
|
||||
sm.update(0)
|
||||
cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
|
||||
if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
|
||||
metered = sm['deviceState'].networkMetered
|
||||
network_type = sm['deviceState'].networkType.raw
|
||||
if metered and (not cur_upload_items[tid].allow_cellular):
|
||||
retry_upload(tid, end_event, False)
|
||||
continue
|
||||
|
||||
try:
|
||||
def cb(sz, cur):
|
||||
# Abort transfer if connection changed to cell after starting upload
|
||||
# Abort transfer if connection changed to metered after starting upload
|
||||
sm.update(0)
|
||||
cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
|
||||
if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
|
||||
metered = sm['deviceState'].networkMetered
|
||||
if metered and (not cur_upload_items[tid].allow_cellular):
|
||||
raise AbortTransferException
|
||||
|
||||
cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
|
||||
|
||||
|
||||
network_type = sm['deviceState'].networkType.raw
|
||||
fn = cur_upload_items[tid].path
|
||||
try:
|
||||
sz = os.path.getsize(fn)
|
||||
except OSError:
|
||||
sz = -1
|
||||
|
||||
cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type)
|
||||
cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
response = _do_upload(cur_upload_items[tid], cb)
|
||||
|
||||
if response.status_code not in (200, 201, 403, 412):
|
||||
cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type)
|
||||
cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
retry_upload(tid, end_event)
|
||||
else:
|
||||
cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type)
|
||||
cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
|
||||
UploadQueueCache.cache(upload_queue)
|
||||
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError):
|
||||
cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type)
|
||||
cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
retry_upload(tid, end_event)
|
||||
except AbortTransferException:
|
||||
cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type)
|
||||
cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
retry_upload(tid, end_event, False)
|
||||
|
||||
except queue.Empty:
|
||||
|
|
@ -459,6 +456,12 @@ def getNetworkType():
|
|||
return HARDWARE.get_network_type()
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getNetworkMetered():
|
||||
network_type = HARDWARE.get_network_type()
|
||||
return HARDWARE.get_network_metered(network_type)
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getNetworks():
|
||||
return HARDWARE.get_networks()
|
||||
|
|
|
|||
|
|
@ -371,7 +371,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
|
|||
|
||||
size_t j = 0;
|
||||
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
|
||||
f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
|
||||
f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_EXTI); f++) {
|
||||
if (fault_bits.test(f)) {
|
||||
faults.set(j, cereal::PandaState::FaultType(f));
|
||||
j++;
|
||||
|
|
|
|||
|
|
@ -338,6 +338,10 @@ void Panda::set_power_saving(bool power_saving) {
|
|||
usb_write(0xe7, power_saving, 0);
|
||||
}
|
||||
|
||||
void Panda::enable_deepsleep() {
|
||||
usb_write(0xfb, 0, 0);
|
||||
}
|
||||
|
||||
void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode) {
|
||||
usb_write(0xe6, (uint16_t)power_mode, 0);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -85,6 +85,7 @@ class Panda {
|
|||
std::optional<std::vector<uint8_t>> get_firmware_version();
|
||||
std::optional<std::string> get_serial();
|
||||
void set_power_saving(bool power_saving);
|
||||
void enable_deepsleep();
|
||||
void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
|
||||
void send_heartbeat(bool engaged);
|
||||
void set_can_speed_kbps(uint16_t bus, uint16_t speed);
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ public:
|
|||
CL_CHECK(clReleaseProgram(prg_debayer));
|
||||
}
|
||||
|
||||
void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, cl_event *debayer_event) {
|
||||
void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, float black_level, cl_event *debayer_event) {
|
||||
CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl));
|
||||
CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl));
|
||||
|
||||
|
|
@ -62,6 +62,7 @@ public:
|
|||
const size_t globalWorkSize[] = {size_t(width), size_t(height)};
|
||||
const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
|
||||
CL_CHECK(clSetKernelArg(krnl_, 2, localMemSize, 0));
|
||||
CL_CHECK(clSetKernelArg(krnl_, 3, sizeof(float), &black_level));
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
|
||||
} else {
|
||||
if (hdr_) {
|
||||
|
|
@ -161,15 +162,19 @@ bool CameraBuf::acquire() {
|
|||
cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
|
||||
cl_event event;
|
||||
|
||||
float start_time = millis_since_boot();
|
||||
|
||||
if (debayer) {
|
||||
float gain = 0.0;
|
||||
|
||||
float black_level = 42.0;
|
||||
#ifndef QCOM2
|
||||
gain = camera_state->digital_gain;
|
||||
if ((int)gain == 0) gain = 1.0;
|
||||
#else
|
||||
if (camera_state->camera_id == CAMERA_ID_IMX390) black_level = 64.0;
|
||||
#endif
|
||||
|
||||
debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, &event);
|
||||
debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, black_level, &event);
|
||||
} else {
|
||||
assert(rgb_stride == camera_state->ci.frame_stride);
|
||||
CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, cur_rgb_buf->len, 0, 0, &event));
|
||||
|
|
@ -181,6 +186,8 @@ bool CameraBuf::acquire() {
|
|||
cur_yuv_buf = vipc_server->get_buffer(yuv_type);
|
||||
rgb2yuv->queue(q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
|
||||
|
||||
cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0;
|
||||
|
||||
VisionIpcBufExtra extra = {
|
||||
cur_frame_data.frame_id,
|
||||
cur_frame_data.timestamp_sof,
|
||||
|
|
@ -219,6 +226,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
|
|||
framed.setLensPos(frame_data.lens_pos);
|
||||
framed.setLensErr(frame_data.lens_err);
|
||||
framed.setLensTruePos(frame_data.lens_true_pos);
|
||||
framed.setProcessingTime(frame_data.processing_time);
|
||||
}
|
||||
|
||||
kj::Array<uint8_t> get_frame_image(const CameraBuf *b) {
|
||||
|
|
|
|||
|
|
@ -25,7 +25,8 @@
|
|||
#define CAMERA_ID_LGC920 6
|
||||
#define CAMERA_ID_LGC615 7
|
||||
#define CAMERA_ID_AR0231 8
|
||||
#define CAMERA_ID_MAX 9
|
||||
#define CAMERA_ID_IMX390 9
|
||||
#define CAMERA_ID_MAX 10
|
||||
|
||||
const int UI_BUF_COUNT = 4;
|
||||
const int YUV_BUFFER_COUNT = Hardware::EON() ? 100 : 40;
|
||||
|
|
@ -75,6 +76,8 @@ typedef struct FrameMetadata {
|
|||
unsigned int lens_pos;
|
||||
float lens_err;
|
||||
float lens_true_pos;
|
||||
|
||||
float processing_time;
|
||||
} FrameMetadata;
|
||||
|
||||
typedef struct CameraExpInfo {
|
||||
|
|
|
|||
|
|
@ -211,12 +211,12 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
|
|||
/*fps*/ 20,
|
||||
#endif
|
||||
device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
|
||||
VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
|
||||
|
||||
camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
|
||||
/*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
|
||||
/*max_gain=*/510, 10, device_id, ctx,
|
||||
VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
|
||||
VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
|
||||
|
||||
s->sm = new SubMaster({"driverState"});
|
||||
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
|
||||
|
|
|
|||
|
|
@ -22,11 +22,14 @@
|
|||
#include "selfdrive/common/swaglog.h"
|
||||
#include "selfdrive/camerad/cameras/sensor2_i2c.h"
|
||||
|
||||
// For debugging:
|
||||
// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
|
||||
extern ExitHandler do_exit;
|
||||
|
||||
const size_t FRAME_WIDTH = 1928;
|
||||
const size_t FRAME_HEIGHT = 1208;
|
||||
const size_t FRAME_STRIDE = 2416; // for 10 bit output
|
||||
const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
|
||||
|
||||
const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py
|
||||
|
||||
|
|
@ -39,6 +42,14 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
|
|||
.bayer_flip = 1,
|
||||
.hdr = false
|
||||
},
|
||||
[CAMERA_ID_IMX390] = {
|
||||
.frame_width = FRAME_WIDTH,
|
||||
.frame_height = FRAME_HEIGHT,
|
||||
.frame_stride = FRAME_STRIDE,
|
||||
.bayer = true,
|
||||
.bayer_flip = 1,
|
||||
.hdr = false
|
||||
},
|
||||
};
|
||||
|
||||
const float DC_GAIN = 2.5;
|
||||
|
|
@ -56,7 +67,7 @@ const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss
|
|||
const int EXPOSURE_TIME_MAX = 1904; // with HDR, slowest ss
|
||||
|
||||
// ************** low level camera helpers ****************
|
||||
int cam_control(int fd, int op_code, void *handle, int size) {
|
||||
int do_cam_control(int fd, int op_code, void *handle, int size) {
|
||||
struct cam_control camcontrol = {0};
|
||||
camcontrol.op_code = op_code;
|
||||
camcontrol.handle = (uint64_t)handle;
|
||||
|
|
@ -83,7 +94,7 @@ std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data
|
|||
.num_resources = (uint32_t)(data ? 1 : 0),
|
||||
.resource_hdl = (uint64_t)data,
|
||||
};
|
||||
int err = cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
|
||||
int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
|
||||
return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt;
|
||||
};
|
||||
|
||||
|
|
@ -93,13 +104,13 @@ int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t p
|
|||
.dev_handle = dev_handle,
|
||||
.packet_handle = packet_handle,
|
||||
};
|
||||
return cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
|
||||
return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
int device_control(int fd, int op_code, int session_handle, int dev_handle) {
|
||||
// start stop and release are all the same
|
||||
struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle };
|
||||
return cam_control(fd, op_code, &cmd, sizeof(cmd));
|
||||
return do_cam_control(fd, op_code, &cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
|
||||
|
|
@ -118,7 +129,7 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, i
|
|||
mem_mgr_alloc_cmd.num_hdl++;
|
||||
}
|
||||
|
||||
cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
|
||||
do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
|
||||
*handle = mem_mgr_alloc_cmd.out.buf_handle;
|
||||
|
||||
void *ptr = NULL;
|
||||
|
|
@ -137,7 +148,7 @@ void release(int video0_fd, uint32_t handle) {
|
|||
struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
|
||||
mem_mgr_release_cmd.buf_handle = handle;
|
||||
|
||||
ret = cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
|
||||
ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
|
||||
assert(ret == 0);
|
||||
}
|
||||
|
||||
|
|
@ -153,34 +164,46 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
|
|||
req_mgr_flush_request.link_hdl = link_hdl;
|
||||
req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL;
|
||||
int ret;
|
||||
ret = cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
|
||||
ret = do_cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
|
||||
// LOGD("flushed all req: %d", ret);
|
||||
}
|
||||
|
||||
// ************** high level camera helpers ****************
|
||||
|
||||
void sensors_poke(struct CameraState *s, int request_id) {
|
||||
void CameraState::sensors_start() {
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
int start_reg_len = sizeof(start_reg_array_ar0231) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(start_reg_array_ar0231, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
int start_reg_len = sizeof(start_reg_array_imx390) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(start_reg_array_imx390, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
}
|
||||
|
||||
void CameraState::sensors_poke(int request_id) {
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 0;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
pkt->header.op_code = 0x7f;
|
||||
pkt->header.request_id = request_id;
|
||||
|
||||
int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
|
||||
int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
|
||||
assert(ret == 0);
|
||||
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) {
|
||||
void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) {
|
||||
// LOGD("sensors_i2c: %d", len);
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 1;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
|
|
@ -190,22 +213,23 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
|
|||
buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload);
|
||||
buf_desc[0].type = CAM_CMD_BUF_I2C;
|
||||
|
||||
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
i2c_random_wr->header.count = len;
|
||||
i2c_random_wr->header.op_code = 1;
|
||||
i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR;
|
||||
i2c_random_wr->header.data_type = CAMERA_SENSOR_I2C_TYPE_WORD;
|
||||
i2c_random_wr->header.data_type = data_word ? CAMERA_SENSOR_I2C_TYPE_WORD : CAMERA_SENSOR_I2C_TYPE_BYTE;
|
||||
i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
|
||||
memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload));
|
||||
|
||||
int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
|
||||
int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
|
||||
assert(ret == 0);
|
||||
|
||||
munmap(i2c_random_wr, buf_desc[0].size);
|
||||
release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
|
||||
cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings)));
|
||||
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
|
||||
|
|
@ -214,7 +238,8 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
|
|||
return (struct cam_cmd_power *)(unconditional_wait + 1);
|
||||
};
|
||||
|
||||
void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
||||
int CameraState::sensors_init() {
|
||||
int video0_fd = multi_cam_state->video0_fd;
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle);
|
||||
|
|
@ -232,17 +257,17 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
|||
switch (camera_num) {
|
||||
case 0:
|
||||
// port 0
|
||||
i2c_info->slave_addr = 0x20;
|
||||
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34;
|
||||
probe->camera_id = 0;
|
||||
break;
|
||||
case 1:
|
||||
// port 1
|
||||
i2c_info->slave_addr = 0x30;
|
||||
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x36;
|
||||
probe->camera_id = 1;
|
||||
break;
|
||||
case 2:
|
||||
// port 2
|
||||
i2c_info->slave_addr = 0x20;
|
||||
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34;
|
||||
probe->camera_id = 2;
|
||||
break;
|
||||
}
|
||||
|
|
@ -256,8 +281,15 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
|||
probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
|
||||
probe->op_code = 3; // don't care?
|
||||
probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE;
|
||||
probe->reg_addr = 0x3000; //0x300a; //0x300b;
|
||||
probe->expected_data = 0x354; //0x7750; //0x885a;
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
probe->reg_addr = 0x3000;
|
||||
probe->expected_data = 0x354;
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
probe->reg_addr = 0x330;
|
||||
probe->expected_data = 0x1538;
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
probe->data_mask = 0;
|
||||
|
||||
//buf_desc[1].size = buf_desc[1].length = 148;
|
||||
|
|
@ -286,7 +318,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
|||
power->count = 1;
|
||||
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
|
||||
power->power_settings[0].power_seq_type = 0;
|
||||
power->power_settings[0].config_val_low = 19200000; //Hz
|
||||
power->power_settings[0].config_val_low = (camera_id == CAMERA_ID_AR0231) ? 19200000 : 24000000; //Hz
|
||||
power = power_set_wait(power, 10);
|
||||
|
||||
// 8,1 is this reset?
|
||||
|
|
@ -333,8 +365,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
|||
power->power_settings[2].power_seq_type = 3;
|
||||
|
||||
LOGD("probing the sensor");
|
||||
int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
|
||||
assert(ret == 0);
|
||||
int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
|
||||
|
||||
munmap(i2c_info, buf_desc[0].size);
|
||||
release_fd(video0_fd, buf_desc[0].mem_handle);
|
||||
|
|
@ -342,15 +373,17 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
|||
release_fd(video0_fd, buf_desc[1].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(video0_fd, cam_packet_handle);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
|
||||
void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
|
||||
if (io_mem_handle != 0) {
|
||||
size += sizeof(struct cam_buf_io_cfg);
|
||||
}
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 2;
|
||||
pkt->kmd_cmd_buf_index = 0;
|
||||
// YUV has kmd_cmd_buf_offset = 1780
|
||||
|
|
@ -445,7 +478,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
|||
buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
|
||||
buf_desc[1].type = CAM_CMD_BUF_GENERIC;
|
||||
buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
|
||||
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
|
||||
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
|
||||
memcpy(buf2, &tmp, sizeof(tmp));
|
||||
|
||||
if (io_mem_handle != 0) {
|
||||
|
|
@ -464,10 +497,10 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
|||
.h_init = 0x0,
|
||||
.v_init = 0x0,
|
||||
};
|
||||
io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; // CAM_FORMAT_UBWC_TP10 for YUV
|
||||
io_cfg[0].format = CAM_FORMAT_MIPI_RAW_12; // CAM_FORMAT_UBWC_TP10 for YUV
|
||||
io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV
|
||||
io_cfg[0].color_pattern = 0x5; // 0x0 for YUV
|
||||
io_cfg[0].bpp = 0xa;
|
||||
io_cfg[0].bpp = 0xc;
|
||||
io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV
|
||||
io_cfg[0].fence = fence;
|
||||
io_cfg[0].direction = CAM_BUF_OUTPUT;
|
||||
|
|
@ -475,142 +508,149 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
|||
io_cfg[0].framedrop_pattern = 0x1;
|
||||
}
|
||||
|
||||
int ret = device_config(s->multi_cam_state->isp_fd, s->session_handle, s->isp_dev_handle, cam_packet_handle);
|
||||
int ret = device_config(multi_cam_state->isp_fd, session_handle, isp_dev_handle, cam_packet_handle);
|
||||
assert(ret == 0);
|
||||
if (ret != 0) {
|
||||
printf("ISP CONFIG FAILED\n");
|
||||
}
|
||||
|
||||
munmap(buf2, buf_desc[1].size);
|
||||
release_fd(s->multi_cam_state->video0_fd, buf_desc[1].mem_handle);
|
||||
// release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle);
|
||||
// release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
void enqueue_buffer(struct CameraState *s, int i, bool dp) {
|
||||
void CameraState::enqueue_buffer(int i, bool dp) {
|
||||
int ret;
|
||||
int request_id = s->request_ids[i];
|
||||
int request_id = request_ids[i];
|
||||
|
||||
if (s->buf_handle[i]) {
|
||||
release(s->multi_cam_state->video0_fd, s->buf_handle[i]);
|
||||
if (buf_handle[i]) {
|
||||
release(multi_cam_state->video0_fd, buf_handle[i]);
|
||||
// wait
|
||||
struct cam_sync_wait sync_wait = {0};
|
||||
sync_wait.sync_obj = s->sync_objs[i];
|
||||
sync_wait.sync_obj = sync_objs[i];
|
||||
sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
|
||||
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
|
||||
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
|
||||
// LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
|
||||
|
||||
s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
|
||||
if (dp) s->buf.queue(i);
|
||||
buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
|
||||
if (dp) buf.queue(i);
|
||||
|
||||
// destroy old output fence
|
||||
struct cam_sync_info sync_destroy = {0};
|
||||
strcpy(sync_destroy.name, "NodeOutputPortFence");
|
||||
sync_destroy.sync_obj = s->sync_objs[i];
|
||||
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
|
||||
sync_destroy.sync_obj = sync_objs[i];
|
||||
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
|
||||
// LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
|
||||
}
|
||||
|
||||
// do stuff
|
||||
struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
|
||||
req_mgr_sched_request.session_hdl = s->session_handle;
|
||||
req_mgr_sched_request.link_hdl = s->link_handle;
|
||||
req_mgr_sched_request.session_hdl = session_handle;
|
||||
req_mgr_sched_request.link_hdl = link_handle;
|
||||
req_mgr_sched_request.req_id = request_id;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
|
||||
// LOGD("sched req: %d %d", ret, request_id);
|
||||
|
||||
// create output fence
|
||||
struct cam_sync_info sync_create = {0};
|
||||
strcpy(sync_create.name, "NodeOutputPortFence");
|
||||
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
|
||||
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
|
||||
// LOGD("fence req: %d %d", ret, sync_create.sync_obj);
|
||||
s->sync_objs[i] = sync_create.sync_obj;
|
||||
sync_objs[i] = sync_create.sync_obj;
|
||||
|
||||
// configure ISP to put the image in place
|
||||
struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
|
||||
mem_mgr_map_cmd.mmu_hdls[0] = s->multi_cam_state->device_iommu;
|
||||
mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu;
|
||||
mem_mgr_map_cmd.num_hdl = 1;
|
||||
mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE;
|
||||
mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
|
||||
// LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
|
||||
s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
|
||||
mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
|
||||
// LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
|
||||
buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
|
||||
|
||||
// poke sensor
|
||||
sensors_poke(s, request_id);
|
||||
sensors_poke(request_id);
|
||||
// LOGD("Poked sensor");
|
||||
|
||||
// push the buffer
|
||||
config_isp(s, s->buf_handle[i], s->sync_objs[i], request_id, s->buf0_handle, 65632*(i+1));
|
||||
config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1));
|
||||
}
|
||||
|
||||
void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) {
|
||||
void CameraState::enqueue_req_multi(int start, int n, bool dp) {
|
||||
for (int i=start;i<start+n;++i) {
|
||||
s->request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
|
||||
enqueue_buffer(s, (i - 1) % FRAME_BUF_COUNT, dp);
|
||||
request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
|
||||
enqueue_buffer((i - 1) % FRAME_BUF_COUNT, dp);
|
||||
}
|
||||
}
|
||||
|
||||
// ******************* camera *******************
|
||||
|
||||
static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
|
||||
void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id_, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
|
||||
LOGD("camera init %d", camera_num);
|
||||
s->multi_cam_state = multi_cam_state;
|
||||
multi_cam_state = multi_cam_state_;
|
||||
camera_id = camera_id_;
|
||||
assert(camera_id < std::size(cameras_supported));
|
||||
s->ci = cameras_supported[camera_id];
|
||||
assert(s->ci.frame_width != 0);
|
||||
ci = cameras_supported[camera_id];
|
||||
assert(ci.frame_width != 0);
|
||||
|
||||
s->camera_num = camera_num;
|
||||
camera_num = camera_num_;
|
||||
|
||||
s->request_id_last = 0;
|
||||
s->skipped = true;
|
||||
request_id_last = 0;
|
||||
skipped = true;
|
||||
|
||||
s->min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
|
||||
s->max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
|
||||
s->target_grey_fraction = 0.3;
|
||||
min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
|
||||
max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
|
||||
target_grey_fraction = 0.3;
|
||||
|
||||
s->dc_gain_enabled = false;
|
||||
s->gain_idx = ANALOG_GAIN_REC_IDX;
|
||||
s->exposure_time = 5;
|
||||
s->cur_ev[0] = s->cur_ev[1] = s->cur_ev[2] = (s->dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[s->gain_idx] * s->exposure_time;
|
||||
dc_gain_enabled = false;
|
||||
gain_idx = ANALOG_GAIN_REC_IDX;
|
||||
exposure_time = 5;
|
||||
cur_ev[0] = cur_ev[1] = cur_ev[2] = (dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[gain_idx] * exposure_time;
|
||||
|
||||
s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
|
||||
buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
|
||||
}
|
||||
|
||||
static void camera_open(CameraState *s) {
|
||||
s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
|
||||
assert(s->sensor_fd >= 0);
|
||||
LOGD("opened sensor for %d", s->camera_num);
|
||||
void CameraState::camera_open() {
|
||||
int ret;
|
||||
sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num);
|
||||
assert(sensor_fd >= 0);
|
||||
LOGD("opened sensor for %d", camera_num);
|
||||
|
||||
// probe the sensor
|
||||
LOGD("-- Probing sensor %d", s->camera_num);
|
||||
sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
|
||||
LOGD("-- Probing sensor %d", camera_num);
|
||||
ret = sensors_init();
|
||||
if (ret != 0) {
|
||||
LOGD("AR0231 init failed, trying IMX390");
|
||||
camera_id = CAMERA_ID_IMX390;
|
||||
ret = sensors_init();
|
||||
}
|
||||
assert(ret == 0);
|
||||
|
||||
// create session
|
||||
struct cam_req_mgr_session_info session_info = {};
|
||||
int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
|
||||
LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
|
||||
s->session_handle = session_info.session_hdl;
|
||||
session_handle = session_info.session_hdl;
|
||||
|
||||
// access the sensor
|
||||
LOGD("-- Accessing sensor");
|
||||
auto sensor_dev_handle = device_acquire(s->sensor_fd, s->session_handle, nullptr);
|
||||
assert(sensor_dev_handle);
|
||||
s->sensor_dev_handle = *sensor_dev_handle;
|
||||
auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr);
|
||||
assert(sensor_dev_handle_);
|
||||
sensor_dev_handle = *sensor_dev_handle_;
|
||||
LOGD("acquire sensor dev");
|
||||
|
||||
struct cam_isp_in_port_info in_port_info = {
|
||||
.res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[s->camera_num],
|
||||
.res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num],
|
||||
|
||||
.lane_type = CAM_ISP_LANE_TYPE_DPHY,
|
||||
.lane_num = 4,
|
||||
.lane_cfg = 0x3210,
|
||||
|
||||
.vc = 0x0,
|
||||
// .dt = 0x2C; //CSI_RAW12
|
||||
.dt = 0x2B, //CSI_RAW10
|
||||
.format = CAM_FORMAT_MIPI_RAW_10,
|
||||
.dt = 0x2C, // CSI_RAW12
|
||||
.format = CAM_FORMAT_MIPI_RAW_12,
|
||||
|
||||
.test_pattern = 0x2, // 0x3?
|
||||
.usage_type = 0x0,
|
||||
|
|
@ -636,7 +676,7 @@ static void camera_open(CameraState *s) {
|
|||
.num_out_res = 0x1,
|
||||
.data[0] = (struct cam_isp_out_port_info){
|
||||
.res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
|
||||
.format = CAM_FORMAT_MIPI_RAW_10,
|
||||
.format = CAM_FORMAT_MIPI_RAW_12,
|
||||
.width = FRAME_WIDTH,
|
||||
.height = FRAME_HEIGHT,
|
||||
.comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
|
||||
|
|
@ -649,37 +689,40 @@ static void camera_open(CameraState *s) {
|
|||
.length = sizeof(in_port_info),
|
||||
};
|
||||
|
||||
auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
|
||||
assert(isp_dev_handle);
|
||||
s->isp_dev_handle = *isp_dev_handle;
|
||||
auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource);
|
||||
assert(isp_dev_handle_);
|
||||
isp_dev_handle = *isp_dev_handle_;
|
||||
LOGD("acquire isp dev");
|
||||
|
||||
s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
|
||||
assert(s->csiphy_fd >= 0);
|
||||
LOGD("opened csiphy for %d", s->camera_num);
|
||||
csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", camera_num);
|
||||
assert(csiphy_fd >= 0);
|
||||
LOGD("opened csiphy for %d", camera_num);
|
||||
|
||||
struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
|
||||
auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
|
||||
assert(csiphy_dev_handle);
|
||||
s->csiphy_dev_handle = *csiphy_dev_handle;
|
||||
auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info);
|
||||
assert(csiphy_dev_handle_);
|
||||
csiphy_dev_handle = *csiphy_dev_handle_;
|
||||
LOGD("acquire csiphy dev");
|
||||
|
||||
// config ISP
|
||||
alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu);
|
||||
config_isp(s, 0, 0, 1, s->buf0_handle, 0);
|
||||
alloc_w_mmu_hdl(multi_cam_state->video0_fd, 984480, (uint32_t*)&buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, multi_cam_state->device_iommu, multi_cam_state->cdm_iommu);
|
||||
config_isp(0, 0, 1, buf0_handle, 0);
|
||||
|
||||
LOG("-- Configuring sensor");
|
||||
sensors_i2c(s, init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
//sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
|
||||
//sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
|
||||
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
sensors_i2c(init_array_imx390, std::size(init_array_imx390), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
// config csiphy
|
||||
LOG("-- Config CSI PHY");
|
||||
{
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 1;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
|
|
@ -688,7 +731,7 @@ static void camera_open(CameraState *s) {
|
|||
buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info);
|
||||
buf_desc[0].type = CAM_CMD_BUF_GENERIC;
|
||||
|
||||
struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
csiphy_info->lane_mask = 0x1f;
|
||||
csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels??
|
||||
csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane
|
||||
|
|
@ -698,54 +741,51 @@ static void camera_open(CameraState *s) {
|
|||
csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
|
||||
csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py
|
||||
|
||||
int ret_ = device_config(s->csiphy_fd, s->session_handle, s->csiphy_dev_handle, cam_packet_handle);
|
||||
int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle);
|
||||
assert(ret_ == 0);
|
||||
|
||||
munmap(csiphy_info, buf_desc[0].size);
|
||||
release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
// link devices
|
||||
LOG("-- Link devices");
|
||||
struct cam_req_mgr_link_info req_mgr_link_info = {0};
|
||||
req_mgr_link_info.session_hdl = s->session_handle;
|
||||
req_mgr_link_info.session_hdl = session_handle;
|
||||
req_mgr_link_info.num_devices = 2;
|
||||
req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
|
||||
req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
|
||||
s->link_handle = req_mgr_link_info.link_hdl;
|
||||
LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
|
||||
req_mgr_link_info.dev_hdls[0] = isp_dev_handle;
|
||||
req_mgr_link_info.dev_hdls[1] = sensor_dev_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
|
||||
link_handle = req_mgr_link_info.link_hdl;
|
||||
LOGD("link: %d hdl: 0x%X", ret, link_handle);
|
||||
|
||||
struct cam_req_mgr_link_control req_mgr_link_control = {0};
|
||||
req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
|
||||
req_mgr_link_control.session_hdl = s->session_handle;
|
||||
req_mgr_link_control.session_hdl = session_handle;
|
||||
req_mgr_link_control.num_links = 1;
|
||||
req_mgr_link_control.link_hdls[0] = s->link_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
req_mgr_link_control.link_hdls[0] = link_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
LOGD("link control: %d", ret);
|
||||
|
||||
ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
|
||||
ret = device_control(csiphy_fd, CAM_START_DEV, session_handle, csiphy_dev_handle);
|
||||
LOGD("start csiphy: %d", ret);
|
||||
ret = device_control(s->multi_cam_state->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
|
||||
ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle);
|
||||
LOGD("start isp: %d", ret);
|
||||
ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
|
||||
ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle);
|
||||
LOGD("start sensor: %d", ret);
|
||||
|
||||
enqueue_req_multi(s, 1, FRAME_BUF_COUNT, 0);
|
||||
enqueue_req_multi(1, FRAME_BUF_COUNT, 0);
|
||||
}
|
||||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
|
||||
camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
|
||||
s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
|
||||
printf("driver camera initted \n");
|
||||
if (!env_only_driver) {
|
||||
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
|
||||
s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD); // swap left/right
|
||||
printf("road camera initted \n");
|
||||
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
|
||||
s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD);
|
||||
printf("wide road camera initted \n");
|
||||
}
|
||||
|
||||
|
|
@ -768,7 +808,7 @@ void cameras_open(MultiCameraState *s) {
|
|||
LOGD("opened video1");
|
||||
|
||||
// looks like there's only one of these
|
||||
s->isp_fd = HANDLE_EINTR(open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK));
|
||||
s->isp_fd = open_v4l_by_name_and_index("cam-isp");
|
||||
assert(s->isp_fd >= 0);
|
||||
LOGD("opened isp");
|
||||
|
||||
|
|
@ -779,7 +819,7 @@ void cameras_open(MultiCameraState *s) {
|
|||
query_cap_cmd.handle_type = 1;
|
||||
query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd;
|
||||
query_cap_cmd.size = sizeof(isp_query_cap_cmd);
|
||||
ret = cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
|
||||
ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
|
||||
assert(ret == 0);
|
||||
LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure);
|
||||
LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure);
|
||||
|
|
@ -794,74 +834,72 @@ void cameras_open(MultiCameraState *s) {
|
|||
ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
|
||||
printf("req mgr subscribe: %d\n", ret);
|
||||
|
||||
camera_open(&s->driver_cam);
|
||||
s->driver_cam.camera_open();
|
||||
printf("driver camera opened \n");
|
||||
if (!env_only_driver) {
|
||||
camera_open(&s->road_cam);
|
||||
s->road_cam.camera_open();
|
||||
printf("road camera opened \n");
|
||||
camera_open(&s->wide_road_cam);
|
||||
s->wide_road_cam.camera_open();
|
||||
printf("wide road camera opened \n");
|
||||
}
|
||||
}
|
||||
|
||||
static void camera_close(CameraState *s) {
|
||||
void CameraState::camera_close() {
|
||||
int ret;
|
||||
|
||||
// stop devices
|
||||
LOG("-- Stop devices");
|
||||
// ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
|
||||
// ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle);
|
||||
// LOGD("stop sensor: %d", ret);
|
||||
ret = device_control(s->multi_cam_state->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
|
||||
ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle);
|
||||
LOGD("stop isp: %d", ret);
|
||||
ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle);
|
||||
ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle);
|
||||
LOGD("stop csiphy: %d", ret);
|
||||
// link control stop
|
||||
LOG("-- Stop link control");
|
||||
static struct cam_req_mgr_link_control req_mgr_link_control = {0};
|
||||
req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE;
|
||||
req_mgr_link_control.session_hdl = s->session_handle;
|
||||
req_mgr_link_control.session_hdl = session_handle;
|
||||
req_mgr_link_control.num_links = 1;
|
||||
req_mgr_link_control.link_hdls[0] = s->link_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
req_mgr_link_control.link_hdls[0] = link_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
LOGD("link control stop: %d", ret);
|
||||
|
||||
// unlink
|
||||
LOG("-- Unlink");
|
||||
static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0};
|
||||
req_mgr_unlink_info.session_hdl = s->session_handle;
|
||||
req_mgr_unlink_info.link_hdl = s->link_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
|
||||
req_mgr_unlink_info.session_hdl = session_handle;
|
||||
req_mgr_unlink_info.link_hdl = link_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
|
||||
LOGD("unlink: %d", ret);
|
||||
|
||||
// release devices
|
||||
LOGD("-- Release devices");
|
||||
ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle);
|
||||
ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle);
|
||||
LOGD("release sensor: %d", ret);
|
||||
ret = device_control(s->multi_cam_state->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle);
|
||||
ret = device_control(multi_cam_state->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle);
|
||||
LOGD("release isp: %d", ret);
|
||||
ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle);
|
||||
ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle);
|
||||
LOGD("release csiphy: %d", ret);
|
||||
|
||||
// destroyed session
|
||||
struct cam_req_mgr_session_info session_info = {.session_hdl = s->session_handle};
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
|
||||
struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle};
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
|
||||
LOGD("destroyed session: %d", ret);
|
||||
}
|
||||
|
||||
void cameras_close(MultiCameraState *s) {
|
||||
camera_close(&s->driver_cam);
|
||||
s->driver_cam.camera_close();
|
||||
if (!env_only_driver) {
|
||||
camera_close(&s->road_cam);
|
||||
camera_close(&s->wide_road_cam);
|
||||
s->road_cam.camera_close();
|
||||
s->wide_road_cam.camera_close();
|
||||
}
|
||||
|
||||
delete s->sm;
|
||||
delete s->pm;
|
||||
}
|
||||
|
||||
// ******************* just a helper *******************
|
||||
|
||||
void handle_camera_event(CameraState *s, void *evdat) {
|
||||
void CameraState::handle_camera_event(void *evdat) {
|
||||
struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
|
||||
|
||||
uint64_t timestamp = event_data->u.frame_msg.timestamp;
|
||||
|
|
@ -869,53 +907,53 @@ void handle_camera_event(CameraState *s, void *evdat) {
|
|||
int real_id = event_data->u.frame_msg.request_id;
|
||||
|
||||
if (real_id != 0) { // next ready
|
||||
if (real_id == 1) {s->idx_offset = main_id;}
|
||||
if (real_id == 1) {idx_offset = main_id;}
|
||||
int buf_idx = (real_id - 1) % FRAME_BUF_COUNT;
|
||||
|
||||
// check for skipped frames
|
||||
if (main_id > s->frame_id_last + 1 && !s->skipped) {
|
||||
if (main_id > frame_id_last + 1 && !skipped) {
|
||||
// realign
|
||||
clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0);
|
||||
s->skipped = true;
|
||||
} else if (main_id == s->frame_id_last + 1) {
|
||||
s->skipped = false;
|
||||
clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(real_id + 1, FRAME_BUF_COUNT - 1, 0);
|
||||
skipped = true;
|
||||
} else if (main_id == frame_id_last + 1) {
|
||||
skipped = false;
|
||||
}
|
||||
|
||||
// check for dropped requests
|
||||
if (real_id > s->request_id_last + 1) {
|
||||
enqueue_req_multi(s, s->request_id_last + 1 + FRAME_BUF_COUNT, real_id - (s->request_id_last + 1), 0);
|
||||
if (real_id > request_id_last + 1) {
|
||||
enqueue_req_multi(request_id_last + 1 + FRAME_BUF_COUNT, real_id - (request_id_last + 1), 0);
|
||||
}
|
||||
|
||||
// metas
|
||||
s->frame_id_last = main_id;
|
||||
s->request_id_last = real_id;
|
||||
frame_id_last = main_id;
|
||||
request_id_last = real_id;
|
||||
|
||||
auto &meta_data = s->buf.camera_bufs_metadata[buf_idx];
|
||||
meta_data.frame_id = main_id - s->idx_offset;
|
||||
auto &meta_data = buf.camera_bufs_metadata[buf_idx];
|
||||
meta_data.frame_id = main_id - idx_offset;
|
||||
meta_data.timestamp_sof = timestamp;
|
||||
s->exp_lock.lock();
|
||||
meta_data.gain = s->dc_gain_enabled ? s->analog_gain_frac * DC_GAIN : s->analog_gain_frac;
|
||||
meta_data.high_conversion_gain = s->dc_gain_enabled;
|
||||
meta_data.integ_lines = s->exposure_time;
|
||||
meta_data.measured_grey_fraction = s->measured_grey_fraction;
|
||||
meta_data.target_grey_fraction = s->target_grey_fraction;
|
||||
s->exp_lock.unlock();
|
||||
exp_lock.lock();
|
||||
meta_data.gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac;
|
||||
meta_data.high_conversion_gain = dc_gain_enabled;
|
||||
meta_data.integ_lines = exposure_time;
|
||||
meta_data.measured_grey_fraction = measured_grey_fraction;
|
||||
meta_data.target_grey_fraction = target_grey_fraction;
|
||||
exp_lock.unlock();
|
||||
|
||||
// dispatch
|
||||
enqueue_req_multi(s, real_id + FRAME_BUF_COUNT, 1, 1);
|
||||
enqueue_req_multi(real_id + FRAME_BUF_COUNT, 1, 1);
|
||||
} else { // not ready
|
||||
// reset after half second of no response
|
||||
if (main_id > s->frame_id_last + 10) {
|
||||
clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0);
|
||||
s->frame_id_last = main_id;
|
||||
s->skipped = true;
|
||||
if (main_id > frame_id_last + 10) {
|
||||
clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(request_id_last + 1, FRAME_BUF_COUNT, 0);
|
||||
frame_id_last = main_id;
|
||||
skipped = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void set_camera_exposure(CameraState *s, float grey_frac) {
|
||||
void CameraState::set_camera_exposure(float grey_frac) {
|
||||
const float dt = 0.05;
|
||||
|
||||
const float ts_grey = 10.0;
|
||||
|
|
@ -929,15 +967,15 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
|||
// Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
|
||||
// TODO: Lower latency to 2 frames, by using the histogram outputed by the sensor we can do AE before the debayering is complete
|
||||
|
||||
const float cur_ev = s->cur_ev[s->buf.cur_frame_data.frame_id % 3];
|
||||
const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3];
|
||||
|
||||
// Scale target grey between 0.1 and 0.4 depending on lighting conditions
|
||||
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev) / log2(6000.0), 0.1, 0.4);
|
||||
float target_grey = (1.0 - k_grey) * s->target_grey_fraction + k_grey * new_target_grey;
|
||||
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev_) / log2(6000.0), 0.1, 0.4);
|
||||
float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey;
|
||||
|
||||
float desired_ev = std::clamp(cur_ev * target_grey / grey_frac, s->min_ev, s->max_ev);
|
||||
float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev);
|
||||
float k = (1.0 - k_ev) / 3.0;
|
||||
desired_ev = (k * s->cur_ev[0]) + (k * s->cur_ev[1]) + (k * s->cur_ev[2]) + (k_ev * desired_ev);
|
||||
desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev);
|
||||
|
||||
float best_ev_score = 1e6;
|
||||
int new_g = 0;
|
||||
|
|
@ -945,7 +983,7 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
|||
|
||||
// Hysteresis around high conversion gain
|
||||
// We usually want this on since it results in lower noise, but turn off in very bright day scenes
|
||||
bool enable_dc_gain = s->dc_gain_enabled;
|
||||
bool enable_dc_gain = dc_gain_enabled;
|
||||
if (!enable_dc_gain && target_grey < 0.2) {
|
||||
enable_dc_gain = true;
|
||||
} else if (enable_dc_gain && target_grey > 0.3) {
|
||||
|
|
@ -954,14 +992,14 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
|||
|
||||
// Simple brute force optimizer to choose sensor parameters
|
||||
// to reach desired EV
|
||||
for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, s->gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, s->gain_idx + 1); g++) {
|
||||
for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, gain_idx + 1); g++) {
|
||||
float gain = sensor_analog_gains[g] * (enable_dc_gain ? DC_GAIN : 1);
|
||||
|
||||
// Compute optimal time for given gain
|
||||
int t = std::clamp(int(std::round(desired_ev / gain)), EXPOSURE_TIME_MIN, EXPOSURE_TIME_MAX);
|
||||
|
||||
// Only go below recomended gain when absolutely necessary to not overexpose
|
||||
if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < s->gain_idx) {
|
||||
if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < gain_idx) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -972,10 +1010,10 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
|||
float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1;
|
||||
score += std::abs(g - (int)ANALOG_GAIN_REC_IDX) * m;
|
||||
|
||||
// LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", s->camera_num, g, t, desired_ev / gain, score, score + std::abs(g - s->gain_idx) * (score + 1.0) / 10.0, desired_ev, s->min_ev);
|
||||
// LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev);
|
||||
|
||||
// Small penalty on changing gain
|
||||
score += std::abs(g - s->gain_idx) * (score + 1.0) / 10.0;
|
||||
score += std::abs(g - gain_idx) * (score + 1.0) / 10.0;
|
||||
|
||||
if (score < best_ev_score) {
|
||||
new_t = t;
|
||||
|
|
@ -984,42 +1022,55 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
|||
}
|
||||
}
|
||||
|
||||
s->exp_lock.lock();
|
||||
exp_lock.lock();
|
||||
|
||||
s->measured_grey_fraction = grey_frac;
|
||||
s->target_grey_fraction = target_grey;
|
||||
measured_grey_fraction = grey_frac;
|
||||
target_grey_fraction = target_grey;
|
||||
|
||||
s->analog_gain_frac = sensor_analog_gains[new_g];
|
||||
s->gain_idx = new_g;
|
||||
s->exposure_time = new_t;
|
||||
s->dc_gain_enabled = enable_dc_gain;
|
||||
analog_gain_frac = sensor_analog_gains[new_g];
|
||||
gain_idx = new_g;
|
||||
exposure_time = new_t;
|
||||
dc_gain_enabled = enable_dc_gain;
|
||||
|
||||
float gain = s->analog_gain_frac * (s->dc_gain_enabled ? DC_GAIN : 1.0);
|
||||
s->cur_ev[s->buf.cur_frame_data.frame_id % 3] = s->exposure_time * gain;
|
||||
float gain = analog_gain_frac * (dc_gain_enabled ? DC_GAIN : 1.0);
|
||||
cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain;
|
||||
|
||||
s->exp_lock.unlock();
|
||||
exp_lock.unlock();
|
||||
|
||||
// Processing a frame takes right about 50ms, so we need to wait a few ms
|
||||
// so we don't send i2c commands around the frame start.
|
||||
int ms = (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof) / 1000000;
|
||||
int ms = (nanos_since_boot() - buf.cur_frame_data.timestamp_sof) / 1000000;
|
||||
if (ms < 60) {
|
||||
util::sleep_for(60 - ms);
|
||||
}
|
||||
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", s->camera_num, 1e-9 * nanos_since_boot(), 1e-9 * s->buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof));
|
||||
|
||||
uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x3366, analog_gain_reg},
|
||||
{0x3362, (uint16_t)(s->dc_gain_enabled ? 0x1 : 0x0)},
|
||||
{0x3012, (uint16_t)s->exposure_time},
|
||||
};
|
||||
sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
|
||||
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
|
||||
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x3366, analog_gain_reg},
|
||||
{0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
|
||||
{0x3012, (uint16_t)exposure_time},
|
||||
};
|
||||
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
// if gain is sub 1, we have to use exposure to mimic sub 1 gains
|
||||
uint32_t real_exposure_time = (gain < 1.0) ? (exposure_time*gain) : exposure_time;
|
||||
// invert real_exposure_time, max exposure is 2
|
||||
real_exposure_time = (exposure_time >= 0x7cf) ? 2 : (0x7cf - exposure_time);
|
||||
uint32_t real_gain = int((10*log10(fmax(1.0, gain)))/0.3);
|
||||
//printf("%d expose: %d gain: %f = %d\n", camera_num, exposure_time, gain, real_gain);
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x000c, real_exposure_time&0xFF}, {0x000d, real_exposure_time>>8},
|
||||
{0x0010, real_exposure_time&0xFF}, {0x0011, real_exposure_time>>8},
|
||||
{0x0018, real_gain&0xFF}, {0x0019, real_gain>>8},
|
||||
};
|
||||
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
|
||||
}
|
||||
}
|
||||
|
||||
void camera_autoexposure(CameraState *s, float grey_frac) {
|
||||
set_camera_exposure(s, grey_frac);
|
||||
s->set_camera_exposure(grey_frac);
|
||||
}
|
||||
|
||||
// called by processing_thread
|
||||
|
|
@ -1053,11 +1104,10 @@ void cameras_run(MultiCameraState *s) {
|
|||
|
||||
// start devices
|
||||
LOG("-- Starting devices");
|
||||
int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
s->driver_cam.sensors_start();
|
||||
if (!env_only_driver) {
|
||||
sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
s->road_cam.sensors_start();
|
||||
s->wide_road_cam.sensors_start();
|
||||
}
|
||||
|
||||
// poll events
|
||||
|
|
@ -1088,11 +1138,11 @@ void cameras_run(MultiCameraState *s) {
|
|||
}
|
||||
|
||||
if (event_data->session_hdl == s->road_cam.session_handle) {
|
||||
handle_camera_event(&s->road_cam, event_data);
|
||||
s->road_cam.handle_camera_event(event_data);
|
||||
} else if (event_data->session_hdl == s->wide_road_cam.session_handle) {
|
||||
handle_camera_event(&s->wide_road_cam, event_data);
|
||||
s->wide_road_cam.handle_camera_event(event_data);
|
||||
} else if (event_data->session_hdl == s->driver_cam.session_handle) {
|
||||
handle_camera_event(&s->driver_cam, event_data);
|
||||
s->driver_cam.handle_camera_event(event_data);
|
||||
} else {
|
||||
printf("Unknown vidioc event source\n");
|
||||
assert(false);
|
||||
|
|
|
|||
|
|
@ -9,7 +9,8 @@
|
|||
|
||||
#define FRAME_BUF_COUNT 4
|
||||
|
||||
typedef struct CameraState {
|
||||
class CameraState {
|
||||
public:
|
||||
MultiCameraState *multi_cam_state;
|
||||
CameraInfo ci;
|
||||
|
||||
|
|
@ -31,6 +32,21 @@ typedef struct CameraState {
|
|||
|
||||
int camera_num;
|
||||
|
||||
void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
|
||||
void enqueue_req_multi(int start, int n, bool dp);
|
||||
void enqueue_buffer(int i, bool dp);
|
||||
void handle_camera_event(void *evdat);
|
||||
void set_camera_exposure(float grey_frac);
|
||||
|
||||
void sensors_start();
|
||||
void sensors_poke(int request_id);
|
||||
void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
|
||||
int sensors_init();
|
||||
|
||||
void camera_open();
|
||||
void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type);
|
||||
void camera_close();
|
||||
|
||||
int32_t session_handle;
|
||||
int32_t sensor_dev_handle;
|
||||
int32_t isp_dev_handle;
|
||||
|
|
@ -46,9 +62,10 @@ typedef struct CameraState {
|
|||
int frame_id_last;
|
||||
int idx_offset;
|
||||
bool skipped;
|
||||
int camera_id;
|
||||
|
||||
CameraBuf buf;
|
||||
} CameraState;
|
||||
};
|
||||
|
||||
typedef struct MultiCameraState {
|
||||
unique_fd video0_fd;
|
||||
|
|
@ -57,7 +74,6 @@ typedef struct MultiCameraState {
|
|||
int device_iommu;
|
||||
int cdm_iommu;
|
||||
|
||||
|
||||
CameraState road_cam;
|
||||
CameraState wide_road_cam;
|
||||
CameraState driver_cam;
|
||||
|
|
|
|||
|
|
@ -98,9 +98,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
|
|||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
|
||||
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
|
||||
VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
|
||||
// camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
|
||||
// VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
|
||||
// VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
|
||||
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -141,9 +141,9 @@ void driver_camera_thread(CameraState *s) {
|
|||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
|
||||
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
|
||||
VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
|
||||
camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
|
||||
VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
|
||||
VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
|
||||
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,5 @@
|
|||
#pragma OPENCL EXTENSION cl_khr_fp16 : enable
|
||||
|
||||
const half black_level = 42.0;
|
||||
|
||||
const __constant half3 color_correction[3] = {
|
||||
// post wb CCM
|
||||
(half3)(1.82717181, -0.31231438, 0.07307673),
|
||||
|
|
@ -39,17 +37,17 @@ half3 color_correct(half3 rgb) {
|
|||
return ret;
|
||||
}
|
||||
|
||||
half val_from_10(const uchar * source, int gx, int gy) {
|
||||
// parse 10bit
|
||||
int start = gy * FRAME_STRIDE + (5 * (gx / 4));
|
||||
int offset = gx % 4;
|
||||
uint major = (uint)source[start + offset] << 2;
|
||||
uint minor = (source[start + 4] >> (2 * offset)) & 3;
|
||||
half pv = (half)(major + minor);
|
||||
inline half val_from_10(const uchar * source, int gx, int gy, half black_level) {
|
||||
// parse 12bit
|
||||
int start = gy * FRAME_STRIDE + (3 * (gx / 2));
|
||||
int offset = gx % 2;
|
||||
uint major = (uint)source[start + offset] << 4;
|
||||
uint minor = (source[start + 2] >> (4 * offset)) & 0xf;
|
||||
half pv = (half)((major + minor)/4);
|
||||
|
||||
// normalize
|
||||
pv = max(0.0h, pv - black_level);
|
||||
pv *= 0.00101833h; // /= (1024.0f - black_level);
|
||||
pv /= (1024.0f - black_level);
|
||||
|
||||
// correct vignetting
|
||||
if (CAM_NUM == 1) { // fcamera
|
||||
|
|
@ -89,7 +87,8 @@ half phi(half x) {
|
|||
|
||||
__kernel void debayer10(const __global uchar * in,
|
||||
__global uchar * out,
|
||||
__local half * cached
|
||||
__local half * cached,
|
||||
float black_level
|
||||
)
|
||||
{
|
||||
const int x_global = get_global_id(0);
|
||||
|
|
@ -102,7 +101,7 @@ __kernel void debayer10(const __global uchar * in,
|
|||
|
||||
int out_idx = 3 * x_global + 3 * y_global * RGB_WIDTH;
|
||||
|
||||
half pv = val_from_10(in, x_global, y_global);
|
||||
half pv = val_from_10(in, x_global, y_global, black_level);
|
||||
cached[localOffset] = pv;
|
||||
|
||||
// don't care
|
||||
|
|
@ -118,22 +117,22 @@ __kernel void debayer10(const __global uchar * in,
|
|||
if (x_local < 1) {
|
||||
localColOffset = x_local;
|
||||
globalColOffset = -1;
|
||||
cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-1, y_global);
|
||||
cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-1, y_global, black_level);
|
||||
} else if (x_local >= get_local_size(0) - 1) {
|
||||
localColOffset = x_local + 2;
|
||||
globalColOffset = 1;
|
||||
cached[localOffset + 1] = val_from_10(in, x_global+1, y_global);
|
||||
cached[localOffset + 1] = val_from_10(in, x_global+1, y_global, black_level);
|
||||
}
|
||||
|
||||
if (y_local < 1) {
|
||||
cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-1);
|
||||
cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-1, black_level);
|
||||
if (localColOffset != -1) {
|
||||
cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global-1);
|
||||
cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global-1, black_level);
|
||||
}
|
||||
} else if (y_local >= get_local_size(1) - 1) {
|
||||
cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+1);
|
||||
cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+1, black_level);
|
||||
if (localColOffset != -1) {
|
||||
cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global+1);
|
||||
cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global+1, black_level);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,49 @@
|
|||
struct i2c_random_wr_payload start_reg_array[] = {{0x301A, 0x91C}};
|
||||
struct i2c_random_wr_payload stop_reg_array[] = {{0x301A, 0x918}};
|
||||
struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
|
||||
struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
|
||||
struct i2c_random_wr_payload start_reg_array_imx390[] = {{0x0, 0}};
|
||||
struct i2c_random_wr_payload stop_reg_array_imx390[] = {{0x0, 1}};
|
||||
|
||||
struct i2c_random_wr_payload init_array_imx390[] = {
|
||||
{0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames
|
||||
{0x200C, 0xe4}, {0x200D, 0x0c}, // MODE_HMAX
|
||||
|
||||
// crop
|
||||
{0x3410, 0x88}, {0x3411, 0x7}, // CROP_H_SIZE
|
||||
{0x3418, 0xb8}, {0x3419, 0x4}, // CROP_V_SIZE
|
||||
{0x0078, 1}, {0x03c0, 1},
|
||||
|
||||
// external trigger (off)
|
||||
// while images still come in, they are blank with this
|
||||
{0x3650, 0}, // CU_MODE
|
||||
|
||||
// exposure
|
||||
{0x000c, 0xc0}, {0x000d, 0x07},
|
||||
{0x0010, 0xc0}, {0x0011, 0x07},
|
||||
|
||||
// WUXGA mode
|
||||
// not in datasheet, from https://github.com/bogsen/STLinux-Kernel/blob/master/drivers/media/platform/tegra/imx185.c
|
||||
{0x0086, 0xc4}, {0x0087, 0xff}, // WND_SHIFT_V = -60
|
||||
{0x03c6, 0xc4}, {0x03c7, 0xff}, // SM_WND_SHIFT_V_APL = -60
|
||||
|
||||
{0x201c, 0xe1}, {0x201d, 0x12}, // image read amount
|
||||
{0x21ee, 0xc4}, {0x21ef, 0x04}, // image send amount (1220 is the end)
|
||||
{0x21f0, 0xc4}, {0x21f1, 0x04}, // image processing amount
|
||||
|
||||
// disable a bunch of errors causing blanking
|
||||
{0x0390, 0x00}, {0x0391, 0x00}, {0x0392, 0x00},
|
||||
|
||||
// flip bayer
|
||||
{0x2D64, 0x64 + 2},
|
||||
|
||||
// color correction
|
||||
{0x0030, 0xf8}, {0x0031, 0x00}, // red gain
|
||||
{0x0032, 0x9a}, {0x0033, 0x00}, // gr gain
|
||||
{0x0034, 0x9a}, {0x0035, 0x00}, // gb gain
|
||||
{0x0036, 0x22}, {0x0037, 0x01}, // blue gain
|
||||
|
||||
// hdr enable (noise with this on for now)
|
||||
{0x00f9, 0}
|
||||
};
|
||||
|
||||
struct i2c_random_wr_payload init_array_ar0231[] = {
|
||||
{0x301A, 0x0018}, // RESET_REGISTER
|
||||
|
|
@ -9,7 +53,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
|
|||
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
|
||||
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
|
||||
{0x3030, 0x0032}, // PLL_MULTIPLIER
|
||||
{0x3036, 0x000A}, // OP_WORD_CLK_DIV
|
||||
{0x3036, 0x000C}, // OP_WORD_CLK_DIV
|
||||
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
|
||||
|
||||
// FORMAT
|
||||
|
|
@ -46,11 +90,11 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
|
|||
|
||||
// Readout Settings
|
||||
{0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
|
||||
{0x31AC, 0x0C0A}, // DATA_FORMAT_BITS, 12 -> 10
|
||||
{0x3342, 0x122B}, // MIPI_F1_PDT_EDT
|
||||
{0x3346, 0x122B}, // MIPI_F2_PDT_EDT
|
||||
{0x334A, 0x122B}, // MIPI_F3_PDT_EDT
|
||||
{0x334E, 0x122B}, // MIPI_F4_PDT_EDT
|
||||
{0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
|
||||
{0x3342, 0x122C}, // MIPI_F1_PDT_EDT
|
||||
{0x3346, 0x122C}, // MIPI_F2_PDT_EDT
|
||||
{0x334A, 0x122C}, // MIPI_F3_PDT_EDT
|
||||
{0x334E, 0x122C}, // MIPI_F4_PDT_EDT
|
||||
{0x3344, 0x0011}, // MIPI_F1_VDT_VC
|
||||
{0x3348, 0x0111}, // MIPI_F2_VDT_VC
|
||||
{0x334C, 0x0211}, // MIPI_F3_VDT_VC
|
||||
|
|
|
|||
|
|
@ -17,9 +17,9 @@ from selfdrive.manager.process_config import managed_processes
|
|||
LM_THRESH = 120 # defined in selfdrive/camerad/imgproc/utils.h
|
||||
|
||||
VISION_STREAMS = {
|
||||
"roadCameraState": VisionStreamType.VISION_STREAM_RGB_BACK,
|
||||
"driverCameraState": VisionStreamType.VISION_STREAM_RGB_FRONT,
|
||||
"wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE,
|
||||
"roadCameraState": VisionStreamType.VISION_STREAM_RGB_ROAD,
|
||||
"driverCameraState": VisionStreamType.VISION_STREAM_RGB_DRIVER,
|
||||
"wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE_ROAD,
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,54 @@
|
|||
# Supported Cars
|
||||
|
||||
A supported vehicle is one that just works when you install openpilot on a compatible device. Every car performs differently with openpilot, but we aim for all supported cars to provide a solid highway experience in the US market.
|
||||
|
||||
Cars are organized into three tiers:
|
||||
|
||||
- Gold - The best openpilot experience. Great highway driving with continual updates.
|
||||
- Silver - A solid highway experience, but is limited by stock longitudinal.
|
||||
- Bronze - A solid highway experience, but will have limited performance in stop-and-go. May have ACC and ALC speed limitations.
|
||||
|
||||
How We Rate The Cars
|
||||
---
|
||||
|
||||
### openpilot Adaptive Cruise Control (ACC)
|
||||
- {{Star.FULL.icon}} - openpilot is able to control the gas and brakes.
|
||||
- {{Star.HALF.icon}} - openpilot is able to control the gas and brakes with some restrictions.
|
||||
- {{Star.EMPTY.icon}} - The gas and brakes are controlled by the car's stock Adaptive Cruise Control (ACC) system.
|
||||
|
||||
### Stop and Go
|
||||
- {{Star.FULL.icon}} - Adaptive Cruise Control (ACC) operates down to 0 mph.
|
||||
- {{Star.EMPTY.icon}} - Adaptive Cruise Control (ACC) available only above certain speeds. See your car's manual for the minimum speed.
|
||||
|
||||
### Steer to 0
|
||||
- {{Star.FULL.icon}} - openpilot can control the steering wheel down to 0 mph.
|
||||
- {{Star.EMPTY.icon}} - No steering control below certain speeds.
|
||||
|
||||
### Steering Torque
|
||||
- {{Star.FULL.icon}} - Car has enough steering torque for comfortable highway driving.
|
||||
- {{Star.EMPTY.icon}} - Limited ability to make turns.
|
||||
|
||||
### Actively Maintained
|
||||
- {{Star.FULL.icon}} - Mainline software support, harness hardware sold by comma, lots of users, primary development target.
|
||||
- {{Star.EMPTY.icon}} - Low user count, community maintained, harness hardware not sold by comma.
|
||||
|
||||
**All supported cars can move between the tiers as support changes.**
|
||||
|
||||
{% for tier, car_rows in tiers %}
|
||||
## {{tier}} Cars
|
||||
|
||||
|{{columns | join('|')}}|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|
|
||||
{% for row in car_rows %}
|
||||
|{{row | join('|')}}|
|
||||
{% endfor %}
|
||||
|
||||
{% endfor %}
|
||||
|
||||
## Footnotes
|
||||
{% for footnote in footnotes %}
|
||||
<sup>{{loop.index}}</sup>{{footnote}} <br />
|
||||
{% endfor %}
|
||||
|
||||
## Community Maintained Cars
|
||||
Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
|
||||
|
|
@ -1,4 +1,6 @@
|
|||
import os
|
||||
from typing import Any, Dict, List
|
||||
|
||||
from common.params import Params
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.version import is_comma_remote, is_tested_branch
|
||||
|
|
@ -57,19 +59,25 @@ def load_interfaces(brand_names):
|
|||
return ret
|
||||
|
||||
|
||||
def _get_interface_names():
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car names that which we have an interface for
|
||||
# - values are lists of spefic car models for a given car
|
||||
def get_interface_attr(attr: str) -> Dict[str, Any]:
|
||||
# returns given attribute from each interface
|
||||
brand_names = {}
|
||||
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
||||
for car_folder in sorted([x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]):
|
||||
try:
|
||||
brand_name = car_folder.split('/')[-1]
|
||||
model_names = __import__(f'selfdrive.car.{brand_name}.values', fromlist=['CAR']).CAR
|
||||
model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")]
|
||||
brand_names[brand_name] = model_names
|
||||
attr_data = getattr(__import__(f'selfdrive.car.{brand_name}.values', fromlist=[attr]), attr, None)
|
||||
brand_names[brand_name] = attr_data
|
||||
except (ImportError, OSError):
|
||||
pass
|
||||
return brand_names
|
||||
|
||||
|
||||
def _get_interface_names() -> Dict[str, List[str]]:
|
||||
# returns a dict of brand name and its respective models
|
||||
brand_names = {}
|
||||
for brand_name, model_names in get_interface_attr("CAR").items():
|
||||
model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")]
|
||||
brand_names[brand_name] = model_names
|
||||
|
||||
return brand_names
|
||||
|
||||
|
|
@ -173,11 +181,13 @@ def get_car(logcan, sendcan):
|
|||
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
|
||||
candidate = "mock"
|
||||
|
||||
CarInterface, CarController, CarState = interfaces[candidate]
|
||||
car_params = CarInterface.get_params(candidate, fingerprints, car_fw)
|
||||
car_params.carVin = vin
|
||||
car_params.carFw = car_fw
|
||||
car_params.fingerprintSource = source
|
||||
car_params.fuzzyFingerprint = not exact_match
|
||||
disable_radar = Params().get_bool("DisableRadar")
|
||||
|
||||
return CarInterface(car_params, CarController, CarState), car_params
|
||||
CarInterface, CarController, CarState = interfaces[candidate]
|
||||
CP = CarInterface.get_params(candidate, fingerprints, car_fw, disable_radar)
|
||||
CP.carVin = vin
|
||||
CP.carFw = car_fw
|
||||
CP.fingerprintSource = source
|
||||
CP.fuzzyFingerprint = not exact_match
|
||||
|
||||
return CarInterface(CP, CarController, CarState), CP
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@ from opendbc.can.packer import CANPacker
|
|||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.ccframe = 0
|
||||
self.prev_frame = -1
|
||||
|
|
@ -29,11 +30,11 @@ class CarController():
|
|||
CS.out.steeringTorqueEps, CarControllerParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
|
||||
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
|
||||
moving_fast = CS.out.vEgo > self.CP.minSteerSpeed # for status message
|
||||
if CS.out.vEgo > (self.CP.minSteerSpeed - 0.5): # for command high bit
|
||||
self.gone_fast_yet = True
|
||||
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019):
|
||||
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
|
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
|
||||
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
|
||||
lkas_active = moving_fast and enabled
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
|
||||
|
||||
|
|
@ -58,7 +58,7 @@ class CarState(CarStateBase):
|
|||
ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"]
|
||||
ret.steerError = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
|
||||
ret.steerFaultPermanent = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
|
||||
|
||||
ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]["HIGH_BEAM_FLASH"])
|
||||
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
|
|||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "chrysler"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler)]
|
||||
|
|
|
|||
|
|
@ -1,4 +1,7 @@
|
|||
from typing import Dict, List, Union
|
||||
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
from cereal import car
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
|
@ -18,6 +21,16 @@ class CAR:
|
|||
JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk
|
||||
JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.PACIFICA_2017_HYBRID: CarInfo("Chrysler Pacifica Hybrid 2017-18", "Adaptive Cruise"),
|
||||
CAR.PACIFICA_2019_HYBRID: CarInfo("Chrysler Pacifica Hybrid 2019-21", "Adaptive Cruise"),
|
||||
CAR.PACIFICA_2018: CarInfo("Chrysler Pacifica 2017-18", "Adaptive Cruise"),
|
||||
CAR.PACIFICA_2020: CarInfo("Chrysler Pacifica 2020", "Adaptive Cruise"),
|
||||
CAR.JEEP_CHEROKEE: CarInfo("Jeep Grand Cherokee 2016-18", "Adaptive Cruise", "https://www.youtube.com/watch?v=eLR9o2JkuRk"),
|
||||
CAR.JEEP_CHEROKEE_2019: CarInfo("Jeep Grand Cherokee 2019-20", "Adaptive Cruise", "https://www.youtube.com/watch?v=jBe4lWnRSu4"),
|
||||
}
|
||||
|
||||
# Unique CAN messages:
|
||||
# Only the hybrids have 270: 8
|
||||
# Only the gas have 55: 8, 416: 7
|
||||
|
|
|
|||
|
|
@ -0,0 +1,66 @@
|
|||
#!/usr/bin/env python3
|
||||
import jinja2
|
||||
import os
|
||||
from enum import Enum
|
||||
from typing import Dict, Iterator, List, Tuple
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.car.docs_definitions import Column, Star, Tier
|
||||
from selfdrive.car.car_helpers import interfaces, get_interface_attr
|
||||
from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR as HKG_RADAR_START_ADDR
|
||||
from selfdrive.car.tests.routes import non_tested_cars
|
||||
|
||||
|
||||
def get_all_footnotes():
|
||||
all_footnotes = []
|
||||
for _, footnotes in get_interface_attr("Footnote").items():
|
||||
if footnotes is not None:
|
||||
all_footnotes += footnotes
|
||||
return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)}
|
||||
|
||||
|
||||
ALL_FOOTNOTES: Dict[Enum, int] = get_all_footnotes()
|
||||
CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md")
|
||||
CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md")
|
||||
|
||||
|
||||
def get_tier_car_rows() -> Iterator[Tuple[str, List[str]]]:
|
||||
tier_car_rows: Dict[Tier, list] = {tier: [] for tier in Tier}
|
||||
|
||||
for models in get_interface_attr("CAR_INFO").values():
|
||||
for model, car_info in models.items():
|
||||
# Hyundai exception: those with radar have openpilot longitudinal
|
||||
fingerprint = {0: {}, 1: {HKG_RADAR_START_ADDR: 8}, 2: {}, 3: {}}
|
||||
CP = interfaces[model][0].get_params(model, fingerprint=fingerprint, disable_radar=True)
|
||||
|
||||
if CP.dashcamOnly:
|
||||
continue
|
||||
|
||||
# A platform can include multiple car models
|
||||
if not isinstance(car_info, list):
|
||||
car_info = (car_info,)
|
||||
|
||||
for _car_info in car_info:
|
||||
stars = _car_info.get_stars(CP, non_tested_cars)
|
||||
tier = {5: Tier.GOLD, 4: Tier.SILVER}.get(stars.count(Star.FULL), Tier.BRONZE)
|
||||
tier_car_rows[tier].append(_car_info.get_row(ALL_FOOTNOTES, stars))
|
||||
|
||||
# Return tier title and car rows for each tier
|
||||
for tier, car_rows in tier_car_rows.items():
|
||||
yield tier.name.title(), sorted(car_rows)
|
||||
|
||||
|
||||
def generate_cars_md(tier_car_rows: Iterator[Tuple[str, List[str]]], template_fn: str) -> str:
|
||||
with open(template_fn, "r") as f:
|
||||
template = jinja2.Template(f.read(), trim_blocks=True)
|
||||
|
||||
footnotes = [fn.value.text for fn in ALL_FOOTNOTES]
|
||||
return template.render(tiers=tier_car_rows, columns=[column.value for column in Column],
|
||||
footnotes=footnotes, Star=Star)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# Auto generates supported cars documentation
|
||||
with open(CARS_MD_OUT, 'w') as f:
|
||||
f.write(generate_cars_md(get_tier_car_rows(), CARS_MD_TEMPLATE))
|
||||
print(f"Generated and written to {CARS_MD_OUT}")
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
from collections import namedtuple
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import List, Optional
|
||||
|
||||
|
||||
@dataclass
|
||||
class CarInfo:
|
||||
name: str
|
||||
package: str
|
||||
video_link: Optional[str] = None
|
||||
footnotes: Optional[List[Enum]] = None
|
||||
min_steer_speed: Optional[float] = None
|
||||
min_enable_speed: Optional[float] = None
|
||||
good_torque: bool = False
|
||||
|
||||
def get_stars(self, CP, non_tested_cars):
|
||||
# TODO: set all the min steer speeds in carParams and remove this
|
||||
min_steer_speed = CP.minSteerSpeed
|
||||
if self.min_steer_speed is not None:
|
||||
min_steer_speed = self.min_steer_speed
|
||||
assert CP.minSteerSpeed == 0, f"Minimum steer speed set in both CarInfo and CarParams for {CP.carFingerprint}"
|
||||
|
||||
# TODO: set all the min enable speeds in carParams correctly and remove this
|
||||
min_enable_speed = CP.minEnableSpeed
|
||||
if self.min_enable_speed is not None:
|
||||
min_enable_speed = self.min_enable_speed
|
||||
|
||||
stars = {
|
||||
Column.LONGITUDINAL: CP.openpilotLongitudinalControl and not CP.radarOffCan,
|
||||
Column.FSR_LONGITUDINAL: min_enable_speed <= 0.,
|
||||
Column.FSR_STEERING: min_steer_speed <= 0.,
|
||||
Column.STEERING_TORQUE: self.good_torque,
|
||||
Column.MAINTAINED: CP.carFingerprint not in non_tested_cars,
|
||||
}
|
||||
|
||||
for column in StarColumns:
|
||||
stars[column] = Star.FULL if stars[column] else Star.EMPTY
|
||||
|
||||
# Demote if footnote specifies a star
|
||||
footnote = get_footnote(self.footnotes, column)
|
||||
if footnote is not None and footnote.value.star is not None:
|
||||
stars[column] = footnote.value.star
|
||||
|
||||
return [stars[column] for column in StarColumns]
|
||||
|
||||
def get_row(self, all_footnotes, stars):
|
||||
# TODO: add YouTube vidos
|
||||
make, model = self.name.split(' ', 1)
|
||||
row = [make, model, self.package, *stars]
|
||||
|
||||
# Check for car footnotes and get star icons
|
||||
for row_idx, column in enumerate(Column):
|
||||
if column in StarColumns:
|
||||
row[row_idx] = row[row_idx].icon
|
||||
|
||||
footnote = get_footnote(self.footnotes, column)
|
||||
if footnote is not None:
|
||||
row[row_idx] += f"[<sup>{all_footnotes[footnote]}</sup>](#Footnotes)"
|
||||
|
||||
return row
|
||||
|
||||
|
||||
class Tier(Enum):
|
||||
GOLD = "Gold"
|
||||
SILVER = "Silver"
|
||||
BRONZE = "Bronze"
|
||||
|
||||
|
||||
class Column(Enum):
|
||||
MAKE = "Make"
|
||||
MODEL = "Model"
|
||||
PACKAGE = "Supported Package"
|
||||
LONGITUDINAL = "openpilot ACC"
|
||||
FSR_LONGITUDINAL = "Stop and Go"
|
||||
FSR_STEERING = "Steer to 0"
|
||||
STEERING_TORQUE = "Steering Torque"
|
||||
MAINTAINED = "Actively Maintained"
|
||||
|
||||
|
||||
class Star(Enum):
|
||||
FULL = "full"
|
||||
HALF = "half"
|
||||
EMPTY = "empty"
|
||||
|
||||
@property
|
||||
def icon(self):
|
||||
return f'<a href="#"><img valign="top" src="assets/icon-star-{self.value}.svg" width="22" /></a>'
|
||||
|
||||
|
||||
StarColumns = list(Column)[3:]
|
||||
CarFootnote = namedtuple("CarFootnote", ["text", "column", "star"], defaults=[None])
|
||||
|
||||
|
||||
def get_footnote(footnotes: Optional[List[Enum]], column: Column) -> Optional[Enum]:
|
||||
# Returns applicable footnote given current column
|
||||
if footnotes is not None:
|
||||
for fn in footnotes:
|
||||
if fn.value.column == column:
|
||||
return fn
|
||||
return None
|
||||
|
|
@ -1,7 +1,7 @@
|
|||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from common.conversions import Conversions as CV
|
||||
from common.numpy_fast import mean
|
||||
from selfdrive.config import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.ford.values import DBC
|
||||
|
||||
|
|
@ -23,7 +23,7 @@ class CarState(CarStateBase):
|
|||
ret.standstill = not ret.vEgoRaw > 0.001
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"]
|
||||
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"]
|
||||
ret.steerError = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1
|
||||
ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1
|
||||
ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS
|
||||
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in (0, 3))
|
||||
ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car.ford.values import MAX_ANGLE
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
|
@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
|
|||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "ford"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
|
|
@ -25,6 +25,8 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.steerRateCost = 1.0
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
tire_stiffness_factor = 0.5328
|
||||
# TODO: add minSteerSpeed
|
||||
ret.minEnableSpeed = 12. * CV.MPH_TO_MS
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
|
|
@ -64,7 +66,7 @@ class CarInterface(CarInterfaceBase):
|
|||
def apply(self, c):
|
||||
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.hudControl.visualAlert, c.cruiseControl.cancel)
|
||||
c.hudControl.visualAlert, c.cruiseControl.cancel)
|
||||
|
||||
self.frame += 1
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.ford.values import DBC
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_MSGS = list(range(0x500, 0x540))
|
||||
|
|
|
|||
|
|
@ -1,12 +1,21 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
from typing import Dict, List, Union
|
||||
|
||||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault
|
||||
|
||||
|
||||
class CAR:
|
||||
FUSION = "FORD FUSION 2018"
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.FUSION: CarInfo("Ford Fusion 2018", "All")
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,11 +1,11 @@
|
|||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import interp
|
||||
from selfdrive.config import Conversions as CV
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.gm import gmcan
|
||||
from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
|
@ -27,8 +27,7 @@ class CarController():
|
|||
self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar'])
|
||||
self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis'])
|
||||
|
||||
def update(self, c, enabled, CS, frame, actuators,
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
|
||||
P = self.params
|
||||
|
||||
|
|
@ -41,7 +40,7 @@ class CarController():
|
|||
if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
|
||||
self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
|
||||
elif (frame % P.STEER_STEP) == 0:
|
||||
lkas_enabled = c.active and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED
|
||||
lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED
|
||||
if lkas_enabled:
|
||||
new_steer = int(round(actuators.steer * P.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
|
||||
|
|
@ -58,7 +57,7 @@ class CarController():
|
|||
|
||||
# Gas/regen and brakes - all at 25Hz
|
||||
if (frame % 4) == 0:
|
||||
if not c.active:
|
||||
if not c.longActive:
|
||||
# Stock ECU sends max regen when not enabled.
|
||||
self.apply_gas = P.MAX_ACC_REGEN
|
||||
self.apply_brake = 0
|
||||
|
|
@ -68,15 +67,15 @@ class CarController():
|
|||
|
||||
idx = (frame // 4) % 4
|
||||
|
||||
at_full_stop = enabled and CS.out.standstill
|
||||
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
|
||||
at_full_stop = c.longActive and CS.out.standstill
|
||||
near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop))
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop))
|
||||
|
||||
# Send dashboard UI commands (ACC status), 25hz
|
||||
if (frame % 4) == 0:
|
||||
send_fcw = hud_alert == VisualAlert.fcw
|
||||
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
|
||||
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
|
||||
|
||||
# Radar needs to know current speed and yaw rate (50hz),
|
||||
# and that ADAS is alive (10hz)
|
||||
|
|
|
|||
|
|
@ -45,8 +45,8 @@ class CarState(CarStateBase):
|
|||
|
||||
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
|
||||
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
|
||||
ret.steerWarning = self.lkas_status == 2
|
||||
ret.steerError = self.lkas_status == 3
|
||||
ret.steerFaultTemporary = self.lkas_status == 2
|
||||
ret.steerFaultPermanent = self.lkas_status == 3
|
||||
|
||||
# 1 - open, 0 - closed
|
||||
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
|
||||
|
|
@ -59,7 +59,7 @@ class CarState(CarStateBase):
|
|||
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
|
||||
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
|
||||
|
||||
self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"]
|
||||
ret.parkingBrake = pt_cp.vl["EPBStatus"]["EPBClosed"] == 1
|
||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
|
||||
|
|
|
|||
|
|
@ -1,10 +1,11 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from math import fabs
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.gm.values import CAR, CruiseButtons, \
|
||||
AccState, CarControllerParams
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
|
@ -38,7 +39,7 @@ class CarInterface(CarInterfaceBase):
|
|||
return CarInterfaceBase.get_steer_feedforward_default
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "gm"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
|
||||
|
|
@ -46,7 +47,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/test/test_routes, we can remove it from this list.
|
||||
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
|
||||
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL}
|
||||
|
||||
# Presence of a camera on the object bus is ok.
|
||||
|
|
@ -191,8 +192,6 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
if ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if self.CS.park_brake:
|
||||
events.add(EventName.parkBrake)
|
||||
if ret.cruiseState.standstill:
|
||||
events.add(EventName.resumeRequired)
|
||||
if self.CS.pcm_acc_status == AccState.FAULTED:
|
||||
|
|
@ -222,12 +221,7 @@ class CarInterface(CarInterfaceBase):
|
|||
if hud_v_cruise > 70:
|
||||
hud_v_cruise = 0
|
||||
|
||||
# For Openpilot, "enabled" includes pre-enable.
|
||||
# In GM, PCM faults out if ACC command overlaps user gas.
|
||||
enabled = c.enabled and not self.CS.out.gasPressed
|
||||
|
||||
ret = self.CC.update(c, enabled, self.CS, self.frame,
|
||||
c.actuators,
|
||||
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
|
||||
hud_v_cruise, hud_control.lanesVisible,
|
||||
hud_control.leadVisible, hud_control.visualAlert)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,9 +1,9 @@
|
|||
#!/usr/bin/env python3
|
||||
import math
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.gm.values import DBC, CAR, CanBus
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_HEADER_MSG = 1120
|
||||
|
|
|
|||
|
|
@ -1,8 +1,13 @@
|
|||
from enum import Enum
|
||||
from typing import Dict, List, Union
|
||||
|
||||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
class CarControllerParams():
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_MAX = 300 # Safety limit, not LKA max. Trucks use 600.
|
||||
STEER_STEP = 2 # control frames per command
|
||||
STEER_DELTA_UP = 7
|
||||
|
|
@ -38,6 +43,7 @@ class CarControllerParams():
|
|||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
|
||||
class CAR:
|
||||
HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
|
||||
VOLT = "CHEVROLET VOLT PREMIER 2017"
|
||||
|
|
@ -47,6 +53,25 @@ class CAR:
|
|||
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
|
||||
ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016"
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
OBD_II = CarFootnote(
|
||||
"Requires an [OBD-II](https://comma.ai/shop/products/comma-car-harness) car harness and [community built ASCM harness]" +
|
||||
"(https://github.com/commaai/openpilot/wiki/GM#hardware). NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).",
|
||||
Column.MODEL)
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.HOLDEN_ASTRA: CarInfo("Holden Astra 2017", "Adaptive Cruise"),
|
||||
CAR.VOLT: CarInfo("Chevrolet Volt 2017-18", "Adaptive Cruise", footnotes=[Footnote.OBD_II], min_enable_speed=0),
|
||||
CAR.CADILLAC_ATS: CarInfo("Cadillac ATS Premium Performance 2018", "Adaptive Cruise"),
|
||||
CAR.MALIBU: CarInfo("Chevrolet Malibu Premier 2017", "Adaptive Cruise"),
|
||||
CAR.ACADIA: CarInfo("GMC Acadia 2018", "Adaptive Cruise", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo", footnotes=[Footnote.OBD_II]),
|
||||
CAR.BUICK_REGAL: CarInfo("Buick Regal Essence 2018", "Adaptive Cruise"),
|
||||
CAR.ESCALADE_ESV: CarInfo("Cadillac Escalade ESV 2016", "ACC + LKAS", footnotes=[Footnote.OBD_II]),
|
||||
}
|
||||
|
||||
|
||||
class CruiseButtons:
|
||||
INIT = 0
|
||||
UNPRESS = 1
|
||||
|
|
|
|||
|
|
@ -100,6 +100,7 @@ HUDData = namedtuple("HUDData",
|
|||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
|
|
@ -114,19 +115,19 @@ class CarController:
|
|||
|
||||
self.params = CarControllerParams(CP)
|
||||
|
||||
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
|
||||
def update(self, c, CS, frame, actuators, pcm_cancel_cmd,
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
|
||||
if active:
|
||||
if c.longActive:
|
||||
accel = actuators.accel
|
||||
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
|
||||
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint)
|
||||
else:
|
||||
accel = 0.0
|
||||
gas, brake = 0.0, 0.0
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
|
||||
CS.out.vEgo, CS.CP.carFingerprint)
|
||||
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady,
|
||||
CS.out.vEgo, self.CP.carFingerprint)
|
||||
|
||||
# *** rate limit after the enable check ***
|
||||
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
|
||||
|
|
@ -137,7 +138,7 @@ class CarController:
|
|||
else:
|
||||
hud_lanes = 0
|
||||
|
||||
if enabled:
|
||||
if c.enabled:
|
||||
if hud_show_car:
|
||||
hud_car = 2
|
||||
else:
|
||||
|
|
@ -153,19 +154,17 @@ class CarController:
|
|||
apply_steer = int(interp(-actuators.steer * self.params.STEER_MAX,
|
||||
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
|
||||
|
||||
lkas_active = active and not CS.steer_not_allowed
|
||||
|
||||
# Send CAN commands
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
|
||||
if self.CP.carFingerprint in HONDA_BOSCH and self.CP.openpilotLongitudinalControl:
|
||||
if frame % 10 == 0:
|
||||
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
|
||||
|
||||
# Send steering command.
|
||||
idx = frame % 4
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, c.latActive, self.CP.carFingerprint,
|
||||
idx, CS.CP.openpilotLongitudinalControl))
|
||||
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
|
|
@ -181,10 +180,10 @@ class CarController:
|
|||
0.5]
|
||||
# The Honda ODYSSEY seems to have different PCM_ACCEL
|
||||
# msgs, is it other cars too?
|
||||
if CS.CP.enableGasInterceptor:
|
||||
if self.CP.enableGasInterceptor:
|
||||
pcm_speed = 0.0
|
||||
pcm_accel = int(0.0)
|
||||
elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
pcm_speed_V = [0.0,
|
||||
clip(CS.out.vEgo - 3.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 0.0, 0.0, 100.0),
|
||||
|
|
@ -199,15 +198,15 @@ class CarController:
|
|||
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6)
|
||||
|
||||
if not CS.CP.openpilotLongitudinalControl:
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if frame % 2 == 0:
|
||||
idx = frame // 2
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx))
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint, idx))
|
||||
# If using stock ACC, spam cancel command to kill gas when OP disengages.
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, self.CP.carFingerprint))
|
||||
elif CS.out.cruiseState.standstill:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, self.CP.carFingerprint))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
|
|
@ -215,11 +214,11 @@ class CarController:
|
|||
idx = frame // 2
|
||||
ts = frame * DT_CTRL
|
||||
|
||||
if CS.CP.carFingerprint in HONDA_BOSCH:
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)
|
||||
self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas,
|
||||
idx, stopping, CS.CP.carFingerprint))
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas,
|
||||
idx, stopping, self.CP.carFingerprint))
|
||||
else:
|
||||
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))
|
||||
|
|
@ -228,18 +227,18 @@ class CarController:
|
|||
pcm_override = True
|
||||
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, fcw_display, idx,
|
||||
CS.CP.carFingerprint, CS.stock_brake))
|
||||
self.CP.carFingerprint, CS.stock_brake))
|
||||
self.apply_brake_last = apply_brake
|
||||
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
|
||||
|
||||
if CS.CP.enableGasInterceptor:
|
||||
if self.CP.enableGasInterceptor:
|
||||
# way too aggressive at low speed without this
|
||||
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
|
||||
# when you do enable.
|
||||
if active:
|
||||
if c.longActive:
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
|
||||
else:
|
||||
self.gas = 0.0
|
||||
|
|
@ -250,12 +249,12 @@ class CarController:
|
|||
idx = (frame // 10) % 4
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
|
||||
hud_lanes, fcw_display, acc_alert, steer_required)
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
|
||||
|
||||
if CS.CP.openpilotLongitudinalControl and (CS.CP.carFingerprint not in HONDA_BOSCH):
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
self.speed = pcm_speed
|
||||
|
||||
if not CS.CP.enableGasInterceptor:
|
||||
if not self.CP.enableGasInterceptor:
|
||||
self.gas = pcm_accel / 0xc6
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
|
|
|
|||
|
|
@ -1,9 +1,10 @@
|
|||
from cereal import car
|
||||
from collections import defaultdict
|
||||
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from common.numpy_fast import interp
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
|
||||
|
||||
|
|
@ -197,11 +198,10 @@ class CarState(CarStateBase):
|
|||
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
|
||||
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
|
||||
ret.steerError = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
|
||||
self.steer_not_allowed = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_2")
|
||||
ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
ret.steerWarning = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
|
||||
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
|
||||
|
|
@ -230,11 +230,10 @@ class CarState(CarStateBase):
|
|||
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
|
||||
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
|
||||
# TODO: set for all cars
|
||||
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
|
||||
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
|
||||
self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
|
||||
else:
|
||||
self.park_brake = 0 # TODO
|
||||
ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
|
||||
|
||||
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, CAR, CarControllerParams
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
# CAN bus layout with relay
|
||||
# 0 = ACC-CAN - radar side
|
||||
|
|
|
|||
|
|
@ -1,13 +1,12 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from panda import Panda
|
||||
from common.conversions import Conversions as CV
|
||||
from common.numpy_fast import interp
|
||||
from common.params import Params
|
||||
from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
|
||||
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
from selfdrive.car.disable_ecu import disable_ecu
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
|
@ -28,7 +27,7 @@ class CarInterface(CarInterfaceBase):
|
|||
return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "honda"
|
||||
|
||||
|
|
@ -38,7 +37,7 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
# Disable the radar and let openpilot control longitudinal
|
||||
# WARNING: THIS DISABLES AEB!
|
||||
ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar")
|
||||
ret.openpilotLongitudinalControl = disable_radar
|
||||
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
|
|
@ -380,8 +379,6 @@ class CarInterface(CarInterfaceBase):
|
|||
events = self.create_common_events(ret, pcm_enable=False)
|
||||
if self.CS.brake_error:
|
||||
events.add(EventName.brakeUnavailable)
|
||||
if self.CS.park_brake:
|
||||
events.add(EventName.parkBrake)
|
||||
|
||||
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
|
|
@ -405,9 +402,9 @@ class CarInterface(CarInterfaceBase):
|
|||
for b in ret.buttonEvents:
|
||||
|
||||
# do enable on both accel and decel buttons
|
||||
if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed:
|
||||
if not self.CP.pcmCruise:
|
||||
events.add(EventName.buttonEnable)
|
||||
if not self.CP.pcmCruise:
|
||||
if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed:
|
||||
events.add(EventName.buttonEnable)
|
||||
|
||||
# do disable on button down
|
||||
if b.type == ButtonType.cancel and b.pressed:
|
||||
|
|
@ -427,11 +424,9 @@ class CarInterface(CarInterfaceBase):
|
|||
else:
|
||||
hud_v_cruise = 255
|
||||
|
||||
ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
|
||||
c.actuators,
|
||||
c.cruiseControl.cancel,
|
||||
hud_v_cruise,
|
||||
hud_control.lanesVisible,
|
||||
ret = self.CC.update(c, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel,
|
||||
hud_v_cruise, hud_control.lanesVisible,
|
||||
hud_show_car=hud_control.leadVisible,
|
||||
hud_alert=hud_control.visualAlert)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,13 +1,15 @@
|
|||
from enum import IntFlag
|
||||
from enum import Enum, IntFlag
|
||||
from typing import Dict, List, Union
|
||||
|
||||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column
|
||||
from common.conversions import Conversions as CV
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarControllerParams():
|
||||
class CarControllerParams:
|
||||
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
|
||||
# perform the closed loop control, and might need some
|
||||
# to apply some more braking if we're on a downhill slope.
|
||||
|
|
@ -63,6 +65,7 @@ VISUAL_HUD = {
|
|||
VisualAlert.speedTooHigh: 8
|
||||
}
|
||||
|
||||
|
||||
class CAR:
|
||||
ACCORD = "HONDA ACCORD 2018"
|
||||
ACCORDH = "HONDA ACCORD HYBRID 2018"
|
||||
|
|
@ -87,6 +90,43 @@ class CAR:
|
|||
INSIGHT = "HONDA INSIGHT 2019"
|
||||
HONDA_E = "HONDA E 2020"
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
CIVIC_DIESEL = CarFootnote(
|
||||
"2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.",
|
||||
Column.FSR_STEERING)
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.ACCORD: [
|
||||
CarInfo("Honda Accord 2018-21", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
],
|
||||
CAR.ACCORDH: CarInfo("Honda Accord Hybrid 2018-21", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.CIVIC: CarInfo("Honda Civic 2016-18", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.CIVIC_BOSCH: [
|
||||
CarInfo("Honda Civic 2019-20", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS),
|
||||
CarInfo("Honda Civic Hatchback 2017-21", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
],
|
||||
CAR.ACURA_ILX: CarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS),
|
||||
CAR.CRV: CarInfo("Honda CR-V 2015-16", "Touring", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.CRV_5G: CarInfo("Honda CR-V 2017-21", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
# CAR.CRV_EU: CarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring
|
||||
CAR.CRV_HYBRID: CarInfo("Honda CR-V Hybrid 2017-19", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.FIT: CarInfo("Honda Fit 2018-19", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.FREED: CarInfo("Honda Freed 2020", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.HRV: CarInfo("Honda HR-V 2019-20", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.ODYSSEY: CarInfo("Honda Odyssey 2018-20", "Honda Sensing"),
|
||||
CAR.ACURA_RDX: CarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.ACURA_RDX_3G: CarInfo("Acura RDX 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.PILOT: CarInfo("Honda Pilot 2016-21", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.PASSPORT: CarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.RIDGELINE: CarInfo("Honda Ridgeline 2017-21", "Honda Sensing", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.INSIGHT: CarInfo("Honda Insight 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.HONDA_E: CarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
}
|
||||
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.ACCORD: {
|
||||
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
from cereal import car
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt
|
||||
from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
|
||||
|
|
@ -37,6 +37,7 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
|
|||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.p = CarControllerParams(CP)
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
|
|
@ -46,38 +47,35 @@ class CarController():
|
|||
self.last_resume_frame = 0
|
||||
self.accel = 0
|
||||
|
||||
def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
|
||||
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
|
||||
left_lane, right_lane, left_lane_depart, right_lane_depart):
|
||||
# Steering Torque
|
||||
new_steer = int(round(actuators.steer * self.p.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# disable when temp fault is active, or below LKA minimum speed
|
||||
lkas_active = c.active and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
|
||||
|
||||
if not lkas_active:
|
||||
if not c.latActive:
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = \
|
||||
process_hud_alert(enabled, self.car_fingerprint, visual_alert,
|
||||
process_hud_alert(c.enabled, self.car_fingerprint, visual_alert,
|
||||
left_lane, right_lane, left_lane_depart, right_lane_depart)
|
||||
|
||||
can_sends = []
|
||||
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
if CS.CP.openpilotLongitudinalControl:
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if (frame % 100) == 0:
|
||||
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
|
||||
|
||||
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
|
||||
CS.lkas11, sys_warning, sys_state, enabled,
|
||||
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, c.latActive,
|
||||
CS.lkas11, sys_warning, sys_state, c.enabled,
|
||||
left_lane, right_lane,
|
||||
left_lane_warning, right_lane_warning))
|
||||
|
||||
if not CS.CP.openpilotLongitudinalControl:
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
|
||||
elif CS.out.cruiseState.standstill:
|
||||
|
|
@ -87,9 +85,9 @@ class CarController():
|
|||
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
|
||||
self.last_resume_frame = frame
|
||||
|
||||
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
if frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
lead_visible = False
|
||||
accel = actuators.accel if c.active else 0
|
||||
accel = actuators.accel if c.longActive else 0
|
||||
|
||||
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
|
||||
|
||||
|
|
@ -100,7 +98,8 @@ class CarController():
|
|||
|
||||
stopping = (actuators.longControlState == LongCtrlState.stopping)
|
||||
set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
|
||||
can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
|
||||
can_sends.extend(create_acc_commands(self.packer, c.enabled, accel, jerk, int(frame / 2), lead_visible,
|
||||
set_speed_in_units, stopping, CS.out.gasPressed))
|
||||
self.accel = accel
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
|
|
@ -108,14 +107,14 @@ class CarController():
|
|||
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
|
||||
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
|
||||
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
|
||||
can_sends.append(create_lfahda_mfc(self.packer, enabled))
|
||||
can_sends.append(create_lfahda_mfc(self.packer, c.enabled))
|
||||
|
||||
# 5 Hz ACC options
|
||||
if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
if frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
can_sends.extend(create_acc_opt(self.packer))
|
||||
|
||||
# 2 Hz front radar options
|
||||
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
if frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_frt_radar_opt(self.packer))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
import copy
|
||||
from cereal import car
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
|
|
@ -47,7 +47,7 @@ class CarState(CarStateBase):
|
|||
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
|
||||
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
ret.steerWarning = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
|
||||
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
|
||||
|
||||
# cruise state
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
|
|
@ -70,6 +70,7 @@ class CarState(CarStateBase):
|
|||
ret.brake = 0
|
||||
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
|
||||
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
|
||||
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
|
||||
|
||||
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
|
||||
if self.CP.carFingerprint in HYBRID_CAR:
|
||||
|
|
@ -109,7 +110,6 @@ class CarState(CarStateBase):
|
|||
# save the entire LKAS11 and CLU11
|
||||
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
|
||||
self.clu11 = copy.copy(cp.vl["CLU11"])
|
||||
self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
|
||||
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
|
||||
self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
|
|||
}
|
||||
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|
||||
|
||||
def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping):
|
||||
def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping, gas_pressed):
|
||||
commands = []
|
||||
|
||||
scc11_values = {
|
||||
|
|
@ -95,7 +95,7 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
|
|||
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
|
||||
|
||||
scc12_values = {
|
||||
"ACCMode": 1 if enabled else 0,
|
||||
"ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 0,
|
||||
"StopReq": 1 if enabled and stopping else 0,
|
||||
"aReqRaw": accel if enabled else 0,
|
||||
"aReqValue": accel if enabled else 0, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
|
||||
|
|
@ -111,7 +111,7 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
|
|||
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"JerkUpperLimit": max(jerk, 1.0) if (enabled and not stopping) else 0, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": max(-jerk, 1.0) if enabled else 0, # stock usually is 0.5 but sometimes uses higher values
|
||||
"ACCMode": 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
|
||||
|
|
|
|||
|
|
@ -1,9 +1,8 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from panda import Panda
|
||||
from common.params import Params
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car.hyundai.values import CAR, DBC, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
|
||||
from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
|
@ -18,21 +17,21 @@ class CarInterface(CarInterfaceBase):
|
|||
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "hyundai"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundai, 0)]
|
||||
ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1]
|
||||
ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
|
||||
|
||||
# WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB)
|
||||
ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar") and (candidate not in LEGACY_SAFETY_MODE_CAR)
|
||||
ret.openpilotLongitudinalControl = disable_radar and (candidate not in LEGACY_SAFETY_MODE_CAR)
|
||||
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
|
||||
# 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/test/test_routes, we can remove it from this list.
|
||||
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
|
||||
ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay
|
||||
|
|
@ -302,8 +301,6 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
if self.CS.brake_error:
|
||||
events.add(EventName.brakeUnavailable)
|
||||
if self.CS.park_brake:
|
||||
events.add(EventName.parkBrake)
|
||||
|
||||
if self.CS.CP.openpilotLongitudinalControl:
|
||||
buttonEvents = []
|
||||
|
|
@ -352,8 +349,8 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
def apply(self, c):
|
||||
hud_control = c.hudControl
|
||||
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
|
||||
hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||
ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert,
|
||||
hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -1,5 +1,10 @@
|
|||
from dataclasses import dataclass
|
||||
from typing import Dict, List, Union
|
||||
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
# Steer torque limits
|
||||
|
|
@ -11,8 +16,8 @@ class CarControllerParams:
|
|||
# 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.
|
||||
if CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
|
||||
CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
|
||||
CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER):
|
||||
CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
|
||||
CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER):
|
||||
self.STEER_MAX = 255
|
||||
else:
|
||||
self.STEER_MAX = 384
|
||||
|
|
@ -22,6 +27,7 @@ class CarControllerParams:
|
|||
self.STEER_DRIVER_MULTIPLIER = 2
|
||||
self.STEER_DRIVER_FACTOR = 1
|
||||
|
||||
|
||||
class CAR:
|
||||
# Hyundai
|
||||
ELANTRA = "HYUNDAI ELANTRA 2017"
|
||||
|
|
@ -67,6 +73,60 @@ class CAR:
|
|||
GENESIS_G90 = "GENESIS G90 2017"
|
||||
|
||||
|
||||
@dataclass
|
||||
class HyundaiCarInfo(CarInfo):
|
||||
package: str = "SCC + LKAS"
|
||||
good_torque: bool = True
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[HyundaiCarInfo, List[HyundaiCarInfo]]] = {
|
||||
CAR.ELANTRA: HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS),
|
||||
CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c"),
|
||||
CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021", video_link="https://youtu.be/_EdYQtV52-c"),
|
||||
CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS),
|
||||
CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19"),
|
||||
CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "SCC + LFA"),
|
||||
CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019"),
|
||||
CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020"),
|
||||
CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-In Hybrid 2020-21"),
|
||||
CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020"),
|
||||
CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-19"),
|
||||
CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/_EdYQtV52-c"),
|
||||
CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All"),
|
||||
CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All"),
|
||||
CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All"),
|
||||
CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-In Hybrid 2022", "All"),
|
||||
CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw"),
|
||||
CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19"),
|
||||
CAR.PALISADE: [
|
||||
HyundaiCarInfo("Hyundai Palisade 2020-21", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456"),
|
||||
HyundaiCarInfo("Kia Telluride 2020"),
|
||||
],
|
||||
CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS),
|
||||
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2021-22", "All"),
|
||||
|
||||
# Kia
|
||||
CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2018-21"),
|
||||
CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "SCC + LFA"),
|
||||
CAR.KIA_NIRO_EV: HyundaiCarInfo("Kia Niro Electric 2019-22", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo"),
|
||||
CAR.KIA_NIRO_HEV: HyundaiCarInfo("Kia Niro Plug-In Hybrid 2019", min_enable_speed=10. * CV.MPH_TO_MS),
|
||||
CAR.KIA_NIRO_HEV_2021: HyundaiCarInfo("Kia Niro Hybrid 2021-22"),
|
||||
CAR.KIA_OPTIMA: [
|
||||
HyundaiCarInfo("Kia Optima 2017", min_steer_speed=32. * CV.MPH_TO_MS),
|
||||
HyundaiCarInfo("Kia Optima 2019"),
|
||||
],
|
||||
CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021"),
|
||||
CAR.KIA_SORENTO: HyundaiCarInfo("Kia Sorento 2018-19", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8"),
|
||||
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0"),
|
||||
CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019"),
|
||||
|
||||
# Genesis
|
||||
CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018", "All"),
|
||||
CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All"),
|
||||
CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018", "All"),
|
||||
CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2018", "All"),
|
||||
}
|
||||
|
||||
class Buttons:
|
||||
NONE = 0
|
||||
RES_ACCEL = 1
|
||||
|
|
@ -184,21 +244,27 @@ FW_VERSIONS = {
|
|||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\000AEhe SCC FHCUP 1.00 1.02 99110-G2100 ',
|
||||
b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2200 ',
|
||||
b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ',
|
||||
],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\000AE MDPS C 1.00 1.01 56310/G2510 4APHC101',
|
||||
b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2560 4APHC101',
|
||||
b'\xf1\x00AE MDPS C 1.00 1.01 56310G2510\x00 4APHC101',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\000AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819',
|
||||
b'\xf1\x00AEP MFC AT EUR RHD 1.00 1.01 95740-G2600 190819',
|
||||
b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2700 201027',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x816U3J9051\000\000\xf1\0006U3H1_C2\000\0006U3J9051\000\000PAE0G16NL0\x82zT\xd2',
|
||||
b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00',
|
||||
b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt',
|
||||
b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.IONIQ_EV_2020: {
|
||||
|
|
@ -460,6 +526,7 @@ FW_VERSIONS = {
|
|||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ',
|
||||
b'\xf1\x8799110S1500\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ',
|
||||
b'\xf1\x8799110S1500\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
|
||||
|
|
@ -467,11 +534,14 @@ FW_VERSIONS = {
|
|||
b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
|
||||
b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
|
||||
b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0',
|
||||
b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x82TACVN5GMI3XXXH0A',
|
||||
b'\xf1\x82TMBZN5TMD3XXXG2E',
|
||||
b'\xf1\x82TACVN5GSI3XXXH0A',
|
||||
b'\xf1\x82TMCFD5MMCXXXXG0A',
|
||||
b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82TMDWN5TMD3TXXJ1A',
|
||||
],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x00TM MDPS C 1.00 1.02 56370-S2AA0 0B19',
|
||||
|
|
@ -481,12 +551,15 @@ FW_VERSIONS = {
|
|||
b'\xf1\x00TMA MFC AT MEX LHD 1.00 1.01 99211-S2500 210205',
|
||||
b'\xf1\x00TMA MFC AT USA LHD 1.00 1.00 99211-S2500 200720',
|
||||
b'\xf1\x00TM MFC AT EUR LHD 1.00 1.03 99211-S1500 210224',
|
||||
b'\xf1\x00TMA MFC AT USA LHD 1.00 1.01 99211-S2500 210205',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x87SDMXCA9087684GN1VfvgUUeVwwgwwwwwffffU?\xfb\xff\x97\x88\x7f\xff+\xa4\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
|
||||
b'\xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7',
|
||||
b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
|
||||
b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7',
|
||||
b'\xf1\x87KMMYBU034207SB72x\x89\x88\x98h\x88\x98\x89\x87fhvvfWf33_\xff\x87\xff\x8f\xfa\x81\xe5\xf1\x89HT6TAF00A1\xf1\x82STM0M25GS1\x00\x00\x00\x00\x00\x00',
|
||||
b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6',
|
||||
],
|
||||
},
|
||||
CAR.SANTA_FE_HEV_2022: {
|
||||
|
|
@ -855,6 +928,7 @@ FW_VERSIONS = {
|
|||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00DEH MFC AT USA LHD 1.00 1.07 99211-G5000 201221',
|
||||
b'\xf1\x00DEH MFC AT USA LHD 1.00 1.00 99211-G5500 210428',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00DEhe SCC FHCUP 1.00 1.00 99110-G5600 ',
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ from cereal import car
|
|||
from common.kalman.simple_kalman import KF1D
|
||||
from common.realtime import DT_CTRL
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
||||
from selfdrive.controls.lib.events import Events
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
|
@ -50,7 +50,7 @@ class CarInterfaceBase(ABC):
|
|||
|
||||
@staticmethod
|
||||
@abstractmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
|
|
@ -72,7 +72,6 @@ class CarInterfaceBase(ABC):
|
|||
def get_std_params(candidate, fingerprint):
|
||||
ret = car.CarParams.new_message()
|
||||
ret.carFingerprint = candidate
|
||||
ret.unsafeMode = 0 # see panda/board/safety_declarations.h for allowed values
|
||||
|
||||
# standard ALC params
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
|
@ -88,7 +87,7 @@ class CarInterfaceBase(ABC):
|
|||
ret.stopAccel = -2.0
|
||||
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
|
||||
ret.vEgoStopping = 0.5
|
||||
ret.vEgoStarting = 0.5 # needs to be >= vEgoStopping to avoid state transition oscillation
|
||||
ret.vEgoStarting = 0.5
|
||||
ret.stoppingControl = True
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
|
|
@ -139,11 +138,12 @@ class CarInterfaceBase(ABC):
|
|||
events.add(EventName.wrongCruiseMode)
|
||||
if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
|
||||
events.add(EventName.brakeHold)
|
||||
|
||||
if cs_out.parkingBrake:
|
||||
events.add(EventName.parkBrake)
|
||||
|
||||
# Handle permanent and temporary steering faults
|
||||
self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
|
||||
if cs_out.steerWarning:
|
||||
if cs_out.steerFaultTemporary:
|
||||
# if the user overrode recently, show a less harsh alert
|
||||
if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
|
||||
self.silent_steer_warning = True
|
||||
|
|
@ -152,14 +152,9 @@ class CarInterfaceBase(ABC):
|
|||
events.add(EventName.steerTempUnavailable)
|
||||
else:
|
||||
self.silent_steer_warning = False
|
||||
if cs_out.steerError:
|
||||
if cs_out.steerFaultPermanent:
|
||||
events.add(EventName.steerUnavailable)
|
||||
|
||||
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
|
||||
if (cs_out.gasPressed and not self.CS.out.gasPressed) or \
|
||||
(cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)):
|
||||
events.add(EventName.pedalPressed)
|
||||
|
||||
# we engage when pcm is active (rising edge)
|
||||
if pcm_enable:
|
||||
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
|||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.steer_rate_limited = False
|
||||
|
|
@ -19,7 +20,7 @@ class CarController():
|
|||
apply_steer = 0
|
||||
self.steer_rate_limited = False
|
||||
|
||||
if c.active:
|
||||
if c.latActive:
|
||||
# calculate steer and also set limits due to driver torque
|
||||
new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
|
||||
|
|
@ -30,9 +31,9 @@ class CarController():
|
|||
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
|
||||
# Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
|
||||
# TODO: improve the resume trigger logic by looking at actual radar data
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
|
||||
|
||||
if c.cruiseControl.cancel or (CS.out.cruiseState.enabled and not c.enabled):
|
||||
if c.cruiseControl.cancel:
|
||||
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
|
||||
# a race condition with the stock system, where the second cancel from openpilot
|
||||
# will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
|
||||
|
|
@ -41,7 +42,7 @@ class CarController():
|
|||
if frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
|
||||
# Cancel Stock ACC if it's enabled while OP is disengaged
|
||||
# Send at a rate of 10hz until we sync with stock ACC state
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
|
||||
else:
|
||||
self.brake_counter = 0
|
||||
|
||||
|
|
@ -56,7 +57,7 @@ class CarController():
|
|||
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
|
||||
|
||||
# send steering command
|
||||
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
|
||||
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
|
||||
frame, apply_steer, CS.cam_lkas))
|
||||
|
||||
new_actuators = c.actuators.copy()
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.conversions import Conversions as CV
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
|
|
@ -88,7 +88,7 @@ class CarState(CarStateBase):
|
|||
# Check if LKAS is disabled due to lack of driver torque when all other states indicate
|
||||
# it should be enabled (steer lockout). Don't warn until we actually get lkas active
|
||||
# and lose it again, i.e, after initial lkas activation
|
||||
ret.steerWarning = self.lkas_allowed_speed and lkas_blocked
|
||||
ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked
|
||||
|
||||
self.acc_active_last = ret.cruiseState.enabled
|
||||
|
||||
|
|
@ -98,7 +98,7 @@ class CarState(CarStateBase):
|
|||
self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
|
||||
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
|
||||
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
|
||||
ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
|
||||
ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
|
||||
|
||||
return ret
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
|
@ -15,7 +15,7 @@ class CarInterface(CarInterfaceBase):
|
|||
return float(accel) / 4.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "mazda"
|
||||
|
|
|
|||
|
|
@ -1,4 +1,7 @@
|
|||
from typing import Dict, List, Union
|
||||
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
from cereal import car
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
|
@ -14,6 +17,7 @@ class CarControllerParams:
|
|||
STEER_DRIVER_FACTOR = 1 # from dbc
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
|
||||
class CAR:
|
||||
CX5 = "MAZDA CX-5"
|
||||
CX9 = "MAZDA CX-9"
|
||||
|
|
@ -22,6 +26,17 @@ class CAR:
|
|||
CX9_2021 = "MAZDA CX-9 2021"
|
||||
CX5_2022 = "MAZDA CX-5 2022"
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.CX5: CarInfo("Mazda CX-5 2017, 2019", "All"), # TODO: verify years and torque for first 4
|
||||
CAR.CX9: CarInfo("Mazda CX-9 2016-17", "All"),
|
||||
CAR.MAZDA3: CarInfo("Mazda 3 2017", "All"),
|
||||
CAR.MAZDA6: CarInfo("Mazda 6 2017", "All"),
|
||||
CAR.CX9_2021: CarInfo("Mazda CX-9 2021", "All", good_torque=True),
|
||||
CAR.CX5_2022: CarInfo("Mazda CX-5 2022", "All", good_torque=True),
|
||||
}
|
||||
|
||||
|
||||
class LKAS_LIMITS:
|
||||
STEER_THRESHOLD = 15
|
||||
DISABLE_SPEED = 45 # kph
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#!/usr/bin/env python3
|
||||
import math
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.swaglog import cloudlog
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.car import gen_empty_fingerprint, get_safety_config
|
||||
|
|
@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
|
|||
return accel
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "mock"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)]
|
||||
|
|
|
|||
|
|
@ -1,2 +1,10 @@
|
|||
from typing import Dict, List, Union
|
||||
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
|
||||
|
||||
class CAR:
|
||||
MOCK = 'mock'
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {}
|
||||
|
|
|
|||
|
|
@ -18,20 +18,19 @@ class CarController():
|
|||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
|
||||
def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert,
|
||||
left_line, right_line, left_lane_depart, right_lane_depart):
|
||||
|
||||
can_sends = []
|
||||
|
||||
### STEER ###
|
||||
acc_active = CS.out.cruiseState.enabled
|
||||
lkas_hud_msg = CS.lkas_hud_msg
|
||||
lkas_hud_info_msg = CS.lkas_hud_info_msg
|
||||
apply_angle = actuators.steeringAngleDeg
|
||||
|
||||
steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
|
||||
|
||||
if c.active:
|
||||
if c.latActive:
|
||||
# # windup slower
|
||||
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
|
||||
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
|
||||
|
|
@ -58,10 +57,6 @@ class CarController():
|
|||
|
||||
self.last_angle = apply_angle
|
||||
|
||||
if not enabled and acc_active:
|
||||
# send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
|
||||
cruise_cancel = 1
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel:
|
||||
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame))
|
||||
|
||||
|
|
@ -73,12 +68,12 @@ class CarController():
|
|||
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
|
||||
|
||||
can_sends.append(nissancan.create_steering_control(
|
||||
self.packer, apply_angle, frame, enabled, self.lkas_max_torque))
|
||||
self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque))
|
||||
|
||||
if lkas_hud_msg and lkas_hud_info_msg:
|
||||
if frame % 2 == 0:
|
||||
can_sends.append(nissancan.create_lkas_hud_msg(
|
||||
self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
|
||||
if frame % 50 == 0:
|
||||
can_sends.append(nissancan.create_lkas_hud_info_msg(
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@ from collections import deque
|
|||
from cereal import car
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
|
||||
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ class CarInterface(CarInterfaceBase):
|
|||
self.cp_adas = self.CS.get_adas_can_parser(CP)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "nissan"
|
||||
|
|
@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
def apply(self, c):
|
||||
hud_control = c.hudControl
|
||||
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
|
||||
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||
|
|
|
|||
|
|
@ -1,4 +1,7 @@
|
|||
from typing import Dict, List, Union
|
||||
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
from cereal import car
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
|
@ -10,6 +13,7 @@ class CarControllerParams:
|
|||
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
|
||||
class CAR:
|
||||
XTRAIL = "NISSAN X-TRAIL 2017"
|
||||
LEAF = "NISSAN LEAF 2018"
|
||||
|
|
@ -20,6 +24,13 @@ class CAR:
|
|||
ALTIMA = "NISSAN ALTIMA 2020"
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.XTRAIL: CarInfo("Nissan X-Trail 2017", "ProPILOT"),
|
||||
CAR.LEAF: CarInfo("Nissan Leaf 2018-22", "ProPILOT"),
|
||||
CAR.ROGUE: CarInfo("Nissan Rogue 2018-20", "ProPILOT"),
|
||||
CAR.ALTIMA: CarInfo("Nissan Altima 2019-20", "ProPILOT"),
|
||||
}
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.XTRAIL: [
|
||||
{
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@ from opendbc.can.packer import CANPacker
|
|||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.es_distance_cnt = -1
|
||||
self.es_lkas_cnt = -1
|
||||
|
|
@ -15,7 +16,7 @@ class CarController():
|
|||
self.p = CarControllerParams(CP)
|
||||
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
|
||||
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
|
||||
|
||||
can_sends = []
|
||||
|
||||
|
|
@ -30,10 +31,10 @@ class CarController():
|
|||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
if not c.active:
|
||||
if not c.latActive:
|
||||
apply_steer = 0
|
||||
|
||||
if CS.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
if self.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
|
||||
else:
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
|
||||
|
|
@ -43,7 +44,7 @@ class CarController():
|
|||
|
||||
# *** alerts and pcm cancel ***
|
||||
|
||||
if CS.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
if self.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
|
||||
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
|
||||
# disengage ACC when OP is disengaged
|
||||
|
|
@ -69,7 +70,7 @@ class CarController():
|
|||
self.es_distance_cnt = CS.es_distance_msg["Counter"]
|
||||
|
||||
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
|
||||
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, c.enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue