Vehicle Researcher
parent
92c596544d
commit
860a48765d
131
README.md
131
README.md
|
|
@ -3,9 +3,26 @@
|
|||
Welcome to openpilot
|
||||
======
|
||||
|
||||
[openpilot](http://github.com/commaai/openpilot) is an open source driving agent. Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for selected Honda, Toyota, Acura, Lexus, Chevrolet, Hyundai, Kia. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
|
||||
[openpilot](http://github.com/commaai/openpilot) is an open source driving agent. Currently, it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for selected Honda, Toyota, Acura, Lexus, Chevrolet, Hyundai, Kia. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
|
||||
|
||||
The openpilot codebase has been written to be concise and enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier.
|
||||
The openpilot codebase has been written to be concise and to enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier.
|
||||
|
||||
Table of Contents
|
||||
=======================
|
||||
|
||||
* [Community](#community)
|
||||
* [Hardware](#hardware)
|
||||
* [Supported Cars](#supported-cars)
|
||||
* [Community Maintained Cars](#community-maintained-cars)
|
||||
* [In Progress Cars](#in-progress-cars)
|
||||
* [How can I add support for my car?](#how-can-i-add-support-for-my-car-)
|
||||
* [Directory structure](#directory-structure)
|
||||
* [User Data / chffr Account / Crash Reporting](#user-data-chffr-account-crash-reporting)
|
||||
* [Testing on PC](#testing-on-pc)
|
||||
* [Contributing](#contributing)
|
||||
* [Licensing](#licensing)
|
||||
|
||||
---
|
||||
|
||||
Community
|
||||
------
|
||||
|
|
@ -41,73 +58,59 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur
|
|||
Supported Cars
|
||||
------
|
||||
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| ---------------------| ----------------------| ---------------------| --------| ---------------| -----------------| ---------------|
|
||||
| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Chevrolet<sup>3</sup>| Volt 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Chevrolet<sup>3</sup>| Volt 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph |
|
||||
| Honda | Civic 2016 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2017 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | Civic 2018 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2018 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V 2015 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2019 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Hyundai<sup>6</sup> | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Hyundai<sup>6</sup> | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph |
|
||||
| Hyundai<sup>6</sup> | Genesis 2018 | All | Yes | Stock | 19mph | 34mph |
|
||||
| Kia<sup>6</sup> | Sorento 2018 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Kia<sup>6</sup> | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Camry 2018<sup>4</sup>| All | Yes | Stock | 0mph<sup>5</sup> | 0mph |
|
||||
| Toyota | C-HR 2018<sup>4</sup> | All | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Corolla 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Highlander 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2018| All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
|
||||
| ---------------------| ------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
|
||||
| Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | Bosch |
|
||||
| Honda | Civic 2016-18 | Honda Sensing | Yes | Yes | 0mph | 12mph | Nidec |
|
||||
| Honda | Civic 2017-18 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
|
||||
| Honda | Pilot 2017-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Ridgeline 2017-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Lexus | RX Hybrid 2016-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-18<sup>4</sup>| All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Highlander 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Highlander Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius Prime 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
|
||||
<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/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. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
|
||||
<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)
|
||||
<sup>3</sup>[GM installation guide](https://www.zoneos.com/volt.htm)
|
||||
<sup>3</sup>[GM installation guide](https://zoneos.com/volt/).
|
||||
<sup>4</sup>It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).
|
||||
<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
|
||||
<sup>6</sup>Giraffe is under development: architecture similar to Toyota giraffe, with an extra 120Ohm resistor on bus 3.
|
||||
<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed ofor the 2019 Sante Fe; pinout may differ for other Hyundais. <br />
|
||||
<sup>7</sup>Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/) <br />
|
||||
|
||||
Community Maintained Cars
|
||||
------
|
||||
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- |
|
||||
| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph |
|
||||
| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph |
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
|
||||
| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- | ------------------|
|
||||
| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph | Custom<sup>8</sup>|
|
||||
| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph | Custom<sup>8</sup>|
|
||||
|
||||
[[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266).
|
||||
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246)
|
||||
[[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266). <br />
|
||||
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246) <br />
|
||||
<sup>8</sup>Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla) <br />
|
||||
|
||||
Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.
|
||||
|
||||
|
|
@ -116,6 +119,7 @@ In Progress Cars
|
|||
- All TSS-P Toyota with Steering Assist.
|
||||
- 'Full Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the Prius, Camry and C-HR have this option.
|
||||
- Even though the Tundra, Sequoia and the Land Cruiser have TSS-P, they don't have Steering Assist and are not supported.
|
||||
- Only remaining Toyota cars with no port yet are the Avalon and the Sienna.
|
||||
- All LSS-P Lexus with Steering Assist or Lane Keep Assist.
|
||||
- 'All-Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the GS, GSH, F, RX, RXH, LX, NX, NXH, LC, LCH, LS, LSH have this option.
|
||||
- Even though the LX have TSS-P, it does not have Steering Assist and is not supported.
|
||||
|
|
@ -131,7 +135,7 @@ We've written guides for [Brand](https://medium.com/@comma_ai/how-to-write-a-car
|
|||
|
||||
- BMW, Audi, Volvo, and Mercedes all use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) and are unlikely to be supported any time soon.
|
||||
- We put time into a Ford port, but the steering has a 10 second cutout limitation that makes it unusable.
|
||||
- The 2016-2017 Honda Accord use a custom signaling protocol for steering that's unlikely to ever be upstreamed.
|
||||
- The 2016-2017 Honda Accord uses a custom signaling protocol for steering that's unlikely to ever be upstreamed.
|
||||
|
||||
Directory structure
|
||||
------
|
||||
|
|
@ -166,7 +170,7 @@ To understand how the services interact, see `selfdrive/service_list.yaml`
|
|||
User Data / chffr Account / Crash Reporting
|
||||
------
|
||||
|
||||
By default openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone.
|
||||
By default, openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone.
|
||||
|
||||
It's open source software, so you are free to disable it if you wish.
|
||||
|
||||
|
|
@ -190,6 +194,7 @@ The resulting plots are displayed in `selfdrive/test/tests/plant/out/longitudina
|
|||
|
||||
More extensive testing infrastructure and simulation environments are coming soon.
|
||||
|
||||
|
||||
Contributing
|
||||
------
|
||||
|
||||
|
|
@ -208,3 +213,7 @@ Any user of this software shall indemnify and hold harmless Comma.ai, Inc. and i
|
|||
**THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT.
|
||||
YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS.
|
||||
NO WARRANTY EXPRESSED OR IMPLIED.**
|
||||
|
||||
---
|
||||
|
||||
<img src="https://d1qb2nb5cznatu.cloudfront.net/startups/i/1061157-bc7e9bf3b246ece7322e6ffe653f6af8-medium_jpg.jpg?buster=1458363130" width="75"></img> <img src="https://cdn-images-1.medium.com/max/1600/1*C87EjxGeMPrkTuVRVWVg4w.png" width="225"></img>
|
||||
|
|
|
|||
10
RELEASES.md
10
RELEASES.md
|
|
@ -1,3 +1,13 @@
|
|||
Version 0.5.6 (2018-11-16)
|
||||
========================
|
||||
* Refresh settings layout and add feature descriptions
|
||||
* In Honda, keep stock camera on for logging and extra stock features; new openpilot giraffe setting is 0111!
|
||||
* In Toyota, option to keep stock camera on for logging and extra stock features (e.g. AHB); 120Ohm resistor required on giraffe.
|
||||
* Improve camera calibration stability
|
||||
* More tuning to Honda positive accelerations
|
||||
* Reduce brake pump use on Hondas
|
||||
* Chevrolet Malibu support thanks to tylergets!
|
||||
|
||||
Version 0.5.5 (2018-10-20)
|
||||
========================
|
||||
* Increase allowed Honda positive accelerations
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -71,6 +71,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
|||
belowSteerSpeed @46;
|
||||
calibrationProgress @47;
|
||||
lowBattery @48;
|
||||
invalidGiraffeHonda @49;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -260,13 +261,13 @@ struct CarControl {
|
|||
# these are the choices from the Honda
|
||||
# map as good as you can for your car
|
||||
none @0;
|
||||
beepSingle @1;
|
||||
beepTriple @2;
|
||||
beepRepeated @3;
|
||||
chimeSingle @4;
|
||||
chimeDouble @5;
|
||||
chimeRepeated @6;
|
||||
chimeContinuous @7;
|
||||
chimeEngage @1;
|
||||
chimeDisengage @2;
|
||||
chimeError @3;
|
||||
chimeWarning1 @4;
|
||||
chimeWarning2 @5;
|
||||
chimeWarningRepeat @6;
|
||||
chimePrompt @7;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -411,6 +411,7 @@ struct Live100Data {
|
|||
alertSize @39 :AlertSize;
|
||||
alertBlinkingRate @42 :Float32;
|
||||
alertType @44 :Text;
|
||||
alertSound @45 :Text;
|
||||
awarenessStatus @26 :Float32;
|
||||
angleOffset @27 :Float32;
|
||||
gpsPlannerActive @40 :Bool;
|
||||
|
|
@ -1565,6 +1566,11 @@ struct LiveParametersData {
|
|||
angleOffset @2 :Float32;
|
||||
}
|
||||
|
||||
struct LiveMapData {
|
||||
valid @0 :Bool;
|
||||
speedLimit @1 :Float32;
|
||||
}
|
||||
|
||||
|
||||
struct Event {
|
||||
# in nanoseconds?
|
||||
|
|
@ -1632,5 +1638,6 @@ struct Event {
|
|||
driverMonitoring @59 :DriverMonitoring;
|
||||
boot @60 :Boot;
|
||||
liveParameters @61 :LiveParametersData;
|
||||
liveMapData @62 :LiveMapData;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
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.
|
|
@ -1,4 +1,8 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# TODO: merge the extra functionalities of this file (like MOCK) in boardd.c and
|
||||
# delete this python version of boardd
|
||||
|
||||
import os
|
||||
import struct
|
||||
import zmq
|
||||
|
|
@ -16,8 +20,6 @@ try:
|
|||
except Exception:
|
||||
pass
|
||||
|
||||
# TODO: rewrite in C to save CPU
|
||||
|
||||
SAFETY_NOOUTPUT = 0
|
||||
SAFETY_HONDA = 1
|
||||
SAFETY_TOYOTA = 2
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ from selfdrive.can.packer import CANPacker
|
|||
|
||||
class CarControllerParams():
|
||||
def __init__(self, car_fingerprint):
|
||||
if car_fingerprint == CAR.VOLT:
|
||||
if car_fingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
self.STEER_MAX = 300
|
||||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s)
|
||||
|
|
@ -104,7 +104,7 @@ class CarController(object):
|
|||
self.apply_steer_last = apply_steer
|
||||
idx = (frame / P.STEER_STEP) % 4
|
||||
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt,
|
||||
canbus.powertrain, apply_steer, idx, lkas_enabled))
|
||||
if self.car_fingerprint == CAR.CADILLAC_CT6:
|
||||
|
|
@ -113,7 +113,7 @@ class CarController(object):
|
|||
|
||||
### GAS/BRAKE ###
|
||||
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
# no output if not enabled, but keep sending keepalive messages
|
||||
# treat pedals as one
|
||||
final_pedal = actuators.gas - actuators.brake
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ def get_powertrain_can_parser(CP, canbus):
|
|||
("LKATorqueDeliveredStatus", "PSCMStatus", 0),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.VOLT:
|
||||
if CP.carFingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
signals += [
|
||||
("RegenPaddle", "EBCMRegenPaddle", 0),
|
||||
("TractionControlOn", "ESPStatus", 0),
|
||||
|
|
@ -117,14 +117,14 @@ class CarState(object):
|
|||
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
|
||||
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
|
||||
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
|
||||
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
|
||||
self.acc_active = False
|
||||
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
|
||||
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
|
||||
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
|
||||
else:
|
||||
else:
|
||||
self.park_brake = False
|
||||
self.main_on = False
|
||||
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
|
||||
|
|
@ -141,4 +141,3 @@ class CarState(object):
|
|||
self.brake_pressed = self.user_brake > 10 or self.regen_pressed
|
||||
|
||||
self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ from common.realtime import sec_since_boot
|
|||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS
|
||||
from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD
|
||||
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
|
||||
|
||||
try:
|
||||
|
|
@ -85,6 +85,16 @@ class CarInterface(object):
|
|||
ret.steerRatioRear = 0.
|
||||
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
||||
|
||||
elif candidate == CAR.MALIBU:
|
||||
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.mass = 1496 + std_cargo
|
||||
ret.safetyModel = car.CarParams.SafetyModels.gm
|
||||
ret.wheelbase = 2.83
|
||||
ret.steerRatio = 15.8
|
||||
ret.steerRatioRear = 0.
|
||||
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
||||
|
||||
elif candidate == CAR.CADILLAC_CT6:
|
||||
# engage speed is decided by pcm
|
||||
ret.minEnableSpeed = -1
|
||||
|
|
@ -254,7 +264,7 @@ class CarInterface(object):
|
|||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if self.CS.car_fingerprint == CAR.VOLT:
|
||||
if self.CS.car_fingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
|
||||
if self.CS.brake_error:
|
||||
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
|
|
@ -312,15 +322,7 @@ class CarInterface(object):
|
|||
if hud_v_cruise > 70:
|
||||
hud_v_cruise = 0
|
||||
|
||||
chime, chime_count = {
|
||||
"none": (0, 0),
|
||||
"beepSingle": (CM.HIGH_CHIME, 1),
|
||||
"beepTriple": (CM.HIGH_CHIME, 3),
|
||||
"beepRepeated": (CM.LOW_CHIME, -1),
|
||||
"chimeSingle": (CM.LOW_CHIME, 1),
|
||||
"chimeDouble": (CM.LOW_CHIME, 2),
|
||||
"chimeRepeated": (CM.LOW_CHIME, -1),
|
||||
"chimeContinuous": (CM.LOW_CHIME, -1)}[str(c.hudControl.audibleAlert)]
|
||||
chime, chime_count = AUDIO_HUD[c.hudControl.audibleAlert.raw]
|
||||
|
||||
# For Openpilot, "enabled" includes pre-enable.
|
||||
# In GM, PCM faults out if ACC command overlaps user gas.
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
|
|||
def create_radard_can_parser(canbus, car_fingerprint):
|
||||
|
||||
dbc_f = DBC[car_fingerprint]['radar']
|
||||
if car_fingerprint == CAR.VOLT:
|
||||
if car_fingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
# C1A-ARS3-A by Continental
|
||||
radar_targets = range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)
|
||||
signals = zip(['FLRRNumValidTargets',
|
||||
|
|
@ -125,5 +125,3 @@ if __name__ == "__main__":
|
|||
ret = RI.update()
|
||||
print(chr(27) + "[2J")
|
||||
print ret
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,9 +1,12 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
class CAR:
|
||||
VOLT = "CHEVROLET VOLT PREMIER 2017"
|
||||
CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018"
|
||||
MALIBU = "CHEVROLET MALIBU PREMIER 2017"
|
||||
|
||||
class CruiseButtons:
|
||||
UNPRESS = 1
|
||||
|
|
@ -12,9 +15,28 @@ class CruiseButtons:
|
|||
MAIN = 5
|
||||
CANCEL = 6
|
||||
|
||||
# Car chimes, beeps, blinker sounds etc
|
||||
class CM:
|
||||
TOCK = 0x81
|
||||
TICK = 0x82
|
||||
LOW_BEEP = 0x84
|
||||
HIGH_BEEP = 0x85
|
||||
LOW_CHIME = 0x86
|
||||
HIGH_CHIME = 0x87
|
||||
|
||||
AUDIO_HUD = {
|
||||
AudibleAlert.none: (0, 0),
|
||||
AudibleAlert.chimeEngage: (CM.HIGH_CHIME, 1),
|
||||
AudibleAlert.chimeDisengage: (CM.HIGH_CHIME, 1),
|
||||
AudibleAlert.chimeError: (CM.LOW_CHIME, 2),
|
||||
AudibleAlert.chimePrompt: (CM.LOW_CHIME, 1),
|
||||
AudibleAlert.chimeWarning1: (CM.LOW_CHIME, 2),
|
||||
AudibleAlert.chimeWarning2: (CM.LOW_CHIME, -1),
|
||||
AudibleAlert.chimeWarningRepeat: (CM.LOW_CHIME, -1)}
|
||||
|
||||
def is_eps_status_ok(eps_status, car_fingerprint):
|
||||
valid_eps_status = []
|
||||
if car_fingerprint == CAR.VOLT:
|
||||
if car_fingerprint in (CAR.VOLT, CAR.MALIBU):
|
||||
valid_eps_status += [0, 1]
|
||||
elif car_fingerprint == CAR.CADILLAC_CT6:
|
||||
valid_eps_status += [0, 1, 4, 5, 6]
|
||||
|
|
@ -45,16 +67,23 @@ FINGERPRINTS = {
|
|||
CAR.CADILLAC_CT6: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 336: 1, 338: 6, 340: 6, 352: 5, 354: 5, 356: 8, 368: 3, 372: 5, 381: 8, 386: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 458: 5, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 800: 6, 801: 8, 804: 3, 810: 8, 832: 8, 833: 8, 834: 8, 835: 6, 836: 5, 837: 8, 838: 8, 839: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 884: 8, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1934: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.MALIBU: [
|
||||
# Malibu Premier w/ ACC 2017
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8,
|
||||
}],
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
STOCK_CONTROL_MSGS = {
|
||||
CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.MALIBU: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.CADILLAC_CT6: [], # Cadillac does not require ASCMs to be disconnected
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'),
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
from cereal import car
|
||||
from collections import namedtuple
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.controls.lib.drive_helpers import rate_limit
|
||||
from common.numpy_fast import clip
|
||||
|
|
@ -8,7 +8,7 @@ from selfdrive.car.honda.values import AH, CruiseButtons, CAR
|
|||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
# hyst params... TODO: move these to VehicleParams
|
||||
# hyst params
|
||||
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
|
||||
|
|
@ -33,6 +33,24 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
|
|||
return brake, braking, brake_steady
|
||||
|
||||
|
||||
def brake_pump_hysteresys(apply_brake, apply_brake_last, last_pump_ts):
|
||||
ts = sec_since_boot()
|
||||
pump_on = False
|
||||
|
||||
# reset pump timer if:
|
||||
# - there is an increment in brake request
|
||||
# - we are applying steady state brakes and we haven't been running the pump
|
||||
# for more than 20s (to prevent pressure bleeding)
|
||||
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20 and apply_brake > 0):
|
||||
last_pump_ts = ts
|
||||
|
||||
# once the pump is on, run it for at least 0.2s
|
||||
if ts - last_pump_ts < 0.2 and apply_brake > 0:
|
||||
pump_on = True
|
||||
|
||||
return pump_on, last_pump_ts
|
||||
|
||||
|
||||
def process_hud_alert(hud_alert):
|
||||
# initialize to no alert
|
||||
fcw_display = 0
|
||||
|
|
@ -60,6 +78,8 @@ class CarController(object):
|
|||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
self.apply_brake_last = 0
|
||||
self.last_pump_ts = 0
|
||||
self.enable_camera = enable_camera
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.new_radar_config = False
|
||||
|
|
@ -109,9 +129,6 @@ class CarController(object):
|
|||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
|
||||
0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
|
||||
|
||||
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
|
||||
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
|
@ -128,7 +145,6 @@ class CarController(object):
|
|||
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
|
||||
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
|
||||
|
||||
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
|
||||
lkas_active = enabled and not CS.steer_not_allowed
|
||||
|
||||
# Send CAN commands.
|
||||
|
|
@ -136,7 +152,8 @@ class CarController(object):
|
|||
|
||||
# Send steering command.
|
||||
idx = frame % 4
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx))
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
|
||||
lkas_active, CS.CP.carFingerprint, idx))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
|
|
@ -149,28 +166,19 @@ class CarController(object):
|
|||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx))
|
||||
elif CS.stopped:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if (frame % 2) == 0:
|
||||
idx = (frame / 2) % 4
|
||||
can_sends.append(
|
||||
hondacan.create_brake_command(self.packer, apply_brake, pcm_override,
|
||||
pcm_cancel_cmd, hud.chime, hud.fcw, idx))
|
||||
pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts)
|
||||
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx))
|
||||
self.apply_brake_last = apply_brake
|
||||
|
||||
if CS.CP.enableGasInterceptor:
|
||||
# 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
|
||||
can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx))
|
||||
|
||||
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
|
||||
if CS.CP.carFingerprint == CAR.ACURA_ILX:
|
||||
radar_send_step = 2
|
||||
else:
|
||||
radar_send_step = 5
|
||||
|
||||
if (frame % radar_send_step) == 0:
|
||||
idx = (frame/radar_send_step) % 4
|
||||
if not self.new_radar_config: # only change state once
|
||||
self.new_radar_config = car.RadarState.Error.wrongConfig in radar_error
|
||||
can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, self.new_radar_config, idx))
|
||||
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@ from common.numpy_fast import interp
|
|||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.can.parser import CANParser, CANDefine
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR
|
||||
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
|
||||
|
||||
def parse_gear_shifter(gear, vals):
|
||||
|
||||
|
|
@ -129,6 +129,17 @@ def get_can_parser(CP):
|
|||
signals, checks = get_can_signals(CP)
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
def get_cam_can_parser(CP):
|
||||
signals = []
|
||||
|
||||
# all hondas except CRV and RDX use 0xe4 for steering
|
||||
checks = [(0xe4, 100)]
|
||||
if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX]:
|
||||
checks = [(0x194, 100)]
|
||||
|
||||
cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus)
|
||||
|
||||
class CarState(object):
|
||||
def __init__(self, CP):
|
||||
|
|
@ -160,10 +171,11 @@ class CarState(object):
|
|||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
def update(self, cp):
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
# copy can_valid
|
||||
# copy can_valid on buses 0 and 2
|
||||
self.can_valid = cp.can_valid
|
||||
self.cam_can_valid = cp_cam.can_valid
|
||||
|
||||
# car params
|
||||
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
|
|
|
|||
|
|
@ -1,8 +1,6 @@
|
|||
import struct
|
||||
|
||||
import common.numpy_fast as np
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.honda.values import CAR, HONDA_BOSCH, VEHICLE_STATE_MSG
|
||||
from selfdrive.car.honda.values import CAR, HONDA_BOSCH
|
||||
|
||||
# *** Honda specific ***
|
||||
def can_cksum(mm):
|
||||
|
|
@ -21,16 +19,8 @@ def fix(msg, addr):
|
|||
return msg2
|
||||
|
||||
|
||||
def make_can_msg(addr, dat, idx, alt):
|
||||
if idx is not None:
|
||||
dat += chr(idx << 4)
|
||||
dat = fix(dat, addr)
|
||||
return [addr, 0, dat, alt]
|
||||
|
||||
|
||||
def create_brake_command(packer, apply_brake, pcm_override, pcm_cancel_cmd, chime, fcw, idx):
|
||||
"""Creates a CAN message for the Honda DBC BRAKE_COMMAND."""
|
||||
pump_on = apply_brake > 0
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx):
|
||||
# TODO: do we loose pressure if we keep pump off for long?
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
pcm_fault_cmd = False
|
||||
|
|
@ -45,13 +35,13 @@ def create_brake_command(packer, apply_brake, pcm_override, pcm_cancel_cmd, chim
|
|||
"SET_ME_0X80": 0x80,
|
||||
"BRAKE_LIGHTS": brakelights,
|
||||
"CHIME": chime,
|
||||
"FCW": fcw << 1, # TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
|
||||
# TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
|
||||
"FCW": fcw << 1,
|
||||
}
|
||||
return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
|
||||
|
||||
|
||||
def create_gas_command(packer, gas_amount, idx):
|
||||
"""Creates a CAN message for the Honda DBC GAS_COMMAND."""
|
||||
enable = gas_amount > 0.001
|
||||
|
||||
values = {"ENABLE": enable}
|
||||
|
|
@ -64,7 +54,6 @@ def create_gas_command(packer, gas_amount, idx):
|
|||
|
||||
|
||||
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx):
|
||||
"""Creates a CAN message for the Honda DBC STEERING_CONTROL."""
|
||||
values = {
|
||||
"STEER_TORQUE": apply_steer if lkas_active else 0,
|
||||
"STEER_TORQUE_REQUEST": lkas_active,
|
||||
|
|
@ -75,7 +64,6 @@ def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, i
|
|||
|
||||
|
||||
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
|
||||
"""Creates an iterable of CAN messages for the UIs."""
|
||||
commands = []
|
||||
bus = 0
|
||||
|
||||
|
|
@ -105,7 +93,6 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
|
|||
commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values, idx))
|
||||
|
||||
if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY):
|
||||
commands.append(packer.make_can_msg('HIGHBEAM_CONTROL', 0, {'HIGHBEAMS_ON': False}, idx))
|
||||
|
||||
radar_hud_values = {
|
||||
'ACC_ALERTS': hud.acc_alert,
|
||||
|
|
@ -117,26 +104,6 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
|
|||
return commands
|
||||
|
||||
|
||||
def create_radar_commands(v_ego, car_fingerprint, new_radar_config, idx):
|
||||
"""Creates an iterable of CAN messages for the radar system."""
|
||||
commands = []
|
||||
v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255)
|
||||
speed = struct.pack('!B', v_ego_kph)
|
||||
|
||||
msg_0x300 = ("\xf9" + speed + "\x8a\xd0" +
|
||||
("\x20" if idx == 0 or idx == 3 else "\x00") +
|
||||
"\x00\x00")
|
||||
msg_0x301 = VEHICLE_STATE_MSG[car_fingerprint]
|
||||
|
||||
idx_0x300 = idx
|
||||
if car_fingerprint == CAR.CIVIC:
|
||||
idx_offset = 0xc if new_radar_config else 0x8 # radar in civic 2018 requires 0xc
|
||||
idx_0x300 += idx_offset
|
||||
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx_0x300, 1))
|
||||
commands.append(make_can_msg(0x301, msg_0x301, idx, 1))
|
||||
return commands
|
||||
|
||||
def spam_buttons_command(packer, button_val, idx):
|
||||
values = {
|
||||
'CRUISE_BUTTONS': button_val,
|
||||
|
|
|
|||
|
|
@ -8,8 +8,9 @@ from selfdrive.swaglog import cloudlog
|
|||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.honda.carstate import CarState, get_can_parser
|
||||
from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH, CAR, HONDA_BOSCH
|
||||
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, VISUAL_HUD
|
||||
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
|
||||
|
||||
try:
|
||||
from selfdrive.car.honda.carcontroller import CarController
|
||||
|
|
@ -21,6 +22,8 @@ except ImportError:
|
|||
# those messages are mutually exclusive on CRV and non-CRV cars
|
||||
CAMERA_MSGS = [0xe4, 0x194]
|
||||
|
||||
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
|
||||
|
||||
|
||||
def compute_gb_honda(accel, speed):
|
||||
creep_brake = 0.0
|
||||
|
|
@ -86,8 +89,10 @@ class CarInterface(object):
|
|||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
self.can_invalid_count = 0
|
||||
self.cam_can_invalid_count = 0
|
||||
|
||||
self.cp = get_can_parser(CP)
|
||||
self.cp_cam = get_cam_can_parser(CP)
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
|
|
@ -105,6 +110,12 @@ class CarInterface(object):
|
|||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
|
||||
# normalized max accel. Allowing max accel at low speed causes speed overshoots
|
||||
max_accel_bp = [10, 20] # m/s
|
||||
max_accel_v = [0.714, 1.0] # unit of max accel
|
||||
max_accel = interp(v_ego, max_accel_bp, max_accel_v)
|
||||
|
||||
# limit the pcm accel cmd if:
|
||||
# - v_ego exceeds v_target, or
|
||||
# - a_ego exceeds a_target and v_ego is close to v_target
|
||||
|
|
@ -127,7 +138,7 @@ class CarInterface(object):
|
|||
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
|
||||
# unless aTargetMax is very high and then we scale with it; this help in quicker restart
|
||||
|
||||
return float(min(speedLimiter, accelLimiter))
|
||||
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint):
|
||||
|
|
@ -369,8 +380,9 @@ class CarInterface(object):
|
|||
canMonoTimes = []
|
||||
|
||||
self.cp.update(int(sec_since_boot() * 1e9), False)
|
||||
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
|
||||
|
||||
self.CS.update(self.cp)
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
|
@ -482,6 +494,12 @@ class CarInterface(object):
|
|||
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
else:
|
||||
self.can_invalid_count = 0
|
||||
if not self.CS.cam_can_valid and self.CP.enableCamera:
|
||||
self.cam_can_invalid_count += 1
|
||||
if self.cam_can_invalid_count >= 5 and self.CS.CP.carFingerprint not in HONDA_BOSCH:
|
||||
events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
else:
|
||||
self.cam_can_invalid_count = 0
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
elif self.CS.steer_warning:
|
||||
|
|
@ -573,24 +591,8 @@ class CarInterface(object):
|
|||
else:
|
||||
hud_v_cruise = 255
|
||||
|
||||
hud_alert = {
|
||||
"none": AH.NONE,
|
||||
"fcw": AH.FCW,
|
||||
"steerRequired": AH.STEER,
|
||||
"brakePressed": AH.BRAKE_PRESSED,
|
||||
"wrongGear": AH.GEAR_NOT_D,
|
||||
"seatbeltUnbuckled": AH.SEATBELT,
|
||||
"speedTooHigh": AH.SPEED_TOO_HIGH}[str(c.hudControl.visualAlert)]
|
||||
|
||||
snd_beep, snd_chime = {
|
||||
"none": (BP.MUTE, CM.MUTE),
|
||||
"beepSingle": (BP.SINGLE, CM.MUTE),
|
||||
"beepTriple": (BP.TRIPLE, CM.MUTE),
|
||||
"beepRepeated": (BP.REPEATED, CM.MUTE),
|
||||
"chimeSingle": (BP.MUTE, CM.SINGLE),
|
||||
"chimeDouble": (BP.MUTE, CM.DOUBLE),
|
||||
"chimeRepeated": (BP.MUTE, CM.REPEATED),
|
||||
"chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
|
||||
hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw]
|
||||
snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw]
|
||||
|
||||
pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,9 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
# Car button codes
|
||||
class CruiseButtons:
|
||||
RES_ACCEL = 4
|
||||
|
|
@ -15,13 +19,23 @@ class CM:
|
|||
REPEATED = 1
|
||||
CONTINUOUS = 2
|
||||
|
||||
#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
|
||||
#car beeps: enumeration from dbc file. Beeps are for engage and disengage
|
||||
class BP:
|
||||
MUTE = 0
|
||||
SINGLE = 3
|
||||
TRIPLE = 2
|
||||
REPEATED = 1
|
||||
|
||||
AUDIO_HUD = {
|
||||
AudibleAlert.none: (BP.MUTE, CM.MUTE),
|
||||
AudibleAlert.chimeEngage: (BP.SINGLE, CM.MUTE),
|
||||
AudibleAlert.chimeDisengage: (BP.SINGLE, CM.MUTE),
|
||||
AudibleAlert.chimeError: (BP.MUTE, CM.DOUBLE),
|
||||
AudibleAlert.chimePrompt: (BP.MUTE, CM.SINGLE),
|
||||
AudibleAlert.chimeWarning1: (BP.MUTE, CM.DOUBLE),
|
||||
AudibleAlert.chimeWarning2: (BP.MUTE, CM.REPEATED),
|
||||
AudibleAlert.chimeWarningRepeat: (BP.MUTE, CM.REPEATED)}
|
||||
|
||||
class AH:
|
||||
#[alert_idx, value]
|
||||
# See dbc files for info on values"
|
||||
|
|
@ -33,6 +47,15 @@ class AH:
|
|||
SEATBELT = [5, 5]
|
||||
SPEED_TOO_HIGH = [6, 8]
|
||||
|
||||
VISUAL_HUD = {
|
||||
VisualAlert.none: AH.NONE,
|
||||
VisualAlert.fcw: AH.FCW,
|
||||
VisualAlert.steerRequired: AH.STEER,
|
||||
VisualAlert.brakePressed: AH.BRAKE_PRESSED,
|
||||
VisualAlert.wrongGear: AH.GEAR_NOT_D,
|
||||
VisualAlert.seatbeltUnbuckled: AH.SEATBELT,
|
||||
VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH}
|
||||
|
||||
class CAR:
|
||||
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
|
||||
ACCORD_15 = "HONDA ACCORD 2018 LX 1.5T"
|
||||
|
|
@ -146,18 +169,5 @@ SPEED_FACTOR = {
|
|||
CAR.RIDGELINE: 1.,
|
||||
}
|
||||
|
||||
# This message sends car info to the radar that is specific to the model. You
|
||||
# can determine this message by monitoring the OEM system.
|
||||
VEHICLE_STATE_MSG = {
|
||||
CAR.ACURA_ILX: "\x0f\x18\x51\x02\x5a\x00\x00",
|
||||
CAR.ACURA_RDX: "\x0f\x57\x4f\x02\x5a\x00\x00",
|
||||
CAR.CIVIC: "\x02\x38\x44\x32\x4f\x00\x00",
|
||||
CAR.CRV: "\x00\x00\x50\x02\x51\x00\x00",
|
||||
CAR.ODYSSEY: "\x00\x00\x56\x02\x55\x00\x00",
|
||||
CAR.PILOT: "\x00\x00\x56\x02\x58\x00\x00",
|
||||
CAR.PILOT_2019: "\x00\x00\x58\x02\x5c\x00\x00",
|
||||
CAR.RIDGELINE: "\x00\x00\x56\x02\x57\x00\x00",
|
||||
}
|
||||
|
||||
# TODO: get these from dbc file
|
||||
HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_HATCH, CAR.CRV_5G]
|
||||
|
|
|
|||
|
|
@ -1,8 +1,12 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
|
||||
def get_hud_alerts(visual_alert, audble_alert):
|
||||
if visual_alert == "steerRequired":
|
||||
return 4 if audble_alert != "none" else 5
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
def get_hud_alerts(visual_alert, audible_alert):
|
||||
if visual_alert == VisualAlert.steerRequired:
|
||||
return 4 if audible_alert != AudibleAlert.none else 5
|
||||
else:
|
||||
return 0
|
||||
|
||||
|
|
|
|||
|
|
@ -1,12 +1,16 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
|
||||
create_steer_command, create_ui_command, \
|
||||
create_ipas_steer_command, create_accel_command, \
|
||||
create_fcw_command
|
||||
from selfdrive.car.toyota.values import ECU, STATIC_MSGS, NO_DSU_CAR
|
||||
from selfdrive.car.toyota.values import ECU, STATIC_MSGS
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
# Accel limits
|
||||
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
||||
ACCEL_MAX = 1.5 # 1.5 m/s2
|
||||
|
|
@ -54,14 +58,14 @@ def process_hud_alert(hud_alert, audible_alert):
|
|||
sound1 = 0
|
||||
sound2 = 0
|
||||
|
||||
if hud_alert == 'fcw':
|
||||
if hud_alert == VisualAlert.fcw:
|
||||
fcw = 1
|
||||
elif hud_alert == 'steerRequired':
|
||||
elif hud_alert == VisualAlert.steerRequired:
|
||||
steer = 1
|
||||
|
||||
if audible_alert == 'chimeRepeated':
|
||||
if audible_alert == AudibleAlert.chimeWarningRepeat:
|
||||
sound1 = 1
|
||||
elif audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']:
|
||||
elif audible_alert != AudibleAlert.none:
|
||||
# TODO: find a way to send single chimes
|
||||
sound2 = 1
|
||||
|
||||
|
|
@ -108,6 +112,7 @@ class CarController(object):
|
|||
|
||||
self.steer_angle_enabled = False
|
||||
self.ipas_reset_counter = 0
|
||||
self.last_fault_frame = -200
|
||||
|
||||
self.fake_ecus = set()
|
||||
if enable_camera: self.fake_ecus.add(ECU.CAM)
|
||||
|
|
@ -117,7 +122,7 @@ class CarController(object):
|
|||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, sendcan, enabled, CS, frame, actuators,
|
||||
pcm_cancel_cmd, hud_alert, audible_alert):
|
||||
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera):
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
||||
|
|
@ -143,7 +148,11 @@ class CarController(object):
|
|||
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
|
||||
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
|
||||
# only cut torque when steer state is a known fault
|
||||
if not enabled or CS.steer_state in [9, 25]:
|
||||
if CS.steer_state in [9, 25]:
|
||||
self.last_fault_frame = frame
|
||||
|
||||
# Cut steering for 2s after fault
|
||||
if not enabled or (frame - self.last_fault_frame < 200):
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
else:
|
||||
|
|
@ -212,7 +221,7 @@ class CarController(object):
|
|||
else:
|
||||
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False))
|
||||
|
||||
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and self.car_fingerprint not in NO_DSU_CAR:
|
||||
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
|
||||
for addr in TARGET_IDS:
|
||||
can_sends.append(create_video_target(frame/10, addr))
|
||||
|
||||
|
|
@ -231,12 +240,14 @@ class CarController(object):
|
|||
|
||||
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
|
||||
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2))
|
||||
|
||||
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw))
|
||||
|
||||
#*** static msgs ***
|
||||
|
||||
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
|
||||
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
|
||||
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
|
||||
# special cases
|
||||
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
|
||||
cnt = (((frame / 5) % 7) + 1) << 5
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ def get_can_parser(CP):
|
|||
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
|
||||
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
|
||||
("GAS_RELEASED", "PCM_CRUISE", 0),
|
||||
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
|
||||
("CRUISE_STATE", "PCM_CRUISE", 0),
|
||||
("MAIN_ON", "PCM_CRUISE_2", 0),
|
||||
("SET_SPEED", "PCM_CRUISE_2", 0),
|
||||
|
|
@ -65,6 +66,16 @@ def get_can_parser(CP):
|
|||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
def get_cam_can_parser(CP):
|
||||
|
||||
signals = []
|
||||
|
||||
# use steering message to check if panda is connected to frc
|
||||
checks = [("STEERING_LKA", 42)]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
def __init__(self, CP):
|
||||
|
||||
|
|
@ -87,9 +98,10 @@ class CarState(object):
|
|||
K=np.matrix([[0.12287673], [0.29666309]]))
|
||||
self.v_ego = 0.0
|
||||
|
||||
def update(self, cp):
|
||||
def update(self, cp, cp_cam):
|
||||
# copy can_valid
|
||||
self.can_valid = cp.can_valid
|
||||
self.cam_can_valid = cp_cam.can_valid
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
|
|
@ -142,6 +154,7 @@ class CarState(object):
|
|||
self.user_brake = 0
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
|
||||
self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ from cereal import car, log
|
|||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.toyota.carstate import CarState, get_can_parser
|
||||
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
|
|
@ -23,12 +23,16 @@ class CarInterface(object):
|
|||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
self.can_invalid_count = 0
|
||||
self.cam_can_valid_count = 0
|
||||
self.cruise_enabled_prev = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
|
||||
self.cp = get_can_parser(CP)
|
||||
self.cp_cam = get_cam_can_parser(CP)
|
||||
|
||||
self.forwarding_camera = False
|
||||
|
||||
# sending if read only is False
|
||||
if sendcan is not None:
|
||||
|
|
@ -204,7 +208,11 @@ class CarInterface(object):
|
|||
|
||||
self.cp.update(int(sec_since_boot() * 1e9), False)
|
||||
|
||||
self.CS.update(self.cp)
|
||||
# run the cam can update for 10s as we just need to know if the camera is alive
|
||||
if self.frame < 1000:
|
||||
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
|
@ -240,7 +248,7 @@ class CarInterface(object):
|
|||
ret.steeringPressed = self.CS.steer_override
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_active
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
ret.cruiseState.speedOffset = 0.
|
||||
|
|
@ -281,6 +289,12 @@ class CarInterface(object):
|
|||
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
else:
|
||||
self.can_invalid_count = 0
|
||||
|
||||
if self.CS.cam_can_valid:
|
||||
self.cam_can_valid_count += 1
|
||||
if self.cam_can_valid_count >= 5:
|
||||
self.forwarding_camera = True
|
||||
|
||||
if not ret.gearShifter == 'drive' and self.CP.enableDsu:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.doorOpen:
|
||||
|
|
@ -335,7 +349,7 @@ class CarInterface(object):
|
|||
|
||||
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
|
||||
c.hudControl.audibleAlert)
|
||||
c.hudControl.audibleAlert, self.forwarding_camera)
|
||||
|
||||
self.frame += 1
|
||||
return False
|
||||
|
|
|
|||
|
|
@ -65,7 +65,7 @@ def create_steer_command(packer, steer, steer_req, raw_cnt):
|
|||
|
||||
|
||||
def create_accel_command(packer, accel, pcm_cancel, standstill_req):
|
||||
# TODO: find the exact canceling bit
|
||||
# TODO: find the exact canceling bit that does not create a chime
|
||||
values = {
|
||||
"ACCEL_CMD": accel,
|
||||
"SET_ME_X63": 0x63,
|
||||
|
|
|
|||
|
|
@ -24,15 +24,14 @@ static int find_dev() {
|
|||
int fd = openat(dirfd(dir), de->d_name, O_RDONLY);
|
||||
assert(fd >= 0);
|
||||
|
||||
char name[128] = {0};
|
||||
err = ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name);
|
||||
unsigned char ev_bits[KEY_MAX / 8 + 1];
|
||||
err = ioctl(fd, EVIOCGBIT(EV_ABS, sizeof(ev_bits)), ev_bits);
|
||||
assert(err >= 0);
|
||||
|
||||
unsigned long ev_bits[8] = {0};
|
||||
err = ioctl(fd, EVIOCGBIT(0, sizeof(ev_bits)), ev_bits);
|
||||
assert(err >= 0);
|
||||
|
||||
if (strncmp(name, "synaptics", 9) == 0 && ev_bits[0] == 0xb) {
|
||||
const int x_key = ABS_MT_POSITION_X / 8;
|
||||
const int y_key = ABS_MT_POSITION_Y / 8;
|
||||
if ((ev_bits[x_key] & (ABS_MT_POSITION_X - x_key)) &&
|
||||
(ev_bits[y_key] & (ABS_MT_POSITION_Y - y_key))) {
|
||||
ret = fd;
|
||||
break;
|
||||
}
|
||||
|
|
@ -77,12 +76,7 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
|
|||
} else if (event.code == ABS_MT_POSITION_Y) {
|
||||
s->last_y = event.value;
|
||||
}
|
||||
break;
|
||||
case EV_KEY:
|
||||
if (event.code == BTN_TOOL_FINGER && event.value == 0) {
|
||||
// finger up
|
||||
up = true;
|
||||
}
|
||||
up = true;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
#define COMMA_VERSION "0.5.5-release"
|
||||
#define COMMA_VERSION "0.5.6-release"
|
||||
|
|
|
|||
|
|
@ -19,28 +19,6 @@ class Conversions:
|
|||
|
||||
RADAR_TO_CENTER = 2.7 # RADAR is ~ 2.7m ahead from center of car
|
||||
|
||||
# Image params for color cam on acura, calibrated on pre las vegas drive (2016-05-21)
|
||||
class ImageParams:
|
||||
def __init__(self):
|
||||
self.SX_R = 160 # top left corner pixel shift of the visual region considered by the model
|
||||
self.SY_R = 180 # top left corner pixel shift of the visual region considered by the model
|
||||
self.VPX_R = 319 # vanishing point reference, as calibrated in Vegas drive
|
||||
self.VPY_R = 201 # vanishing point reference, as calibrated in Vegas drive
|
||||
self.X = 320 # pixel length of image for model
|
||||
self.Y = 160 # pixel length of image for model
|
||||
self.SX = self.SX_R # current visual region with shift
|
||||
self.SY = self.SY_R # current visual region with shift
|
||||
self.VPX = self.VPX_R # current vanishing point with shift
|
||||
self.VPY = self.VPY_R # current vanishing point with shift
|
||||
def shift(self, shift):
|
||||
def to_int(fl):
|
||||
return int(round(fl))
|
||||
# shift comes from calibration and says how much to shift the viual region
|
||||
self.SX = self.SX_R + to_int(shift[0]) # current visual region with shift
|
||||
self.SY = self.SY_R + to_int(shift[1]) # current visual region with shift
|
||||
self.VPX = self.VPX_R + to_int(shift[0]) # current vanishing point with shift
|
||||
self.VPY = self.VPY_R + to_int(shift[1]) # current vanishing point with shift
|
||||
|
||||
class UIParams:
|
||||
lidar_x, lidar_y, lidar_zoom = 384, 960, 6
|
||||
lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1
|
||||
|
|
|
|||
|
|
@ -1,12 +1,14 @@
|
|||
#!/usr/bin/env python
|
||||
import gc
|
||||
import json
|
||||
import zmq
|
||||
import json
|
||||
|
||||
from cereal import car, log
|
||||
from common.numpy_fast import clip
|
||||
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
|
||||
from common.profiler import Profiler
|
||||
from common.params import Params
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.services import service_list
|
||||
|
|
@ -17,8 +19,7 @@ from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
|
|||
create_event, \
|
||||
EventTypes as ET, \
|
||||
update_v_cruise, \
|
||||
initialize_v_cruise, \
|
||||
kill_defaultd
|
||||
initialize_v_cruise
|
||||
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
|
||||
from selfdrive.controls.lib.latcontrol import LatControl
|
||||
from selfdrive.controls.lib.alertmanager import AlertManager
|
||||
|
|
@ -35,25 +36,27 @@ class Calibration:
|
|||
INVALID = 2
|
||||
|
||||
|
||||
# True when actuators are controlled
|
||||
def isActive(state):
|
||||
"""Check if the actuators are enabled"""
|
||||
return state in [State.enabled, State.softDisabling]
|
||||
|
||||
|
||||
# True if system is engaged
|
||||
def isEnabled(state):
|
||||
"""Check if openpilot is engaged"""
|
||||
return (isActive(state) or state == State.preEnabled)
|
||||
|
||||
|
||||
def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_location,
|
||||
poller, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
driver_status, geofence, state, mismatch_counter, params):
|
||||
"""Receive data from sockets and create events for battery, temperature and disk space"""
|
||||
|
||||
# *** read can and compute car states ***
|
||||
# Update carstate from CAN and create events
|
||||
CS = CI.update(CC)
|
||||
events = list(CS.events)
|
||||
enabled = isEnabled(state)
|
||||
|
||||
# Receive from sockets
|
||||
td = None
|
||||
cal = None
|
||||
hh = None
|
||||
|
|
@ -72,27 +75,20 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
|
|||
elif socket is gps_location:
|
||||
gps = messaging.recv_one(socket)
|
||||
|
||||
# *** thermal checking logic ***
|
||||
# thermal data, checked every second
|
||||
if td is not None:
|
||||
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
|
||||
free_space = td.thermal.freeSpace < 0.15 # under 15% of space free no enable allowed
|
||||
low_battery = td.thermal.batteryPercent < 1 # at zero percent battery, OP should not be allowed
|
||||
|
||||
# under 15% of space free no enable allowed
|
||||
free_space = td.thermal.freeSpace < 0.15
|
||||
|
||||
# at zero percent battery, OP should not be allowed
|
||||
low_battery = td.thermal.batteryPercent < 1
|
||||
|
||||
# Create events for battery, temperature and disk space
|
||||
if low_battery:
|
||||
events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if overtemp:
|
||||
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if free_space:
|
||||
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
|
||||
|
||||
# *** read calibration status ***
|
||||
# Handle calibration
|
||||
if cal is not None:
|
||||
cal_status = cal.liveCalibration.calStatus
|
||||
cal_perc = cal.liveCalibration.calPerc
|
||||
|
|
@ -103,24 +99,27 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
|
|||
else:
|
||||
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
# When the panda and controlsd do not agree on controls_allowed
|
||||
# we want to disengage openpilot. However the status from the panda goes through
|
||||
# another socket than the CAN messages, therefore one can arrive earlier than the other.
|
||||
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
|
||||
if not enabled:
|
||||
mismatch_counter = 0
|
||||
|
||||
# *** health checking logic ***
|
||||
if hh is not None:
|
||||
controls_allowed = hh.health.controlsAllowed
|
||||
if not controls_allowed and enabled:
|
||||
mismatch_counter += 1
|
||||
|
||||
if mismatch_counter >= 2:
|
||||
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
|
||||
|
||||
# Driver monitoring
|
||||
if dm is not None:
|
||||
driver_status.get_pose(dm.driverMonitoring, params)
|
||||
|
||||
# Geofence
|
||||
if geofence is not None and gps is not None:
|
||||
geofence.update_geofence_status(gps.gpsLocationExternal, params)
|
||||
|
||||
if geofence is not None and not geofence.in_geofence:
|
||||
events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING]))
|
||||
|
||||
|
|
@ -128,25 +127,26 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
|
|||
|
||||
|
||||
def calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence):
|
||||
# plan runs always, independently of the state
|
||||
force_decel = driver_status.awareness < 0. or (geofence is not None and not geofence.in_geofence)
|
||||
plan_packet = PL.update(CS, LaC, LoC, v_cruise_kph, force_decel)
|
||||
plan = plan_packet.plan
|
||||
plan_ts = plan_packet.logMonoTime
|
||||
"""Calculate a longitudinal plan using MPC"""
|
||||
|
||||
# add events from planner
|
||||
events += list(plan.events)
|
||||
# Slow down when based on driver monitoring or geofence
|
||||
force_decel = driver_status.awareness < 0. or (geofence is not None and not geofence.in_geofence)
|
||||
|
||||
# disable if lead isn't close when system is active and brake is pressed to avoid
|
||||
# unexpected vehicle accelerations
|
||||
if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
|
||||
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
# Update planner
|
||||
plan_packet = PL.update(CS, LaC, LoC, v_cruise_kph, force_decel)
|
||||
plan = plan_packet.plan
|
||||
plan_ts = plan_packet.logMonoTime
|
||||
events += list(plan.events)
|
||||
|
||||
return plan, plan_ts
|
||||
# Only allow engagement with brake pressed when stopped behind another stopped car
|
||||
if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
|
||||
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
|
||||
return plan, plan_ts
|
||||
|
||||
|
||||
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
|
||||
# compute conditional state transitions and execute actions on state transitions
|
||||
"""Compute conditional state transitions and execute actions on state transitions"""
|
||||
enabled = isEnabled(state)
|
||||
|
||||
v_cruise_kph_last = v_cruise_kph
|
||||
|
|
@ -161,8 +161,6 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
|
|||
# entrance in SOFT_DISABLING state
|
||||
soft_disable_timer = max(0, soft_disable_timer - 1)
|
||||
|
||||
# ***** handle state transitions *****
|
||||
|
||||
# DISABLED
|
||||
if state == State.disabled:
|
||||
if get_events(events, [ET.ENABLE]):
|
||||
|
|
@ -191,7 +189,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
|
|||
|
||||
elif get_events(events, [ET.SOFT_DISABLE]):
|
||||
state = State.softDisabling
|
||||
soft_disable_timer = 300 # 3s TODO: use rate
|
||||
soft_disable_timer = 300 # 3s
|
||||
for e in get_events(events, [ET.SOFT_DISABLE]):
|
||||
AM.add(e, enabled)
|
||||
|
||||
|
|
@ -210,6 +208,10 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
|
|||
# no more soft disabling condition, so go back to ENABLED
|
||||
state = State.enabled
|
||||
|
||||
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
|
||||
for e in get_events(events, [ET.SOFT_DISABLE]):
|
||||
AM.add(e, enabled)
|
||||
|
||||
elif soft_disable_timer <= 0:
|
||||
state = State.disabled
|
||||
|
||||
|
|
@ -232,9 +234,8 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
|
|||
|
||||
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
|
||||
driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc):
|
||||
# Given the state, this function returns the actuators
|
||||
"""Given the state, this function returns an actuators packet"""
|
||||
|
||||
# reset actuators to zero
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
|
||||
enabled = isEnabled(state)
|
||||
|
|
@ -252,17 +253,13 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
|||
if plan.fcw:
|
||||
AM.add("fcw", enabled)
|
||||
|
||||
# ***** state specific actions *****
|
||||
# State specific actions
|
||||
|
||||
# DISABLED
|
||||
if state in [State.preEnabled, State.disabled]:
|
||||
|
||||
LaC.reset()
|
||||
LoC.reset(v_pid=CS.vEgo)
|
||||
|
||||
# ENABLED or SOFT_DISABLING
|
||||
elif state in [State.enabled, State.softDisabling]:
|
||||
|
||||
# parse warnings from car specific interface
|
||||
for e in get_events(events, [ET.WARNING]):
|
||||
extra_text = ""
|
||||
|
|
@ -273,28 +270,25 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
|||
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
|
||||
AM.add(e, enabled, extra_text_2=extra_text)
|
||||
|
||||
# *** angle offset learning ***
|
||||
|
||||
# Run angle offset learner at 20 Hz
|
||||
if rk.frame % 5 == 2 and plan.lateralValid:
|
||||
# *** run this at 20hz again ***
|
||||
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
|
||||
PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle,
|
||||
CS.steeringPressed)
|
||||
|
||||
# *** gas/brake PID loop ***
|
||||
# Gas/Brake PID loop
|
||||
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
|
||||
v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget,
|
||||
CP, PL.lead_1)
|
||||
|
||||
# *** steering PID loop ***
|
||||
# Steering PID loop and lateral MPC
|
||||
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
|
||||
CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
|
||||
CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL)
|
||||
|
||||
# send a "steering required alert" if saturation count has reached the limit
|
||||
# Send a "steering required alert" if saturation count has reached the limit
|
||||
if LaC.sat_flag and CP.steerLimitAlert:
|
||||
AM.add("steerSaturated", enabled)
|
||||
|
||||
# parse permanent warnings to display constantly
|
||||
# Parse permanent warnings to display constantly
|
||||
for e in get_events(events, [ET.PERMANENT]):
|
||||
extra_text_1, extra_text_2 = "", ""
|
||||
if e == "calibrationIncomplete":
|
||||
|
|
@ -302,7 +296,6 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
|||
extra_text_2 = "35 kph" if is_metric else "15 mph"
|
||||
AM.add(str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
|
||||
|
||||
# *** process alerts ***
|
||||
AM.process_alerts(sec_since_boot())
|
||||
|
||||
return actuators, v_cruise_kph, driver_status, angle_offset
|
||||
|
|
@ -311,23 +304,19 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
|||
def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
|
||||
carcontrol, live100, livempc, AM, driver_status,
|
||||
LaC, LoC, angle_offset, passive):
|
||||
|
||||
# ***** control the car *****
|
||||
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
|
||||
|
||||
CC = car.CarControl.new_message()
|
||||
|
||||
if not passive:
|
||||
|
||||
CC.enabled = isEnabled(state)
|
||||
|
||||
CC.actuators = actuators
|
||||
|
||||
CC.cruiseControl.override = True
|
||||
# always cancel if we have an interceptor
|
||||
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
|
||||
|
||||
# brake discount removes a sharp nonlinearity
|
||||
brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
|
||||
# Some override values for Honda
|
||||
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity
|
||||
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
|
||||
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget)
|
||||
|
||||
|
|
@ -341,11 +330,9 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
|
|||
# send car controls over can
|
||||
CI.apply(CC, perception_state)
|
||||
|
||||
# ***** publish state to logger *****
|
||||
# publish controls state at 100Hz
|
||||
# live100
|
||||
dat = messaging.new_message()
|
||||
dat.init('live100')
|
||||
|
||||
dat.live100 = {
|
||||
"alertText1": AM.alert_text_1,
|
||||
"alertText2": AM.alert_text_2,
|
||||
|
|
@ -353,6 +340,7 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
|
|||
"alertStatus": AM.alert_status,
|
||||
"alertBlinkingRate": AM.alert_rate,
|
||||
"alertType": AM.alert_type,
|
||||
"alertSound": "", # no EON sounds yet
|
||||
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
|
||||
"driverMonitoringOn": bool(driver_status.monitor_on),
|
||||
"canMonoTimes": list(CS.canMonoTimes),
|
||||
|
|
@ -381,24 +369,24 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
|
|||
"jerkFactor": float(plan.jerkFactor),
|
||||
"angleOffset": float(angle_offset),
|
||||
"gpsPlannerActive": plan.gpsPlannerActive,
|
||||
"cumLagMs": -rk.remaining*1000.,
|
||||
"cumLagMs": -rk.remaining * 1000.,
|
||||
}
|
||||
live100.send(dat.to_bytes())
|
||||
|
||||
# broadcast carState
|
||||
# carState
|
||||
cs_send = messaging.new_message()
|
||||
cs_send.init('carState')
|
||||
cs_send.carState = CS
|
||||
cs_send.carState.events = events
|
||||
carstate.send(cs_send.to_bytes())
|
||||
|
||||
# broadcast carControl
|
||||
# carControl
|
||||
cc_send = messaging.new_message()
|
||||
cc_send.init('carControl')
|
||||
cc_send.carControl = CC
|
||||
carcontrol.send(cc_send.to_bytes())
|
||||
|
||||
# publish mpc state at 20Hz
|
||||
# send MPC when updated (20 Hz)
|
||||
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
|
||||
dat = messaging.new_message()
|
||||
dat.init('liveMpc')
|
||||
|
|
@ -421,7 +409,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
context = zmq.Context()
|
||||
params = Params()
|
||||
|
||||
# pub
|
||||
# Pub Sockets
|
||||
live100 = messaging.pub_sock(context, service_list['live100'].port)
|
||||
carstate = messaging.pub_sock(context, service_list['carState'].port)
|
||||
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
|
||||
|
|
@ -429,28 +417,23 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
|
||||
is_metric = params.get("IsMetric") == "1"
|
||||
passive = params.get("Passive") != "0"
|
||||
|
||||
# No sendcan if passive
|
||||
if not passive:
|
||||
while 1:
|
||||
try:
|
||||
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
|
||||
break
|
||||
except zmq.error.ZMQError:
|
||||
kill_defaultd()
|
||||
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
|
||||
else:
|
||||
sendcan = None
|
||||
|
||||
# sub
|
||||
# Sub sockets
|
||||
poller = zmq.Poller()
|
||||
thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
|
||||
health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
|
||||
cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller)
|
||||
driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller)
|
||||
gps_location = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller)
|
||||
|
||||
logcan = messaging.sub_sock(context, service_list['can'].port)
|
||||
|
||||
CC = car.CarControl.new_message()
|
||||
|
||||
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
|
||||
|
||||
if CI is None:
|
||||
|
|
@ -464,20 +447,21 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
if passive:
|
||||
CP.safetyModel = car.CarParams.SafetyModels.noOutput
|
||||
|
||||
# Get FCW toggle from settings
|
||||
fcw_enabled = params.get("IsFcwEnabled") == "1"
|
||||
geofence = None
|
||||
|
||||
PL = Planner(CP, fcw_enabled)
|
||||
LoC = LongControl(CP, CI.compute_gb)
|
||||
VM = VehicleModel(CP)
|
||||
LaC = LatControl(VM)
|
||||
LaC = LatControl(CP)
|
||||
AM = AlertManager()
|
||||
driver_status = DriverStatus()
|
||||
|
||||
if not passive:
|
||||
AM.add("startup", False)
|
||||
|
||||
# write CarParams
|
||||
# Write CarParams for radard and boardd safety mode
|
||||
params.put("CarParams", CP.to_bytes())
|
||||
|
||||
state = State.disabled
|
||||
|
|
@ -491,9 +475,9 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
mismatch_counter = 0
|
||||
low_battery = False
|
||||
|
||||
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
|
||||
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
|
||||
|
||||
# learned angle offset
|
||||
# Read angle offset from previous drive, fallback to default
|
||||
angle_offset = default_bias
|
||||
calibration_params = params.get("CalibrationParams")
|
||||
if calibration_params:
|
||||
|
|
@ -505,16 +489,15 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
|
||||
prof = Profiler(False) # off by default
|
||||
|
||||
while 1:
|
||||
|
||||
while True:
|
||||
prof.checkpoint("Ratekeeper", ignore=True)
|
||||
|
||||
# sample data and compute car events
|
||||
# Sample data and compute car events
|
||||
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(CI, CC, thermal, cal, health,
|
||||
driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params)
|
||||
prof.checkpoint("Sample")
|
||||
|
||||
# define plan
|
||||
# Define longitudinal plan (MPC)
|
||||
plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence)
|
||||
prof.checkpoint("Plan")
|
||||
|
||||
|
|
@ -524,24 +507,23 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
|
||||
prof.checkpoint("State transition")
|
||||
|
||||
# compute actuators
|
||||
# Compute actuators (runs PID loops and lateral MPC)
|
||||
actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph,
|
||||
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
|
||||
prof.checkpoint("State Control")
|
||||
|
||||
# publish data
|
||||
# Publish data
|
||||
CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
|
||||
live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive)
|
||||
prof.checkpoint("Sent")
|
||||
|
||||
# *** run loop at fixed rate ***
|
||||
rk.keep_time()
|
||||
|
||||
rk.keep_time() # Run at 100Hz
|
||||
prof.display()
|
||||
|
||||
|
||||
def main(gctx=None):
|
||||
controlsd_thread(gctx, 100)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -1,619 +1,19 @@
|
|||
from cereal import car, log
|
||||
from cereal import log
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.controls.lib.alerts import ALERTS
|
||||
from common.realtime import sec_since_boot
|
||||
import copy
|
||||
|
||||
|
||||
# Priority
|
||||
class Priority:
|
||||
LOWEST = 0
|
||||
LOW_LOWEST = 1
|
||||
LOW = 2
|
||||
MID = 3
|
||||
HIGH = 4
|
||||
HIGHEST = 5
|
||||
|
||||
AlertSize = log.Live100Data.AlertSize
|
||||
AlertStatus = log.Live100Data.AlertStatus
|
||||
|
||||
class Alert(object):
|
||||
def __init__(self,
|
||||
alert_type,
|
||||
alert_text_1,
|
||||
alert_text_2,
|
||||
alert_status,
|
||||
alert_size,
|
||||
alert_priority,
|
||||
visual_alert,
|
||||
audible_alert,
|
||||
duration_sound,
|
||||
duration_hud_alert,
|
||||
duration_text,
|
||||
alert_rate=0.):
|
||||
|
||||
self.alert_type = alert_type
|
||||
self.alert_text_1 = alert_text_1
|
||||
self.alert_text_2 = alert_text_2
|
||||
self.alert_status = alert_status
|
||||
self.alert_size = alert_size
|
||||
self.alert_priority = alert_priority
|
||||
self.visual_alert = visual_alert if visual_alert is not None else "none"
|
||||
self.audible_alert = audible_alert if audible_alert is not None else "none"
|
||||
|
||||
self.duration_sound = duration_sound
|
||||
self.duration_hud_alert = duration_hud_alert
|
||||
self.duration_text = duration_text
|
||||
|
||||
self.start_time = 0.
|
||||
self.alert_rate = alert_rate
|
||||
|
||||
# typecheck that enums are valid on startup
|
||||
tst = car.CarControl.new_message()
|
||||
tst.hudControl.visualAlert = self.visual_alert
|
||||
tst.hudControl.audibleAlert = self.audible_alert
|
||||
|
||||
def __str__(self):
|
||||
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str(
|
||||
self.visual_alert) + " " + str(self.audible_alert)
|
||||
|
||||
def __gt__(self, alert2):
|
||||
return self.alert_priority > alert2.alert_priority
|
||||
|
||||
|
||||
class AlertManager(object):
|
||||
alerts = {
|
||||
alert.alert_type: alert
|
||||
for alert in [
|
||||
# Miscellaneous alerts
|
||||
Alert(
|
||||
"enable",
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, None, "beepSingle", .2, 0., 0.),
|
||||
|
||||
Alert(
|
||||
"disable",
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, None, "beepSingle", .2, 0., 0.),
|
||||
|
||||
Alert(
|
||||
"fcw",
|
||||
"BRAKE!",
|
||||
"Risk of Collision",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "fcw", "chimeRepeated", 1., 2., 2.),
|
||||
|
||||
Alert(
|
||||
"steerSaturated",
|
||||
"TAKE CONTROL",
|
||||
"Turn Exceeds Steering Limit",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
|
||||
|
||||
Alert(
|
||||
"steerTempUnavailable",
|
||||
"TAKE CONTROL",
|
||||
"Steering Temporarily Unavailable",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"steerTempUnavailableMute",
|
||||
"TAKE CONTROL",
|
||||
"Steering Temporarily Unavailable",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, None, None, .2, .2, .2),
|
||||
|
||||
Alert(
|
||||
"preDriverDistracted",
|
||||
"KEEP EYES ON ROAD: User Appears Distracted",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, "steerRequired", None, 0., .1, .1, alert_rate=0.75),
|
||||
|
||||
Alert(
|
||||
"promptDriverDistracted",
|
||||
"KEEP EYES ON ROAD",
|
||||
"User Appears Distracted",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"driverDistracted",
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"User Was Distracted",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"preDriverUnresponsive",
|
||||
"TOUCH STEERING WHEEL: No Driver Monitoring",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, "steerRequired", None, 0., .1, .1, alert_rate=0.75),
|
||||
|
||||
Alert(
|
||||
"promptDriverUnresponsive",
|
||||
"TOUCH STEERING WHEEL",
|
||||
"User Is Unresponsive",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"driverUnresponsive",
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"User Was Unresponsive",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"driverMonitorOff",
|
||||
"DRIVER MONITOR IS UNAVAILABLE",
|
||||
"Accuracy Is Low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, None, .4, 0., 4.),
|
||||
|
||||
Alert(
|
||||
"driverMonitorOn",
|
||||
"DRIVER MONITOR IS AVAILABLE",
|
||||
"Accuracy Is High",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, None, .4, 0., 4.),
|
||||
|
||||
Alert(
|
||||
"geofence",
|
||||
"DISENGAGEMENT REQUIRED",
|
||||
"Not in Geofenced Area",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"startup",
|
||||
"Be ready to take over at any time",
|
||||
"Always keep hands on wheel and eyes on road",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., 15.),
|
||||
|
||||
Alert(
|
||||
"ethicalDilemma",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Ethical Dilemma Detected",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"steerTempUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Steering Temporarily Unavailable",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"manualRestart",
|
||||
"TAKE CONTROL",
|
||||
"Resume Driving Manually",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, None, None, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"resumeRequired",
|
||||
"STOPPED",
|
||||
"Press Resume to Move",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, None, None, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"belowSteerSpeed",
|
||||
"TAKE CONTROL",
|
||||
"Steer Unavailable Below ",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, "steerRequired", None, 0., 0., .1),
|
||||
|
||||
Alert(
|
||||
"debugAlert",
|
||||
"DEBUG ALERT",
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, None, None, .1, .1, .1),
|
||||
|
||||
# Non-entry only alerts
|
||||
Alert(
|
||||
"wrongCarModeNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Main Switch Off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"dataNeededNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Data Needed for Calibration. Upload Drive, Try Again",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"outOfSpaceNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Out of Storage Space",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"pedalPressedNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Pedal Pressed During Attempt",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"speedTooLowNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Speed Too Low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"brakeHoldNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Brake Hold Active",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"parkBrakeNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Park Brake Engaged",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"lowSpeedLockoutNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"lowBatteryNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Low Battery",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# Cancellation alerts causing soft disabling
|
||||
Alert(
|
||||
"overheat",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"System Overheated",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"wrongGear",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gear not D",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"calibrationInvalid",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"calibrationIncomplete",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"doorOpen",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Door Open",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"seatbeltNotLatched",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Seatbelt Unlatched",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"espDisabled",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"ESP Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"lowBattery",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Low Battery",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
# Cancellation alerts causing immediate disabling
|
||||
Alert(
|
||||
"radarCommIssue",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"radarFault",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"modelCommIssue",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Model Error: Check Internet Connection",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"controlsFailed",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Failed",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"controlsMismatch",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Mismatch",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"commIssue",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"CAN Error: Check Connections",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"steerUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"LKAS Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"brakeUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"gasUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gas Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"reverseGear",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Reverse Gear",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"cruiseDisabled",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Is Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"plannerError",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Planner Solution Error",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
# not loud cancellations (user is in control)
|
||||
Alert(
|
||||
"noTarget",
|
||||
"openpilot Canceled",
|
||||
"No close lead car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"speedTooLow",
|
||||
"openpilot Canceled",
|
||||
"Speed too low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# Cancellation alerts causing non-entry
|
||||
Alert(
|
||||
"overheatNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"System overheated",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"wrongGearNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Gear not D",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"calibrationInvalidNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"calibrationIncompleteNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"doorOpenNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Door open",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"seatbeltNotLatchedNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Seatbelt unlatched",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"espDisabledNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"ESP Off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"geofenceNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Not in Geofenced Area",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.MID, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"radarCommIssueNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"radarFaultNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"modelCommIssueNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Model Error: Check Internet Connection",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"controlsFailedNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Controls Failed",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"commIssueNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"CAN Error: Check Connections",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"steerUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"LKAS Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"brakeUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"gasUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Gas Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"reverseGearNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Reverse Gear",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"cruiseDisabledNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Cruise is Off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"noTargetNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"No Close Lead Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"plannerErrorNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Planner Solution Error",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# permanent alerts
|
||||
Alert(
|
||||
"steerUnavailablePermanent",
|
||||
"LKAS Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"brakeUnavailablePermanent",
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"lowSpeedLockoutPermanent",
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"calibrationIncompletePermanent",
|
||||
"Calibration in Progress: ",
|
||||
"Drive Above ",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, None, None, 0., 0., .2),
|
||||
]
|
||||
}
|
||||
|
||||
def __init__(self):
|
||||
self.activealerts = []
|
||||
self.alerts = {alert.alert_type: alert for alert in ALERTS}
|
||||
|
||||
def alertPresent(self):
|
||||
return len(self.activealerts) > 0
|
||||
|
|
@ -626,24 +26,21 @@ class AlertManager(object):
|
|||
added_alert.start_time = sec_since_boot()
|
||||
|
||||
# if new alert is higher priority, log it
|
||||
if not self.alertPresent() or \
|
||||
added_alert.alert_priority > self.activealerts[0].alert_priority:
|
||||
cloudlog.event('alert_add',
|
||||
alert_type=alert_type,
|
||||
enabled=enabled)
|
||||
if not self.alertPresent() or added_alert.alert_priority > self.activealerts[0].alert_priority:
|
||||
cloudlog.event('alert_add', alert_type=alert_type, enabled=enabled)
|
||||
|
||||
self.activealerts.append(added_alert)
|
||||
|
||||
# sort by priority first and then by start_time
|
||||
self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True)
|
||||
|
||||
# TODO: cycle through alerts?
|
||||
def process_alerts(self, cur_time):
|
||||
|
||||
# first get rid of all the expired alerts
|
||||
self.activealerts = [a for a in self.activealerts if a.start_time +
|
||||
max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time]
|
||||
|
||||
ca = self.activealerts[0] if self.alertPresent() else None
|
||||
current_alert = self.activealerts[0] if self.alertPresent() else None
|
||||
|
||||
# start with assuming no alerts
|
||||
self.alert_type = ""
|
||||
|
|
@ -655,17 +52,18 @@ class AlertManager(object):
|
|||
self.audible_alert = "none"
|
||||
self.alert_rate = 0.
|
||||
|
||||
if ca:
|
||||
if ca.start_time + ca.duration_sound > cur_time:
|
||||
self.audible_alert = ca.audible_alert
|
||||
if current_alert:
|
||||
self.alert_type = current_alert.alert_type
|
||||
|
||||
if ca.start_time + ca.duration_hud_alert > cur_time:
|
||||
self.visual_alert = ca.visual_alert
|
||||
if current_alert.start_time + current_alert.duration_sound > cur_time:
|
||||
self.audible_alert = current_alert.audible_alert
|
||||
|
||||
if ca.start_time + ca.duration_text > cur_time:
|
||||
self.alert_type = ca.alert_type
|
||||
self.alert_text_1 = ca.alert_text_1
|
||||
self.alert_text_2 = ca.alert_text_2
|
||||
self.alert_status = ca.alert_status
|
||||
self.alert_size = ca.alert_size
|
||||
self.alert_rate = ca.alert_rate
|
||||
if current_alert.start_time + current_alert.duration_hud_alert > cur_time:
|
||||
self.visual_alert = current_alert.visual_alert
|
||||
|
||||
if current_alert.start_time + current_alert.duration_text > cur_time:
|
||||
self.alert_text_1 = current_alert.alert_text_1
|
||||
self.alert_text_2 = current_alert.alert_text_2
|
||||
self.alert_status = current_alert.alert_status
|
||||
self.alert_size = current_alert.alert_size
|
||||
self.alert_rate = current_alert.alert_rate
|
||||
|
|
|
|||
|
|
@ -0,0 +1,627 @@
|
|||
from cereal import car, log
|
||||
|
||||
# Priority
|
||||
class Priority:
|
||||
LOWEST = 0
|
||||
LOW_LOWEST = 1
|
||||
LOW = 2
|
||||
MID = 3
|
||||
HIGH = 4
|
||||
HIGHEST = 5
|
||||
|
||||
AlertSize = log.Live100Data.AlertSize
|
||||
AlertStatus = log.Live100Data.AlertStatus
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
class Alert(object):
|
||||
def __init__(self,
|
||||
alert_type,
|
||||
alert_text_1,
|
||||
alert_text_2,
|
||||
alert_status,
|
||||
alert_size,
|
||||
alert_priority,
|
||||
visual_alert,
|
||||
audible_alert,
|
||||
duration_sound,
|
||||
duration_hud_alert,
|
||||
duration_text,
|
||||
alert_rate=0.):
|
||||
|
||||
self.alert_type = alert_type
|
||||
self.alert_text_1 = alert_text_1
|
||||
self.alert_text_2 = alert_text_2
|
||||
self.alert_status = alert_status
|
||||
self.alert_size = alert_size
|
||||
self.alert_priority = alert_priority
|
||||
self.visual_alert = visual_alert
|
||||
self.audible_alert = audible_alert
|
||||
|
||||
self.duration_sound = duration_sound
|
||||
self.duration_hud_alert = duration_hud_alert
|
||||
self.duration_text = duration_text
|
||||
|
||||
self.start_time = 0.
|
||||
self.alert_rate = alert_rate
|
||||
|
||||
# typecheck that enums are valid on startup
|
||||
tst = car.CarControl.new_message()
|
||||
tst.hudControl.visualAlert = self.visual_alert
|
||||
|
||||
def __str__(self):
|
||||
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str(
|
||||
self.visual_alert) + " " + str(self.audible_alert)
|
||||
|
||||
def __gt__(self, alert2):
|
||||
return self.alert_priority > alert2.alert_priority
|
||||
|
||||
|
||||
ALERTS = [
|
||||
# Miscellaneous alerts
|
||||
Alert(
|
||||
"enable",
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.chimeEngage, .2, 0., 0.),
|
||||
|
||||
Alert(
|
||||
"disable",
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.chimeDisengage, .2, 0., 0.),
|
||||
|
||||
Alert(
|
||||
"fcw",
|
||||
"BRAKE!",
|
||||
"Risk of Collision",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.chimeWarningRepeat, 1., 2., 2.),
|
||||
|
||||
Alert(
|
||||
"steerSaturated",
|
||||
"TAKE CONTROL",
|
||||
"Turn Exceeds Steering Limit",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.),
|
||||
|
||||
Alert(
|
||||
"steerTempUnavailable",
|
||||
"TAKE CONTROL",
|
||||
"Steering Temporarily Unavailable",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"steerTempUnavailableMute",
|
||||
"TAKE CONTROL",
|
||||
"Steering Temporarily Unavailable",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2, .2, .2),
|
||||
|
||||
Alert(
|
||||
"preDriverDistracted",
|
||||
"KEEP EYES ON ROAD: User Appears Distracted",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
|
||||
|
||||
Alert(
|
||||
"promptDriverDistracted",
|
||||
"KEEP EYES ON ROAD",
|
||||
"User Appears Distracted",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"driverDistracted",
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"User Was Distracted",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"preDriverUnresponsive",
|
||||
"TOUCH STEERING WHEEL: No Driver Monitoring",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
|
||||
|
||||
Alert(
|
||||
"promptDriverUnresponsive",
|
||||
"TOUCH STEERING WHEEL",
|
||||
"User Is Unresponsive",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"driverUnresponsive",
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"User Was Unresponsive",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"driverMonitorOff",
|
||||
"DRIVER MONITOR IS UNAVAILABLE",
|
||||
"Accuracy Is Low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .4, 0., 4.),
|
||||
|
||||
Alert(
|
||||
"driverMonitorOn",
|
||||
"DRIVER MONITOR IS AVAILABLE",
|
||||
"Accuracy Is High",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .4, 0., 4.),
|
||||
|
||||
Alert(
|
||||
"geofence",
|
||||
"DISENGAGEMENT REQUIRED",
|
||||
"Not in Geofenced Area",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
|
||||
|
||||
Alert(
|
||||
"startup",
|
||||
"Be ready to take over at any time",
|
||||
"Always keep hands on wheel and eyes on road",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
|
||||
|
||||
Alert(
|
||||
"ethicalDilemma",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Ethical Dilemma Detected",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 3.),
|
||||
|
||||
Alert(
|
||||
"steerTempUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Steering Temporarily Unavailable",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"manualRestart",
|
||||
"TAKE CONTROL",
|
||||
"Resume Driving Manually",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"resumeRequired",
|
||||
"STOPPED",
|
||||
"Press Resume to Move",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"belowSteerSpeed",
|
||||
"TAKE CONTROL",
|
||||
"Steer Unavailable Below ",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, 0., 0., .1),
|
||||
|
||||
Alert(
|
||||
"debugAlert",
|
||||
"DEBUG ALERT",
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1),
|
||||
|
||||
# Non-entry only alerts
|
||||
Alert(
|
||||
"wrongCarModeNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Main Switch Off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"dataNeededNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Data Needed for Calibration. Upload Drive, Try Again",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"outOfSpaceNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Out of Storage Space",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
|
||||
|
||||
Alert(
|
||||
"pedalPressedNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Pedal Pressed During Attempt",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, "brakePressed", AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"speedTooLowNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Speed Too Low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"brakeHoldNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Brake Hold Active",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"parkBrakeNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Park Brake Engaged",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"lowSpeedLockoutNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"lowBatteryNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Low Battery",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
# Cancellation alerts causing soft disabling
|
||||
Alert(
|
||||
"overheat",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"System Overheated",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
Alert(
|
||||
"wrongGear",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gear not D",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
Alert(
|
||||
"calibrationInvalid",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
Alert(
|
||||
"calibrationIncomplete",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
Alert(
|
||||
"doorOpen",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Door Open",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
Alert(
|
||||
"seatbeltNotLatched",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Seatbelt Unlatched",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
Alert(
|
||||
"espDisabled",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"ESP Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
Alert(
|
||||
"lowBattery",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Low Battery",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
|
||||
|
||||
# Cancellation alerts causing immediate disabling
|
||||
Alert(
|
||||
"radarCommIssue",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"radarFault",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"modelCommIssue",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Model Error: Check Internet Connection",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"controlsFailed",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Failed",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"controlsMismatch",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Mismatch",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"commIssue",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"CAN Error: Check Connections",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"steerUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"LKAS Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"brakeUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"gasUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gas Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"reverseGear",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Reverse Gear",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"cruiseDisabled",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Is Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
Alert(
|
||||
"plannerError",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Planner Solution Error",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
|
||||
# not loud cancellations (user is in control)
|
||||
Alert(
|
||||
"noTarget",
|
||||
"openpilot Canceled",
|
||||
"No close lead car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"speedTooLow",
|
||||
"openpilot Canceled",
|
||||
"Speed too low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"invalidGiraffeHonda",
|
||||
"Invalid Giraffe Configuration",
|
||||
"Set 0111 for openpilot. 1011 for stock",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
# Cancellation alerts causing non-entry
|
||||
Alert(
|
||||
"overheatNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"System overheated",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"wrongGearNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Gear not D",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"calibrationInvalidNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"calibrationIncompleteNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"doorOpenNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Door open",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"seatbeltNotLatchedNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Seatbelt unlatched",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"espDisabledNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"ESP Off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"geofenceNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Not in Geofenced Area",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"radarCommIssueNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"radarFaultNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"modelCommIssueNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Model Error: Check Internet Connection",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"controlsFailedNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Controls Failed",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"commIssueNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"CAN Error: Check Connections",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"steerUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"LKAS Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"brakeUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"gasUnavailableNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Gas Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"reverseGearNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Reverse Gear",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"cruiseDisabledNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Cruise is Off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"noTargetNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"No Close Lead Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"plannerErrorNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Planner Solution Error",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"invalidGiraffeHondaNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Set 0111 for openpilot. 1011 for stock",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
# permanent alerts
|
||||
Alert(
|
||||
"steerUnavailablePermanent",
|
||||
"LKAS Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"brakeUnavailablePermanent",
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"lowSpeedLockoutPermanent",
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"calibrationIncompletePermanent",
|
||||
"Calibration in Progress: ",
|
||||
"Drive Above ",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"invalidGiraffeHondaPermanent",
|
||||
"Invalid Giraffe Configuration",
|
||||
"Set 0111 for openpilot. 1011 for stock",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
]
|
||||
|
|
@ -1,5 +1,3 @@
|
|||
import os
|
||||
import signal
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
|
@ -10,6 +8,7 @@ V_CRUISE_MIN = 8
|
|||
V_CRUISE_DELTA = 8
|
||||
V_CRUISE_ENABLE_MIN = 40
|
||||
|
||||
|
||||
class MPC_COST_LAT:
|
||||
PATH = 1.0
|
||||
LANE = 3.0
|
||||
|
|
@ -60,8 +59,8 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, ang
|
|||
# simple integral controller that learns how much steering offset to put to have the car going straight
|
||||
# while being in the middle of the lane
|
||||
min_offset = -5. # deg
|
||||
max_offset = 5. # deg
|
||||
alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
|
||||
max_offset = 5. # deg
|
||||
alpha = 1. / 36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
|
||||
min_learn_speed = 1.
|
||||
|
||||
# learn less at low speed or when turning
|
||||
|
|
@ -97,12 +96,3 @@ def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last):
|
|||
return v_cruise_last
|
||||
|
||||
return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX)))
|
||||
|
||||
|
||||
def kill_defaultd():
|
||||
# defaultd is used to send can messages when controlsd is off to make car test easier
|
||||
if os.path.isfile("/tmp/defaultd_pid"):
|
||||
with open("/tmp/defaultd_pid") as f:
|
||||
ddpid = int(f.read())
|
||||
print("signalling defaultd with pid %d" % ddpid)
|
||||
os.kill(ddpid, signal.SIGUSR1)
|
||||
|
|
|
|||
|
|
@ -23,12 +23,12 @@ def get_steer_max(CP, v_ego):
|
|||
|
||||
|
||||
class LatControl(object):
|
||||
def __init__(self, VM):
|
||||
self.pid = PIController((VM.CP.steerKpBP, VM.CP.steerKpV),
|
||||
(VM.CP.steerKiBP, VM.CP.steerKiV),
|
||||
k_f=VM.CP.steerKf, pos_limit=1.0)
|
||||
def __init__(self, CP):
|
||||
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
|
||||
(CP.steerKiBP, CP.steerKiV),
|
||||
k_f=CP.steerKf, pos_limit=1.0)
|
||||
self.last_cloudlog_t = 0.0
|
||||
self.setup_mpc(VM.CP.steerRateCost)
|
||||
self.setup_mpc(CP.steerRateCost)
|
||||
|
||||
def setup_mpc(self, steer_rate_cost):
|
||||
self.libmpc = libmpc_py.libmpc
|
||||
|
|
@ -52,7 +52,7 @@ class LatControl(object):
|
|||
def reset(self):
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
|
||||
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL):
|
||||
cur_time = sec_since_boot()
|
||||
self.mpc_updated = False
|
||||
# TODO: this creates issues in replay when rewinding time: mpc won't run
|
||||
|
|
@ -67,7 +67,7 @@ class LatControl(object):
|
|||
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
|
||||
|
||||
# account for actuation delay
|
||||
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.steerRatio, VM.CP.steerActuatorDelay)
|
||||
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
|
||||
|
||||
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
|
||||
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
|
||||
|
|
@ -78,11 +78,11 @@ class LatControl(object):
|
|||
if active:
|
||||
delta_desired = self.mpc_solution[0].delta[1]
|
||||
else:
|
||||
delta_desired = math.radians(angle_steers - angle_offset) / VM.CP.steerRatio
|
||||
delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio
|
||||
|
||||
self.cur_state[0].delta = delta_desired
|
||||
|
||||
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset)
|
||||
self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset)
|
||||
self.angle_steers_des_time = cur_time
|
||||
self.mpc_updated = True
|
||||
|
||||
|
|
@ -90,8 +90,8 @@ class LatControl(object):
|
|||
self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
|
||||
t = sec_since_boot()
|
||||
if self.mpc_nans:
|
||||
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost)
|
||||
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
|
||||
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
|
||||
self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio
|
||||
|
||||
if t > self.last_cloudlog_t + 5.0:
|
||||
self.last_cloudlog_t = t
|
||||
|
|
@ -106,11 +106,11 @@ class LatControl(object):
|
|||
#dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
|
||||
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
|
||||
self.angle_steers_des = self.angle_steers_des_mpc
|
||||
steers_max = get_steer_max(VM.CP, v_ego)
|
||||
steers_max = get_steer_max(CP, v_ego)
|
||||
self.pid.pos_limit = steers_max
|
||||
self.pid.neg_limit = -steers_max
|
||||
steer_feedforward = self.angle_steers_des # feedforward desired angle
|
||||
if VM.CP.steerControlType == car.CarParams.SteerControlType.torque:
|
||||
if CP.steerControlType == car.CarParams.SteerControlType.torque:
|
||||
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
|
||||
deadzone = 0.0
|
||||
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
|
||||
|
|
|
|||
|
|
@ -1,26 +1,28 @@
|
|||
from cereal import log
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.controls.lib.pid import PIController
|
||||
|
||||
LongCtrlState = log.Live100Data.LongControlState
|
||||
|
||||
STOPPING_EGO_SPEED = 0.5
|
||||
MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface
|
||||
MIN_CAN_SPEED = 0.3 # TODO: parametrize this in car interface
|
||||
STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
|
||||
STARTING_TARGET_SPEED = 0.5
|
||||
BRAKE_THRESHOLD_TO_PID = 0.2
|
||||
|
||||
STOPPING_BRAKE_RATE = 0.2 # brake_travel/s while trying to stop
|
||||
STARTING_BRAKE_RATE = 0.8 # brake_travel/s while releasing on restart
|
||||
BRAKE_STOPPING_TARGET = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
|
||||
|
||||
class LongCtrlState:
|
||||
#*** this function handles the long control state transitions
|
||||
# long_control_state labels (see capnp enum):
|
||||
off = 'off' # Off
|
||||
pid = 'pid' # moving and tracking targets, with PID control running
|
||||
stopping = 'stopping' # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
|
||||
starting = 'starting' # starting and releasing brakes in open loop before giving back to PID
|
||||
_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints
|
||||
_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
|
||||
|
||||
RATE = 100.0
|
||||
|
||||
|
||||
def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
|
||||
output_gb, brake_pressed, cruise_standstill):
|
||||
|
||||
"""Update longitudinal control state machine"""
|
||||
stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
|
||||
(v_ego < STOPPING_EGO_SPEED and \
|
||||
((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or
|
||||
|
|
@ -53,74 +55,71 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
|
|||
return long_control_state
|
||||
|
||||
|
||||
stopping_brake_rate = 0.2 # brake_travel/s while trying to stop
|
||||
starting_brake_rate = 0.8 # brake_travel/s while releasing on restart
|
||||
brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
|
||||
|
||||
_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints
|
||||
_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
|
||||
|
||||
|
||||
class LongControl(object):
|
||||
def __init__(self, CP, compute_gb):
|
||||
self.long_control_state = LongCtrlState.off # initialized to off
|
||||
self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV),
|
||||
(CP.longitudinalKiBP, CP.longitudinalKiV),
|
||||
rate=100.0,
|
||||
rate=RATE,
|
||||
sat_limit=0.8,
|
||||
convert=compute_gb)
|
||||
self.v_pid = 0.0
|
||||
self.last_output_gb = 0.0
|
||||
|
||||
def reset(self, v_pid):
|
||||
"""Reset PID controller and change setpoint"""
|
||||
self.pid.reset()
|
||||
self.v_pid = v_pid
|
||||
|
||||
def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1):
|
||||
# actuation limits
|
||||
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
|
||||
# Actuation limits
|
||||
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
|
||||
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
|
||||
|
||||
# Update state machine
|
||||
output_gb = self.last_output_gb
|
||||
rate = 100.0
|
||||
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,
|
||||
v_target_future, self.v_pid, output_gb,
|
||||
brake_pressed, cruise_standstill)
|
||||
|
||||
v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
|
||||
|
||||
# *** long control behavior based on state
|
||||
if self.long_control_state == LongCtrlState.off:
|
||||
self.v_pid = v_ego_pid # do nothing
|
||||
output_gb = 0.
|
||||
self.v_pid = v_ego_pid
|
||||
self.pid.reset()
|
||||
output_gb = 0.
|
||||
|
||||
# tracking objects and driving
|
||||
elif self.long_control_state == LongCtrlState.pid:
|
||||
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
|
||||
|
||||
self.v_pid = v_target
|
||||
self.pid.pos_limit = gas_max
|
||||
self.pid.neg_limit = - brake_max
|
||||
|
||||
# Toyota starts braking more when it thinks you want to stop
|
||||
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
|
||||
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
|
||||
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)
|
||||
|
||||
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
|
||||
|
||||
if prevent_overshoot:
|
||||
output_gb = min(output_gb, 0.0)
|
||||
|
||||
# intention is to stop, switch to a different brake control until we stop
|
||||
# Intention is to stop, switch to a different brake control until we stop
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
# TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego
|
||||
if not standstill or output_gb > -brake_stopping_target:
|
||||
output_gb -= stopping_brake_rate / rate
|
||||
# Keep applying brakes until the car is stopped
|
||||
if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
|
||||
output_gb -= STOPPING_BRAKE_RATE / RATE
|
||||
output_gb = clip(output_gb, -brake_max, gas_max)
|
||||
|
||||
self.v_pid = v_ego
|
||||
self.pid.reset()
|
||||
|
||||
# intention is to move again, release brake fast before handling control to PID
|
||||
# Intention is to move again, release brake fast before handing control to PID
|
||||
elif self.long_control_state == LongCtrlState.starting:
|
||||
if output_gb < -0.2:
|
||||
output_gb += starting_brake_rate / rate
|
||||
output_gb += STARTING_BRAKE_RATE / RATE
|
||||
self.v_pid = v_ego
|
||||
self.pid.reset()
|
||||
|
||||
|
|
|
|||
|
|
@ -2,15 +2,41 @@
|
|||
import numpy as np
|
||||
from numpy.linalg import solve
|
||||
|
||||
# dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"##
|
||||
# Xdot = A*X + B*U
|
||||
# where X = [v, r], with v and r lateral speed and rotational speed, respectively
|
||||
# and U is the steering angle (controller input)
|
||||
#
|
||||
# A depends on longitudinal speed, u, and vehicle parameters CP
|
||||
"""
|
||||
Dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"
|
||||
|
||||
The state is x = [v, r]^T
|
||||
with v lateral speed [m/s], and r rotational speed [rad/s]
|
||||
|
||||
The input u is the steering angle [rad]
|
||||
|
||||
The system is defined by
|
||||
x_dot = A*x + B*u
|
||||
|
||||
A depends on longitudinal speed, u [m/s], and vehicle parameters CP
|
||||
"""
|
||||
|
||||
|
||||
def create_dyn_state_matrices(u, VM):
|
||||
"""Returns the A and B matrix for the dynamics system
|
||||
|
||||
Args:
|
||||
u: Vehicle speed [m/s]
|
||||
VM: Vehicle model
|
||||
|
||||
Returns:
|
||||
A tuple with the 2x2 A matrix, and 2x1 B matrix
|
||||
|
||||
Parameters in the vehicle model:
|
||||
cF: Tire stiffnes Front [N/rad]
|
||||
cR: Tire stiffnes Front [N/rad]
|
||||
aF: Distance from CG to front wheels [m]
|
||||
aR: Distance from CG to rear wheels [m]
|
||||
m: Mass [kg]
|
||||
j: Rotational inertia [kg m^2]
|
||||
sR: Steering ratio [-]
|
||||
chi: Steer ratio rear [-]
|
||||
"""
|
||||
A = np.zeros((2, 2))
|
||||
B = np.zeros((2, 1))
|
||||
A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
|
||||
|
|
@ -23,7 +49,18 @@ def create_dyn_state_matrices(u, VM):
|
|||
|
||||
|
||||
def kin_ss_sol(sa, u, VM):
|
||||
# kinematic solution, useful when speed ~ 0
|
||||
"""Calculate the steady state solution at low speeds
|
||||
At low speeds the tire slip is undefined, so a kinematic
|
||||
model is used.
|
||||
|
||||
Args:
|
||||
sa: Steering angle [rad]
|
||||
u: Speed [m/s]
|
||||
VM: Vehicle model
|
||||
|
||||
Returns:
|
||||
2x1 matrix with steady state solution
|
||||
"""
|
||||
K = np.zeros((2, 1))
|
||||
K[0, 0] = VM.aR / VM.sR / VM.l * u
|
||||
K[1, 0] = 1. / VM.sR / VM.l * u
|
||||
|
|
@ -31,25 +68,34 @@ def kin_ss_sol(sa, u, VM):
|
|||
|
||||
|
||||
def dyn_ss_sol(sa, u, VM):
|
||||
# Dynamic solution, useful when speed > 0
|
||||
"""Calculate the steady state solution when x_dot = 0,
|
||||
Ax + Bu = 0 => x = A^{-1} B u
|
||||
|
||||
Args:
|
||||
sa: Steering angle [rad]
|
||||
u: Speed [m/s]
|
||||
VM: Vehicle model
|
||||
|
||||
Returns:
|
||||
2x1 matrix with steady state solution
|
||||
"""
|
||||
A, B = create_dyn_state_matrices(u, VM)
|
||||
return - solve(A, B) * sa
|
||||
return -solve(A, B) * sa
|
||||
|
||||
|
||||
def calc_slip_factor(VM):
|
||||
# the slip factor is a measure of how the curvature changes with speed
|
||||
# it's positive for Oversteering vehicle, negative (usual case) otherwise
|
||||
"""The slip factor is a measure of how the curvature changes with speed
|
||||
it's positive for Oversteering vehicle, negative (usual case) otherwise.
|
||||
"""
|
||||
return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)
|
||||
|
||||
|
||||
class VehicleModel(object):
|
||||
def __init__(self, CP, init_state=np.asarray([[0.], [0.]])):
|
||||
self.dt = 0.1
|
||||
lookahead = 2. # s
|
||||
self.steps = int(lookahead / self.dt)
|
||||
self.update_state(init_state)
|
||||
self.state_pred = np.zeros((self.steps, self.state.shape[0]))
|
||||
self.CP = CP
|
||||
def __init__(self, CP):
|
||||
"""
|
||||
Args:
|
||||
CP: Car Parameters
|
||||
"""
|
||||
# for math readability, convert long names car params into short names
|
||||
self.m = CP.mass
|
||||
self.j = CP.rotationalInertia
|
||||
|
|
@ -61,45 +107,80 @@ class VehicleModel(object):
|
|||
self.sR = CP.steerRatio
|
||||
self.chi = CP.steerRatioRear
|
||||
|
||||
def update_state(self, state):
|
||||
self.state = state
|
||||
|
||||
def steady_state_sol(self, sa, u):
|
||||
# if the speed is too small we can't use the dynamic model
|
||||
# (tire slip is undefined), we then use the kinematic model
|
||||
"""Returns the steady state solution.
|
||||
|
||||
If the speed is too small we can't use the dynamic model (tire slip is undefined),
|
||||
we then have to use the kinematic model
|
||||
|
||||
Args:
|
||||
sa: Steering wheel angle [rad]
|
||||
u: Speed [m/s]
|
||||
|
||||
Returns:
|
||||
2x1 matrix with steady state solution (lateral speed, rotational speed)
|
||||
"""
|
||||
if u > 0.1:
|
||||
return dyn_ss_sol(sa, u, self)
|
||||
else:
|
||||
return kin_ss_sol(sa, u, self)
|
||||
|
||||
def calc_curvature(self, sa, u):
|
||||
# this formula can be derived from state equations in steady state conditions
|
||||
"""Returns the curvature. Multiplied by the speed this will give the yaw rate.
|
||||
|
||||
Args:
|
||||
sa: Steering wheel angle [rad]
|
||||
u: Speed [m/s]
|
||||
|
||||
Returns:
|
||||
Curvature factor [rad/m]
|
||||
"""
|
||||
return self.curvature_factor(u) * sa / self.sR
|
||||
|
||||
def curvature_factor(self, u):
|
||||
"""Returns the curvature factor.
|
||||
Multiplied by wheel angle (not steering wheel angle) this will give the curvature.
|
||||
|
||||
Args:
|
||||
u: Speed [m/s]
|
||||
|
||||
Returns:
|
||||
Curvature factor [1/m]
|
||||
"""
|
||||
sf = calc_slip_factor(self)
|
||||
return (1. - self.chi)/(1. - sf * u**2) / self.l
|
||||
return (1. - self.chi) / (1. - sf * u**2) / self.l
|
||||
|
||||
def get_steer_from_curvature(self, curv, u):
|
||||
"""Calculates the required steering wheel angle for a given curvature
|
||||
|
||||
Args:
|
||||
curv: Desired curvature [rad/s]
|
||||
u: Speed [m/s]
|
||||
|
||||
Returns:
|
||||
Steering wheel angle [rad]
|
||||
"""
|
||||
|
||||
return curv * self.sR * 1.0 / self.curvature_factor(u)
|
||||
|
||||
def state_prediction(self, sa, u):
|
||||
# U is the matrix of the controls
|
||||
# u is the long speed
|
||||
A, B = create_dyn_state_matrices(u, self)
|
||||
return np.matmul((A * self.dt + np.eye(2)), self.state) + B * sa * self.dt
|
||||
|
||||
def yaw_rate(self, sa, u):
|
||||
"""Calculate yaw rate
|
||||
|
||||
Args:
|
||||
sa: Steering wheel angle [rad]
|
||||
u: Speed [m/s]
|
||||
|
||||
Returns:
|
||||
Yaw rate [rad/s]
|
||||
"""
|
||||
return self.calc_curvature(sa, u) * u
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import math
|
||||
from selfdrive.car.honda.interface import CarInterface
|
||||
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
|
||||
#from selfdrive.car.hyundai.interface import CarInterface
|
||||
#CP = CarInterface.get_params("HYUNDAI SANTA FE UNLIMITED 2019", {})
|
||||
#print CP
|
||||
from selfdrive.car.honda.values import CAR
|
||||
|
||||
CP = CarInterface.get_params(CAR.CIVIC, {})
|
||||
VM = VehicleModel(CP)
|
||||
#print VM.steady_state_sol(.1, 0.15)
|
||||
#print calc_slip_factor(VM)
|
||||
print VM.yaw_rate(1.*np.pi/180, 32.) * 180./np.pi
|
||||
#print VM.curvature_factor(32)
|
||||
print(VM.yaw_rate(math.radians(20), 10.))
|
||||
|
|
|
|||
|
|
@ -141,6 +141,14 @@ def radard_thread(gctx=None):
|
|||
reading = speedSensorV.read(PP.lead_dist, covar=np.matrix(PP.lead_var))
|
||||
ekfv.update_scalar(reading)
|
||||
ekfv.predict(tsv)
|
||||
|
||||
# When changing lanes the distance to the lead car can suddenly change,
|
||||
# which makes the Kalman filter output large relative acceleration
|
||||
if mocked and abs(PP.lead_dist - ekfv.state[XV]) > 2.0:
|
||||
ekfv.state[XV] = PP.lead_dist
|
||||
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
|
||||
ekfv.state[SPEEDV] = 0.
|
||||
|
||||
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
|
||||
float(ekfv.state[SPEEDV]), False)
|
||||
else:
|
||||
|
|
@ -279,8 +287,8 @@ def radard_thread(gctx=None):
|
|||
"yRel": float(tracks[ids].yRel),
|
||||
"vRel": float(tracks[ids].vRel),
|
||||
"aRel": float(tracks[ids].aRel),
|
||||
"stationary": tracks[ids].stationary,
|
||||
"oncoming": tracks[ids].oncoming,
|
||||
"stationary": bool(tracks[ids].stationary),
|
||||
"oncoming": bool(tracks[ids].oncoming),
|
||||
}
|
||||
liveTracks.send(dat.to_bytes())
|
||||
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -95,7 +95,7 @@ managed_processes = {
|
|||
"proclogd": ("selfdrive/proclogd", ["./proclogd"]),
|
||||
"boardd": ("selfdrive/boardd", ["./boardd"]), # not used directly
|
||||
"pandad": "selfdrive.pandad",
|
||||
"ui": ("selfdrive/ui", ["./ui"]),
|
||||
"ui": ("selfdrive/ui", ["./start.sh"]),
|
||||
"calibrationd": "selfdrive.locationd.calibrationd",
|
||||
"visiond": ("selfdrive/visiond", ["./visiond"]),
|
||||
"sensord": ("selfdrive/sensord", ["./sensord"]),
|
||||
|
|
@ -448,8 +448,6 @@ def main():
|
|||
if os.getenv("NOCONTROL") is not None:
|
||||
del managed_processes['controlsd']
|
||||
del managed_processes['radard']
|
||||
if os.getenv("DEFAULTD") is not None:
|
||||
managed_processes["controlsd"] = "selfdrive.controls.defaultd"
|
||||
|
||||
# support additional internal only extensions
|
||||
try:
|
||||
|
|
@ -521,4 +519,3 @@ if __name__ == "__main__":
|
|||
main()
|
||||
# manual exit because we are forked
|
||||
sys.exit(0)
|
||||
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -71,6 +71,7 @@ frontEncodeIdx: [8061, true]
|
|||
orbFeaturesSummary: [8062, true]
|
||||
driverMonitoring: [8063, true]
|
||||
liveParameters: [8064, true]
|
||||
liveMapData: [8065, true]
|
||||
|
||||
testModel: [8040, false]
|
||||
testLiveLocation: [8045, false]
|
||||
|
|
|
|||
|
|
@ -242,7 +242,7 @@ class Plant(object):
|
|||
'INTERCEPTOR_GAS',
|
||||
])
|
||||
vls = vls_tuple(
|
||||
self.speed_sensor(speed),
|
||||
self.speed_sensor(speed),
|
||||
self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
|
||||
self.angle_steer, self.angle_steer_rate, 0, #Steer torque sensor
|
||||
0, 0, # Blinkers
|
||||
|
|
@ -302,6 +302,14 @@ class Plant(object):
|
|||
"0f00000"
|
||||
can_msgs.append([0x400, 0, radar_state_msg, 1])
|
||||
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
|
||||
|
||||
# add camera msg so controlsd thinks it's alive
|
||||
msg_struct["COUNTER"] = self.rk.frame % 4
|
||||
msg = honda.lookup_msg_id(0xe4)
|
||||
msg_data = honda.encode(msg, msg_struct)
|
||||
msg_data = fix(msg_data, 0xe4)
|
||||
can_msgs.append([0xe4, 0, msg_data, 2])
|
||||
|
||||
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
|
||||
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
|
|
|
|||
|
|
@ -30,12 +30,15 @@ OPENCL_LIBS = -lgsl -lCB -lOpenCL
|
|||
|
||||
OPENGL_LIBS = -lGLESv3
|
||||
|
||||
OPENSL_LIBS = -lOpenSLES
|
||||
|
||||
FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
|
||||
|
||||
CFLAGS += -DQCOM
|
||||
CXXFLAGS += -DQCOM
|
||||
|
||||
OBJS = ui.o \
|
||||
OBJS = slplay.o \
|
||||
ui.o \
|
||||
../common/glutil.o \
|
||||
../common/visionipc.o \
|
||||
../common/ipc.o \
|
||||
|
|
@ -64,8 +67,17 @@ ui: $(OBJS)
|
|||
-lhardware -lui \
|
||||
$(OPENGL_LIBS) \
|
||||
$(OPENCL_LIBS) \
|
||||
${OPENSL_LIBS} \
|
||||
-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib \
|
||||
-lcutils -lm -llog -lui -ladreno_utils
|
||||
|
||||
slplay.o: slplay.c
|
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -fPIC \
|
||||
-I../ \
|
||||
$(OPENSL_LIBS) \
|
||||
-c -o '$@' $^
|
||||
|
||||
%.o: %.cc
|
||||
@echo "[ CXX ] $@"
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
|
|
|
|||
|
|
@ -0,0 +1,184 @@
|
|||
#include <stdio.h>
|
||||
#include <assert.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <getopt.h>
|
||||
|
||||
#include "common/timing.h"
|
||||
#include "slplay.h"
|
||||
|
||||
SLEngineItf engineInterface = NULL;
|
||||
SLObjectItf outputMix = NULL;
|
||||
SLObjectItf engine = NULL;
|
||||
uri_player players[32] = {{NULL, NULL, NULL}};
|
||||
|
||||
uint64_t loop_start = 0;
|
||||
uint64_t loop_start_ctx = 0;
|
||||
|
||||
uri_player* get_player_by_uri(const char* uri) {
|
||||
for (uri_player *s = players; s->uri != NULL; s++) {
|
||||
if (strcmp(s->uri, uri) == 0) {
|
||||
return s;
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
uri_player* slplay_create_player_for_uri(const char* uri, char **error) {
|
||||
uri_player player = { uri, NULL, NULL };
|
||||
|
||||
SLresult result;
|
||||
SLDataLocator_URI locUri = {SL_DATALOCATOR_URI, (SLchar *) uri};
|
||||
SLDataFormat_MIME formatMime = {SL_DATAFORMAT_MIME, NULL, SL_CONTAINERTYPE_UNSPECIFIED};
|
||||
SLDataSource audioSrc = {&locUri, &formatMime};
|
||||
|
||||
SLDataLocator_OutputMix outMix = {SL_DATALOCATOR_OUTPUTMIX, outputMix};
|
||||
SLDataSink audioSnk = {&outMix, NULL};
|
||||
|
||||
result = (*engineInterface)->CreateAudioPlayer(engineInterface, &player.player, &audioSrc, &audioSnk, 0, NULL, NULL);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to create audio player");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
result = (*(player.player))->Realize(player.player, SL_BOOLEAN_FALSE);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to realize audio player");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
result = (*(player.player))->GetInterface(player.player, SL_IID_PLAY, &(player.playInterface));
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to get player interface");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
result = (*(player.playInterface))->SetPlayState(player.playInterface, SL_PLAYSTATE_PAUSED);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to initialize playstate to SL_PLAYSTATE_PAUSED");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
uri_player *p = players;
|
||||
while (p->uri != NULL) {
|
||||
p++;
|
||||
}
|
||||
*p = player;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
void slplay_setup(char **error) {
|
||||
SLresult result;
|
||||
SLEngineOption engineOptions[] = {{SL_ENGINEOPTION_THREADSAFE, SL_BOOLEAN_TRUE}};
|
||||
result = slCreateEngine(&engine, 1, engineOptions, 0, NULL, NULL);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to create OpenSL engine");
|
||||
}
|
||||
|
||||
result = (*engine)->Realize(engine, SL_BOOLEAN_FALSE);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to realize OpenSL engine");
|
||||
}
|
||||
|
||||
result = (*engine)->GetInterface(engine, SL_IID_ENGINE, &engineInterface);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to realize OpenSL engine");
|
||||
}
|
||||
|
||||
const SLInterfaceID ids[1] = {SL_IID_VOLUME};
|
||||
const SLboolean req[1] = {SL_BOOLEAN_FALSE};
|
||||
result = (*engineInterface)->CreateOutputMix(engineInterface, &outputMix, 1, ids, req);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to create output mix");
|
||||
}
|
||||
|
||||
result = (*outputMix)->Realize(outputMix, SL_BOOLEAN_FALSE);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to realize output mix");
|
||||
}
|
||||
}
|
||||
|
||||
void slplay_destroy() {
|
||||
for (uri_player *player = players; player->uri != NULL; player++) {
|
||||
if (player->player) {
|
||||
(*(player->player))->Destroy(player->player);
|
||||
}
|
||||
}
|
||||
|
||||
(*outputMix)->Destroy(outputMix);
|
||||
(*engine)->Destroy(engine);
|
||||
}
|
||||
|
||||
void slplay_stop (uri_player* player, char **error) {
|
||||
SLPlayItf playInterface = player->playInterface;
|
||||
SLresult result;
|
||||
|
||||
// stop a loop
|
||||
loop_start = 0;
|
||||
|
||||
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to set SL_PLAYSTATE_STOPPED");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void slplay_stop_uri(const char* uri, char **error) {
|
||||
uri_player* player = get_player_by_uri(uri);
|
||||
slplay_stop(player, error);
|
||||
}
|
||||
|
||||
void SLAPIENTRY slplay_callback(SLPlayItf playItf, void *context, SLuint32 event) {
|
||||
uint64_t cb_loop_start = *((uint64_t*)context);
|
||||
if (event == SL_PLAYEVENT_HEADATEND && cb_loop_start == loop_start) {
|
||||
(*playItf)->SetPlayState(playItf, SL_PLAYSTATE_STOPPED);
|
||||
(*playItf)->SetMarkerPosition(playItf, 0);
|
||||
(*playItf)->SetPlayState(playItf, SL_PLAYSTATE_PLAYING);
|
||||
}
|
||||
}
|
||||
|
||||
void slplay_play (const char *uri, bool loop, char **error) {
|
||||
SLresult result;
|
||||
|
||||
uri_player* player = get_player_by_uri(uri);
|
||||
if (player == NULL) {
|
||||
player = slplay_create_player_for_uri(uri, error);
|
||||
if (*error) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
SLPlayItf playInterface = player->playInterface;
|
||||
if (loop) {
|
||||
loop_start = nanos_since_boot();
|
||||
loop_start_ctx = loop_start;
|
||||
result = (*playInterface)->RegisterCallback(playInterface, slplay_callback, &loop_start_ctx);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
char error[64];
|
||||
snprintf(error, sizeof(error), "Failed to register callback. %d", result);
|
||||
*error = error[0];
|
||||
return;
|
||||
}
|
||||
|
||||
result = (*playInterface)->SetCallbackEventsMask(playInterface, SL_PLAYEVENT_HEADATEND);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to set callback event mask");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset the audio player
|
||||
result = (*playInterface)->ClearMarkerPosition(playInterface);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to clear marker position");
|
||||
return;
|
||||
}
|
||||
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED);
|
||||
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_STOPPED);
|
||||
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PLAYING);
|
||||
if (result != SL_RESULT_SUCCESS) {
|
||||
*error = strdup("Failed to set SL_PLAYSTATE_PLAYING");
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
#ifndef SLPLAY_H
|
||||
#define SLPLAY_H
|
||||
|
||||
#include <SLES/OpenSLES.h>
|
||||
#include <SLES/OpenSLES_Android.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef struct {
|
||||
const char* uri;
|
||||
SLObjectItf player;
|
||||
SLPlayItf playInterface;
|
||||
} uri_player;
|
||||
|
||||
void slplay_setup(char **error);
|
||||
uri_player* slplay_create_player_for_uri(const char* uri, char **error);
|
||||
void slplay_play (const char *uri, bool loop, char **error);
|
||||
void slplay_stop_uri (const char* uri, char **error);
|
||||
void slplay_destroy();
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
#!/bin/sh
|
||||
set -e
|
||||
|
||||
make
|
||||
export LD_LIBRARY_PATH=/system/lib64:$LD_LIBRARY_PATH
|
||||
exec ./ui
|
||||
|
|
@ -33,6 +33,7 @@
|
|||
#include "common/params.h"
|
||||
|
||||
#include "cereal/gen/c/log.capnp.h"
|
||||
#include "slplay.h"
|
||||
|
||||
#define STATUS_STOPPED 0
|
||||
#define STATUS_DISENGAGED 1
|
||||
|
|
@ -112,6 +113,10 @@ typedef struct UIScene {
|
|||
float v_cruise;
|
||||
uint64_t v_cruise_update_ts;
|
||||
float v_ego;
|
||||
|
||||
float speedlimit;
|
||||
bool speedlimit_valid;
|
||||
|
||||
float curvature;
|
||||
int engaged;
|
||||
bool engageable;
|
||||
|
|
@ -142,6 +147,7 @@ typedef struct UIScene {
|
|||
// Used to show gps planner status
|
||||
bool gps_planner_active;
|
||||
|
||||
bool is_playing_alert;
|
||||
} UIScene;
|
||||
|
||||
typedef struct UIState {
|
||||
|
|
@ -176,6 +182,8 @@ typedef struct UIState {
|
|||
void *livempc_sock_raw;
|
||||
zsock_t *plus_sock;
|
||||
void *plus_sock_raw;
|
||||
zsock_t *map_data_sock;
|
||||
void *map_data_sock_raw;
|
||||
|
||||
zsock_t *uilayout_sock;
|
||||
void *uilayout_sock_raw;
|
||||
|
|
@ -215,9 +223,13 @@ typedef struct UIState {
|
|||
bool awake;
|
||||
int awake_timeout;
|
||||
|
||||
int volume_timeout;
|
||||
|
||||
int status;
|
||||
bool is_metric;
|
||||
bool passive;
|
||||
char alert_type[64];
|
||||
char alert_sound[64];
|
||||
int alert_size;
|
||||
float alert_blinking_alpha;
|
||||
bool alert_blinked;
|
||||
|
|
@ -256,6 +268,15 @@ static void set_awake(UIState *s, bool awake) {
|
|||
}
|
||||
}
|
||||
|
||||
static void set_volume(UIState *s, int volume) {
|
||||
char volume_change_cmd[64];
|
||||
sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1", volume);
|
||||
|
||||
// 5 second timeout at 60fps
|
||||
s->volume_timeout = 5 * 60;
|
||||
int volume_changed = system(volume_change_cmd);
|
||||
}
|
||||
|
||||
volatile int do_exit = 0;
|
||||
static void set_do_exit(int sig) {
|
||||
do_exit = 1;
|
||||
|
|
@ -322,6 +343,43 @@ static const mat4 full_to_wide_frame_transform = {{
|
|||
0.0, 0.0, 0.0, 1.0,
|
||||
}};
|
||||
|
||||
typedef struct {
|
||||
const char* name;
|
||||
const char* uri;
|
||||
bool loop;
|
||||
} sound_file;
|
||||
|
||||
sound_file sound_table[] = {
|
||||
{ "chimeDisengage", "../assets/sounds/disengaged.wav", false },
|
||||
{ "chimeEngage", "../assets/sounds/engaged.wav", false },
|
||||
{ "chimeWarning1", "../assets/sounds/warning_1.wav", false },
|
||||
{ "chimeWarning2", "../assets/sounds/warning_2.wav", false },
|
||||
{ "chimeWarningRepeat", "../assets/sounds/warning_2.wav", true },
|
||||
{ "chimeError", "../assets/sounds/error.wav", false },
|
||||
{ "chimePrompt", "../assets/sounds/error.wav", false },
|
||||
{ NULL, NULL, false },
|
||||
};
|
||||
|
||||
sound_file* get_sound_file_by_name(const char* name) {
|
||||
for (sound_file *s = sound_table; s->name != NULL; s++) {
|
||||
if (strcmp(s->name, name) == 0) {
|
||||
return s;
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void ui_sound_init(char **error) {
|
||||
slplay_setup(error);
|
||||
if (*error) return;
|
||||
|
||||
for (sound_file *s = sound_table; s->name != NULL; s++) {
|
||||
slplay_create_player_for_uri(s->uri, error);
|
||||
if (*error) return;
|
||||
}
|
||||
}
|
||||
|
||||
static void ui_init(UIState *s) {
|
||||
memset(s, 0, sizeof(UIState));
|
||||
|
||||
|
|
@ -362,6 +420,10 @@ static void ui_init(UIState *s) {
|
|||
assert(s->plus_sock);
|
||||
s->plus_sock_raw = zsock_resolve(s->plus_sock);
|
||||
|
||||
s->map_data_sock = zsock_new_sub(">tcp://127.0.0.1:8065", "");
|
||||
assert(s->map_data_sock);
|
||||
s->map_data_sock_raw = zsock_resolve(s->map_data_sock);
|
||||
|
||||
s->ipc_fd = -1;
|
||||
|
||||
// init display
|
||||
|
|
@ -890,6 +952,76 @@ static void ui_draw_vision_maxspeed(UIState *s) {
|
|||
}
|
||||
}
|
||||
|
||||
static void ui_draw_vision_speedlimit(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
int ui_viz_rw = scene->ui_viz_rw;
|
||||
|
||||
if (!s->scene.speedlimit_valid){
|
||||
return;
|
||||
}
|
||||
|
||||
float speedlimit = s->scene.speedlimit;
|
||||
|
||||
const int viz_maxspeed_w = 180;
|
||||
const int viz_maxspeed_h = 202;
|
||||
|
||||
const int viz_event_w = 220;
|
||||
const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2)));
|
||||
|
||||
const int viz_maxspeed_x = viz_event_x + (viz_event_w-viz_maxspeed_w);
|
||||
const int viz_maxspeed_y = (footer_y + ((footer_h - viz_maxspeed_h) / 2)) - 20;
|
||||
|
||||
char maxspeed_str[32];
|
||||
|
||||
if (s->is_metric) {
|
||||
nvgBeginPath(s->vg);
|
||||
nvgCircle(s->vg, viz_maxspeed_x + viz_maxspeed_w / 2, viz_maxspeed_y + viz_maxspeed_h / 2, 127);
|
||||
nvgFillColor(s->vg, nvgRGBA(195, 0, 0, 255));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgCircle(s->vg, viz_maxspeed_x + viz_maxspeed_w / 2, viz_maxspeed_y + viz_maxspeed_h / 2, 100);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 130);
|
||||
nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255));
|
||||
|
||||
snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(speedlimit * 3.6 + 0.5));
|
||||
nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 135, maxspeed_str, NULL);
|
||||
} else {
|
||||
const int border = 10;
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRoundedRect(s->vg, viz_maxspeed_x - border, viz_maxspeed_y - border, viz_maxspeed_w + 2 * border, viz_maxspeed_h + 2 * border, 30);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20);
|
||||
nvgStrokeColor(s->vg, nvgRGBA(0, 0, 0, 255));
|
||||
nvgStrokeWidth(s->vg, 8);
|
||||
nvgStroke(s->vg);
|
||||
|
||||
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFontSize(s->vg, 50);
|
||||
nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255));
|
||||
nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 50, "SPEED", NULL);
|
||||
nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 90, "LIMIT", NULL);
|
||||
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 120);
|
||||
nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255));
|
||||
|
||||
snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(speedlimit * 2.2369363 + 0.5));
|
||||
nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 170, maxspeed_str, NULL);
|
||||
}
|
||||
}
|
||||
|
||||
static void ui_draw_vision_speed(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
|
|
@ -1019,6 +1151,8 @@ static void ui_draw_vision_footer(UIState *s) {
|
|||
|
||||
// Driver Monitoring
|
||||
ui_draw_vision_face(s);
|
||||
|
||||
ui_draw_vision_speedlimit(s);
|
||||
}
|
||||
|
||||
static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
|
||||
|
|
@ -1267,7 +1401,7 @@ static void ui_update(UIState *s) {
|
|||
|
||||
// poll for events
|
||||
while (true) {
|
||||
zmq_pollitem_t polls[9] = {{0}};
|
||||
zmq_pollitem_t polls[10] = {{0}};
|
||||
polls[0].socket = s->live100_sock_raw;
|
||||
polls[0].events = ZMQ_POLLIN;
|
||||
polls[1].socket = s->livecalibration_sock_raw;
|
||||
|
|
@ -1282,14 +1416,16 @@ static void ui_update(UIState *s) {
|
|||
polls[5].events = ZMQ_POLLIN;
|
||||
polls[6].socket = s->uilayout_sock_raw;
|
||||
polls[6].events = ZMQ_POLLIN;
|
||||
polls[7].socket = s->plus_sock_raw;
|
||||
polls[7].socket = s->map_data_sock_raw;
|
||||
polls[7].events = ZMQ_POLLIN;
|
||||
polls[8].socket = s->plus_sock_raw; // plus_sock should be last
|
||||
polls[8].events = ZMQ_POLLIN;
|
||||
|
||||
int num_polls = 8;
|
||||
int num_polls = 9;
|
||||
if (s->vision_connected) {
|
||||
assert(s->ipc_fd >= 0);
|
||||
polls[8].fd = s->ipc_fd;
|
||||
polls[8].events = ZMQ_POLLIN;
|
||||
polls[9].fd = s->ipc_fd;
|
||||
polls[9].events = ZMQ_POLLIN;
|
||||
num_polls++;
|
||||
}
|
||||
|
||||
|
|
@ -1303,12 +1439,13 @@ static void ui_update(UIState *s) {
|
|||
}
|
||||
|
||||
if (polls[0].revents || polls[1].revents || polls[2].revents ||
|
||||
polls[3].revents || polls[4].revents || polls[6].revents || polls[7].revents) {
|
||||
polls[3].revents || polls[4].revents || polls[6].revents ||
|
||||
polls[7].revents || polls[8].revents) {
|
||||
// awake on any (old) activity
|
||||
set_awake(s, true);
|
||||
}
|
||||
|
||||
if (s->vision_connected && polls[8].revents) {
|
||||
if (s->vision_connected && polls[9].revents) {
|
||||
// vision ipc event
|
||||
VisionPacket rp;
|
||||
err = vipc_recv(s->ipc_fd, &rp);
|
||||
|
|
@ -1351,7 +1488,7 @@ static void ui_update(UIState *s) {
|
|||
} else {
|
||||
assert(false);
|
||||
}
|
||||
} else if (polls[7].revents) {
|
||||
} else if (polls[8].revents) {
|
||||
// plus socket
|
||||
|
||||
zmq_msg_t msg;
|
||||
|
|
@ -1407,9 +1544,40 @@ static void ui_update(UIState *s) {
|
|||
s->scene.engageable = datad.engageable;
|
||||
s->scene.gps_planner_active = datad.gpsPlannerActive;
|
||||
s->scene.monitoring_active = datad.driverMonitoringOn;
|
||||
// printf("recv %f\n", datad.vEgo);
|
||||
|
||||
s->scene.frontview = datad.rearViewCam;
|
||||
|
||||
if (datad.alertSound.str && datad.alertSound.str[0] != '\0' && strcmp(s->alert_type, datad.alertType.str) != 0) {
|
||||
char* error = NULL;
|
||||
if (s->alert_sound[0] != '\0') {
|
||||
sound_file* active_sound = get_sound_file_by_name(s->alert_sound);
|
||||
slplay_stop_uri(active_sound->uri, &error);
|
||||
if (error) {
|
||||
LOGW("error stopping active sound %s", error);
|
||||
}
|
||||
}
|
||||
|
||||
sound_file* sound = get_sound_file_by_name(datad.alertSound.str);
|
||||
slplay_play(sound->uri, sound->loop, &error);
|
||||
if(error) {
|
||||
LOGW("error playing sound: %s", error);
|
||||
}
|
||||
|
||||
snprintf(s->alert_sound, sizeof(s->alert_sound), "%s", datad.alertSound.str);
|
||||
snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str);
|
||||
} else if ((!datad.alertSound.str || datad.alertSound.str[0] == '\0') && s->alert_sound[0] != '\0') {
|
||||
sound_file* sound = get_sound_file_by_name(s->alert_sound);
|
||||
|
||||
char* error = NULL;
|
||||
|
||||
slplay_stop_uri(sound->uri, &error);
|
||||
if(error) {
|
||||
LOGW("error stopping sound: %s", error);
|
||||
}
|
||||
s->alert_type[0] = '\0';
|
||||
s->alert_sound[0] = '\0';
|
||||
}
|
||||
|
||||
if (datad.alertText1.str) {
|
||||
snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", datad.alertText1.str);
|
||||
} else {
|
||||
|
|
@ -1462,7 +1630,6 @@ static void ui_update(UIState *s) {
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (eventd.which == cereal_Event_live20) {
|
||||
struct cereal_Live20Data datad;
|
||||
cereal_read_Live20Data(&datad, eventd.live20);
|
||||
|
|
@ -1523,22 +1690,27 @@ static void ui_update(UIState *s) {
|
|||
|
||||
s->scene.started_ts = datad.startedTs;
|
||||
} else if (eventd.which == cereal_Event_uiLayoutState) {
|
||||
struct cereal_UiLayoutState datad;
|
||||
cereal_read_UiLayoutState(&datad, eventd.uiLayoutState);
|
||||
s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed;
|
||||
s->scene.uilayout_mapenabled = datad.mapEnabled;
|
||||
struct cereal_UiLayoutState datad;
|
||||
cereal_read_UiLayoutState(&datad, eventd.uiLayoutState);
|
||||
s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed;
|
||||
s->scene.uilayout_mapenabled = datad.mapEnabled;
|
||||
|
||||
bool hasSidebar = !s->scene.uilayout_sidebarcollapsed;
|
||||
bool mapEnabled = s->scene.uilayout_mapenabled;
|
||||
if (mapEnabled) {
|
||||
s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4));
|
||||
s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4));
|
||||
s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s);
|
||||
} else {
|
||||
s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2);
|
||||
s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2));
|
||||
s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0;
|
||||
}
|
||||
bool hasSidebar = !s->scene.uilayout_sidebarcollapsed;
|
||||
bool mapEnabled = s->scene.uilayout_mapenabled;
|
||||
if (mapEnabled) {
|
||||
s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4));
|
||||
s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4));
|
||||
s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s);
|
||||
} else {
|
||||
s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2);
|
||||
s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2));
|
||||
s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0;
|
||||
}
|
||||
} else if (eventd.which == cereal_Event_liveMapData) {
|
||||
struct cereal_LiveMapData datad;
|
||||
cereal_read_LiveMapData(&datad, eventd.liveMapData);
|
||||
s->scene.speedlimit = datad.speedLimit;
|
||||
s->scene.speedlimit_valid = datad.valid;
|
||||
}
|
||||
capn_free(&ctx);
|
||||
zmq_msg_close(&msg);
|
||||
|
|
@ -1738,6 +1910,13 @@ int main() {
|
|||
TouchState touch = {0};
|
||||
touch_init(&touch);
|
||||
|
||||
char* error = NULL;
|
||||
ui_sound_init(&error);
|
||||
if (error) {
|
||||
LOGW(error);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// light sensor scaling params
|
||||
const int EON = (access("/EON", F_OK) != -1);
|
||||
const int LEON = is_leon();
|
||||
|
|
@ -1748,6 +1927,8 @@ int main() {
|
|||
|
||||
float smooth_brightness = BRIGHTNESS_B;
|
||||
|
||||
set_volume(s, 0);
|
||||
|
||||
while (!do_exit) {
|
||||
bool should_swap = false;
|
||||
pthread_mutex_lock(&s->lock);
|
||||
|
|
@ -1787,6 +1968,13 @@ int main() {
|
|||
should_swap = true;
|
||||
}
|
||||
|
||||
if (s->volume_timeout > 0) {
|
||||
s->volume_timeout--;
|
||||
} else {
|
||||
int volume = min(13, 11 + s->scene.v_ego / 15); // up one notch every 15 m/s, starting at 11
|
||||
set_volume(s, volume);
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&s->lock);
|
||||
|
||||
// the bg thread needs to be scheduled, so the main thread needs time without the lock
|
||||
|
|
@ -1798,6 +1986,8 @@ int main() {
|
|||
|
||||
set_awake(s, true);
|
||||
|
||||
slplay_destroy();
|
||||
|
||||
// wake up bg thread to exit
|
||||
pthread_mutex_lock(&s->lock);
|
||||
pthread_cond_signal(&s->bg_cond);
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in New Issue