mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-10 18:14:25 +08:00
Compare commits
6 Commits
archive/fc
...
accel-prof
| Author | SHA1 | Date | |
|---|---|---|---|
|
ac65fb5f54 | ||
|
|
801a17edea | ||
|
|
f4f48ee55c | ||
|
|
8bddfda0dd | ||
|
|
a9aa92bfcf | ||
|
|
a29714a610 |
@@ -87,6 +87,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
|
||||
struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
dec @0 :DynamicExperimentalControl;
|
||||
accelPersonality @1 :AccelerationPersonality;
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
@@ -98,6 +99,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
blended @1;
|
||||
}
|
||||
}
|
||||
|
||||
enum AccelerationPersonality {
|
||||
sport @0;
|
||||
normal @1;
|
||||
eco @2;
|
||||
stock @3;
|
||||
}
|
||||
}
|
||||
|
||||
struct OnroadEventSP @0xda96579883444c35 {
|
||||
|
||||
@@ -6,12 +6,12 @@
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
static std::vector<std::string> get_services(const std::string &whitelist_str, bool zmq_to_msgq) {
|
||||
static std::vector<std::string> get_services(std::string whitelist_str, bool zmq_to_msgq) {
|
||||
std::vector<std::string> service_list;
|
||||
for (const auto& it : services) {
|
||||
std::string name = it.second.name;
|
||||
bool in_whitelist = whitelist_str.find(name) != std::string::npos;
|
||||
if (zmq_to_msgq && !in_whitelist) {
|
||||
if (name == "plusFrame" || name == "uiLayoutState" || (zmq_to_msgq && !in_whitelist)) {
|
||||
continue;
|
||||
}
|
||||
service_list.push_back(name);
|
||||
|
||||
@@ -8,7 +8,6 @@ import uuid
|
||||
import socket
|
||||
import logging
|
||||
import traceback
|
||||
import numpy as np
|
||||
from threading import local
|
||||
from collections import OrderedDict
|
||||
from contextlib import contextmanager
|
||||
@@ -16,8 +15,6 @@ from contextlib import contextmanager
|
||||
LOG_TIMESTAMPS = "LOG_TIMESTAMPS" in os.environ
|
||||
|
||||
def json_handler(obj):
|
||||
if isinstance(obj, np.bool_):
|
||||
return bool(obj)
|
||||
# if isinstance(obj, (datetime.date, datetime.time)):
|
||||
# return obj.isoformat()
|
||||
return repr(obj)
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
#define DEFAULT_MODEL "Notre Dame (Default)"
|
||||
@@ -89,7 +89,6 @@ private:
|
||||
|
||||
std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
|
||||
{"AdbEnabled", PERSISTENT},
|
||||
{"AlwaysOnDM", PERSISTENT},
|
||||
{"ApiCache_Device", PERSISTENT},
|
||||
{"AssistNowToken", PERSISTENT},
|
||||
@@ -206,7 +205,6 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"CarParamsSP", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"CarParamsSPCache", CLEAR_ON_MANAGER_START},
|
||||
{"CarParamsSPPersistent", PERSISTENT},
|
||||
{"CarPlatformBundle", PERSISTENT},
|
||||
{"EnableGithubRunner", PERSISTENT | BACKUP},
|
||||
{"ModelRunnerTypeCache", CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"OffroadMode", CLEAR_ON_MANAGER_START},
|
||||
@@ -238,6 +236,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"HyundaiRadarTracksToggle", PERSISTENT},
|
||||
|
||||
{"DynamicExperimentalControl", PERSISTENT},
|
||||
{"AccelPersonality", PERSISTENT},
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -10,7 +10,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||
|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|26 mph|25 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Acura&model=ILX 2016-19">Buy Here</a></sub></details>||
|
||||
|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Acura&model=RDX 2016-18">Buy Here</a></sub></details>||
|
||||
|Acura|RDX 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Acura&model=RDX 2019-21">Buy Here</a></sub></details>||
|
||||
|Acura|RDX 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Acura&model=RDX 2019-22">Buy Here</a></sub></details>||
|
||||
|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=A3 2014-19">Buy Here</a></sub></details>||
|
||||
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=A3 Sportback e-tron 2017-18">Buy Here</a></sub></details>||
|
||||
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=Q2 2018">Buy Here</a></sub></details>||
|
||||
@@ -128,7 +128,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Jeep&model=Grand Cherokee 2019-21">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=jBe4lWnRSu4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Carnival 2022-24[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Carnival 2022-24">Buy Here</a></sub></details>||
|
||||
|Kia|Carnival (China only) 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Carnival (China only) 2023">Buy Here</a></sub></details>||
|
||||
|Kia|Ceed 2019-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Ceed 2019-21">Buy Here</a></sub></details>||
|
||||
|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Ceed 2019">Buy Here</a></sub></details>||
|
||||
|Kia|EV6 (Southeast Asia only) 2022-24[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=EV6 (Southeast Asia only) 2022-24">Buy Here</a></sub></details>||
|
||||
|Kia|EV6 (with HDA II) 2022-24[<sup>5</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=EV6 (with HDA II) 2022-24">Buy Here</a></sub></details>||
|
||||
|Kia|EV6 (without HDA II) 2022-24[<sup>5</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=EV6 (without HDA II) 2022-24">Buy Here</a></sub></details>||
|
||||
@@ -260,7 +260,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2017-18">Buy Here</a></sub></details>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2019-21">Buy Here</a></sub></details>||
|
||||
|Toyota|RAV4 Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2022">Buy Here</a></sub></details>|<a href="https://youtu.be/U0nH9cnrFB0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 Hybrid 2023-25|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2023-25">Buy Here</a></sub></details>|<a href="https://youtu.be/4eIsEq4L4Ng" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 Hybrid 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2023-24">Buy Here</a></sub></details>||
|
||||
|Toyota|Sienna 2018-20|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Sienna 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=q1UPOo4Sh68" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Arteon 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Arteon eHybrid 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|
||||
@@ -63,12 +63,3 @@ A good pull request has all of the following:
|
||||
* Connect your device to Wi-Fi regularly, so that we can pull data for training better driving models.
|
||||
* Run the `nightly` branch and report issues. This branch is like `master` but it's built just like a release.
|
||||
* Annotate images in the [comma10k dataset](https://github.com/commaai/comma10k).
|
||||
|
||||
## Contributing Training Data
|
||||
|
||||
### A guide for forks
|
||||
|
||||
In order for your fork's data to be eligible for the training set:
|
||||
* **Your cereal messaging structs must be [compatible](../cereal#custom-forks)**
|
||||
* **The definitions of all the stock messaging structs must not change**: Do not change how any of the fields are set, including everything from `selfdriveState.enabled` to `carState.steeringAngleDeg`. Instead, create your own structs and set them however you'd like.
|
||||
* **Do not include cars that are not supported in upstream platforms**: Instead, create new opendbc platforms for cars that you'd like to support outside of upstream, even if it's just a trim-level difference.
|
||||
|
||||
@@ -29,22 +29,6 @@ Here's an example command for connecting to your device using its tethered conne
|
||||
|
||||
For doing development work on device, it's recommended to use [SSH agent forwarding](https://docs.github.com/en/developers/overview/using-ssh-agent-forwarding).
|
||||
|
||||
|
||||
## ADB
|
||||
|
||||
In order to use ADB on your device, you'll need to enable it in the device's settings.
|
||||
|
||||
* Enable ADB in your device's settings
|
||||
* Connect to your device
|
||||
* `adb shell` over USB
|
||||
* `adb connect` over WiFi
|
||||
* Here's an example command for connecting to your device using its tethered connection: `adb connect 192.168.43.1:5555`
|
||||
|
||||
> [!NOTE]
|
||||
> The default port for ADB is 5555 on the comma 3/3X.
|
||||
|
||||
For more info on ADB, see the [Android Debug Bridge (ADB) documentation](https://developer.android.com/tools/adb).
|
||||
|
||||
### Notes
|
||||
|
||||
The public keys are only fetched from your GitHub account once. In order to update your device's authorized keys, you'll need to re-enter your GitHub username.
|
||||
|
||||
@@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
|
||||
export VECLIB_MAXIMUM_THREADS=1
|
||||
|
||||
if [ -z "$AGNOS_VERSION" ]; then
|
||||
export AGNOS_VERSION="11.6"
|
||||
export AGNOS_VERSION="11.4"
|
||||
fi
|
||||
|
||||
export STAGING_ROOT="/data/safe_staging"
|
||||
|
||||
Submodule msgq_repo updated: 102befe731...5bb86f8bc7
Submodule opendbc_repo updated: cd086f3e5e...5e5885c75b
2
panda
2
panda
Submodule panda updated: 4ca963345a...84836fd802
@@ -165,7 +165,7 @@ testpaths = [
|
||||
[tool.codespell]
|
||||
quiet-level = 3
|
||||
# if you've got a short variable name that's getting flagged, add it here
|
||||
ignore-words-list = "bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints,whit,indexIn,ws,uint,grey,deque,stdio,amin,BA,LITE,atEnd,UIs,errorString,arange,FocusIn,od,tim,relA,hist,copyable,jupyter,thead,TGE,abl"
|
||||
ignore-words-list = "bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints,whit,indexIn,ws,uint,grey,deque,stdio,amin,BA,LITE,atEnd,UIs,errorString,arange,FocusIn,od,tim,relA,hist,copyable,jupyter,thead"
|
||||
builtin = "clear,rare,informal,code,names,en-GB_to_en-US"
|
||||
skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*"
|
||||
|
||||
|
||||
@@ -42,19 +42,19 @@ class CarSpecificEvents:
|
||||
self.cruise_buttons: deque = deque([], maxlen=HYUNDAI_PREV_BUTTON_SAMPLES)
|
||||
|
||||
def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl):
|
||||
if self.CP.brand in ('body', 'mock'):
|
||||
if self.CP.carName in ('body', 'mock'):
|
||||
events = Events()
|
||||
|
||||
elif self.CP.brand in ('subaru', 'mazda'):
|
||||
elif self.CP.carName in ('subaru', 'mazda'):
|
||||
events = self.create_common_events(CS, CS_prev)
|
||||
|
||||
elif self.CP.brand == 'ford':
|
||||
elif self.CP.carName == 'ford':
|
||||
events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.manumatic])
|
||||
|
||||
elif self.CP.brand == 'nissan':
|
||||
elif self.CP.carName == 'nissan':
|
||||
events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.brake])
|
||||
|
||||
elif self.CP.brand == 'chrysler':
|
||||
elif self.CP.carName == 'chrysler':
|
||||
events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.low])
|
||||
|
||||
# Low speed steer alert hysteresis logic
|
||||
@@ -65,7 +65,7 @@ class CarSpecificEvents:
|
||||
if self.low_speed_alert:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
elif self.CP.brand == 'honda':
|
||||
elif self.CP.carName == 'honda':
|
||||
events = self.create_common_events(CS, CS_prev, pcm_enable=False)
|
||||
|
||||
if self.CP.pcmCruise and CS.vEgo < self.CP.minEnableSpeed:
|
||||
@@ -86,7 +86,7 @@ class CarSpecificEvents:
|
||||
if self.CP.minEnableSpeed > 0 and CS.vEgo < 0.001:
|
||||
events.add(EventName.manualRestart)
|
||||
|
||||
elif self.CP.brand == 'toyota':
|
||||
elif self.CP.carName == 'toyota':
|
||||
events = self.create_common_events(CS, CS_prev)
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
@@ -101,7 +101,7 @@ class CarSpecificEvents:
|
||||
# while in standstill, send a user alert
|
||||
events.add(EventName.manualRestart)
|
||||
|
||||
elif self.CP.brand == 'gm':
|
||||
elif self.CP.carName == 'gm':
|
||||
# The ECM allows enabling on falling edge of set, but only rising edge of resume
|
||||
events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.sport, GearShifter.low,
|
||||
GearShifter.eco, GearShifter.manumatic],
|
||||
@@ -120,7 +120,7 @@ class CarSpecificEvents:
|
||||
if CS.vEgo < self.CP.minSteerSpeed:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
elif self.CP.brand == 'volkswagen':
|
||||
elif self.CP.carName == 'volkswagen':
|
||||
events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
|
||||
pcm_enable=self.CP.pcmCruise,
|
||||
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
|
||||
@@ -143,7 +143,7 @@ class CarSpecificEvents:
|
||||
# if CC.eps_timer_soft_disable_alert: # type: ignore[attr-defined]
|
||||
# events.add(EventName.steerTimeLimit)
|
||||
|
||||
elif self.CP.brand == 'hyundai':
|
||||
elif self.CP.carName == 'hyundai':
|
||||
# On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
|
||||
# To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
|
||||
# Main button also can trigger an engagement on these cars
|
||||
@@ -160,7 +160,7 @@ class CarSpecificEvents:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
else:
|
||||
raise ValueError(f"Unsupported car: {self.CP.brand}")
|
||||
raise ValueError(f"Unsupported car: {self.CP.carName}")
|
||||
|
||||
return events
|
||||
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
import json
|
||||
import os
|
||||
import time
|
||||
import threading
|
||||
@@ -105,9 +104,7 @@ class Car:
|
||||
with car.CarParams.from_bytes(cached_params_raw) as _cached_params:
|
||||
cached_params = _cached_params
|
||||
|
||||
fixed_fingerprint = json.loads(self.params.get("CarPlatformBundle", encoding='utf-8') or "{}").get("platform", None)
|
||||
|
||||
self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params, fixed_fingerprint)
|
||||
self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params)
|
||||
interfaces.setup_car_interface_sp(self.CI.CP, self.CI.CP_SP, self.params)
|
||||
self.RI = get_radar_interface(self.CI.CP, self.CI.CP_SP)
|
||||
self.CP = self.CI.CP
|
||||
@@ -202,7 +199,7 @@ class Car:
|
||||
|
||||
# Update carState from CAN
|
||||
CS = self.CI.update(can_list)
|
||||
if self.CP.brand == 'mock':
|
||||
if self.CP.carName == 'mock':
|
||||
CS = self.mock_carstate.update(CS)
|
||||
|
||||
# Update radar tracks from CAN
|
||||
|
||||
@@ -367,11 +367,11 @@ class TestCarModelBase(unittest.TestCase):
|
||||
if self.safety.get_vehicle_moving() != prev_panda_vehicle_moving:
|
||||
self.assertEqual(not CS.standstill, self.safety.get_vehicle_moving())
|
||||
|
||||
if not (self.CP.brand == "honda" and not (self.CP.flags & HondaFlags.BOSCH)):
|
||||
if not (self.CP.carName == "honda" and not (self.CP.flags & HondaFlags.BOSCH)):
|
||||
if self.safety.get_cruise_engaged_prev() != prev_panda_cruise_engaged:
|
||||
self.assertEqual(CS.cruiseState.enabled, self.safety.get_cruise_engaged_prev())
|
||||
|
||||
if self.CP.brand == "honda":
|
||||
if self.CP.carName == "honda":
|
||||
if self.safety.get_acc_main_on() != prev_panda_acc_main_on:
|
||||
self.assertEqual(CS.cruiseState.available, self.safety.get_acc_main_on())
|
||||
|
||||
@@ -426,7 +426,7 @@ class TestCarModelBase(unittest.TestCase):
|
||||
# On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state.
|
||||
# On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but
|
||||
# openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages).
|
||||
if self.CP.brand == "honda" and not (self.CP.flags & HondaFlags.BOSCH):
|
||||
if self.CP.carName == "honda" and not (self.CP.flags & HondaFlags.BOSCH):
|
||||
# only the rising edges are expected to match
|
||||
if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
|
||||
checks['controlsAllowed'] += not self.safety.get_controls_allowed()
|
||||
@@ -448,7 +448,7 @@ class TestCarModelBase(unittest.TestCase):
|
||||
if button_enable and not mismatch:
|
||||
self.safety.set_controls_allowed(False)
|
||||
|
||||
if self.CP.brand == "honda":
|
||||
if self.CP.carName == "honda":
|
||||
checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on()
|
||||
|
||||
CS_prev = CS
|
||||
|
||||
@@ -141,6 +141,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
accel_limits = [ACCEL_MIN, ACCEL_MAX]
|
||||
accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
|
||||
|
||||
if (accel_control := self.compute_accel_limits(v_ego, sm, self.CP)):
|
||||
accel_limits, accel_limits_turns = accel_control
|
||||
|
||||
if reset_state:
|
||||
self.v_desired_filter.x = v_ego
|
||||
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
|
||||
|
||||
@@ -14,7 +14,7 @@ def main():
|
||||
cloudlog.info("plannerd is waiting for CarParams")
|
||||
params = Params()
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("plannerd got CarParams: %s", CP.brand)
|
||||
cloudlog.info("plannerd got CarParams: %s", CP.carName)
|
||||
|
||||
ldw = LaneDepartureWarning()
|
||||
longitudinal_planner = LongitudinalPlanner(CP)
|
||||
|
||||
11
selfdrive/debug/adb.sh
Executable file
11
selfdrive/debug/adb.sh
Executable file
@@ -0,0 +1,11 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
PORT=5555
|
||||
|
||||
setprop service.adb.tcp.port $PORT
|
||||
sudo systemctl start adbd
|
||||
|
||||
IP=$(echo $SSH_CONNECTION | awk '{ print $3}')
|
||||
echo "then, connect on your computer:"
|
||||
echo "adb connect $IP:$PORT"
|
||||
@@ -24,16 +24,14 @@ MIN_STD_SANITY_CHECK = 1e-5 # m or rad
|
||||
MAX_FILTER_REWIND_TIME = 0.8 # s
|
||||
MAX_SENSOR_TIME_DIFF = 0.1 # s
|
||||
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
|
||||
INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
|
||||
INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
|
||||
INPUT_INVALID_THRESHOLD = 0.5 # 0 bad inputs ignored
|
||||
TIMING_INVALID_THRESHOLD = 2.5 # 2 bad timings ignored
|
||||
INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after exceeding allowed bad inputs by one (at 100hz)
|
||||
TIMING_INVALID_DECAY = 0.9990 # ~2 secs to resume after exceeding allowed bad timings by one (at 100hz)
|
||||
POSENET_STD_INITIAL_VALUE = 10.0
|
||||
POSENET_STD_HIST_HALF = 20
|
||||
|
||||
|
||||
def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
|
||||
return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
|
||||
|
||||
|
||||
def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
|
||||
assert len(values) == len(stds) == 3
|
||||
measurement.x, measurement.y, measurement.z = map(float, values)
|
||||
@@ -271,11 +269,11 @@ def main():
|
||||
|
||||
filter_initialized = False
|
||||
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
|
||||
observation_timing_invalid = defaultdict(int)
|
||||
observation_input_invalid = defaultdict(int)
|
||||
|
||||
input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
|
||||
input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
|
||||
input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
|
||||
input_invalid_decay = {s: INPUT_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
|
||||
timing_invalid_decay = {s: TIMING_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
|
||||
|
||||
initial_pose = params.get("LocationFilterInitialState")
|
||||
if initial_pose is not None:
|
||||
@@ -308,20 +306,19 @@ def main():
|
||||
continue
|
||||
|
||||
if res == HandleLogResult.TIMING_INVALID:
|
||||
print(f"Observation {which} ignored due to failed timing check")
|
||||
observation_input_invalid[which] += 1
|
||||
print(observation_input_invalid[which])
|
||||
observation_timing_invalid[which] += 1
|
||||
elif res == HandleLogResult.INPUT_INVALID:
|
||||
print(f"Observation {which} ignored due to failed sanity check")
|
||||
observation_input_invalid[which] += 1
|
||||
else:
|
||||
observation_input_invalid[which] *= input_invalid_decay[which]
|
||||
observation_timing_invalid[which] *= timing_invalid_decay[which]
|
||||
else:
|
||||
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
|
||||
|
||||
if sm.updated["cameraOdometry"]:
|
||||
critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
|
||||
inputs_valid = sm.all_valid() and critical_service_inputs_valid
|
||||
critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services)
|
||||
critical_service_timing_valid = all(observation_timing_invalid[s] < TIMING_INVALID_THRESHOLD for s in critcal_services)
|
||||
inputs_valid = sm.all_valid() and critical_service_inputs_valid and critical_service_timing_valid
|
||||
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
|
||||
|
||||
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
|
||||
|
||||
56
selfdrive/locationd/test/test_locationd.py
Normal file
56
selfdrive/locationd/test/test_locationd.py
Normal file
@@ -0,0 +1,56 @@
|
||||
import capnp
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.params import Params
|
||||
|
||||
from openpilot.system.manager.process_config import managed_processes
|
||||
|
||||
|
||||
class TestLocationdProc:
|
||||
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
|
||||
'accelerometer', 'gyroscope', 'magnetometer']
|
||||
|
||||
def setup_method(self):
|
||||
self.pm = messaging.PubMaster(self.LLD_MSGS)
|
||||
|
||||
self.params = Params()
|
||||
self.params.put_bool("UbloxAvailable", True)
|
||||
managed_processes['locationd'].prepare()
|
||||
managed_processes['locationd'].start()
|
||||
|
||||
def teardown_method(self):
|
||||
managed_processes['locationd'].stop()
|
||||
|
||||
def get_msg(self, name, t):
|
||||
try:
|
||||
msg = messaging.new_message(name)
|
||||
except capnp.lib.capnp.KjException:
|
||||
msg = messaging.new_message(name, 0)
|
||||
|
||||
if name == "gpsLocationExternal":
|
||||
msg.gpsLocationExternal.flags = 1
|
||||
msg.gpsLocationExternal.hasFix = True
|
||||
msg.gpsLocationExternal.verticalAccuracy = 1.0
|
||||
msg.gpsLocationExternal.speedAccuracy = 1.0
|
||||
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
|
||||
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
|
||||
msg.gpsLocationExternal.latitude = float(self.lat)
|
||||
msg.gpsLocationExternal.longitude = float(self.lon)
|
||||
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
|
||||
msg.gpsLocationExternal.altitude = float(self.alt)
|
||||
#if name == "gnssMeasurements":
|
||||
# msg.gnssMeasurements.measTime = t
|
||||
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
|
||||
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
|
||||
# msg.gnssMeasurements.positionECEF.valid = True
|
||||
# msg.gnssMeasurements.velocityECEF.value = []
|
||||
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
|
||||
# msg.gnssMeasurements.velocityECEF.valid = True
|
||||
elif name == 'cameraOdometry':
|
||||
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
|
||||
msg.logMonoTime = t
|
||||
msg.valid = True
|
||||
return msg
|
||||
@@ -23,10 +23,8 @@ class Scenario(Enum):
|
||||
BASE = 'base'
|
||||
GYRO_OFF = 'gyro_off'
|
||||
GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
|
||||
GYRO_CONSISTENT_SPIKES = 'gyro_consistent_spikes'
|
||||
ACCEL_OFF = 'accel_off'
|
||||
ACCEL_SPIKE_MIDWAY = 'accel_spike_midway'
|
||||
ACCEL_CONSISTENT_SPIKES = 'accel_consistent_spikes'
|
||||
SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes'
|
||||
SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes'
|
||||
|
||||
@@ -65,20 +63,18 @@ def run_scenarios(scenario, logs):
|
||||
elif scenario == Scenario.GYRO_OFF:
|
||||
logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
|
||||
|
||||
elif scenario == Scenario.GYRO_SPIKE_MIDWAY or scenario == Scenario.GYRO_CONSISTENT_SPIKES:
|
||||
elif scenario == Scenario.GYRO_SPIKE_MIDWAY:
|
||||
def gyro_spike(msg):
|
||||
msg.gyroscope.gyroUncalibrated.v[0] += 3.0
|
||||
count = 1 if scenario == Scenario.GYRO_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
|
||||
logs = modify_logs_midway(logs, 'gyroscope', count, gyro_spike)
|
||||
logs = modify_logs_midway(logs, 'gyroscope', 1, gyro_spike)
|
||||
|
||||
elif scenario == Scenario.ACCEL_OFF:
|
||||
logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime)
|
||||
|
||||
elif scenario == Scenario.ACCEL_SPIKE_MIDWAY or scenario == Scenario.ACCEL_CONSISTENT_SPIKES:
|
||||
elif scenario == Scenario.ACCEL_SPIKE_MIDWAY:
|
||||
def acc_spike(msg):
|
||||
msg.accelerometer.acceleration.v[0] += 100.0
|
||||
count = 1 if scenario == Scenario.ACCEL_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
|
||||
logs = modify_logs_midway(logs, 'accelerometer', count, acc_spike)
|
||||
msg.accelerometer.acceleration.v[0] += 10.0
|
||||
logs = modify_logs_midway(logs, 'accelerometer', 1, acc_spike)
|
||||
|
||||
elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES:
|
||||
def timing_spike(msg):
|
||||
@@ -125,7 +121,7 @@ class TestLocationdScenarios:
|
||||
assert np.allclose(replayed_data['roll'], 0.0)
|
||||
assert np.all(replayed_data['sensors_flag'] == 0.0)
|
||||
|
||||
def test_gyro_spike(self):
|
||||
def test_gyro_spikes(self):
|
||||
"""
|
||||
Test: a gyroscope spike in the middle of the segment
|
||||
Expected Result:
|
||||
@@ -136,17 +132,8 @@ class TestLocationdScenarios:
|
||||
orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
|
||||
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
|
||||
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
|
||||
assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
|
||||
assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
|
||||
|
||||
def test_consistent_gyro_spikes(self):
|
||||
"""
|
||||
Test: consistent timing spikes for N gyroscope messages in the middle of the segment
|
||||
Expected Result: inputsOK becomes False after N of bad measurements
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.GYRO_CONSISTENT_SPIKES, self.logs)
|
||||
assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[708] == 1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[499] == -1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[704] == 1.0
|
||||
|
||||
def test_accel_off(self):
|
||||
"""
|
||||
@@ -161,7 +148,7 @@ class TestLocationdScenarios:
|
||||
assert np.allclose(replayed_data['roll'], 0.0)
|
||||
assert np.all(replayed_data['sensors_flag'] == 0.0)
|
||||
|
||||
def test_accel_spike(self):
|
||||
def test_accel_spikes(self):
|
||||
"""
|
||||
ToDo:
|
||||
Test: an accelerometer spike in the middle of the segment
|
||||
@@ -186,5 +173,5 @@ class TestLocationdScenarios:
|
||||
Expected Result: inputsOK becomes False after N of bad measurements
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs)
|
||||
assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[707] == 1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[500] == -1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[787] == 1.0
|
||||
|
||||
@@ -71,7 +71,7 @@ class TorqueEstimator(ParameterEstimator):
|
||||
self.offline_friction = 0.0
|
||||
self.offline_latAccelFactor = 0.0
|
||||
self.resets = 0.0
|
||||
self.use_params = CP.brand in ALLOWED_CARS and CP.lateralTuning.which() == 'torque'
|
||||
self.use_params = CP.carName in ALLOWED_CARS and CP.lateralTuning.which() == 'torque'
|
||||
|
||||
if CP.lateralTuning.which() == 'torque':
|
||||
self.offline_friction = CP.lateralTuning.torque.friction
|
||||
|
||||
@@ -26,7 +26,6 @@ from openpilot.common.transformations.model import dmonitoringmodel_intrinsics,
|
||||
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
|
||||
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
|
||||
from openpilot.system import sentry
|
||||
|
||||
MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE
|
||||
CALIB_LEN = 3
|
||||
@@ -38,7 +37,6 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
MODEL_PATH = Path(__file__).parent / 'models/dmonitoring_model.onnx'
|
||||
MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
|
||||
|
||||
|
||||
class DriverStateResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("face_orientation", ctypes.c_float*3),
|
||||
@@ -57,7 +55,6 @@ class DriverStateResult(ctypes.Structure):
|
||||
("ready_prob", ctypes.c_float*4),
|
||||
("not_ready_prob", ctypes.c_float*2)]
|
||||
|
||||
|
||||
class DMonitoringModelResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("driver_state_lhd", DriverStateResult),
|
||||
@@ -66,7 +63,6 @@ class DMonitoringModelResult(ctypes.Structure):
|
||||
("wheel_on_right_prob", ctypes.c_float),
|
||||
("features", ctypes.c_float*FEATURE_LEN)]
|
||||
|
||||
|
||||
class ModelState:
|
||||
inputs: dict[str, np.ndarray]
|
||||
output: np.ndarray
|
||||
@@ -86,7 +82,7 @@ class ModelState:
|
||||
else:
|
||||
self.onnx_cpu_runner = make_onnx_cpu_runner(MODEL_PATH)
|
||||
|
||||
def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
|
||||
def run(self, buf:VisionBuf, calib:np.ndarray, transform:np.ndarray) -> tuple[np.ndarray, float]:
|
||||
self.numpy_inputs['calib'][0,:] = calib
|
||||
|
||||
t1 = time.perf_counter()
|
||||
@@ -123,7 +119,6 @@ def fill_driver_state(msg, ds_result: DriverStateResult):
|
||||
msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob]
|
||||
msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob]
|
||||
|
||||
|
||||
def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, gpu_execution_time: float):
|
||||
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
|
||||
msg = messaging.new_message('driverStateV2', valid=True)
|
||||
@@ -144,9 +139,6 @@ def main():
|
||||
setproctitle(PROCESS_NAME)
|
||||
set_realtime_priority(1)
|
||||
|
||||
sentry.set_tag("daemon", PROCESS_NAME)
|
||||
cloudlog.bind(daemon=PROCESS_NAME)
|
||||
|
||||
cl_context = CLContext()
|
||||
model = ModelState(cl_context)
|
||||
cloudlog.warning("models loaded, dmonitoringmodeld starting")
|
||||
@@ -185,10 +177,4 @@ def main():
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except KeyboardInterrupt:
|
||||
cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
|
||||
except Exception:
|
||||
sentry.capture_exception()
|
||||
raise
|
||||
main()
|
||||
|
||||
@@ -93,7 +93,9 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
driving_model_data.frameIdExtra = vipc_frame_id_extra
|
||||
driving_model_data.frameDropPerc = frame_drop_perc
|
||||
driving_model_data.modelExecutionTime = model_execution_time
|
||||
driving_model_data.action.desiredCurvature = desired_curvature
|
||||
|
||||
action = driving_model_data.action
|
||||
action.desiredCurvature = desired_curvature
|
||||
|
||||
modelV2 = extended_msg.modelV2
|
||||
modelV2.frameId = vipc_frame_id
|
||||
@@ -104,11 +106,16 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
modelV2.modelExecutionTime = model_execution_time
|
||||
|
||||
# plan
|
||||
fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
|
||||
fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
|
||||
fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
|
||||
fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
|
||||
fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
|
||||
position = modelV2.position
|
||||
fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
|
||||
velocity = modelV2.velocity
|
||||
fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
|
||||
acceleration = modelV2.acceleration
|
||||
fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
|
||||
orientation = modelV2.orientation
|
||||
fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
|
||||
orientation_rate = modelV2.orientationRate
|
||||
fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
|
||||
|
||||
# temporal pose
|
||||
temporal_pose = modelV2.temporalPose
|
||||
@@ -118,10 +125,12 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
|
||||
|
||||
# poly path
|
||||
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
|
||||
poly_path = driving_model_data.path
|
||||
fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
|
||||
|
||||
# lateral planning
|
||||
modelV2.action.desiredCurvature = desired_curvature
|
||||
action = modelV2.action
|
||||
action.desiredCurvature = desired_curvature
|
||||
|
||||
# times at X_IDXS according to model plan
|
||||
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
|
||||
@@ -150,7 +159,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
|
||||
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
|
||||
|
||||
fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs)
|
||||
lane_line_meta = driving_model_data.laneLineMeta
|
||||
fill_lane_line_meta(lane_line_meta, modelV2.laneLines, modelV2.laneLineProbs)
|
||||
|
||||
# road edges
|
||||
modelV2.init('roadEdges', 2)
|
||||
|
||||
@@ -194,7 +194,7 @@ def main(demo=False):
|
||||
CP = get_demo_car_params()
|
||||
else:
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("modeld got CarParams: %s", CP.brand)
|
||||
cloudlog.info("modeld got CarParams: %s", CP.carName)
|
||||
|
||||
# TODO this needs more thought, use .2s extra for now to estimate other delays
|
||||
steer_delay = CP.steerActuatorDelay + .2
|
||||
|
||||
@@ -105,6 +105,9 @@ void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
|
||||
while (!do_exit && check_all_connected(pandas)) {
|
||||
std::unique_ptr<Message> msg(subscriber->receive());
|
||||
if (!msg) {
|
||||
if (errno == EINTR) {
|
||||
do_exit = true;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -202,7 +205,7 @@ void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const
|
||||
cs.setCanCoreResetCnt(can_health.can_core_reset_cnt);
|
||||
}
|
||||
|
||||
std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started, bool always_offroad) {
|
||||
std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started, PandaSafety *panda_safety) {
|
||||
bool ignition_local = false;
|
||||
const uint32_t pandas_cnt = pandas.size();
|
||||
|
||||
@@ -250,7 +253,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
|
||||
health.ignition_line_pkt = 0;
|
||||
}
|
||||
|
||||
ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)) && !always_offroad;
|
||||
ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)) && !panda_safety->getOffroadMode();
|
||||
|
||||
pandaStates.push_back(health);
|
||||
}
|
||||
@@ -337,14 +340,16 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
|
||||
pm->send("peripheralState", msg);
|
||||
}
|
||||
|
||||
void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool engaged, bool engaged_mads, bool spoofing_started, bool always_offroad) {
|
||||
void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoofing_started, PandaSafety *panda_safety) {
|
||||
static SubMaster sm({"selfdriveState", "selfdriveStateSP", "carParams"});
|
||||
|
||||
std::vector<std::string> connected_serials;
|
||||
for (Panda *p : pandas) {
|
||||
connected_serials.push_back(p->hw_serial());
|
||||
}
|
||||
|
||||
{
|
||||
auto ignition_opt = send_panda_states(pm, pandas, spoofing_started, always_offroad);
|
||||
auto ignition_opt = send_panda_states(pm, pandas, spoofing_started, panda_safety);
|
||||
if (!ignition_opt) {
|
||||
LOGE("Failed to get ignition_opt");
|
||||
return;
|
||||
@@ -373,6 +378,9 @@ void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool engag
|
||||
}
|
||||
}
|
||||
|
||||
sm.update(0);
|
||||
const bool engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
|
||||
const bool engaged_mads = process_mads_heartbeat(&sm);
|
||||
for (const auto &panda : pandas) {
|
||||
panda->send_heartbeat(engaged, engaged_mads);
|
||||
}
|
||||
@@ -438,13 +446,9 @@ void pandad_run(std::vector<Panda *> &pandas) {
|
||||
std::thread send_thread(can_send_thread, pandas, fake_send);
|
||||
|
||||
RateKeeper rk("pandad", 100);
|
||||
SubMaster sm({"selfdriveState", "selfdriveStateSP", "carParams"});
|
||||
PubMaster pm({"can", "pandaStates", "peripheralState"});
|
||||
PandaSafety panda_safety(pandas);
|
||||
Panda *peripheral_panda = pandas[0];
|
||||
bool engaged = false;
|
||||
bool engaged_mads = false;
|
||||
bool always_offroad = false;
|
||||
|
||||
// Main loop: receive CAN data and process states
|
||||
while (!do_exit && check_all_connected(pandas)) {
|
||||
@@ -457,11 +461,7 @@ void pandad_run(std::vector<Panda *> &pandas) {
|
||||
|
||||
// Process panda state at 10 Hz
|
||||
if (rk.frame() % 10 == 0) {
|
||||
sm.update(0);
|
||||
engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
|
||||
engaged_mads = process_mads_heartbeat(&sm);
|
||||
always_offroad = panda_safety.getOffroadMode();
|
||||
process_panda_state(pandas, &pm, engaged, engaged_mads, spoofing_started, always_offroad);
|
||||
process_panda_state(pandas, &pm, spoofing_started, &panda_safety);
|
||||
panda_safety.configureSafetyMode();
|
||||
}
|
||||
|
||||
@@ -473,16 +473,6 @@ void pandad_run(std::vector<Panda *> &pandas) {
|
||||
rk.keepTime();
|
||||
}
|
||||
|
||||
// Close relay on exit to prevent a fault
|
||||
const bool is_onroad = Params().getBool("IsOnroad");
|
||||
if (is_onroad && !engaged) {
|
||||
for (auto &p : pandas) {
|
||||
if (p->connected()) {
|
||||
p->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
send_thread.join();
|
||||
}
|
||||
|
||||
|
||||
@@ -3,8 +3,8 @@
|
||||
import os
|
||||
import usb1
|
||||
import time
|
||||
import signal
|
||||
import subprocess
|
||||
from typing import NoReturn
|
||||
|
||||
from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
@@ -61,25 +61,13 @@ def flash_panda(panda_serial: str) -> Panda:
|
||||
return panda
|
||||
|
||||
|
||||
def main() -> None:
|
||||
# signal pandad to close the relay and exit
|
||||
def signal_handler(signum, frame):
|
||||
cloudlog.info(f"Caught signal {signum}, exiting")
|
||||
nonlocal do_exit
|
||||
do_exit = True
|
||||
if process is not None:
|
||||
process.send_signal(signal.SIGINT)
|
||||
|
||||
process = None
|
||||
do_exit = False
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
|
||||
def main() -> NoReturn:
|
||||
count = 0
|
||||
first_run = True
|
||||
params = Params()
|
||||
no_internal_panda_count = 0
|
||||
|
||||
while not do_exit:
|
||||
while True:
|
||||
try:
|
||||
count += 1
|
||||
cloudlog.event("pandad.flash_and_connect", count=count)
|
||||
@@ -171,9 +159,8 @@ def main() -> None:
|
||||
|
||||
# run pandad with all connected serials as arguments
|
||||
os.environ['MANAGER_DAEMON'] = 'pandad'
|
||||
process = subprocess.Popen(["./pandad", *panda_serials], cwd=os.path.join(BASEDIR, "selfdrive/pandad"))
|
||||
process.wait()
|
||||
|
||||
os.chdir(os.path.join(BASEDIR, "selfdrive/pandad"))
|
||||
subprocess.run(["./pandad", *panda_serials], check=True)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -154,7 +154,7 @@ def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
|
||||
|
||||
def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
text = "Enable Adaptive Cruise to Engage"
|
||||
if CP.brand == "honda":
|
||||
if CP.carName == "honda":
|
||||
text = "Enable Main Switch to Engage"
|
||||
return NoEntryAlert(text)
|
||||
|
||||
|
||||
@@ -99,7 +99,7 @@ class SelfdriveD(CruiseHelper):
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
|
||||
|
||||
car_recognized = self.CP.brand != 'mock'
|
||||
car_recognized = self.CP.carName != 'mock'
|
||||
|
||||
# cleanup old params
|
||||
if not self.CP.experimentalLongitudinalAvailable:
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user