diff --git a/README.md b/README.md index 442fbd4470d1b2..169417a05da117 100644 --- a/README.md +++ b/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 | 25mph1| 25mph | -| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | -| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | -| Chevrolet3| Volt 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | -| Chevrolet3| 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 | 25mph1| 12mph | -| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph1| 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 | 25mph1| 0mph | -| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph1| 0mph | -| Honda | Odyssey 2019 | Honda Sensing | Yes | Yes | 25mph1| 0mph | -| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | -| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Hyundai6 | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | -| Hyundai6 | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | -| Hyundai6 | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | -| Kia6 | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | -| Kia6 | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | -| Lexus | RX Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | -| Lexus | RX Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Camry 20184| All | Yes | Stock | 0mph5 | 0mph | -| Toyota | C-HR 20184 | All | Yes | Stock | 0mph | 0mph | -| Toyota | Corolla 2017 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Corolla 2018 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Highlander 2017 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Highlander Hybrid 2018| All | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius 2017 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius 2018 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius Prime 2017 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius Prime 2018 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | -| Toyota | Rav4 2017 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Rav4 2018 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | +| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe | +| ---------------------| ------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------| +| Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | Nidec | +| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec | +| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| 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 | 25mph1| 12mph | Nidec | +| Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch | +| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 0mph | Inverted Nidec | +| Honda | Pilot 2017-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | +| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | +| Honda | Ridgeline 2017-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | +| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6| +| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6| +| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6| +| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| +| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| +| Lexus | RX Hybrid 2016-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | Toyota | +| Toyota | C-HR 2017-184| All | Yes | Stock | 0mph | 0mph | Toyota | +| Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota | +| Toyota | Highlander 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius Prime 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | Toyota | +| Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota | +| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | 1[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)*** 2When 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) -3[GM installation guide](https://www.zoneos.com/volt.htm) +3[GM installation guide](https://zoneos.com/volt/). 4It 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/). 528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. -6Giraffe is under development: architecture similar to Toyota giraffe, with an extra 120Ohm resistor on bus 3. +6Open 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.
+7Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/)
Community Maintained Cars ------ -| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | -| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- | -| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph1| 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 | 25mph1| 12mph | Inverted Nidec | +| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph | Custom8| +| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph | Custom8| -[[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).
+[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246)
+8Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla)
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.** + +--- + + diff --git a/RELEASES.md b/RELEASES.md index 82f01208658d6f..3720b53506940c 100644 --- a/RELEASES.md +++ b/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 diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index 5b9dc2666ba331..b44c0e5004e06c 100644 Binary files a/apk/ai.comma.plus.frame.apk and b/apk/ai.comma.plus.frame.apk differ diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 7453cd10a47e87..a9e5738a40cd3c 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/car.capnp b/cereal/car.capnp index 6b2e95f6cf3e15..2f25aa0d1ac02e 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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; } } } diff --git a/cereal/log.capnp b/cereal/log.capnp index f1b4b50b62510d..389b550ad421ae 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; } } diff --git a/selfdrive/assets/sounds/disengaged.wav b/selfdrive/assets/sounds/disengaged.wav new file mode 100644 index 00000000000000..958e08fd85b3e6 Binary files /dev/null and b/selfdrive/assets/sounds/disengaged.wav differ diff --git a/selfdrive/assets/sounds/engaged.wav b/selfdrive/assets/sounds/engaged.wav new file mode 100644 index 00000000000000..c6c088e01c8b73 Binary files /dev/null and b/selfdrive/assets/sounds/engaged.wav differ diff --git a/selfdrive/assets/sounds/error.wav b/selfdrive/assets/sounds/error.wav new file mode 100644 index 00000000000000..1ff0c540d26ab0 Binary files /dev/null and b/selfdrive/assets/sounds/error.wav differ diff --git a/selfdrive/assets/sounds/hq_disengaged.mp3 b/selfdrive/assets/sounds/hq_disengaged.mp3 deleted file mode 100644 index a2e9eb2aa0887e..00000000000000 Binary files a/selfdrive/assets/sounds/hq_disengaged.mp3 and /dev/null differ diff --git a/selfdrive/assets/sounds/hq_engaged.mp3 b/selfdrive/assets/sounds/hq_engaged.mp3 deleted file mode 100644 index dad247493ee82f..00000000000000 Binary files a/selfdrive/assets/sounds/hq_engaged.mp3 and /dev/null differ diff --git a/selfdrive/assets/sounds/hq_warning_0.mp3 b/selfdrive/assets/sounds/hq_warning_0.mp3 deleted file mode 100644 index 5dbe1a4aea9177..00000000000000 Binary files a/selfdrive/assets/sounds/hq_warning_0.mp3 and /dev/null differ diff --git a/selfdrive/assets/sounds/hq_warning_1.mp3 b/selfdrive/assets/sounds/hq_warning_1.mp3 deleted file mode 100644 index 4b658591f3cc2d..00000000000000 Binary files a/selfdrive/assets/sounds/hq_warning_1.mp3 and /dev/null differ diff --git a/selfdrive/assets/sounds/hq_warning_2.mp3 b/selfdrive/assets/sounds/hq_warning_2.mp3 deleted file mode 100644 index 04371e0a81d5e4..00000000000000 Binary files a/selfdrive/assets/sounds/hq_warning_2.mp3 and /dev/null differ diff --git a/selfdrive/assets/sounds/warning_1.wav b/selfdrive/assets/sounds/warning_1.wav new file mode 100644 index 00000000000000..67b8d76fe804fa Binary files /dev/null and b/selfdrive/assets/sounds/warning_1.wav differ diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav new file mode 100644 index 00000000000000..8e1b1d7d9161a7 Binary files /dev/null and b/selfdrive/assets/sounds/warning_2.wav differ diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index 849d43979b9f19..d897a815f2cb03 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -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 @@ except Exception: pass -# TODO: rewrite in C to save CPU - SAFETY_NOOUTPUT = 0 SAFETY_HONDA = 1 SAFETY_TOYOTA = 2 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 71277cf2a7f08b..14b165dc6aea61 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -10,7 +10,7 @@ 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ ### 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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 7c0c63f54631ce..6d5b3a9d505f6c 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -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 @@ def update(self, pt_cp): 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 @@ def update(self, pt_cp): self.brake_pressed = self.user_brake > 10 or self.regen_pressed self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive - diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 27d94b885b4af7..e1f9fc51736174 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -4,7 +4,7 @@ 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 @@ def get_params(candidate, fingerprint): 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 @@ def update(self, c): 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 @@ def apply(self, c, perception_state=log.Live20Data.new_message()): 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. diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index a14cb1603f90f5..8e564b1b924152 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -22,7 +22,7 @@ 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 @@ def update(self): ret = RI.update() print(chr(27) + "[2J") print ret - - diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 8e93b677c8ae66..b4f8d90601418a 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -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 @@ def parse_gear_shifter(can_gear): 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'), } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b1b72321efc7d4..1cb9830e39f26e 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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.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 @@ def __init__(self, dbc_name, enable_camera=True): 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ # 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ 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()) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 3d72442820118c..f46343c18fc7d6 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -2,7 +2,7 @@ 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 @@ def __init__(self, CP): 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 diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 245d2511f17b1a..3d92d24a7c2924 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -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, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 42eb041f3f9bb2..3ab57b899e9344 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -8,8 +8,9 @@ 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 @@ # 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 @@ def __init__(self, CP, sendcan=None): 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 @@ def __init__(self, CP, sendcan=None): @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 @@ def calc_accel_override(a_ego, a_target, v_ego, v_target): # 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 @@ def update(self, c): 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 @@ def update(self, c): 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 @@ def apply(self, c, perception_state=log.Live20Data.new_message()): 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) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index c9dbcdc7a829e4..17492eaf08ad96 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -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 @@ class CAR: 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] diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 0017c0783116f4..ad5191195b92b1 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index cb377fd5412958..b0c6bf5209f63b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_ 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 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_ 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, # 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, 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 @@ def update(self, sendcan, enabled, CS, frame, actuators, 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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 7e518a57de127d..71d33b0ab7c2a2 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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 @@ def __init__(self, CP): 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 @@ def update(self, cp): 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) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index cddd34d38e5318..57e90339cc6ae7 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -4,7 +4,7 @@ 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 @@ def __init__(self, CP, sendcan=None): 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 @@ def update(self, c): 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 @@ def update(self, c): 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 @@ def update(self, c): 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 @@ def apply(self, c, perception_state=log.Live20Data.new_message()): 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 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 2b37b85da4bcbb..6587f32f66c06a 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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, diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c index eb59db965fe327..f21fac8f07afd5 100644 --- a/selfdrive/common/touch.c +++ b/selfdrive/common/touch.c @@ -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; diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index ab4726817a77a5..617c8189cba23d 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.5.5-release" +#define COMMA_VERSION "0.5.6-release" diff --git a/selfdrive/config.py b/selfdrive/config.py index 1219965e11ce84..751a84e285f52c 100644 --- a/selfdrive/config.py +++ b/selfdrive/config.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 470be768b3b4b4..f4fa0d8140f491 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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 @@ 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() diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 99bb9bfc11f93e..5884e987033fc3 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -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 @@ def add(self, alert_type, enabled=True, extra_text_1='', extra_text_2=''): 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 @@ def process_alerts(self, cur_time): 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 current_alert.start_time + current_alert.duration_sound > cur_time: + self.audible_alert = current_alert.audible_alert - if ca.start_time + ca.duration_hud_alert > cur_time: - self.visual_alert = ca.visual_alert + if current_alert.start_time + current_alert.duration_hud_alert > cur_time: + self.visual_alert = current_alert.visual_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_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 diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py new file mode 100644 index 00000000000000..cc9b19d5d4fa70 --- /dev/null +++ b/selfdrive/controls/lib/alerts.py @@ -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), +] diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index d08ad37c04e3ed..9d78c966743569 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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_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) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index e1fd68b90df397..81a371f783ca6c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -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 @@ def setup_mpc(self, steer_rate_cost): 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 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs 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 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs 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 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs 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 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs #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, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index d6a7cc76900d8a..092edd27a1c64d 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -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() diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 7aac7d0a990236..95e3ef37832d87 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -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 @@ def __init__(self, CP, init_state=np.asarray([[0.], [0.]])): 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): - return curv * self.sR * 1.0 / self.curvature_factor(u) + """Calculates the required steering wheel angle for a given curvature + + Args: + curv: Desired curvature [rad/s] + u: Speed [m/s] - 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 + Returns: + Steering wheel angle [rad] + """ + + return curv * self.sR * 1.0 / self.curvature_factor(u) 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.)) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 301d04c6476598..b909b4952af7be 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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()) diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 7a058b743f7419..881b9e84fea041 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index ac49c7a2a7ad72..6b64a4af77f224 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -95,7 +95,7 @@ def unblock_stdout(): "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 @@ def main(): main() # manual exit because we are forked sys.exit(0) - diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index cc5d8d9eb88ad7..9c48a5f6ac02f3 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 63e2ac82d6ff15..84137a9b8c2af2 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index c14f913f474d4f..7a875d219b638a 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -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] diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 6c3eb66555825c..29c9e0c330a195 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -242,7 +242,7 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True) '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 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True) "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 ******** diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index 9f957e239270ed..a78a48cf2a9193 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -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 \ diff --git a/selfdrive/ui/slplay.c b/selfdrive/ui/slplay.c new file mode 100644 index 00000000000000..6e5d8b95389140 --- /dev/null +++ b/selfdrive/ui/slplay.c @@ -0,0 +1,184 @@ +#include +#include +#include +#include +#include + +#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"); + } +} diff --git a/selfdrive/ui/slplay.h b/selfdrive/ui/slplay.h new file mode 100644 index 00000000000000..f8c39ceeb7cb2c --- /dev/null +++ b/selfdrive/ui/slplay.h @@ -0,0 +1,21 @@ +#ifndef SLPLAY_H +#define SLPLAY_H + +#include +#include +#include + +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 + diff --git a/selfdrive/ui/start.sh b/selfdrive/ui/start.sh new file mode 100755 index 00000000000000..89704a5b6edf93 --- /dev/null +++ b/selfdrive/ui/start.sh @@ -0,0 +1,6 @@ +#!/bin/sh +set -e + +make +export LD_LIBRARY_PATH=/system/lib64:$LD_LIBRARY_PATH +exec ./ui diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index e81ab0867a0d82..ce57dd46517e75 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -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; - - 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; - } + 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; + } + } 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); diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 471cd66f7912b7..40259afc55c789 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ