diff --git a/.gitignore b/.gitignore index 47d1a66875..1d91f80a14 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ .tags .ipynb_checkpoints .idea +.sconsign.dblite model2.png a.out @@ -19,6 +20,8 @@ a.out *.class *.pyxbldc *.vcd +lane.cpp +loc*.cpp config.json clcache diff --git a/README.md b/README.md index 5e4a84e439..232d018f3b 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,36 @@ +[![](https://i.imgur.com/RTQYufz.jpg)](#) + 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 Hondas, Acuras and Toyotas. It's about on par with Tesla Autopilot at launch, 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 Hondas, Acuras, Toyotas, and a Chevy. 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. -Here are [some](https://www.youtube.com/watch?v=9OwTJFuDI7g) [videos](https://www.youtube.com/watch?v=64Wvt5pYQmE) [of](https://www.youtube.com/watch?v=6IW7Nejsr3A) [it](https://www.youtube.com/watch?v=-VN1YcC83nA) [running](https://www.youtube.com/watch?v=EQJZvVeihZk). And a really cool [tutorial](https://www.youtube.com/watch?v=PwOnsT2UW5o). - Community ------ -openpilot is supported by [comma.ai](https://comma.ai/) +openpilot is supported by [comma.ai](https://comma.ai/). We have a [Twitter you should follow](https://twitter.com/comma_ai). Also, we have a 3500+ person [community on slack](https://slack.comma.ai). + + + + + + + + + + + + + +
+ Hardware ------ @@ -28,68 +41,57 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur Supported Cars ------ -### Honda + Acura ### - -- Honda Accord 2018 with Honda Sensing (alpha!) - - Uses stock Honda Sensing for longitudinal control - -- Honda Civic 2016+ with Honda Sensing - - Due to limitations in steering firmware, steering is disabled below 12 mph - - Note that the hatchback model is not supported - -- Honda Civic Hatchback 2017+ with Honda Sensing (alpha!) - - Due to limitations in steering firmware, steering is disabled below 12 mph - - Uses stock Honda Sensing for longitudinal control - -- Honda CR-V 2017-2018 with Honda Sensing (alpha!) - - Due to limitations in steering firmware, steering is disabled below 12 mph - - Uses stock Honda Sensing for longitudinal control - -- Honda CR-V Touring 2015-2016 - - Can only be enabled above 25 mph - -- Honda Odyssey 2018 with Honda Sensing (alpha!) - - Can only be enabled above 25 mph - -- Honda Pilot 2017 with Honda Sensing (alpha!) - - Can only be enabled above 27 mph - -- Honda Ridgeline 2017 with Honda Sensing (alpha!) - - Can only be enabled above 27 mph +| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | +| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- | +| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph* | 12mph | +| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph* | 12mph | +| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph* | 12mph | +| GM | Volt 2017 | Driver Confidence II | Yes | Yes | 0mph | 0mph | +| GM | Volt 2018 | Driver Confidence II | Yes | Yes | 0mph | 0mph | +| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | +| Honda | Civic 2016 *(Sedan)* | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | Civic 2017 *(Sedan)* | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | Civic 2018 *(Sedan)* | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | CR-V 2015 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Honda | CR-V 2016 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph* | 12mph | +| Lexus | RX Hybrid 2017 | All | Yes | Yes | 0mph | 0mph | +| Lexus | RX Hybrid 2018 | All | Yes | Yes | 0mph | 0mph | +| Toyota | Corolla 2017 | All | Yes | Yes | 20mph | 0mph | +| Toyota | Corolla 2018 | All | Yes | Yes | 20mph | 0mph | +| Toyota | Prius 2016 | TSS-P | Yes | Yes | 0mph | 0mph | +| Toyota | Prius 2017 | All | Yes | Yes | 0mph | 0mph | +| Toyota | Prius 2018 | All | Yes | Yes | 0mph | 0mph | +| Toyota | Prius Prime 2017 | All | Yes | Yes | 0mph | 0mph | +| Toyota | Prius Prime 2018 | All | Yes | Yes | 0mph | 0mph | +| Toyota | Rav4 2016 | TSS-P | Yes | Yes | 20mph | 0mph | +| Toyota | Rav4 2017 | All | Yes | Yes | 20mph | 0mph | +| Toyota | Rav4 2018 | All | Yes | Yes | 20mph | 0mph | +| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes | 0mph | 0mph | +| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes | 0mph | 0mph | + + +*[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)*** -- Acura ILX 2016 with AcuraWatch Plus - - Due to use of the cruise control for gas, it can only be enabled above 25 mph - -- Acura RDX 2018 with AcuraWatch Plus (alpha!) - - Can only be enabled above 25 mph - -### Toyota + Lexus ### - -- Toyota RAV-4 2016+ non-hybrid with TSS-P - - By default it uses stock Toyota ACC for longitudinal control - - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can be enabled above 20 mph - -- Toyota Prius 2017+ - - By default it uses stock Toyota ACC for longitudinal control - - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) - - Lateral control needs improvements - -- Toyota RAV-4 2017+ hybrid - - By default it uses stock Toyota ACC for longitudinal control - - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can do stop and go - -- Toyota Corolla 2017+ - - By default it uses stock Toyota ACC for longitudinal control - - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Corolla_.28for_openpilot.29) and can be enabled above 20 mph +Community Maintained Cars +------ -- Lexus RX 2017+ hybrid (alpha!) - - By default it uses stock Lexus ACC for longitudinal control - - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Lexus_RX_hybrid) +| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | +| ------- | -----------------------| -------------------- | ------- | ------------ | ----------- | ------------ | +| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph | +| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph | -### GM (Chevrolet + Cadillac) ### +[[Tesla Pull Request]](https://github.com/commaai/openpilot/pull/246) -- Chevrolet Volt Premier 2017+ with Driver Confidence II package - - Read the [installation guide](https://www.zoneos.com/volt.htm) +*Community Maintained Cars are not confirmed by comma.ai to meet our safety model. Be extra cautious using them. In Progress Cars ------ @@ -97,14 +99,9 @@ In Progress Cars - '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. - 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, GS, F, RX, RXH, LX, NX, NXH, LC, LCH, LS, LSH have this option. + - '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. -Community Maintained Cars ------- - -- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/246) - How can I add support for my car? ------ @@ -112,34 +109,53 @@ If your car has adaptive cruise control and lane keep assist, you are in luck. U We've written a [porting guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for Toyota that might help you after you have the basics figured out. -Sadly, BMW, Audi, Volvo, and Mercedes all use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) and are unlikely to be supported any time soon. We also put time into a Ford port, but the steering has a 10 second cutout limitation that makes it unusable. +- 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. Directory structure ------ - -- cereal -- The messaging spec used for all logs on the phone -- common -- Library like functionality we've developed here -- opendbc -- Files showing how to interpret data from cars -- panda -- Code used to communicate on CAN and LIN -- phonelibs -- Libraries used on the phone -- selfdrive -- Code needed to drive the car - - assets -- Fonts for ui - - boardd -- Daemon to talk to the board - - car -- Code that talks to the car and implements CarInterface - - common -- Shared C/C++ code for the daemons - - controls -- Python controls (PID loops etc) for the car - - debug -- Tools to help you debug and do car ports - - logcatd -- Android logcat as a service - - loggerd -- Logger and uploader of car data - - orbd -- Service generating ORB features from road camera - - proclogd -- Logs information from proc - - sensord -- IMU / GPS interface code - - test/plant -- Car simulator running code through virtual maneuvers - - ui -- The UI - - visiond -- embedded vision pipeline + . + ├── apk # The apk files used for the UI + ├── cereal # The messaging spec used for all logs on EON + ├── common # Library like functionality we've developed here + ├── installer/updater # Manages auto-updates of openpilot + ├── opendbc # Files showing how to interpret data from cars + ├── panda # Code used to communicate on CAN and LIN + ├── phonelibs # Libraries used on EON + ├── pyextra # Libraries used on EON + └── selfdrive # Code needed to drive the car + ├── assets # Fonts and images for UI + ├── boardd # Daemon to talk to the board + ├── can # Helpers for parsing CAN messages + ├── car # Car specific code to read states and control actuators + ├── common # Shared C/C++ code for the daemons + ├── controls # Perception, planning and controls + ├── debug # Tools to help you debug and do car ports + ├── locationd # Soon to be home of precise location + ├── logcatd # Android logcat as a service + ├── loggerd # Logger and uploader of car data + ├── proclogd # Logs information from proc + ├── sensord # IMU / GPS interface code + ├── test # Car simulator running code through virtual maneuvers + ├── ui # The UI + └── visiond # Embedded vision pipeline 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. + +It's open source software, so you are free to disable it if you wish. + +It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. +The user facing camera is only logged if you explicitly opt-in in settings. +It does not log the microphone. + +By using it, you agree to [our privacy policy](https://community.comma.ai/privacy.html). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data. + Testing on PC ------ @@ -150,28 +166,15 @@ There is rudimentary infrastructure to run a basic simulation and generate a rep ./run_docker_tests.sh ``` -The results are written to `selfdrive/test/tests/plant/out/longitudinal/index.html` +The resulting plots are displayed in `selfdrive/test/tests/plant/out/longitudinal/index.html` More extensive testing infrastructure and simulation environments are coming soon. -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. - -It's open source software, so you are free to disable it if you wish. - -It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. -It does not log the user facing camera or the microphone. - -By using it, you agree to [our privacy policy](https://community.comma.ai/privacy.html). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data. - Contributing ------ We welcome both pull requests and issues on -[github](http://github.com/commaai/openpilot). See the TODO file for a list of -good places to start. +[github](http://github.com/commaai/openpilot). Bug fixes and new car support encouraged. Want to get paid to work on openpilot? [comma.ai is hiring](https://comma.ai/jobs/) diff --git a/RELEASES.md b/RELEASES.md index 711b5c23fc..a515873642 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,14 @@ +Version 0.5 (2018-07-11) +======================== + * Driver Monitoring (beta) option in settings! + * Make visiond, loggerd and UI use less resources + * 60 FPS UI + * Better car parameters for most cars + * New sidebar with stats + * Remove Waze and Spotify to free up system resources + * Remove rear view mirror option + * Calibration 3x faster + Version 0.4.7.2 (2018-06-25) ========================== * Fix loggerd lag issue diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index 565c2ad395..8291050eee 100644 --- a/apk/ai.comma.plus.frame.apk +++ b/apk/ai.comma.plus.frame.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:51e6a2c14fae295109abb8bdeef0e8c68c36cac5d0480565c6d79fbee2dd5b16 -size 2118013 +oid sha256:db186a9f1e9d1564a6f82cf294eb1322224e4956a6781bfcd6e6acf40dcd92a8 +size 2126976 diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 9b84dae2d4..018d5e9c39 100644 --- a/apk/ai.comma.plus.offroad.apk +++ b/apk/ai.comma.plus.offroad.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9a080ef24391a2e93300e65cb4d38702a3fb634e30f0a48684522a73ab420fb3 -size 17456889 +oid sha256:dd7b7e0a5c84bef76d7f52be9dcf2641ee4ab966901deeff7228f638e112cbe6 +size 18180759 diff --git a/apk/external/.gitignore b/apk/external/.gitignore deleted file mode 100644 index 3cdb953094..0000000000 --- a/apk/external/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0795c5ce6068f7ab3fcf23dccbee8b9c76da86808eacf4814ea99be4057ee83f -size 12 diff --git a/apk/external/com.spotify.music.apkpatch b/apk/external/com.spotify.music.apkpatch deleted file mode 100644 index 79853d461a..0000000000 --- a/apk/external/com.spotify.music.apkpatch +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ce98217d60516c28fd68135a87144be86c63bd8f5e87de64f945925b6cce46af -size 1947345 diff --git a/apk/external/com.waze.apkpatch b/apk/external/com.waze.apkpatch deleted file mode 100644 index 77480a8fc9..0000000000 --- a/apk/external/com.waze.apkpatch +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9e392f4445062fb17266cc260288855ef9e22226910fb00a8d83eea6899c1578 -size 3877605 diff --git a/apk/external/out/.gitkeep b/apk/external/out/.gitkeep deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apk/external/patcher.py b/apk/external/patcher.py deleted file mode 100755 index 114fa73834..0000000000 --- a/apk/external/patcher.py +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:01c13f193fc7e4933bcb99b4f591aaa36881197f056d3d8742c551212159e0d1 -size 3521 diff --git a/apk/external/src/.gitkeep b/apk/external/src/.gitkeep deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apk/external/tools/ApkPatch.android.jar b/apk/external/tools/ApkPatch.android.jar deleted file mode 100644 index 6da31e3c6b..0000000000 --- a/apk/external/tools/ApkPatch.android.jar +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8358afec7442e90c4eeffc4db4b4b00793df0f51c909976b4e95e3c8f8dacdcd -size 1310659 diff --git a/apk/external/tools/apkpatch_android b/apk/external/tools/apkpatch_android deleted file mode 100755 index 6c06520b1e..0000000000 --- a/apk/external/tools/apkpatch_android +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ca197862754835da4f0bc30823845e3448334f8c5e2093a1947348d2fed753a2 -size 176 diff --git a/apk/external/tools/certificate.pem b/apk/external/tools/certificate.pem deleted file mode 100755 index 79acebef04..0000000000 --- a/apk/external/tools/certificate.pem +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b2e32152e7972032fd64ad351323f50008602c60feb19522b1aadd414bf1f7b4 -size 1001 diff --git a/apk/external/tools/key.pk8 b/apk/external/tools/key.pk8 deleted file mode 100755 index 8136ff8c2c..0000000000 --- a/apk/external/tools/key.pk8 +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a07ce769d17334f803a499c520d4a7d88478ceb1a9cf740faf296f8f54616e82 -size 633 diff --git a/apk/external/tools/signapk.android.jar b/apk/external/tools/signapk.android.jar deleted file mode 100644 index d27e59b169..0000000000 --- a/apk/external/tools/signapk.android.jar +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5743dd91db33116bd7444d7407eb14b140ab2694f8525fc34c66d22256f50634 -size 542612 diff --git a/apk/external/tools/signapk_android b/apk/external/tools/signapk_android deleted file mode 100755 index e38ec00139..0000000000 --- a/apk/external/tools/signapk_android +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:123549475cad6ae6ce2261f81caf8f094b3b3d31076e3c320fd08301eb45e657 -size 194 diff --git a/cereal/car.capnp b/cereal/car.capnp index b4c766651b..ffc79e6476 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -59,6 +59,10 @@ struct CarEvent @0x9b1657f34caf3ad3 { debugAlert @34; steerTempUnavailableMute @35; resumeRequired @36; + preDriverDistracted @37; + promptDriverDistracted @38; + driverDistracted @39; + geofence @40; } } @@ -333,7 +337,7 @@ struct CarParams { directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping startAccel @35 :Float32; # Required acceleraton to overcome creep braking - steerRateCostDEPRECATED @40 :Float32; # Lateral MPC cost on steering rate + steerRateCost @40 :Float32; # Lateral MPC cost on steering rate steerControlType @46 :SteerControlType; radarOffCan @47 :Bool; # True when radar objects aren't visible on CAN diff --git a/cereal/log.capnp b/cereal/log.capnp index 84bf1b3b5e..474c0d2410 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -264,6 +264,15 @@ struct ThermalData { fanSpeed @10 :UInt16; started @11 :Bool; startedTs @13 :UInt64; + + thermalStatus @14 :ThermalStatus; + + enum ThermalStatus { + green @0; # all processes run + yellow @1; # critical processes run (kill uploader), engage still allowed + red @2; # no engage, will disengage + danger @3; # immediate process shutdown + } } struct HealthData { @@ -274,6 +283,7 @@ struct HealthData { controlsAllowed @3 :Bool; gasInterceptorDetected @4 :Bool; startedSignalDetected @5 :Bool; + isGreyPanda @6 :Bool; } struct LiveUI { @@ -384,11 +394,11 @@ struct Live100Data { alertText2 @25 :Text; alertStatus @38 :AlertStatus; alertSize @39 :AlertSize; + alertBlinkingRate @42 :Float32; awarenessStatus @26 :Float32; - angleOffset @27 :Float32; - gpsPlannerActive @40 :Bool; + engageable @41 :Bool; # can OP be engaged? enum ControlState { disabled @0; @@ -591,6 +601,7 @@ struct LiveLocationData { poseQuatECEF @19 :List(Float32); pitchCalibration @20 :Float32; yawCalibration @21 :Float32; + imuFrame @22 :List(Float32); struct Accuracy { pNEDError @0 :List(Float32); @@ -1519,6 +1530,11 @@ struct OrbKeyFrame { descriptors @3 :Data; } +struct DriverMonitoring { + frameId @0 :UInt32; + descriptor @1 :List(Float32); +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -1582,5 +1598,6 @@ struct Event { orbKeyFrame @56 :OrbKeyFrame; uiLayoutState @57 :UiLayoutState; orbFeaturesSummary @58 :OrbFeaturesSummary; + driverMonitoring @59 :DriverMonitoring; } } diff --git a/common/fingerprints.py b/common/fingerprints.py index 51d78a34c7..d4845f8d41 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -15,7 +15,7 @@ def get_fingerprint_list(): car_fingerprints = values.FINGERPRINTS else: continue - for f, v in car_fingerprints.iteritems(): + for f, v in car_fingerprints.items(): fingerprints[f] = v except (ImportError, IOError): pass diff --git a/common/kalman/ekf.py b/common/kalman/ekf.py index 4e9a97f9ad..fbed218f7a 100644 --- a/common/kalman/ekf.py +++ b/common/kalman/ekf.py @@ -1,4 +1,5 @@ # pylint: skip-file +from __future__ import print_function import abc import numpy as np # The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use. @@ -92,8 +93,8 @@ class EKF: innovation = reading.data - reading.obs_model * self.state if self.DEBUG: - print "reading:\n",reading.data - print "innovation:\n",innovation + print("reading:\n",reading.data) + print("innovation:\n",innovation) # S = H*P*H' + R innovation_covar = reading.obs_model * self.covar * reading.obs_model.T + reading.covar @@ -103,12 +104,12 @@ class EKF: innovation_covar) if self.DEBUG: - print "gain:\n", kalman_gain - print "innovation_covar:\n", innovation_covar - print "innovation: ", innovation - print "test: ", self.covar * reading.obs_model.T * ( + print("gain:\n", kalman_gain) + print("innovation_covar:\n", innovation_covar) + print("innovation: ", innovation) + print("test: ", self.covar * reading.obs_model.T * ( reading.obs_model * self.covar * reading.obs_model.T + reading.covar * - 0).I + 0).I) # x = x + K*y self.state += kalman_gain*innovation @@ -124,9 +125,9 @@ class EKF: self.covar = aux_mtrx * self.covar * aux_mtrx.T + kalman_gain * reading.covar * kalman_gain.T if self.DEBUG: - print "After update" - print "state\n", self.state - print "covar:\n",self.covar + print("After update") + print("state\n", self.state) + print("covar:\n",self.covar) def update_scalar(self, reading): # like update but knowing that measurement is a scalar diff --git a/common/logging_extra.py b/common/logging_extra.py index bad9ad5165..43ae48882b 100644 --- a/common/logging_extra.py +++ b/common/logging_extra.py @@ -78,7 +78,7 @@ class SwagLogger(logging.Logger): self.log_local = local() self.log_local.ctx = {} - def findCaller(self): + def findCaller(self, stack_info=None): """ Find the stack frame of the caller so that we can note the source file name, line number and function name. @@ -132,6 +132,9 @@ class SwagLogger(logging.Logger): if args: evt['args'] = args evt.update(kwargs) + ctx = self.get_ctx() + if ctx: + evt['ctx'] = self.get_ctx() if 'error' in kwargs: self.error(evt) else: diff --git a/common/params.py b/common/params.py index cd78dc3699..9d1b096af1 100755 --- a/common/params.py +++ b/common/params.py @@ -57,11 +57,12 @@ keys = { # written: baseui # read: ui, controls "IsMetric": TxType.PERSISTENT, - "IsRearViewMirror": TxType.PERSISTENT, "IsFcwEnabled": TxType.PERSISTENT, "HasAcceptedTerms": TxType.PERSISTENT, "CompletedTrainingVersion": TxType.PERSISTENT, "IsUploadVideoOverCellularEnabled": TxType.PERSISTENT, + "IsDriverMonitoringEnabled": TxType.PERSISTENT, + "IsGeofenceEnabled": TxType.PERSISTENT, # written: visiond # read: visiond, controlsd "CalibrationParams": TxType.PERSISTENT, @@ -315,7 +316,6 @@ class Params(object): with self.env.begin(write=True) as txn: txn.put(key, dat) - print "set", key if __name__ == "__main__": params = Params() @@ -325,11 +325,11 @@ if __name__ == "__main__": for k in keys: pp = params.get(k) if pp is None: - print k, "is None" + print("%s is None" % k) elif all(ord(c) < 128 and ord(c) >= 32 for c in pp): - print k, pp + print("%s = %s" % (k, pp)) else: - print k, pp.encode("hex") + print("%s = %s" % (k, pp.encode("hex"))) # Test multiprocess: # seq 0 100000 | xargs -P20 -I{} python common/params.py DongleId {} && sleep 0.05 diff --git a/common/profiler.py b/common/profiler.py index 953f5737d8..7f9b1a41ff 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -36,11 +36,11 @@ class Profiler(object): if not self.enabled: return self.iter += 1 - print "******* Profiling *******" + print("******* Profiling *******") for n, ms in sorted(self.cp.items(), key=lambda x: -x[1]): if n in self.cp_ignored: - print "%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED" + print("%30s: %7.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms/self.tot*100)) else: - print "%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100) - print "Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot) + print("%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100)) + print("Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot)) diff --git a/common/realtime.py b/common/realtime.py index b8d75db09e..7fe183fb23 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -106,5 +106,3 @@ class Ratekeeper(object): self._remaining = remaining return lagged -if __name__ == "__main__": - print sec_since_boot() diff --git a/phonelibs/libgralloc/include/gralloc_priv.h b/phonelibs/libgralloc/include/gralloc_priv.h new file mode 100644 index 0000000000..f46e93cc3d --- /dev/null +++ b/phonelibs/libgralloc/include/gralloc_priv.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c9855edf295958587122a98b881caa4fdfa2c984c8164d95cd7df58153c7304e +size 12842 diff --git a/phonelibs/linux/include/msm_ion.h b/phonelibs/linux/include/msm_ion.h new file mode 100644 index 0000000000..2b0e170b94 --- /dev/null +++ b/phonelibs/linux/include/msm_ion.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2d158f189ca1a26c9c99635d17046826ca72d050491504c48b53b10b8e8e747d +size 5688 diff --git a/phonelibs/opencl/include/CL/cl.h b/phonelibs/opencl/include/CL/cl.h new file mode 100644 index 0000000000..0adb288d92 --- /dev/null +++ b/phonelibs/opencl/include/CL/cl.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1fae6686e23d15a017feac32090724c18dd4c830c354ffbd1841ea57acf08ebd +size 74542 diff --git a/phonelibs/opencl/include/CL/cl_d3d10.h b/phonelibs/opencl/include/CL/cl_d3d10.h new file mode 100644 index 0000000000..56afc0dddb --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_d3d10.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:721e3bfb6a09be9104f733e8813c6609636999b86b7952f4d7cb3032baa74e3c +size 5122 diff --git a/phonelibs/opencl/include/CL/cl_d3d11.h b/phonelibs/opencl/include/CL/cl_d3d11.h new file mode 100644 index 0000000000..37c7ec51c1 --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_d3d11.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dfbd4aa7f6a84e587c986a0b893fde1a06f1534a0643a6d140be7e94df6a45c0 +size 5116 diff --git a/phonelibs/opencl/include/CL/cl_dx9_media_sharing.h b/phonelibs/opencl/include/CL/cl_dx9_media_sharing.h new file mode 100644 index 0000000000..4119561e5d --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_dx9_media_sharing.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2c8661e6fb6fb43123c8e714c021d7741bd6de2db99ab82d50c255294064a321 +size 5421 diff --git a/phonelibs/opencl/include/CL/cl_egl.h b/phonelibs/opencl/include/CL/cl_egl.h new file mode 100644 index 0000000000..edb73d3561 --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_egl.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d9acb9d0766263b0cba5dc6136f01ee72df9c3a526125790b5022ce23183d70a +size 5358 diff --git a/phonelibs/opencl/include/CL/cl_ext.h b/phonelibs/opencl/include/CL/cl_ext.h new file mode 100644 index 0000000000..de18b8efb4 --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_ext.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ae090fc06ae716732a11d886dcf1a679af55a96bfe88c73e4545cae756ddec69 +size 16402 diff --git a/phonelibs/opencl/include/CL/cl_gl.h b/phonelibs/opencl/include/CL/cl_gl.h new file mode 100644 index 0000000000..8c45d148b0 --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_gl.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:130faf7c95336d36c178db95ea211b9f23c6954233e144d1f7de22b95290647d +size 7583 diff --git a/phonelibs/opencl/include/CL/cl_gl_ext.h b/phonelibs/opencl/include/CL/cl_gl_ext.h new file mode 100644 index 0000000000..fdf8768df5 --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_gl_ext.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:231b00744377552b16adf7aaa0580adf2482c0b978b9224b34470606ed0c77cf +size 2869 diff --git a/phonelibs/opencl/include/CL/cl_platform.h b/phonelibs/opencl/include/CL/cl_platform.h new file mode 100644 index 0000000000..90b690dcf1 --- /dev/null +++ b/phonelibs/opencl/include/CL/cl_platform.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dfd4ceeec4ef2b760cfac2e974a4aa6bd1e3b86bba80810c5df9d4084fdfc0b9 +size 43544 diff --git a/phonelibs/opencl/include/CL/opencl.h b/phonelibs/opencl/include/CL/opencl.h new file mode 100644 index 0000000000..d8db352b4e --- /dev/null +++ b/phonelibs/opencl/include/CL/opencl.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:87140d80e5d9e1fdd9a0ba4769d4ba54e6eeadca909a4ac542bf87ec49098249 +size 1993 diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt index 69f135c260..03459175d4 100644 --- a/requirements_openpilot.txt +++ b/requirements_openpilot.txt @@ -15,5 +15,5 @@ sympy==1.1.1 filterpy==1.0.0 smbus2==0.2.0 pyflakes==1.5.0 --e git+https://github.com/commaai/le_python.git#egg=Logentries +-e git+https://github.com/commaai/le_python.git@5eef8f5be5929d33973e1b10e686fa0cdcd6792f#egg=Logentries Flask==1.0.1 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 66eef809a1..669f0fdd8c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -53,7 +53,7 @@ pthread_mutex_t usb_lock; bool spoofing_started = false; bool fake_send = false; bool loopback_can = false; -bool has_pigeon = false; +bool is_grey_panda = false; pthread_t safety_setter_thread_handle = -1; pthread_t pigeon_thread_handle = -1; @@ -173,6 +173,7 @@ bool usb_connect() { if (is_pigeon[0]) { LOGW("grey panda detected"); + is_grey_panda = true; pigeon_needs_init = true; if (pigeon_thread_handle == -1) { err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL); @@ -295,6 +296,7 @@ void can_health(void *s) { healthData.setControlsAllowed(health.controls_allowed); healthData.setGasInterceptorDetected(health.gas_interceptor_detected); healthData.setStartedSignalDetected(health.started_signal_detected); + healthData.setIsGreyPanda(is_grey_panda); // send to health auto words = capnp::messageToFlatArray(msg); diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index 7aca3b8781..849d43979b 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -104,10 +104,10 @@ def can_init(): handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'') if handle is None: - print "CAN NOT FOUND" + cloudlog.warn("CAN NOT FOUND") exit(-1) - print "got handle" + cloudlog.info("got handle") cloudlog.info("can init done") def boardd_mock_loop(): @@ -129,7 +129,7 @@ def boardd_mock_loop(): # recv @ 100hz can_msgs = can_recv() - print "sent %d got %d" % (len(snd), len(can_msgs)) + print("sent %d got %d" % (len(snd), len(can_msgs))) m = can_list_to_can_capnp(can_msgs) sendcan.send(m.to_bytes()) @@ -142,7 +142,7 @@ def boardd_test_loop(): #can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",1]]) # recv @ 100hz can_msgs = can_recv() - print "got %d" % (len(can_msgs)) + print("got %d" % (len(can_msgs))) time.sleep(0.01) cnt += 1 diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py index 5186e17622..58f5e3efc3 100644 --- a/selfdrive/can/packer.py +++ b/selfdrive/can/packer.py @@ -1,6 +1,4 @@ import struct -import numbers - from selfdrive.can.libdbc_py import libdbc, ffi @@ -10,7 +8,6 @@ class CANPacker(object): self.dbc = libdbc.dbc_lookup(dbc_name) self.sig_names = {} self.name_to_address_and_size = {} - self.address_to_size = {} num_msgs = self.dbc[0].num_msgs for i in range(num_msgs): @@ -19,16 +16,11 @@ class CANPacker(object): name = ffi.string(msg.name) address = msg.address self.name_to_address_and_size[name] = (address, msg.size) - self.address_to_size[address] = msg.size + self.name_to_address_and_size[address] = (address, msg.size) def pack(self, addr, values, counter): - # values: [(signal_name, signal_value)] - values_thing = [] - if isinstance(values, dict): - values = values.items() - - for name, value in values: + for name, value in values.iteritems(): if name not in self.sig_names: self.sig_names[name] = ffi.new("char[]", name) @@ -42,10 +34,7 @@ class CANPacker(object): return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c, counter) def pack_bytes(self, addr, values, counter=-1): - if isinstance(addr, numbers.Number): - size = self.address_to_size[addr] - else: - addr, size = self.name_to_address_and_size[addr] + addr, size = self.name_to_address_and_size[addr] val = self.pack(addr, values, counter) r = struct.pack(">Q", val) @@ -62,4 +51,4 @@ if __name__ == "__main__": ("PCM_SPEED", 123), ("PCM_GAS", 10), ]) - print s[1].encode("hex") + print(s[1].encode("hex")) diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py index c4c007d1f5..6abbb3c18a 100644 --- a/selfdrive/can/parser.py +++ b/selfdrive/can/parser.py @@ -203,6 +203,6 @@ if __name__ == "__main__": while True: cp.update(int(sec_since_boot()*1e9), True) # print cp.vl - print cp.ts - print cp.can_valid + print(cp.ts) + print(cp.can_valid) time.sleep(0.01) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 0255137ab0..afe5a9e4c1 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -1,4 +1,5 @@ import os +import time from common.basedir import BASEDIR from common.realtime import sec_since_boot from common.fingerprints import eliminate_incompatible_cars, all_known_cars @@ -49,18 +50,20 @@ def fingerprint(logcan, timeout): candidate_cars = all_known_cars() finger = {} st = None + st_passive = sec_since_boot() # only relevant when passive + can_seen = False while 1: - for a in messaging.drain_sock(logcan, wait_for_one=True): + for a in messaging.drain_sock(logcan): for can in a.can: + can_seen = True # ignore everything not on bus 0 and with more than 11 bits, # which are ussually sporadic and hard to include in fingerprints if can.src == 0 and can.address < 0x800: finger[can.address] = len(can.dat) candidate_cars = eliminate_incompatible_cars(can, candidate_cars) - if st is None: + if st is None and can_seen: st = sec_since_boot() # start time - st_passive = sec_since_boot() # only relevant when passive ts = sec_since_boot() # if we only have one car choice and the time_fingerprint since we got our first # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not @@ -75,13 +78,15 @@ def fingerprint(logcan, timeout): elif len(candidate_cars) == 0 or (timeout and (ts - st_passive) > timeout): return None, finger + time.sleep(0.01) + cloudlog.warning("fingerprinted %s", candidate_cars[0]) return (candidate_cars[0], finger) def get_car(logcan, sendcan=None, passive=True): # TODO: timeout only useful for replays so controlsd can start before unlogger - timeout = 1. if passive else None + timeout = 2. if passive else None candidate, fingerprints = fingerprint(logcan, timeout) if candidate is None: @@ -93,7 +98,7 @@ def get_car(logcan, sendcan=None, passive=True): interface_cls = interfaces[candidate] if interface_cls is None: - cloudlog.warning("car matched %s, but interface wasn't available" % candidate) + cloudlog.warning("car matched %s, but interface wasn't available or failed to import" % candidate) return None, None params = interface_cls.get_params(candidate, fingerprints) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index f4163f526d..6a64183425 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,6 +1,7 @@ #!/usr/bin/env python from common.realtime import sec_since_boot from cereal import car +from selfdrive.swaglog import cloudlog 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 @@ -75,6 +76,7 @@ class CarInterface(object): ret.steerKpV, ret.steerKiV = [[0.01], [0.005]] # TODO: tune this ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + ret.steerRateCost = 1.0 f = 1.2 tireStiffnessFront_civic *= f @@ -117,7 +119,7 @@ class CarInterface(object): ret.brakeMaxV = [1., 0.8] ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) - print "ECU Camera Simulated: ", ret.enableCamera + cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) ret.steerLimitAlert = False ret.stoppingControl = False diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 3f53291f99..53dc8b68e7 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -151,6 +151,7 @@ class CarInterface(object): ret.startAccel = 0.8 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + ret.steerRateCost = 1.0 ret.steerControlType = car.CarParams.SteerControlType.torque return ret diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 1a91a06bd3..0875a67234 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -6,7 +6,6 @@ from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import AH, CruiseButtons, CAR from selfdrive.can.packer import CANPacker - def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params... TODO: move these to VehicleParams brake_hyst_on = 0.02 # to activate brakes exceed this value @@ -109,7 +108,6 @@ class CarController(object): 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): - print "INVALID HUD", hud hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index b926c156c6..36dea1f7ff 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -3,8 +3,6 @@ from common.kalman.simple_kalman import KF1D from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.honda.values import CAR, DBC -import numpy as np - def parse_gear_shifter(can_gear_shifter, car_fingerprint): @@ -73,7 +71,6 @@ def get_can_signals(CP): ("LEFT_BLINKER", "SCM_FEEDBACK", 0), ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), ("GEAR", "GEARBOX", 0), - ("WHEELS_MOVING", "STANDSTILL", 1), ("BRAKE_ERROR_1", "STANDSTILL", 1), ("BRAKE_ERROR_2", "STANDSTILL", 1), ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1), @@ -128,8 +125,10 @@ def get_can_signals(CP): signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1), ("DOOR_OPEN_FR", "DOORS_STATUS", 1), ("DOOR_OPEN_RL", "DOORS_STATUS", 1), - ("DOOR_OPEN_RR", "DOORS_STATUS", 1)] + ("DOOR_OPEN_RR", "DOORS_STATUS", 1), + ("WHEELS_MOVING", "STANDSTILL", 1)] checks += [("DOORS_STATUS", 3)] + if CP.carFingerprint == CAR.CIVIC: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_FEEDBACK", 0), @@ -188,10 +187,10 @@ class CarState(object): dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 - self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), - A=np.matrix([[1.0, dt], [0.0, 1.0]]), - C=np.matrix([1.0, 0.0]), - K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], + A=[[1.0, dt], [0.0, 1.0]], + C=[[1.0, 0.0]], + K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 def update(self, cp): @@ -212,9 +211,12 @@ class CarState(object): self.prev_right_blinker_on = self.right_blinker_on # ******************* parse out can ******************* - if self.CP.carFingerprint in (CAR.ACCORD): + + if self.CP.carFingerprint == CAR.ACCORD: # TODO: find wheels moving bit in dbc + self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] else: + self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'] @@ -232,14 +234,14 @@ class CarState(object): self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS - self.v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])) + self.v_wheel = (self.v_wheel_fl+self.v_wheel_fr+self.v_wheel_rl+self.v_wheel_rr)/4. # blend in transmission speed at low speed, since it has more low speed accuracy self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v) speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS + self.v_weight * self.v_wheel if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed - self.v_ego_x = np.matrix([[speed], [0.0]]) + self.v_ego_x = [[speed], [0.0]] self.v_ego_raw = speed v_ego_x = self.v_ego_kf.update(speed) @@ -283,10 +285,8 @@ class CarState(object): self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] #rdx has different steer override threshold - if self.CP.carFingerprint in (CAR.ACURA_RDX): - self.steer_override = abs(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > 400 - else: - self.steer_override = abs(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > 1200 + steer_thrsld = 400 if self.CP.carFingerprint == CAR.ACURA_RDX else 1200 + self.steer_override = abs(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > steer_thrsld self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] @@ -319,7 +319,6 @@ class CarState(object): self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] - self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 32c32a64f8..db02a9725a 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -4,6 +4,7 @@ import numpy as np from cereal import car from common.numpy_fast import clip, interp from common.realtime import sec_since_boot +from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -132,14 +133,7 @@ class CarInterface(object): @staticmethod def get_params(candidate, fingerprint): - # kg of standard extra cargo to count for drive, gas, etc... - std_cargo = 136 - - # Ridgeline reqires scaled tire stiffness - ts_factor = 1 - ret = car.CarParams.new_message() - ret.carName = "honda" ret.carFingerprint = candidate @@ -151,11 +145,14 @@ class CarInterface(object): ret.safetyModel = car.CarParams.SafetyModels.honda ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.enableGasInterceptor = 0x201 in fingerprint - print "ECU Camera Simulated: ", ret.enableCamera - print "ECU Gas Interceptor: ", ret.enableGasInterceptor + cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) + cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor + # kg of standard extra cargo to count for drive, gas, etc... + std_cargo = 136 + # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars mass_civic = 2923 * CV.LB_TO_KG + std_cargo @@ -163,141 +160,163 @@ class CarInterface(object): centerToFront_civic = wheelbase_civic * 0.4 centerToRear_civic = wheelbase_civic - centerToFront_civic rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 - tireStiffnessRear_civic = 90000 + tireStiffnessFront_civic = 192150 + tireStiffnessRear_civic = 202500 + + # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle + # model optimization process. Certain Hondas have an extra steering sensor at the bottom + # of the steering rack, which improves controls quality as it removes the steering column + # torsion from feedback. + # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. + # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] + + ret.steerKf = 0.00006 # conservative feed-forward + if candidate == CAR.CIVIC: stop_and_go = True ret.mass = mass_civic ret.wheelbase = wheelbase_civic ret.centerToFront = centerToFront_civic - ret.steerRatio = 13.0 + ret.steerRatio = 14.63 # 10.93 is end-to-end spec + tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] - ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] - + ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.8], [0.24]] + if is_fw_modified: + ret.steerKf = 0.00003 ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.54, 0.36] + elif candidate == CAR.CIVIC_HATCH: stop_and_go = True ret.mass = 2916. * CV.LB_TO_KG + std_cargo ret.wheelbase = wheelbase_civic ret.centerToFront = centerToFront_civic - ret.steerRatio = 10.93 + ret.steerRatio = 14.63 # 10.93 is spec end-to-end + tire_stiffness_factor = 1. ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] - ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.ACCORD: stop_and_go = True ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3279. * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 11.82 - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] - + ret.steerRatio = 15.96 # 11.82 is spec end-to-end + tire_stiffness_factor = 0.8467 + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.ACURA_ILX: stop_and_go = False ret.mass = 3095 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 - ret.steerRatio = 15.3 + ret.steerRatio = 18.61 # 15.3 is spec end-to-end + tire_stiffness_factor = 0.72 # Acura at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['ff83f397542ab647'] - ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] - + ret.steerKpV, ret.steerKiV = [[0.45], [0.00]] if is_fw_modified else [[0.8], [0.24]] + if is_fw_modified: + ret.steerKf = 0.00003 ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.CRV: stop_and_go = False ret.mass = 3572 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.3 + ret.steerRatio = 15.3 # as spec + tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] - ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.CRV_5G: stop_and_go = True ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3410. * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 12.30 + ret.steerRatio = 16.0 # 12.3 is spec end-to-end + tire_stiffness_factor = 0.677 ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] - ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.ACURA_RDX: stop_and_go = False ret.mass = 3935 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 - ret.steerRatio = 15.0 + ret.steerRatio = 15.0 # as spec + tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] - ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.ODYSSEY: stop_and_go = False ret.mass = 4354 * CV.LB_TO_KG + std_cargo ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 14.35 + ret.steerRatio = 14.35 # as spec + tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] - ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.PILOT: stop_and_go = False ret.mass = 4303 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.81 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.0 + ret.steerRatio = 16.0 # as spec + tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] - ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.RIDGELINE: stop_and_go = False - ts_factor = 1.4 ret.mass = 4515 * CV.LB_TO_KG + std_cargo ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.59 + ret.steerRatio = 15.59 # as spec + tire_stiffness_factor = 0.444 # not optimized yet ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] - ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + else: raise ValueError("unsupported car %s" % candidate) - ret.steerKf = 0. # TODO: investigate FF steer control for Honda ret.steerControlType = car.CarParams.SteerControlType.torque # min speed to enable ACC. if car can do stop and go, then set enabling speed @@ -313,10 +332,10 @@ class CarInterface(object): # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * ts_factor) * \ + ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ ret.mass / mass_civic * \ (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * ts_factor) * \ + ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ ret.mass / mass_civic * \ (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) @@ -339,7 +358,8 @@ class CarInterface(object): ret.steerLimitAlert = True ret.startAccel = 0.5 - ret.steerActuatorDelay = 0.09 + ret.steerActuatorDelay = 0.1 + ret.steerRateCost = 0.5 return ret @@ -465,7 +485,7 @@ class CarInterface(object): if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: - events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': @@ -514,7 +534,6 @@ class CarInterface(object): # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: - print "enabled pressed at", cur_time self.last_enable_pressed = cur_time enable_pressed = True diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 3b3f0bab05..b15a90a6e2 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -54,42 +54,42 @@ FINGERPRINTS = { 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8 }], CAR.ACURA_ILX: [{ - 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5, + 1024: 5, 513: 6, 1027: 5, 1029: 8, 929: 4, 1057: 5, 777: 8, 1034: 5, 1036: 8, 398: 3, 399: 7, 145: 8, 660: 8, 985: 3, 923: 2, 542: 7, 773: 7, 800: 8, 432: 7, 419: 8, 420: 8, 1030: 5, 422: 8, 808: 8, 428: 8, 304: 8, 819: 7, 821: 5, 57: 3, 316: 8, 545: 4, 464: 8, 1108: 8, 597: 8, 342: 6, 983: 8, 344: 8, 804: 8, 1039: 8, 476: 4, 892: 8, 490: 8, 1064: 7, 882: 2, 884: 7, 887: 8, 888: 8, 380: 8, 1365: 5, # sent messages 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, }], CAR.ACURA_RDX: [{ - 57L: 3, 145L: 8, 229L: 4, 308L: 5, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 398L: 3, 399L: 6, 404L: 4, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 892L: 8, 923L: 2, 929L: 4, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1034L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1365L: 5, 1424L: 5, 1729L: 1 + 57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 392: 6, 398: 3, 399: 6, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 506: 8, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5, 1424: 5, 1729: 1 }], CAR.CIVIC: [{ - 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5, + 1024: 5, 513: 6, 1027: 5, 1029: 8, 777: 8, 1036: 8, 1039: 8, 1424: 5, 401: 8, 148: 8, 662: 4, 985: 3, 795: 8, 773: 7, 800: 8, 545: 6, 420: 8, 806: 8, 808: 8, 1322: 5, 427: 3, 428: 8, 304: 8, 432: 7, 57: 3, 450: 8, 929: 8, 330: 8, 1302: 8, 464: 8, 1361: 5, 1108: 8, 597: 8, 470: 2, 344: 8, 804: 8, 399: 7, 476: 7, 1633: 8, 487: 4, 892: 8, 490: 8, 493: 5, 884: 8, 891: 8, 380: 8, 1365: 5, # sent messages 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8, }], CAR.CIVIC_HATCH: [{ - 57L: 3, 148L: 8, 228L: 5, 304L: 8, 330L: 8, 344L: 8, 380L: 8, 399L: 7, 401L: 8, 420L: 8, 427L: 3, 428L: 8, 432L: 7, 441L: 5, 450L: 8, 464L: 8, 470L: 2, 476L: 7, 477L: 8, 479L: 8, 490L: 8, 493L: 5, 495L: 8, 506L: 8, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 829L: 5, 862L: 8, 884L: 8, 891L: 8, 892L: 8, 927L: 8, 929L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1108L: 8, 1302L: 8, 1322L: 5, 1361L: 5, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, 1633L: 8 + 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8 }], CAR.CRV: [{ - 57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, + 57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 507: 1, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 800: 8, 804: 8, 808: 8, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, # sent messages 0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5, }], CAR.CRV_5G: [{ - 57L: 3, 148L: 8, 199L: 4, 228L: 5, 231L: 5, 232L: 7, 304L: 8, 330L: 8, 340L: 8, 344L: 8, 380L: 8, 399L: 7, 401L: 8, 420L: 8, 423L: 2, 427L: 3, 428L: 8, 432L: 7, 441L: 5, 446L: 3, 450L: 8, 464L: 8, 467L: 2, 469L: 3, 470L: 2, 474L: 8, 476L: 7, 477L: 8, 479L: 8, 490L: 8, 493L: 5, 495L: 8, 507L: 1, 545L: 6, 597L: 8, 661L: 4, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 814L: 4, 815L: 8, 817L: 4, 825L: 4, 829L: 5, 862L: 8, 881L: 8, 882L: 4, 884L: 8, 888L: 8, 891L: 8, 927L: 8, 918L: 7, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1108L: 8, 1092L: 1, 1115L: 4, 1125L: 8, 1127L: 2, 1296L: 8, 1302L: 8, 1322L: 5, 1361L: 5, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, 1618L: 5, 1633L: 8, 1670L: 5 + 57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 4, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5 }], CAR.ODYSSEY: [{ - 57L: 3, 148L: 8, 228L: 5, 229L: 4, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1615L: 8, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 + 57: 3, 148: 8, 228: 5, 229: 4, 316: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1619: 5, 1623: 5, 1668: 5 }, # Odyssey Elite { - 57L: 3, 148L: 8, 228L: 5, 229L: 4, 304L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 440L: 8, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 + 57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5 }], # Includes 2017 Touring and 2016 EX-L messaging. CAR.PILOT: [{ - 57L: 3, 145L: 8, 228L: 5, 229L: 4, 308L: 5, 316L: 8, 334L: 8, 339L: 7, 342L: 6, 344L: 8, 379L: 8, 380L: 8, 392L: 6, 399L: 7, 419L: 8, 420L: 8, 422L: 8, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 538L: 3, 542L: 7, 545L: 5, 546L: 3, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 837L: 5, 856L: 7, 871L: 8, 882L: 2, 884L: 7, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1108L: 8, 1125L: 8, 1296L: 8, 1424L: 5, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1616L: 5, 1618L: 5, 1668L: 5 + 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 339: 7, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 837: 5, 856: 7, 871: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1618: 5, 1668: 5 }], CAR.RIDGELINE: [{ - 57L: 3, 145L: 8, 228L: 5, 229L: 4, 308L: 5, 316L: 8, 339L: 7, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 399L: 7, 419L: 8, 420L: 8, 422L: 8, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 464L: 8, 471L: 3, 476L: 4, 490L: 8, 506L: 8, 545L: 5, 546L: 3, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 871L: 8, 882L: 2, 884L: 7, 892L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, 1613L: 5, 1616L: 5, 1618L: 5, 1668L: 5, 2015L: 3 + 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 471: 3, 476: 4, 490: 8, 506: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1613: 5, 1616: 5, 1618: 5, 1668: 5, 2015: 3 }] } @@ -107,4 +107,5 @@ DBC = { CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), } +# TODO: get these from dbc file HONDA_BOSCH = [CAR.ACCORD, CAR.CIVIC_HATCH, CAR.CRV_5G] diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index ffb7907500..db3b8b6a53 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -3,6 +3,7 @@ import zmq from cereal import car from selfdrive.config import Conversions as CV from selfdrive.services import service_list +from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging # mocked car interface to work with chffrplus @@ -17,7 +18,7 @@ class CarInterface(object): self.CP = CP - print "Using Mock Car Interface" + cloudlog.debug("Using Mock Car Interface") context = zmq.Context() # TODO: subscribe to phone sensor diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index d26992ad1f..0170c4c942 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -6,6 +6,7 @@ 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.values import ECU, check_ecu_msgs, CAR +from selfdrive.swaglog import cloudlog try: from selfdrive.car.toyota.carcontroller import CarController @@ -65,8 +66,8 @@ class CarInterface(object): centerToFront_civic = wheelbase_civic * 0.4 centerToRear_civic = wheelbase_civic - centerToFront_civic rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 - tireStiffnessRear_civic = 90000 + tireStiffnessFront_civic = 192150 + tireStiffnessRear_civic = 202500 ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay @@ -74,39 +75,42 @@ class CarInterface(object): if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 - ret.steerRatio = 15.0 + ret.steerRatio = 15.59 # unknown end-to-end spec + tire_stiffness_factor = 0.7933 ret.mass = 3045 * CV.LB_TO_KG + std_cargo ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 - - f = 1.43353663 - tireStiffnessFront_civic *= f - tireStiffnessRear_civic *= f - - # Prius has a very bad actuator + # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis ret.steerActuatorDelay = 0.25 + elif candidate in [CAR.RAV4, CAR.RAV4H]: ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.65 - ret.steerRatio = 14.5 # Rav4 2017 + ret.steerRatio = 16.30 # 14.5 is spec end-to-end + tire_stiffness_factor = 0.5533 ret.mass = 3650 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + elif candidate == CAR.COROLLA: ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 17.8 + tire_stiffness_factor = 0.444 ret.mass = 2860 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid ret.steerKpV, ret.steerKiV = [[0.2], [0.05]] ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 + elif candidate == CAR.LEXUS_RXH: ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.79 - ret.steerRatio = 16. # official specs say 14.8, but it does not seem right + ret.steerRatio = 16. # 14.8 is spec end-to-end + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4481 * CV.LB_TO_KG + std_cargo # mean between min and max ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 ret.longPidDeadzoneBP = [0., 9.] @@ -127,10 +131,10 @@ class CarInterface(object): # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = tireStiffnessFront_civic * \ + ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ ret.mass / mass_civic * \ (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = tireStiffnessRear_civic * \ + ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ ret.mass / mass_civic * \ (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) @@ -149,9 +153,9 @@ class CarInterface(object): ret.enableCamera = not check_ecu_msgs(fingerprint, candidate, ECU.CAM) ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU) ret.enableApgs = False #not check_ecu_msgs(fingerprint, candidate, ECU.APGS) - print "ECU Camera Simulated: ", ret.enableCamera - print "ECU DSU Simulated: ", ret.enableDsu - print "ECU APGS Simulated: ", ret.enableApgs + cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) + cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu) + cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs) ret.steerLimitAlert = False ret.stoppingControl = False diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 1568ff5fa8..9af6aed47b 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -8,7 +8,7 @@ from selfdrive.services import service_list import selfdrive.messaging as messaging -RADAR_MSGS = range(0x210, 0x220) +RADAR_MSGS = list(range(0x210, 0x220)) def _create_radard_can_parser(): dbc_f = 'toyota_prius_2017_adas.dbc' @@ -90,4 +90,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") - print ret + print(ret) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 4749ea247c..649146073f 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -71,21 +71,21 @@ def check_ecu_msgs(fingerprint, candidate, ecu): FINGERPRINTS = { CAR.RAV4: [{ - 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.RAV4H: [{ - 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 296L: 8, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 4, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 713L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1228L: 8, 1232L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.PRIUS: [{ - 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, # Prius Prime { - 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, # Taiwanese Prius Prime { - 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 845L: 5, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1264L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.COROLLA: [{ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c index 2367d8d98c..eb59db965f 100644 --- a/selfdrive/common/touch.c +++ b/selfdrive/common/touch.c @@ -48,7 +48,7 @@ void touch_init(TouchState *s) { assert(s->fd >= 0); } -int touch_poll(TouchState *s, int* out_x, int* out_y) { +int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) { assert(out_x && out_y); bool up = false; while (true) { @@ -56,7 +56,7 @@ int touch_poll(TouchState *s, int* out_x, int* out_y) { .fd = s->fd, .events = POLLIN, }}; - int err = poll(polls, 1, 0); + int err = poll(polls, 1, timeout); if (err < 0) { return -1; } diff --git a/selfdrive/common/touch.h b/selfdrive/common/touch.h index 3767c06dc3..c2bb6dfece 100644 --- a/selfdrive/common/touch.h +++ b/selfdrive/common/touch.h @@ -11,7 +11,7 @@ typedef struct TouchState { } TouchState; void touch_init(TouchState *s); -int touch_poll(TouchState *s, int *out_x, int *out_y); +int touch_poll(TouchState *s, int *out_x, int *out_y, int timeout); #ifdef __cplusplus } diff --git a/selfdrive/common/util.c b/selfdrive/common/util.c index 172345869e..6f8f0a9a6c 100644 --- a/selfdrive/common/util.c +++ b/selfdrive/common/util.c @@ -2,9 +2,12 @@ #include #include #include +#include #ifdef __linux__ #include +#include +#include #endif void* read_file(const char* path, size_t* out_len) { @@ -39,3 +42,17 @@ void set_thread_name(const char* name) { prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0); #endif } + +int set_realtime_priority(int level) { +#ifdef __linux__ + + long tid = syscall(SYS_gettid); + + // should match python using chrt + struct sched_param sa; + memset(&sa, 0, sizeof(sa)); + sa.sched_priority = level; + return sched_setscheduler(tid, SCHED_FIFO, &sa); +#endif +} + diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index ce18068bbe..65dde16e24 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -24,6 +24,8 @@ #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) +#define ALIGN(x, align) (((x) + (align)-1) & ~((align)-1)) + #ifdef __cplusplus extern "C" { #endif @@ -36,6 +38,8 @@ void* read_file(const char* path, size_t* out_len); void set_thread_name(const char* name); +int set_realtime_priority(int level); + #ifdef __cplusplus } #endif diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 8fcd679d60..2b226fc6e0 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.4.7.2-release" +#define COMMA_VERSION "0.5-release" diff --git a/selfdrive/common/visionbuf.h b/selfdrive/common/visionbuf.h new file mode 100644 index 0000000000..3457f6dff3 --- /dev/null +++ b/selfdrive/common/visionbuf.h @@ -0,0 +1,39 @@ +#ifndef IONBUF_H +#define IONBUF_H + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct VisionBuf { + size_t len; + void* addr; + int handle; + int fd; + + cl_context ctx; + cl_device_id device_id; + cl_mem buf_cl; + cl_command_queue copy_q; +} VisionBuf; + +#define VISIONBUF_SYNC_FROM_DEVICE 0 +#define VISIONBUF_SYNC_TO_DEVICE 1 + +VisionBuf visionbuf_allocate(size_t len); +VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem); +cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx); +void visionbuf_sync(const VisionBuf* buf, int dir); +void visionbuf_free(const VisionBuf* buf); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/selfdrive/common/visionbuf_ion.c b/selfdrive/common/visionbuf_ion.c new file mode 100644 index 0000000000..724e75e9b0 --- /dev/null +++ b/selfdrive/common/visionbuf_ion.c @@ -0,0 +1,141 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "visionbuf.h" + + +// just hard-code these for convenience +// size_t device_page_size = 0; +// clGetDeviceInfo(device_id, CL_DEVICE_PAGE_SIZE_QCOM, +// sizeof(device_page_size), &device_page_size, +// NULL); + +// size_t padding_cl = 0; +// clGetDeviceInfo(device_id, CL_DEVICE_EXT_MEM_PADDING_IN_BYTES_QCOM, +// sizeof(padding_cl), &padding_cl, +// NULL); +#define DEVICE_PAGE_SIZE_CL 4096 +#define PADDING_CL 0 + +static int ion_fd = -1; +static void ion_init() { + if (ion_fd == -1) { + ion_fd = open("/dev/ion", O_RDWR | O_NONBLOCK); + } +} + +VisionBuf visionbuf_allocate(size_t len) { + int err; + + ion_init(); + + struct ion_allocation_data ion_alloc = {0}; + ion_alloc.len = len + PADDING_CL; + ion_alloc.align = 4096; + ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID; + ion_alloc.flags = ION_FLAG_CACHED; + + err = ioctl(ion_fd, ION_IOC_ALLOC, &ion_alloc); + assert(err == 0); + + struct ion_fd_data ion_fd_data = {0}; + ion_fd_data.handle = ion_alloc.handle; + err = ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data); + assert(err == 0); + + void *addr = mmap(NULL, ion_alloc.len, + PROT_READ | PROT_WRITE, + MAP_SHARED, ion_fd_data.fd, 0); + assert(addr != MAP_FAILED); + + memset(addr, 0, ion_alloc.len); + + return (VisionBuf){ + .len = len, + .addr = addr, + .handle = ion_alloc.handle, + .fd = ion_fd_data.fd, + }; +} + +VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) { + VisionBuf r = visionbuf_allocate(len); + *out_mem = visionbuf_to_cl(&r, device_id, ctx); + return r; +} + +cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) { + int err = 0; + + assert(((uintptr_t)buf->addr % DEVICE_PAGE_SIZE_CL) == 0); + + cl_mem_ion_host_ptr ion_cl = {0}; + ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM; + ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM; + ion_cl.ion_filedesc = buf->fd; + ion_cl.ion_hostptr = buf->addr; + + cl_mem mem = clCreateBuffer(ctx, + CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM, + buf->len, &ion_cl, &err); + assert(err == 0); + + return mem; +} + +void visionbuf_sync(const VisionBuf* buf, int dir) { + int err; + + struct ion_fd_data fd_data = {0}; + fd_data.fd = buf->fd; + err = ioctl(ion_fd, ION_IOC_IMPORT, &fd_data); + assert(err == 0); + + struct ion_flush_data flush_data = {0}; + flush_data.handle = fd_data.handle; + flush_data.vaddr = buf->addr; + flush_data.offset = 0; + flush_data.length = buf->len; + + // ION_IOC_INV_CACHES ~= DMA_FROM_DEVICE + // ION_IOC_CLEAN_CACHES ~= DMA_TO_DEVICE + // ION_IOC_CLEAN_INV_CACHES ~= DMA_BIDIRECTIONAL + + struct ion_custom_data custom_data = {0}; + + switch (dir) { + case VISIONBUF_SYNC_FROM_DEVICE: + custom_data.cmd = ION_IOC_INV_CACHES; + break; + case VISIONBUF_SYNC_TO_DEVICE: + custom_data.cmd = ION_IOC_CLEAN_CACHES; + break; + default: + assert(0); + } + + custom_data.arg = (unsigned long)&flush_data; + err = ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data); + assert(err == 0); + + struct ion_handle_data handle_data = {0}; + handle_data.handle = fd_data.handle; + err = ioctl(ion_fd, ION_IOC_FREE, &handle_data); + assert(err == 0); +} + +void visionbuf_free(const VisionBuf* buf) { + struct ion_handle_data handle_data = { + .handle = buf->handle, + }; + int ret = ioctl(ion_fd, ION_IOC_FREE, &handle_data); + assert(ret == 0); +} diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc new file mode 100644 index 0000000000..45bc7bfc4a --- /dev/null +++ b/selfdrive/common/visionimg.cc @@ -0,0 +1,111 @@ +#include + +#ifdef QCOM +#include +#include +#include +#include + +#include +#define GL_GLEXT_PROTOTYPES +#include + +#include +#define EGL_EGLEXT_PROTOTYPES +#include + +#endif + +#include "common/util.h" +#include "common/visionbuf.h" + +#include "common/visionimg.h" + +#ifdef QCOM + +using namespace android; + +// from libadreno_utils.so +extern "C" void compute_aligned_width_and_height(int width, + int height, + int bpp, + int tile_mode, + int raster_mode, + int padding_threshold, + int *aligned_w, + int *aligned_h); +#endif + +VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) { + + int aligned_w = 0, aligned_h = 0; +#ifdef QCOM + compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, &aligned_w, &aligned_h); +#else + aligned_w = width; aligned_h = height; +#endif + + int stride = aligned_w * 3; + size_t size = aligned_w * aligned_h * 3; + + VisionBuf buf = visionbuf_allocate(size); + + *out_buf = buf; + + return (VisionImg){ + .fd = buf.fd, + .format = VISIONIMG_FORMAT_RGB24, + .width = width, + .height = height, + .stride = stride, + .size = size, + .bpp = 3, + }; +} + +#ifdef QCOM + +EGLClientBuffer visionimg_to_egl(const VisionImg *img) { + assert((img->size % img->stride) == 0); + assert((img->stride % img->bpp) == 0); + + int format = 0; + if (img->format == VISIONIMG_FORMAT_RGB24) { + format = HAL_PIXEL_FORMAT_RGB_888; + } else { + assert(false); + } + + private_handle_t* hnd = new private_handle_t(img->fd, img->size, + private_handle_t::PRIV_FLAGS_USES_ION|private_handle_t::PRIV_FLAGS_FRAMEBUFFER, + 0, format, + img->stride/img->bpp, img->size/img->stride, + img->width, img->height); + + GraphicBuffer* gb = new GraphicBuffer(img->width, img->height, (PixelFormat)format, + GraphicBuffer::USAGE_HW_TEXTURE, img->stride/img->bpp, hnd, false); + + return (EGLClientBuffer) gb->getNativeBuffer(); +} + +GLuint visionimg_to_gl(const VisionImg *img) { + + EGLClientBuffer buf = visionimg_to_egl(img); + + EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); + assert(display != EGL_NO_DISPLAY); + + EGLint img_attrs[] = { EGL_IMAGE_PRESERVED_KHR, EGL_TRUE, EGL_NONE }; + EGLImageKHR image = eglCreateImageKHR(display, EGL_NO_CONTEXT, + EGL_NATIVE_BUFFER_ANDROID, buf, img_attrs); + assert(image != EGL_NO_IMAGE_KHR); + + GLuint tex = 0; + glGenTextures(1, &tex); + glBindTexture(GL_TEXTURE_2D, tex); + glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, image); + + return tex; +} + +#endif diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h new file mode 100644 index 0000000000..1a98b55bc7 --- /dev/null +++ b/selfdrive/common/visionimg.h @@ -0,0 +1,37 @@ +#ifndef VISIONIMG_H +#define VISIONIMG_H + +#ifdef QCOM +#include +#include +#include +#endif + +#include "common/visionbuf.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define VISIONIMG_FORMAT_RGB24 1 + +typedef struct VisionImg { + int fd; + int format; + int width, height, stride; + int bpp; + size_t size; +} VisionImg; + +VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf); + +#ifdef QCOM +EGLClientBuffer visionimg_to_egl(const VisionImg *img); +GLuint visionimg_to_gl(const VisionImg *img); +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h index 8bd0dd152f..4844a71b1d 100644 --- a/selfdrive/common/visionipc.h +++ b/selfdrive/common/visionipc.h @@ -21,8 +21,8 @@ typedef enum VisionIPCPacketType { } VisionIPCPacketType; typedef enum VisionStreamType { - VISION_STREAM_UI_BACK, - VISION_STREAM_UI_FRONT, + VISION_STREAM_RGB_BACK, + VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV, VISION_STREAM_YUV_FRONT, VISION_STREAM_MAX, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bcad96b02c..1b8ad21282 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,6 +1,6 @@ #!/usr/bin/env python +import gc import json -from copy import copy import zmq from cereal import car, log from common.numpy_fast import clip @@ -17,18 +17,18 @@ from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \ create_event, \ EventTypes as ET, \ update_v_cruise, \ - initialize_v_cruise + initialize_v_cruise, \ + kill_defaultd from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.controls.lib.driver_monitor import DriverStatus - -AWARENESS_TIME = 360. # 6 minutes limit without user touching steering wheels -AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car - +ThermalStatus = log.ThermalData.ThermalStatus State = log.Live100Data.ControlState + class Calibration: UNCALIBRATED = 0 CALIBRATED = 1 @@ -45,15 +45,20 @@ def isEnabled(state): return (isActive(state) or state == State.preEnabled) -def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overtemp, free_space): +def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_location, + poller, cal_status, overtemp, free_space, driver_status, geofence, + state, mismatch_counter, params): # *** read can and compute car states *** CS = CI.update(CC) events = list(CS.events) + enabled = isEnabled(state) td = None cal = None hh = None + dm = None + gps = None for socket, event in poller.poll(0): if socket is thermal: @@ -62,16 +67,15 @@ def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overte cal = messaging.recv_one(socket) elif socket is health: hh = messaging.recv_one(socket) + elif socket is driver_monitor: + dm = messaging.recv_one(socket) + elif socket is gps_location: + gps = messaging.recv_one(socket) # *** thermal checking logic *** # thermal data, checked every second if td is not None: - # CPU overtemp above 95 deg - overtemp_proc = any(t > 950 for t in - (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, - td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) - overtemp_bat = td.thermal.bat > 60000 # 60c - overtemp = overtemp_proc or overtemp_bat + overtemp = td.thermal.thermalStatus >= ThermalStatus.red # under 15% of space free no enable allowed free_space = td.thermal.freeSpace < 0.15 @@ -92,18 +96,34 @@ def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overte else: events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not enabled: + mismatch_counter = 0 + # *** health checking logic *** if hh is not None: controls_allowed = hh.health.controlsAllowed - if not controls_allowed: + if not controls_allowed and enabled: + mismatch_counter += 1 + + if mismatch_counter >= 2: events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) - return CS, events, cal_status, overtemp, free_space + if dm is not None: + driver_status.get_pose(dm.driverMonitoring, params) + if geofence is not None and gps is not None: + geofence.update_geofence_status(gps.gpsLocationExternal, params) -def calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, awareness_status): + if geofence is not None and not geofence.in_geofence: + events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING])) + + return CS, events, cal_status, overtemp, free_space, mismatch_counter + + +def calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence): # plan runs always, independently of the state - plan_packet = PL.update(CS, LaC, LoC, v_cruise_kph, awareness_status < -0.) + 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 @@ -127,6 +147,8 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM # if stock cruise is completely disabled, then we can use our own set speed logic if not CP.enableCruise: v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled) + elif CP.enableCruise and CS.cruiseState.enabled: + v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # decrease the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -147,7 +169,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM else: state = State.enabled AM.add("enable", enabled) - v_cruise_kph = initialize_v_cruise(CS.vEgo) + v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last) # ENABLED elif state == State.enabled: @@ -202,8 +224,7 @@ 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, - awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, - rear_view_toggle, passive): + driver_status, PL, LaC, LoC, VM, angle_offset, passive): # Given the state, this function returns the actuators # reset actuators to zero @@ -212,14 +233,13 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, enabled = isEnabled(state) active = isActive(state) - for b in CS.buttonEvents: - # button presses for rear view - if b.type == "leftBlinker" or b.type == "rightBlinker": - rear_view_toggle = b.pressed and rear_view_allowed - - if (b.type == "altButton1" and b.pressed) and not passive: - rear_view_toggle = not rear_view_toggle + # check if user has interacted with the car + driver_engaged = len(CS.buttonEvents) > 0 or \ + v_cruise_kph != v_cruise_kph_last or \ + CS.steeringPressed + # add eventual driver distracted events + events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) # send FCW alert if triggered by planner if plan.fcw: @@ -236,14 +256,6 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, # ENABLED or SOFT_DISABLING elif state in [State.enabled, State.softDisabling]: - # decrease awareness status - awareness_status -= 0.01/(AWARENESS_TIME) - if awareness_status <= 0.: - AM.add("driverDistracted", enabled) - elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ - awareness_status >= (AWARENESS_PRE_TIME - 4.) / AWARENESS_TIME: - AM.add("preDriverDistracted", enabled) - # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): AM.add(e, enabled) @@ -265,16 +277,6 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) - if CP.enableCruise and CS.cruiseState.enabled: - v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH - - # reset conditions for the 6 minutes timout - if CS.buttonEvents or \ - v_cruise_kph != v_cruise_kph_last or \ - CS.steeringPressed or \ - state in [State.preEnabled, State.disabled]: - awareness_status = 1. - # send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: AM.add("steerSaturated", enabled) @@ -287,11 +289,11 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM.process_alerts(sec_since_boot()) - return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle + return actuators, v_cruise_kph, driver_status, angle_offset def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, - carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status, + carcontrol, live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive): # ***** control the car ***** @@ -328,74 +330,54 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_ dat = messaging.new_message() dat.init('live100') - # show rear view camera on phone if in reverse gear or when button is pressed - dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle - dat.live100.alertText1 = AM.alert_text_1 - dat.live100.alertText2 = AM.alert_text_2 - dat.live100.alertSize = AM.alert_size - dat.live100.alertStatus = AM.alert_status - dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0 - - # what packets were used to process - dat.live100.canMonoTimes = list(CS.canMonoTimes) - dat.live100.planMonoTime = plan_ts - - # if controls is enabled - dat.live100.enabled = isEnabled(state) - dat.live100.active = isActive(state) - - # car state - dat.live100.vEgo = CS.vEgo - dat.live100.vEgoRaw = CS.vEgoRaw - dat.live100.angleSteers = CS.steeringAngle - dat.live100.curvature = VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo) - dat.live100.steerOverride = CS.steeringPressed - - # high level control state - dat.live100.state = state - - # longitudinal control state - dat.live100.longControlState = LoC.long_control_state - dat.live100.vPid = float(LoC.v_pid) - dat.live100.vCruise = float(v_cruise_kph) - dat.live100.upAccelCmd = float(LoC.pid.p) - dat.live100.uiAccelCmd = float(LoC.pid.i) - dat.live100.ufAccelCmd = float(LoC.pid.f) - - # lateral control state - dat.live100.angleSteersDes = float(LaC.angle_steers_des) - dat.live100.upSteer = float(LaC.pid.p) - dat.live100.uiSteer = float(LaC.pid.i) - dat.live100.ufSteer = float(LaC.pid.f) - - # processed radar state, should add a_pcm? - dat.live100.vTargetLead = float(plan.vTarget) - dat.live100.aTarget = float(plan.aTarget) - dat.live100.jerkFactor = float(plan.jerkFactor) - - # log learned angle offset - dat.live100.angleOffset = float(angle_offset) - - # Save GPS planner status - dat.live100.gpsPlannerActive = plan.gpsPlannerActive - - # lag - dat.live100.cumLagMs = -rk.remaining*1000. - + dat.live100 = { + "alertText1": AM.alert_text_1, + "alertText2": AM.alert_text_2, + "alertSize": AM.alert_size, + "alertStatus": AM.alert_status, + "alertBlinkingRate": AM.alert_rate, + "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0, + "canMonoTimes": list(CS.canMonoTimes), + "planMonoTime": plan_ts, + "enabled": isEnabled(state), + "active": isActive(state), + "vEgo": CS.vEgo, + "vEgoRaw": CS.vEgoRaw, + "angleSteers": CS.steeringAngle, + "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo), + "steerOverride": CS.steeringPressed, + "state": state, + "engageable": not bool(get_events(events, [ET.NO_ENTRY])), + "longControlState": LoC.long_control_state, + "vPid": float(LoC.v_pid), + "vCruise": float(v_cruise_kph), + "upAccelCmd": float(LoC.pid.p), + "uiAccelCmd": float(LoC.pid.i), + "ufAccelCmd": float(LoC.pid.f), + "angleSteersDes": float(LaC.angle_steers_des), + "upSteer": float(LaC.pid.p), + "uiSteer": float(LaC.pid.i), + "ufSteer": float(LaC.pid.f), + "vTargetLead": float(plan.vTarget), + "aTarget": float(plan.aTarget), + "jerkFactor": float(plan.jerkFactor), + "angleOffset": float(angle_offset), + "gpsPlannerActive": plan.gpsPlannerActive, + "cumLagMs": -rk.remaining*1000., + } live100.send(dat.to_bytes()) # broadcast carState cs_send = messaging.new_message() cs_send.init('carState') - # TODO: override CS.events with all the cumulated events - cs_send.carState = copy(CS) + cs_send.carState = CS cs_send.carState.events = events carstate.send(cs_send.to_bytes()) # broadcast carControl cc_send = messaging.new_message() cc_send.init('carControl') - cc_send.carControl = copy(CC) + cc_send.carControl = CC carcontrol.send(cc_send.to_bytes()) # publish mpc state at 20Hz @@ -413,11 +395,12 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_ def controlsd_thread(gctx=None, rate=100, default_bias=0.): + gc.disable() + # start the loop set_realtime_priority(3) context = zmq.Context() - params = Params() # pub @@ -428,7 +411,12 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): passive = params.get("Passive") != "0" if not passive: - sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + while 1: + try: + sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + break + except zmq.error.ZMQError: + kill_defaultd() else: sendcan = None @@ -437,6 +425,8 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): 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) @@ -456,12 +446,21 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): CP.safetyModel = car.CarParams.SafetyModels.noOutput fcw_enabled = params.get("IsFcwEnabled") == "1" + driver_monitor_on = params.get("IsDriverMonitoringEnabled") == "1" + geofence = None + try: + from selfdrive.controls.lib.geofence import Geofence + geofence = Geofence(params.get("IsGeofenceEnabled") == "1") + except ImportError: + # geofence not available + params.put("IsGeofenceEnabled", "-1") PL = Planner(CP, fcw_enabled) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(VM) AM = AlertManager() + driver_status = DriverStatus(driver_monitor_on) if not passive: AM.add("startup", False) @@ -472,15 +471,11 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 + v_cruise_kph_last = 0 overtemp = False free_space = False cal_status = Calibration.UNCALIBRATED - rear_view_toggle = False - rear_view_allowed = params.get("IsRearViewMirror") == "1" - - # 0.0 - 1.0 - awareness_status = 1. - v_cruise_kph_last = 0 + mismatch_counter = 0 rk = Ratekeeper(rate, print_delay_threshold=2./1000) @@ -501,30 +496,28 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): prof.checkpoint("Ratekeeper", ignore=True) # sample data and compute car events - CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status, - overtemp, free_space) + CS, events, cal_status, overtemp, free_space, mismatch_counter = data_sample(CI, CC, thermal, cal, health, + driver_monitor, gps_location, poller, cal_status, overtemp, free_space, driver_status, geofence, state, mismatch_counter, params) prof.checkpoint("Sample") # define plan - plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, awareness_status) + plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence) prof.checkpoint("Plan") if not passive: # update control state - state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer, - v_cruise_kph, AM) + state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ + state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # compute actuators - actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph, - v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM, - angle_offset, rear_view_allowed, rear_view_toggle, passive) + 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) prof.checkpoint("State Control") # publish data - CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, - rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed, - rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive) + CC = data_send(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 *** diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 319e717cd4..9d053c211f 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -6,6 +6,7 @@ import copy # Priority class Priority: + HIGHEST = 4 HIGH = 3 MID = 2 LOW = 1 @@ -25,7 +26,8 @@ class Alert(object): audible_alert, duration_sound, duration_hud_alert, - duration_text): + duration_text, + alert_rate=0.): self.alert_text_1 = alert_text_1 self.alert_text_2 = alert_text_2 @@ -40,6 +42,7 @@ class Alert(object): 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() @@ -74,7 +77,7 @@ class AlertManager(object): "Brake!", "Risk of Collision", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), + Priority.HIGHEST, "fcw", "chimeRepeated", 1., 2., 2.), "steerSaturated": Alert( "TAKE CONTROL", @@ -95,16 +98,28 @@ class AlertManager(object): Priority.LOW, None, None, .2, .2, .2), "preDriverDistracted": Alert( + "TAKE CONTROL: User Appears Distracted", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, "steerRequired", None, 0., .1, .1, alert_rate=0.75), + + "promptDriverDistracted": Alert( "TAKE CONTROL", "User Appears Distracted", AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, "steerRequired", None, 0., .1, .1), + Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), "driverDistracted": Alert( - "TAKE CONTROL TO REGAIN SPEED", - "User Appears Distracted", + "DISENGAGEMENT REQUIRED", + "User Was Distracted", AlertStatus.critical, AlertSize.full, - Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), + Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1), + + "geofence": Alert( + "DISENGAGEMENT REQUIRED", + "Not in Geofenced Area", + AlertStatus.userPrompt, AlertSize.mid, + Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1), "startup": Alert( "Be ready to take over at any time", @@ -116,7 +131,7 @@ class AlertManager(object): "TAKE CONTROL IMMEDIATELY", "Ethical Dilemma Detected", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 3.), "steerTempUnavailableNoEntry": Alert( "openpilot Unavailable", @@ -239,73 +254,73 @@ class AlertManager(object): "TAKE CONTROL IMMEDIATELY", "Radar Error: Restart the Car", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "radarFault": Alert( "TAKE CONTROL IMMEDIATELY", "Radar Error: Restart the Car", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "modelCommIssue": Alert( "TAKE CONTROL IMMEDIATELY", "Model Error: Check Internet Connection", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsFailed": Alert( "TAKE CONTROL IMMEDIATELY", "Controls Failed", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsMismatch": Alert( "TAKE CONTROL IMMEDIATELY", "Controls Mismatch", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "commIssue": Alert( "TAKE CONTROL IMMEDIATELY", "CAN Error: Check Connections", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "steerUnavailable": Alert( "TAKE CONTROL IMMEDIATELY", "Steer Fault: Restart the Car", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "brakeUnavailable": Alert( "TAKE CONTROL IMMEDIATELY", "Brake Fault: Restart the Car", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "gasUnavailable": Alert( "TAKE CONTROL IMMEDIATELY", "Gas Fault: Restart the Car", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "reverseGear": Alert( "TAKE CONTROL IMMEDIATELY", "Reverse Gear", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "cruiseDisabled": Alert( "TAKE CONTROL IMMEDIATELY", "Cruise Is Off", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), "plannerError": Alert( "TAKE CONTROL IMMEDIATELY", "Planner Solution Error", AlertStatus.critical, AlertSize.full, - Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), + Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.), # not loud cancellations (user is in control) "noTarget": Alert( @@ -363,6 +378,12 @@ class AlertManager(object): AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), + "geofenceNoEntry": Alert( + "openpilot Unavailable", + "Not in Geofenced Area", + AlertStatus.normal, AlertSize.mid, + Priority.MID, None, "chimeDouble", .4, 2., 3.), + "radarCommIssueNoEntry": Alert( "openpilot Unavailable", "Radar Error: Restart the Car", @@ -494,6 +515,7 @@ class AlertManager(object): self.alert_size = AlertSize.none self.visual_alert = "none" self.audible_alert = "none" + self.alert_rate = 0. if ca: if ca.start_time + ca.duration_sound > cur_time: @@ -507,3 +529,4 @@ class AlertManager(object): 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 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 37a8adff04..10d3eee73d 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,3 +1,5 @@ +import os +import signal from cereal import car from common.numpy_fast import clip from selfdrive.config import Conversions as CV @@ -88,5 +90,19 @@ def update_v_cruise(v_cruise_kph, buttonEvents, enabled): return v_cruise_kph -def initialize_v_cruise(v_ego): +def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): + for b in buttonEvents: + # 300kph or above probably means we never had a set speed + if b.type == "accelCruise" and v_cruise_last < 300: + return v_cruise_last + return int(round(max(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) + + +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/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py new file mode 100644 index 0000000000..a593f1091d --- /dev/null +++ b/selfdrive/controls/lib/driver_monitor.py @@ -0,0 +1,121 @@ +import numpy as np +from common.realtime import sec_since_boot +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET + +_DT = 0.01 # update runs at 100Hz +_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status +_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration +_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car +_DISTRACTED_TIME = 6. +_DISTRACTED_PRE_TIME = 4. +_DISTRACTED_PROMPT_TIME = 2. +# measured 1 rad in x FOV. 1152x864 is original image, 160x320 is a right crop for model +_CAMERA_FOV_X = 1. # rad +_CAMERA_FOV_Y = 0.75 # 4/3 aspect ratio +# model output refers to center of cropped image, so need to apply the x displacement offset +_CAMERA_OFFSET_X = 0.3125 #(1152/2 - 0.5*(160*864/320))/1152 +_CAMERA_X_CONV = 0.375 # 160*864/320/1152 +_PITCH_WEIGHT = 1.5 # pitch matters a lot more +_METRIC_THRESHOLD = 0.4 +_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch +_DTM = 0.2 # driver monitor runs at 5Hz +_DISTRACTED_FILTER_F = 0.3 # 0.3Hz +_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM) +_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up + + +class _DriverPose(): + def __init__(self): + self.yaw = 0. + self.pitch = 0. + self.roll = 0. + self.yaw_offset = 0. + self.pitch_offset = 0. + +class DriverStatus(): + def __init__(self, monitor_on): + self.pose = _DriverPose() + self.monitor_on = monitor_on + self.awareness = 1. + self.driver_distracted = False + self.driver_distraction_level = 0. + self.ts_last_check = 0. + self._set_timers() + + def _set_timers(self): + if self.monitor_on: + self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME + self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME + self.step_change = _DT / _DISTRACTED_TIME + else: + self.threshold_pre = _AWARENESS_PRE_TIME / _AWARENESS_TIME + self.threshold_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME + self.step_change = _DT / _AWARENESS_TIME + + def _is_driver_distracted(self, pose): + # to be tuned and to learn the driver's normal pose + yaw_error = pose.yaw - pose.yaw_offset + pitch_error = pose.pitch - pose.pitch_offset - _PITCH_NATURAL_OFFSET + # add positive pitch allowance + if pitch_error > 0.: + pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.) + pitch_error *= _PITCH_WEIGHT + metric = np.sqrt(yaw_error**2 + pitch_error**2) + #print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric + return 1 if metric > _METRIC_THRESHOLD else 0 + + def get_pose(self, driver_monitoring, params): + ts = sec_since_boot() + + # don's check for param too often as it's a kernel call + if ts - self.ts_last_check > 1.: + self.monitor_on = params.get("IsDriverMonitoringEnabled") == "1" + self._set_timers() + self.ts_last_check = ts + + self.pose.pitch = driver_monitoring.descriptor[0] + self.pose.yaw = driver_monitoring.descriptor[1] + self.pose.roll = driver_monitoring.descriptor[2] + self.pose.yaw_offset = (driver_monitoring.descriptor[3] * _CAMERA_X_CONV + _CAMERA_OFFSET_X) * _CAMERA_FOV_X + self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y # positive y is down + self.driver_distracted = self._is_driver_distracted(self.pose) + # first order filter + self.driver_distraction_level = (1. - _DISTRACTED_FILTER_K) * self.driver_distraction_level + \ + _DISTRACTED_FILTER_K * self.driver_distracted + + def update(self, events, driver_engaged, ctrl_active, standstill): + + driver_engaged |= (self.driver_distraction_level < 0.37 and self.monitor_on) + + if (driver_engaged and self.awareness > 0.) or not ctrl_active: + # always reset if driver is in control (unless we are in red alert state) or op isn't active + self.awareness = 1. + + if (not self.monitor_on or (self.driver_distraction_level > 0.63 and self.driver_distracted)) and \ + not (standstill and self.awareness - self.step_change <= self.threshold_prompt): + self.awareness = max(self.awareness - self.step_change, -0.1) + + if self.awareness <= 0.: + # terminal red alert: disengagement required + events.append(create_event('driverDistracted', [ET.WARNING])) + elif self.awareness <= self.threshold_prompt: + # prompt orange alert + events.append(create_event('promptDriverDistracted', [ET.WARNING])) + elif self.awareness <= self.threshold_pre: + # pre green alert + events.append(create_event('preDriverDistracted', [ET.WARNING])) + + return events + + +if __name__ == "__main__": + ds = DriverStatus(True) + ds.driver_distraction_level = 1. + ds.driver_distracted = 1 + for i in range(1000): + ds.update([], False, True, True) + print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level) + ds.update([], True, True, False) + print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level) + + diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 101b8368fa..e1fd68b90d 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -28,12 +28,11 @@ class LatControl(object): (VM.CP.steerKiBP, VM.CP.steerKiV), k_f=VM.CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 - self.setup_mpc() + self.setup_mpc(VM.CP.steerRateCost) - def setup_mpc(self): + def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, - MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE) + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -91,7 +90,7 @@ class LatControl(object): self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if self.mpc_nans: - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE) + 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 if t > self.last_cloudlog_t + 5.0: @@ -113,7 +112,9 @@ class LatControl(object): steer_feedforward = self.angle_steers_des # feedforward desired angle if VM.CP.steerControlType == car.CarParams.SteerControlType.torque: steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) - output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego) + deadzone = 0.0 + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, + feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index ab3d8d3db4..178b5e6dbb 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,7 +1,7 @@ from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv -CAMERA_OFFSET = 0.12 # ~12cm from center car to camera +CAMERA_OFFSET = 0.06 # m from center car to camera class PathPlanner(object): def __init__(self): diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 5beec543e7..11d1d847db 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -22,7 +22,6 @@ _DT = 0.01 # 100Hz _DT_MPC = 0.2 # 5Hz MAX_SPEED_ERROR = 2.0 AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted -_DEBUG = False _LEAD_ACCEL_TAU = 1.5 GPS_PLANNER_ADDR = "192.168.5.1" @@ -315,10 +314,11 @@ class Planner(object): slowest = min(solutions, key=solutions.get) - if _DEBUG: - print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol - print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise - print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise + """ + print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol + print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise + print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise + """ self.longitudinalPlanSource = slowest @@ -336,7 +336,7 @@ class Planner(object): self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) # this runs whenever we get a packet that can change the plan - def update(self, CS, LaC, LoC, v_cruise_kph, user_distracted): + def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS @@ -396,8 +396,9 @@ class Planner(object): # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) - if user_distracted: - # if user is not responsive to awareness alerts, then start a smooth deceleration + + if force_slow_decel: + # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 55a6b5b924..b5ee51e485 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -25,12 +25,12 @@ v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary v_ego_stationary = 4. # no stationary object flag below this speed # Lead Kalman Filter params -_VLEAD_A = np.matrix([[1.0, ts], [0.0, 1.0]]) -_VLEAD_C = np.matrix([1.0, 0.0]) +_VLEAD_A = [[1.0, ts], [0.0, 1.0]] +_VLEAD_C = [[1.0, 0.0]] #_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]]) #_VLEAD_R = 1e3 #_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]]) -_VLEAD_K = np.matrix([[ 0.1988689 ], [ 0.28555364]]) +_VLEAD_K = [[ 0.1988689 ], [ 0.28555364]] class Track(object): @@ -65,7 +65,7 @@ class Track(object): self.vision = False self.aRel = 0. # nidec gives no information about this self.vLat = 0. - self.kf = KF1D(np.matrix([[self.vLead], [0.0]]), _VLEAD_A, _VLEAD_C, _VLEAD_K) + self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K) else: # estimate acceleration # TODO: use Kalman filter @@ -82,8 +82,8 @@ class Track(object): self.cnt += 1 - self.vLeadK = float(self.kf.x[SPEED]) - self.aLeadK = float(self.kf.x[ACCEL]) + self.vLeadK = float(self.kf.x[SPEED][0]) + self.aLeadK = float(self.kf.x[ACCEL][0]) if self.stationary: # stationary objects can become non stationary, but not the other way around @@ -200,18 +200,20 @@ class Cluster(object): def oncoming(self): return all([t.oncoming for t in self.tracks]) - def toLive20(self, lead): - lead.dRel = float(self.dRel) - RDR_TO_LDR - lead.yRel = float(self.yRel) - lead.vRel = float(self.vRel) - lead.aRel = float(self.aRel) - lead.vLead = float(self.vLead) - lead.dPath = float(self.dPath) - lead.vLat = float(self.vLat) - lead.vLeadK = float(self.vLeadK) - lead.aLeadK = float(self.aLeadK) - lead.status = True - lead.fcw = self.is_potential_fcw() + def toLive20(self): + return { + "dRel": float(self.dRel) - RDR_TO_LDR, + "yRel": float(self.yRel), + "vRel": float(self.vRel), + "aRel": float(self.aRel), + "vLead": float(self.vLead), + "dPath": float(self.dPath), + "vLat": float(self.vLat), + "vLeadK": float(self.vLeadK), + "aLeadK": float(self.aLeadK), + "status": True, + "fcw": self.is_potential_fcw(), + } def __str__(self): ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index d4a954c9d7..df3bb0a967 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -97,8 +97,9 @@ if __name__ == '__main__': # load car params #CP = CarInterface.get_params("TOYOTA PRIUS 2017", {}) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {}) - print CP + #print CP VM = VehicleModel(CP) - print VM.steady_state_sol(.1, 0.15) - print calc_slip_factor(VM) - print VM.yaw_rate(0.2*np.pi/180, 32.) * 180./np.pi + #print VM.steady_state_sol(.1, 0.15) + #print calc_slip_factor(VM) + #print VM.yaw_rate(3.*np.pi/180, 32.) * 180./np.pi + #print VM.curvature_factor(32) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 00c34557d6..5f69085cf2 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +import gc import zmq import numpy as np import numpy.matlib @@ -44,6 +45,7 @@ class EKFV1D(EKF): # fuses camera and radar data for best lead detection def radard_thread(gctx=None): + gc.disable() set_realtime_priority(2) # wait for stats about the car to come in from controls @@ -195,9 +197,9 @@ def radard_thread(gctx=None): tracks[fused_id].update_vision_fusion() if DEBUG: - print "NEW CYCLE" + print("NEW CYCLE") if VISION_POINT in ar_pts: - print "vision", ar_pts[VISION_POINT] + print("vision", ar_pts[VISION_POINT]) idens = tracks.keys() track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) @@ -223,7 +225,7 @@ def radard_thread(gctx=None): if DEBUG: for i in clusters: - print i + print(i) # *** extract the lead car *** lead_clusters = [c for c in clusters if c.is_potential_lead(v_ego)] @@ -244,9 +246,9 @@ def radard_thread(gctx=None): dat.live20.radarErrors = list(rr.errors) dat.live20.l100MonoTime = last_l100_ts if lead_len > 0: - lead_clusters[0].toLive20(dat.live20.leadOne) + dat.live20.leadOne = lead_clusters[0].toLive20() if lead2_len > 0: - lead2_clusters[0].toLive20(dat.live20.leadTwo) + dat.live20.leadTwo = lead2_clusters[0].toLive20() else: dat.live20.leadTwo.status = False else: @@ -261,20 +263,22 @@ def radard_thread(gctx=None): for cnt, ids in enumerate(tracks.keys()): if DEBUG: - print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ + print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, tracks[ids].dPath, tracks[ids].vLat, tracks[ids].vLead, tracks[ids].vLeadK, tracks[ids].aLeadK, tracks[ids].stationary, - tracks[ids].measured) - dat.liveTracks[cnt].trackId = ids - dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) - dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) - dat.liveTracks[cnt].vRel = float(tracks[ids].vRel) - dat.liveTracks[cnt].aRel = float(tracks[ids].aRel) - dat.liveTracks[cnt].stationary = tracks[ids].stationary - dat.liveTracks[cnt].oncoming = tracks[ids].oncoming + tracks[ids].measured)) + dat.liveTracks[cnt] = { + "trackId": ids, + "dRel": float(tracks[ids].dRel), + "yRel": float(tracks[ids].yRel), + "vRel": float(tracks[ids].vRel), + "aRel": float(tracks[ids].aRel), + "stationary": tracks[ids].stationary, + "oncoming": tracks[ids].oncoming, + } liveTracks.send(dat.to_bytes()) rk.monitor_time() diff --git a/selfdrive/debug/getframes/getframes.py b/selfdrive/debug/getframes/getframes.py index 8ab2811d54..292368d6a9 100755 --- a/selfdrive/debug/getframes/getframes.py +++ b/selfdrive/debug/getframes/getframes.py @@ -14,9 +14,10 @@ ffi = FFI() ffi.cdef(""" typedef enum VisionStreamType { - VISION_STREAM_UI_BACK, - VISION_STREAM_UI_FRONT, + VISION_STREAM_RGB_BACK, + VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV, + VISION_STREAM_YUV_FRONT, VISION_STREAM_MAX, } VisionStreamType; @@ -47,12 +48,15 @@ typedef struct VIPCBuf { } VIPCBuf; typedef struct VIPCBufExtra { - uint32_t frame_id; // only for yuv + // only for yuv + uint32_t frame_id; + uint64_t timestamp_eof; } VIPCBufExtra; typedef struct VisionStream { int ipc_fd; int last_idx; + int last_type; int num_bufs; VisionStreamBufs bufs_info; VIPCBuf *bufs; @@ -68,10 +72,16 @@ void visionstream_destroy(VisionStream *s); clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) -def getframes(): +def getframes(front=False): s = ffi.new("VisionStream*") buf_info = ffi.new("VisionStreamBufs*") - err = clib.visionstream_init(s, clib.VISION_STREAM_UI_BACK, True, buf_info) + + if front: + stream_type = clib.VISION_STREAM_RGB_FRONT + else: + stream_type = clib.VISION_STREAM_RGB_BACK + + err = clib.visionstream_init(s, stream_type, True, buf_info) assert err == 0 w = buf_info.width diff --git a/selfdrive/locationd/locationd_dummy.py b/selfdrive/locationd/locationd_dummy.py deleted file mode 100755 index 2c44932f1e..0000000000 --- a/selfdrive/locationd/locationd_dummy.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python -import zmq -from copy import copy -from selfdrive import messaging -from selfdrive.services import service_list -from cereal import log - -from common.transformations.coordinates import geodetic2ecef - -def main(gctx=None): - context = zmq.Context() - poller = zmq.Poller() - gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, poller) - gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller) - app_sock = messaging.sub_sock(context, service_list['applanixLocation'].port, poller) - loc_sock = messaging.pub_sock(context, service_list['liveLocation'].port) - - last_ext, last_gps, last_app = -1, -1, -1 - # 5 sec - max_gap = 5*1e9 - preferred_type = None - - while 1: - for sock, event in poller.poll(500): - if sock is app_sock: - msg = messaging.recv_one(sock) - last_app = msg.logMonoTime - this_type = 'app' - if sock is gps_sock: - msg = messaging.recv_one(sock) - gps_pkt = msg.gpsLocation - last_gps = msg.logMonoTime - this_type = 'gps' - if sock is gps_ext_sock: - msg = messaging.recv_one(sock) - gps_pkt = msg.gpsLocationExternal - last_ext = msg.logMonoTime - this_type = 'ext' - - last = max(last_gps, last_ext, last_app) - - if last_app > last - max_gap: - new_preferred_type = 'app' - elif last_ext > last - max_gap: - new_preferred_type = 'ext' - else: - new_preferred_type = 'gps' - - if preferred_type != new_preferred_type: - print "switching from %s to %s" % (preferred_type, new_preferred_type) - preferred_type = new_preferred_type - - if this_type == preferred_type: - new_msg = messaging.new_message() - if this_type == 'app': - # straight proxy the applanix - new_msg.init('liveLocation') - new_msg.liveLocation = copy(msg.applanixLocation) - else: - new_msg.logMonoTime = msg.logMonoTime - new_msg.init('liveLocation') - pkt = new_msg.liveLocation - pkt.lat = gps_pkt.latitude - pkt.lon = gps_pkt.longitude - pkt.alt = gps_pkt.altitude - pkt.speed = gps_pkt.speed - pkt.heading = gps_pkt.bearing - pkt.positionECEF = [float(x) for x in geodetic2ecef([pkt.lat, pkt.lon, pkt.alt])] - pkt.source = log.LiveLocationData.SensorSource.dummy - loc_sock.send(new_msg.to_bytes()) - -if __name__ == '__main__': - main() diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index ba34034e28..724f78ac6b 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:fb9ada8972a8a79dcf9af70260b9ac2ada87eea7acf680349a3581e75028587a -size 1622712 +oid sha256:819e9c55b925cdaa7b9f818c09c649926420fb6892f47e4234954961b9d9191c +size 1622728 diff --git a/selfdrive/manager.py b/selfdrive/manager.py index e425304780..5c2d3a4108 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -20,7 +20,7 @@ if __name__ == "__main__": copyfile(os.path.join(BASEDIR, "scripts", "continue.sh"), "/data/data/com.termux/files/continue.sh") # run the updater - print "Starting NEOS updater" + print("Starting NEOS updater") subprocess.check_call(["git", "clean", "-xdf"], cwd=BASEDIR) os.system(os.path.join(BASEDIR, "installer", "updater", "updater")) raise Exception("NEOS outdated") @@ -62,35 +62,29 @@ import subprocess import traceback from multiprocessing import Process -EON = os.path.exists("/EON") -if EON and os.path.exists(os.path.join(BASEDIR, "vpn")): - print "installing vpn" - os.system(os.path.join(BASEDIR, "vpn", "install.sh")) - import zmq from setproctitle import setproctitle #pylint: disable=no-name-in-module from common.params import Params -from common.realtime import sec_since_boot +import cereal +ThermalStatus = cereal.log.ThermalData.ThermalStatus from selfdrive.services import service_list from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging -from selfdrive.thermal import read_thermal from selfdrive.registration import register -from selfdrive.version import version, dirty, training_version +from selfdrive.version import version, dirty import selfdrive.crash as crash from selfdrive.loggerd.config import ROOT - # comment out anything you don't want to run managed_processes = { + "thermald": "selfdrive.thermald", "uploader": "selfdrive.loggerd.uploader", "controlsd": "selfdrive.controls.controlsd", "radard": "selfdrive.controls.radard", "ubloxd": "selfdrive.locationd.ubloxd", - "locationd_dummy": "selfdrive.locationd.locationd_dummy", "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", @@ -102,10 +96,9 @@ managed_processes = { "visiond": ("selfdrive/visiond", ["./visiond"]), "sensord": ("selfdrive/sensord", ["./sensord"]), "gpsd": ("selfdrive/sensord", ["./gpsd"]), - #"orbd": ("selfdrive/orbd", ["./orbd_wrapper.sh"]), "updated": "selfdrive.updated", - #"gpsplanner": "selfdrive.controls.gps_plannerd", } +android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame") running = {} def get_running(): @@ -118,6 +111,7 @@ unkillable_processes = ['visiond'] interrupt_processes = [] persistent_processes = [ + 'thermald', 'logmessaged', 'logcatd', 'tombstoned', @@ -125,7 +119,6 @@ persistent_processes = [ 'ui', 'gpsd', 'ubloxd', - 'locationd_dummy', 'updated', ] @@ -137,12 +130,11 @@ car_started_processes = [ 'visiond', 'proclogd', 'orbd', - # 'gpsplanner, ] def register_managed_process(name, desc, car_started=False): global managed_processes, car_started_processes, persistent_processes - print "registering", name + print("registering %s" % name) managed_processes[name] = desc if car_started: car_started_processes.append(name) @@ -181,7 +173,7 @@ def start_managed_process(name): if name in running or name not in managed_processes: return proc = managed_processes[name] - if isinstance(proc, basestring): + if isinstance(proc, str): cloudlog.info("starting python %s" % proc) running[name] = Process(name=name, target=launcher, args=(proc, gctx)) else: @@ -193,7 +185,7 @@ def start_managed_process(name): def prepare_managed_process(p): proc = managed_processes[p] - if isinstance(proc, basestring): + if isinstance(proc, str): # import this python cloudlog.info("preimporting %s" % proc) importlib.import_module(proc) @@ -238,13 +230,16 @@ def kill_managed_process(name): cloudlog.info("%s is dead with %d" % (name, running[name].exitcode)) del running[name] +def pm_apply_packages(cmd): + for p in android_packages: + system("pm %s %s" % (cmd, p)) + def cleanup_all_processes(signal, frame): cloudlog.info("caught ctrl-c %s %s" % (signal, frame)) - for p in ("com.waze", "com.spotify.music", "ai.comma.plus.offroad", "ai.comma.plus.frame"): - system("am force-stop %s" % p) + pm_apply_packages('disable') - for name in running.keys(): + for name in list(running.keys()): kill_managed_process(name) cloudlog.info("everything is dead") @@ -271,13 +266,13 @@ def manager_init(should_register=True): if not dirty: os.environ['CLEAN'] = '1' - cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, is_eon=EON) + cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, is_eon=True) crash.bind_user(id=dongle_id) - crash.bind_extra(version=version, dirty=dirty, is_eon=EON) + crash.bind_extra(version=version, dirty=dirty, is_eon=True) os.umask(0) try: - os.mkdir(ROOT, 0777) + os.mkdir(ROOT, 0o777) except OSError: pass @@ -288,119 +283,17 @@ def system(cmd): try: cloudlog.info("running %s" % cmd) subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True) - except subprocess.CalledProcessError, e: + except subprocess.CalledProcessError as e: cloudlog.event("running failed", cmd=e.cmd, output=e.output[-1024:], returncode=e.returncode) -LEON = False -def setup_eon_fan(): - global LEON - - if not EON: - return - - os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch") - - from smbus2 import SMBus - bus = SMBus(7, force=True) - try: - bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts - bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable - bus.write_byte_data(0x21, 0x02, 0x2) # needed? - bus.write_byte_data(0x21, 0x04, 0x4) # manual override source - except IOError: - print "LEON detected" - #os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") - LEON = True - bus.close() - -last_eon_fan_val = None -def set_eon_fan(val): - global LEON, last_eon_fan_val - - if not EON: - return - - from smbus2 import SMBus - if last_eon_fan_val is None or last_eon_fan_val != val: - bus = SMBus(7, force=True) - if LEON: - i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val] - bus.write_i2c_block_data(0x3d, 0, [i]) - else: - bus.write_byte_data(0x21, 0x04, 0x2) - bus.write_byte_data(0x21, 0x03, (val*2)+1) - bus.write_byte_data(0x21, 0x04, 0x4) - bus.close() - last_eon_fan_val = val - - -# temp thresholds to control fan speed - high hysteresis -_TEMP_THRS_H = [50., 65., 80., 10000] -# temp thresholds to control fan speed - low hysteresis -_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] -# fan speed options -_FAN_SPEEDS = [0, 16384, 32768, 65535] -# max fan speed only allowed if battery if hot -_BAT_TEMP_THERSHOLD = 45. - -def handle_fan(max_temp, bat_temp, fan_speed): - new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp) - new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp) - - if new_speed_h > fan_speed: - # update speed if using the high thresholds results in fan speed increment - fan_speed = new_speed_h - elif new_speed_l < fan_speed: - # update speed if using the low thresholds results in fan speed decrement - fan_speed = new_speed_l - - if bat_temp < _BAT_TEMP_THERSHOLD: - # no max fan speed unless battery is hot - fan_speed = min(fan_speed, _FAN_SPEEDS[-2]) - - set_eon_fan(fan_speed/16384) - - return fan_speed - -class LocationStarter(object): - def __init__(self): - self.last_good_loc = 0 - def update(self, started_ts, location): - rt = sec_since_boot() - - if location is None or location.accuracy > 50 or location.speed < 2: - # bad location, stop if we havent gotten a location in a while - # dont stop if we're been going for less than a minute - if started_ts: - if rt-self.last_good_loc > 60. and rt-started_ts > 60: - cloudlog.event("location_stop", - ts=rt, - started_ts=started_ts, - last_good_loc=self.last_good_loc, - location=location.to_dict() if location else None) - return False - else: - return True - else: - return False - - self.last_good_loc = rt - - if started_ts: - return True - else: - cloudlog.event("location_start", location=location.to_dict() if location else None) - return location.speed*3.6 > 10 def manager_thread(): # now loop context = zmq.Context() - thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) - health_sock = messaging.sub_sock(context, service_list['health'].port) - location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) + thermal_sock = messaging.sub_sock(context, service_list['thermal'].port) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) @@ -409,156 +302,47 @@ def manager_thread(): start_managed_process(p) # start frame + pm_apply_packages('enable') system("am start -n ai.comma.plus.frame/.MainActivity") - # do this before panda flashing - setup_eon_fan() - if os.getenv("NOBOARD") is None: start_managed_process("pandad") params = Params() - - passive_starter = LocationStarter() - - started_ts = None - off_ts = None logger_dead = False - count = 0 - fan_speed = 0 - ignition_seen = False - started_seen = False - panda_seen = False - - health_sock.RCVTIMEO = 1500 while 1: # get health of board, log this in "thermal" - td = messaging.recv_sock(health_sock, wait=True) - location = messaging.recv_sock(location_sock) - - location = location.gpsLocation if location else None - - print td - - # replace thermald - msg = read_thermal() - - # loggerd is gated based on free space - statvfs = os.statvfs(ROOT) - avail = (statvfs.f_bavail * 1.0)/statvfs.f_blocks - - # thermal message now also includes free space - msg.thermal.freeSpace = avail - with open("/sys/class/power_supply/battery/capacity") as f: - msg.thermal.batteryPercent = int(f.read()) - with open("/sys/class/power_supply/battery/status") as f: - msg.thermal.batteryStatus = f.read().strip() - with open("/sys/class/power_supply/usb/online") as f: - msg.thermal.usbOnline = bool(int(f.read())) - - # TODO: add car battery voltage check - max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, - msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 - bat_temp = msg.thermal.bat/1000. - fan_speed = handle_fan(max_temp, bat_temp, fan_speed) - msg.thermal.fanSpeed = fan_speed - - msg.thermal.started = started_ts is not None - msg.thermal.startedTs = int(1e9*(started_ts or 0)) - - thermal_sock.send(msg.to_bytes()) - print msg + msg = messaging.recv_sock(thermal_sock, wait=True) # uploader is gated based on the phone temperature - if max_temp > 85.0: - cloudlog.warning("over temp: %r", max_temp) + if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") - elif max_temp < 70.0: + else: start_managed_process("uploader") - if avail < 0.05: + if msg.thermal.freeSpace < 0.05: logger_dead = True - # start constellation of processes when the car starts - ignition = td is not None and td.health.started - ignition_seen = ignition_seen or ignition - - # add voltage check for ignition - if not ignition_seen and td is not None and td.health.voltage > 13500: - ignition = True - - do_uninstall = params.get("DoUninstall") == "1" - accepted_terms = params.get("HasAcceptedTerms") == "1" - completed_training = params.get("CompletedTrainingVersion") == training_version - - should_start = ignition - - # have we seen a panda? - panda_seen = panda_seen or td is not None - - # start on gps movement if we haven't seen ignition and are in passive mode - should_start = should_start or (not (ignition_seen and td) # seen ignition and panda is connected - and params.get("Passive") == "1" - and passive_starter.update(started_ts, location)) - - # with 2% left, we killall, otherwise the phone will take a long time to boot - should_start = should_start and avail > 0.02 - - # require usb power - should_start = should_start and msg.thermal.usbOnline - - should_start = should_start and accepted_terms and completed_training and (not do_uninstall) - - # if any CPU gets above 107 or the battery gets above 63, kill all processes - # controls will warn with CPU above 95 or battery above 60 - if max_temp > 107.0 or msg.thermal.bat >= 63000: - # TODO: Add a better warning when this is happening - should_start = False - - if should_start: - off_ts = None - if started_ts is None: - params.car_start() - started_ts = sec_since_boot() - started_seen = True + if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: - started_ts = None - if off_ts is None: - off_ts = sec_since_boot() logger_dead = False for p in car_started_processes: kill_managed_process(p) - # shutdown if the battery gets lower than 3%, t's discharging, we aren't running for - # more than a minute but we were running - if msg.thermal.batteryPercent < 3 and msg.thermal.batteryStatus == "Discharging" and \ - started_seen and (sec_since_boot() - off_ts) > 60: - os.system('LD_LIBRARY_PATH="" svc power shutdown') - # check the status of all processes, did any of them die? for p in running: cloudlog.debug(" running %s %s" % (p, running[p])) - # report to server once per minute - if (count%60) == 0: - cloudlog.event("STATUS_PACKET", - running=running.keys(), - count=count, - health=(td.to_dict() if td else None), - location=(location.to_dict() if location else None), - thermal=msg.to_dict()) - - if do_uninstall: + # is this still needed? + if params.get("DoUninstall") == "1": break - count += 1 - def get_installed_apks(): dat = subprocess.check_output(["pm", "list", "packages", "-f"]).strip().split("\n") ret = {} @@ -578,19 +362,10 @@ def install_apk(path): return ret == 0 def update_apks(): - # patch apks - if os.getenv("PREPAREONLY"): - # assume we have internet, download too - patched = subprocess.call([os.path.join(BASEDIR, "apk/external/patcher.py")]) - else: - patched = subprocess.call([os.path.join(BASEDIR, "apk/external/patcher.py"), "patch"]) - cloudlog.info("patcher: %r" % (patched,)) - # install apks installed = get_installed_apks() - install_apks = (glob.glob(os.path.join(BASEDIR, "apk/*.apk")) - + glob.glob(os.path.join(BASEDIR, "apk/external/out/*.apk"))) + install_apks = glob.glob(os.path.join(BASEDIR, "apk/*.apk")) for apk in install_apks: app = os.path.basename(apk)[:-4] if app not in installed: @@ -601,8 +376,6 @@ def update_apks(): for app in installed.iterkeys(): apk_path = os.path.join(BASEDIR, "apk/"+app+".apk") - if not os.path.exists(apk_path): - apk_path = os.path.join(BASEDIR, "apk/external/out/"+app+".apk") if not os.path.exists(apk_path): continue @@ -624,10 +397,12 @@ def update_apks(): assert success def manager_update(): + if os.path.exists(os.path.join(BASEDIR, "vpn")): + cloudlog.info("installing vpn") + os.system(os.path.join(BASEDIR, "vpn", "install.sh")) update_apks() def manager_prepare(): - # build cereal first subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) @@ -664,13 +439,8 @@ def main(): if os.getenv("NOCONTROL") is not None: del managed_processes['controlsd'] del managed_processes['radard'] - - # disable this until we use it - """ - if os.path.isfile('logserver/logserver.py'): - managed_processes["logserver"] = "selfdrive.logserver.wsgi" - persistent_processes.append("logserver") - """ + if os.getenv("DEFAULTD") is not None: + managed_processes["controlsd"] = "selfdrive.controls.defaultd" # support additional internal only extensions try: @@ -687,14 +457,16 @@ def main(): params.put("IsMetric", "0") if params.get("RecordFront") is None: params.put("RecordFront", "0") - if params.get("IsRearViewMirror") is None: - params.put("IsRearViewMirror", "0") if params.get("IsFcwEnabled") is None: params.put("IsFcwEnabled", "1") if params.get("HasAcceptedTerms") is None: params.put("HasAcceptedTerms", "0") if params.get("IsUploadVideoOverCellularEnabled") is None: params.put("IsUploadVideoOverCellularEnabled", "1") + if params.get("IsDriverMonitoringEnabled") is None: + params.put("IsDriverMonitoringEnabled", "0") + if params.get("IsGeofenceEnabled") is None: + params.put("IsGeofenceEnabled", "-1") # is this chffrplus? if os.getenv("PASSIVE") is not None: @@ -740,3 +512,4 @@ if __name__ == "__main__": main() # manual exit because we are forked sys.exit(0) + diff --git a/selfdrive/messaging.py b/selfdrive/messaging.py index 119e0d68a4..4811f93454 100644 --- a/selfdrive/messaging.py +++ b/selfdrive/messaging.py @@ -18,7 +18,7 @@ def sub_sock(context, port, poller=None, addr="127.0.0.1", conflate=False): if conflate: sock.setsockopt(zmq.CONFLATE, 1) sock.connect("tcp://%s:%d" % (addr, port)) - sock.setsockopt(zmq.SUBSCRIBE, "") + sock.setsockopt(zmq.SUBSCRIBE, b"") if poller is not None: poller.register(sock, zmq.POLLIN) return sock diff --git a/selfdrive/orbd/.gitignore b/selfdrive/orbd/.gitignore deleted file mode 100644 index 829780eb50..0000000000 --- a/selfdrive/orbd/.gitignore +++ /dev/null @@ -1,8 +0,0 @@ -orbd -orbd_cpu -test/turbocv_profile -test/turbocv_test -dspout/* -dumb_test -bilinear_lut.h -orb_lut.h diff --git a/selfdrive/orbd/Makefile b/selfdrive/orbd/Makefile deleted file mode 100644 index c33a8636bc..0000000000 --- a/selfdrive/orbd/Makefile +++ /dev/null @@ -1,105 +0,0 @@ -# CPU - -CC = clang -CXX = clang++ - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -JSON_FLAGS = -I$(PHONELIBS)/json/src - -CFLAGS = -std=gnu11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I. -CXXFLAGS = -std=c++11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I. -LDFLAGS = - -# profile -# CXXFLAGS += -DTURBOCV_PROFILE=1 - -PHONELIBS = ../../phonelibs -BASEDIR = ../.. -EXTERNAL = ../../external -PYTHONLIBS = - -UNAME_M := $(shell uname -m) - -ifeq ($(UNAME_M),x86_64) -# computer - -ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include -ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \ - -l:libczmq.a -l:libzmq.a -lpthread - -OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc - -CXXFLAGS += -fopenmp -LDFLAGS += -lomp - -else -# phone -ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include -ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \ - -l:libczmq.a -l:libzmq.a \ - -lgnustl_shared - -OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include -OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so - -endif - -.PHONY: all -all: orbd - -include ../common/cereal.mk - -DEP_OBJS = ../common/visionipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o - -orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(LDFLAGS) \ - $(ZMQ_LIBS) \ - $(CEREAL_LIBS) \ - -L/usr/lib \ - -L/system/vendor/lib64 \ - -ladsprpc \ - -lm -lz -llog - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) \ - $(ZMQ_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -orbd_dsp.o: orbd.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) \ - $(CEREAL_CXXFLAGS) \ - $(ZMQ_FLAGS) \ - $(OPENCV_FLAGS) \ - -DDSP \ - -I../ \ - -I../../ \ - -I../../../ \ - -I./include \ - -c -o '$@' '$<' - -freethedsp.o: dsp/freethedsp.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) \ - -c -o '$@' '$<' - -calculator_stub.o: dsp/gen/calculator_stub.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -I./include -c -o '$@' '$<' - --include internal.mk - -.PHONY: clean -clean: - rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h - diff --git a/selfdrive/orbd/dsp/freethedsp.c b/selfdrive/orbd/dsp/freethedsp.c deleted file mode 100644 index 298f4fd831..0000000000 --- a/selfdrive/orbd/dsp/freethedsp.c +++ /dev/null @@ -1,119 +0,0 @@ -// freethedsp by geohot -// (because the DSP should be free) -// released under MIT License - -// usage instructions: -// 1. Compile an example from the Qualcomm Hexagon SDK -// 2. Try to run it on your phone -// 3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat -// ...here is where people would give up before freethedsp -// 4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program) -// 5. Run your program with 'LD_PRELOAD=./freethedsp.so ./' -// 6. OMG THE DSP WORKS -// 7. Be happy. - -// *** patch may have to change for your phone *** - -// this is patching /dsp/fastrpc_shell_0 -// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8" -// patch to return 0xFFFFFFFF from is_test_enabled instead of 0 -// your fastrpc_shell_0 may vary -#define PATCH_ADDR 0x5200c -#define PATCH_OLD "\x40\x3f\x20\x50" -#define PATCH_NEW "\x40\x3f\x00\x5a" -#define PATCH_LEN (sizeof(PATCH_OLD)-1) -#define _BITS_IOCTL_H_ - -// under 100 lines of code begins now -#include -#include -#include -#include -#include - -// ioctl stuff -#define IOC_OUT 0x40000000 /* copy out parameters */ -#define IOC_IN 0x80000000 /* copy in parameters */ -#define IOC_INOUT (IOC_IN|IOC_OUT) -#define IOCPARM_MASK 0x1fff /* parameter length, at most 13 bits */ - -#define _IOC(inout,group,num,len) \ - (inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num)) -#define _IOWR(g,n,t) _IOC(IOC_INOUT, (g), (n), sizeof(t)) - -// ion ioctls -#include -#define ION_IOC_MSM_MAGIC 'M' -#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, \ - struct ion_flush_data) - -struct ion_flush_data { - ion_user_handle_t handle; - int fd; - void *vaddr; - unsigned int offset; - unsigned int length; -}; - -// fastrpc ioctls -#define FASTRPC_IOCTL_INIT _IOWR('R', 6, struct fastrpc_ioctl_init) - -struct fastrpc_ioctl_init { - uint32_t flags; /* one of FASTRPC_INIT_* macros */ - uintptr_t __user file; /* pointer to elf file */ - int32_t filelen; /* elf file length */ - int32_t filefd; /* ION fd for the file */ - uintptr_t __user mem; /* mem for the PD */ - int32_t memlen; /* mem length */ - int32_t memfd; /* ION fd for the mem */ -}; - -int ioctl(int fd, unsigned long request, void *arg) { - static void *handle = NULL; - static int (*orig_ioctl)(int, int, void*); - - if (handle == NULL) { - handle = dlopen("/system/lib64/libc.so", RTLD_LAZY); - assert(handle != NULL); - orig_ioctl = dlsym(handle, "ioctl"); - } - - int ret = orig_ioctl(fd, request, arg); - - // carefully modify this one - if (request == FASTRPC_IOCTL_INIT) { - struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg; - - // confirm patch is correct and do the patch - assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0); - memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN); - - // flush cache - int ionfd = open("/dev/ion", O_RDONLY); - assert(ionfd > 0); - - struct ion_fd_data fd_data; - fd_data.fd = init->memfd; - int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data); - assert(ret == 0); - - struct ion_flush_data flush_data; - flush_data.handle = fd_data.handle; - flush_data.vaddr = (void*)init->mem; - flush_data.offset = 0; - flush_data.length = init->memlen; - ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data); - assert(ret == 0); - - struct ion_handle_data handle_data; - handle_data.handle = fd_data.handle; - ret = ioctl(ionfd, ION_IOC_FREE, &handle_data); - assert(ret == 0); - - // cleanup - close(ionfd); - } - - return ret; -} - diff --git a/selfdrive/orbd/dsp/gen/calculator.h b/selfdrive/orbd/dsp/gen/calculator.h deleted file mode 100644 index 86a3de6717..0000000000 --- a/selfdrive/orbd/dsp/gen/calculator.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef _CALCULATOR_H -#define _CALCULATOR_H - -#include -typedef uint8_t uint8; -typedef uint32_t uint32; - -#ifndef __QAIC_HEADER -#define __QAIC_HEADER(ff) ff -#endif //__QAIC_HEADER - -#ifndef __QAIC_HEADER_EXPORT -#define __QAIC_HEADER_EXPORT -#endif // __QAIC_HEADER_EXPORT - -#ifndef __QAIC_HEADER_ATTRIBUTE -#define __QAIC_HEADER_ATTRIBUTE -#endif // __QAIC_HEADER_ATTRIBUTE - -#ifndef __QAIC_IMPL -#define __QAIC_IMPL(ff) ff -#endif //__QAIC_IMPL - -#ifndef __QAIC_IMPL_EXPORT -#define __QAIC_IMPL_EXPORT -#endif // __QAIC_IMPL_EXPORT - -#ifndef __QAIC_IMPL_ATTRIBUTE -#define __QAIC_IMPL_ATTRIBUTE -#endif // __QAIC_IMPL_ATTRIBUTE -#ifdef __cplusplus -extern "C" { -#endif -__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE; -__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE; -#ifdef __cplusplus -} -#endif -#endif //_CALCULATOR_H diff --git a/selfdrive/orbd/dsp/gen/calculator_stub.c b/selfdrive/orbd/dsp/gen/calculator_stub.c deleted file mode 100644 index 66e4a0f822..0000000000 --- a/selfdrive/orbd/dsp/gen/calculator_stub.c +++ /dev/null @@ -1,613 +0,0 @@ -#ifndef _CALCULATOR_STUB_H -#define _CALCULATOR_STUB_H -#include "calculator.h" - -// remote.h -#include -#include - -typedef uint32_t remote_handle; -typedef uint64_t remote_handle64; - -typedef struct { - void *pv; - size_t nLen; -} remote_buf; - -typedef struct { - int32_t fd; - uint32_t offset; -} remote_dma_handle; - -typedef union { - remote_buf buf; - remote_handle h; - remote_handle64 h64; - remote_dma_handle dma; -} remote_arg; - -int remote_handle_open(const char* name, remote_handle *ph); -int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra); -int remote_handle_close(remote_handle h); - -#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \ - ((((uint32_t) (nAttr) & 0x7) << 29) | \ - (((uint32_t) (nMethod) & 0x1f) << 24) | \ - (((uint32_t) (nIn) & 0xff) << 16) | \ - (((uint32_t) (nOut) & 0xff) << 8) | \ - (((uint32_t) (noIn) & 0x0f) << 4) | \ - ((uint32_t) (noOut) & 0x0f)) - -#ifndef _QAIC_ENV_H -#define _QAIC_ENV_H - -#ifdef __GNUC__ -#ifdef __clang__ -#pragma GCC diagnostic ignored "-Wunknown-pragmas" -#else -#pragma GCC diagnostic ignored "-Wpragmas" -#endif -#pragma GCC diagnostic ignored "-Wuninitialized" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wunused-function" -#endif - -#ifndef _ATTRIBUTE_UNUSED - -#ifdef _WIN32 -#define _ATTRIBUTE_UNUSED -#else -#define _ATTRIBUTE_UNUSED __attribute__ ((unused)) -#endif - -#endif // _ATTRIBUTE_UNUSED - -#ifndef __QAIC_REMOTE -#define __QAIC_REMOTE(ff) ff -#endif //__QAIC_REMOTE - -#ifndef __QAIC_HEADER -#define __QAIC_HEADER(ff) ff -#endif //__QAIC_HEADER - -#ifndef __QAIC_HEADER_EXPORT -#define __QAIC_HEADER_EXPORT -#endif // __QAIC_HEADER_EXPORT - -#ifndef __QAIC_HEADER_ATTRIBUTE -#define __QAIC_HEADER_ATTRIBUTE -#endif // __QAIC_HEADER_ATTRIBUTE - -#ifndef __QAIC_IMPL -#define __QAIC_IMPL(ff) ff -#endif //__QAIC_IMPL - -#ifndef __QAIC_IMPL_EXPORT -#define __QAIC_IMPL_EXPORT -#endif // __QAIC_IMPL_EXPORT - -#ifndef __QAIC_IMPL_ATTRIBUTE -#define __QAIC_IMPL_ATTRIBUTE -#endif // __QAIC_IMPL_ATTRIBUTE - -#ifndef __QAIC_STUB -#define __QAIC_STUB(ff) ff -#endif //__QAIC_STUB - -#ifndef __QAIC_STUB_EXPORT -#define __QAIC_STUB_EXPORT -#endif // __QAIC_STUB_EXPORT - -#ifndef __QAIC_STUB_ATTRIBUTE -#define __QAIC_STUB_ATTRIBUTE -#endif // __QAIC_STUB_ATTRIBUTE - -#ifndef __QAIC_SKEL -#define __QAIC_SKEL(ff) ff -#endif //__QAIC_SKEL__ - -#ifndef __QAIC_SKEL_EXPORT -#define __QAIC_SKEL_EXPORT -#endif // __QAIC_SKEL_EXPORT - -#ifndef __QAIC_SKEL_ATTRIBUTE -#define __QAIC_SKEL_ATTRIBUTE -#endif // __QAIC_SKEL_ATTRIBUTE - -#ifdef __QAIC_DEBUG__ - #ifndef __QAIC_DBG_PRINTF__ - #include - #define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0) - #endif -#else - #define __QAIC_DBG_PRINTF__( ee ) (void)0 -#endif - - -#define _OFFSET(src, sof) ((void*)(((char*)(src)) + (sof))) - -#define _COPY(dst, dof, src, sof, sz) \ - do {\ - struct __copy { \ - char ar[sz]; \ - };\ - *(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\ - } while (0) - -#define _COPYIF(dst, dof, src, sof, sz) \ - do {\ - if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\ - _COPY(dst, dof, src, sof, sz); \ - } \ - } while (0) - -_ATTRIBUTE_UNUSED -static __inline void _qaic_memmove(void* dst, void* src, int size) { - int i; - for(i = 0; i < size; ++i) { - ((char*)dst)[i] = ((char*)src)[i]; - } -} - -#define _MEMMOVEIF(dst, src, sz) \ - do {\ - if(dst != src) {\ - _qaic_memmove(dst, src, sz);\ - } \ - } while (0) - - -#define _ASSIGN(dst, src, sof) \ - do {\ - dst = OFFSET(src, sof); \ - } while (0) - -#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str)) - -#define AEE_SUCCESS 0 -#define AEE_EOFFSET 0x80000400 -#define AEE_EBADPARM (AEE_EOFFSET + 0x00E) - -#define _TRY(ee, func) \ - do { \ - if (AEE_SUCCESS != ((ee) = func)) {\ - __QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\ - goto ee##bail;\ - } \ - } while (0) - -#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS) - -#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS) - -#ifdef __QAIC_DEBUG__ -#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv)) -#else -#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv)) -#endif - - -#endif // _QAIC_ENV_H - -#ifndef _ALLOCATOR_H -#define _ALLOCATOR_H - -#include -#include - -typedef struct _heap _heap; -struct _heap { - _heap* pPrev; - const char* loc; - uint64_t buf; -}; - -typedef struct _allocator { - _heap* pheap; - uint8_t* stack; - uint8_t* stackEnd; - int nSize; -} _allocator; - -_ATTRIBUTE_UNUSED -static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) { - _heap* pn = 0; - pn = malloc(size + sizeof(_heap) - sizeof(uint64_t)); - if(pn != 0) { - pn->pPrev = *ppa; - pn->loc = loc; - *ppa = pn; - *ppbuf = (void*)&(pn->buf); - return 0; - } else { - return -1; - } -} -#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1)) - -_ATTRIBUTE_UNUSED -static __inline int _allocator_alloc(_allocator* me, - const char* loc, - int size, - unsigned int al, - void** ppbuf) { - if(size < 0) { - return -1; - } else if (size == 0) { - *ppbuf = 0; - return 0; - } - if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) { - *ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al); - me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size; - return 0; - } else { - return _heap_alloc(&me->pheap, loc, size, ppbuf); - } -} - -_ATTRIBUTE_UNUSED -static __inline void _allocator_deinit(_allocator* me) { - _heap* pa = me->pheap; - while(pa != 0) { - _heap* pn = pa; - const char* loc = pn->loc; - (void)loc; - pa = pn->pPrev; - free(pn); - } -} - -_ATTRIBUTE_UNUSED -static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) { - me->stack = stack; - me->stackEnd = stack + stackSize; - me->nSize = stackSize; - me->pheap = 0; -} - - -#endif // _ALLOCATOR_H - -#ifndef SLIM_H -#define SLIM_H - -#include - -//a C data structure for the idl types that can be used to implement -//static and dynamic language bindings fairly efficiently. -// -//the goal is to have a minimal ROM and RAM footprint and without -//doing too many allocations. A good way to package these things seemed -//like the module boundary, so all the idls within one module can share -//all the type references. - - -#define PARAMETER_IN 0x0 -#define PARAMETER_OUT 0x1 -#define PARAMETER_INOUT 0x2 -#define PARAMETER_ROUT 0x3 -#define PARAMETER_INROUT 0x4 - -//the types that we get from idl -#define TYPE_OBJECT 0x0 -#define TYPE_INTERFACE 0x1 -#define TYPE_PRIMITIVE 0x2 -#define TYPE_ENUM 0x3 -#define TYPE_STRING 0x4 -#define TYPE_WSTRING 0x5 -#define TYPE_STRUCTURE 0x6 -#define TYPE_UNION 0x7 -#define TYPE_ARRAY 0x8 -#define TYPE_SEQUENCE 0x9 - -//these require the pack/unpack to recurse -//so it's a hint to those languages that can optimize in cases where -//recursion isn't necessary. -#define TYPE_COMPLEX_STRUCTURE (0x10 | TYPE_STRUCTURE) -#define TYPE_COMPLEX_UNION (0x10 | TYPE_UNION) -#define TYPE_COMPLEX_ARRAY (0x10 | TYPE_ARRAY) -#define TYPE_COMPLEX_SEQUENCE (0x10 | TYPE_SEQUENCE) - - -typedef struct Type Type; - -#define INHERIT_TYPE\ - int32_t nativeSize; /*in the simple case its the same as wire size and alignment*/\ - union {\ - struct {\ - const uintptr_t p1;\ - const uintptr_t p2;\ - } _cast;\ - struct {\ - uint32_t iid;\ - uint32_t bNotNil;\ - } object;\ - struct {\ - const Type *arrayType;\ - int32_t nItems;\ - } array;\ - struct {\ - const Type *seqType;\ - int32_t nMaxLen;\ - } seqSimple; \ - struct {\ - uint32_t bFloating;\ - uint32_t bSigned;\ - } prim; \ - const SequenceType* seqComplex;\ - const UnionType *unionType;\ - const StructType *structType;\ - int32_t stringMaxLen;\ - uint8_t bInterfaceNotNil;\ - } param;\ - uint8_t type;\ - uint8_t nativeAlignment\ - -typedef struct UnionType UnionType; -typedef struct StructType StructType; -typedef struct SequenceType SequenceType; -struct Type { - INHERIT_TYPE; -}; - -struct SequenceType { - const Type * seqType; - uint32_t nMaxLen; - uint32_t inSize; - uint32_t routSizePrimIn; - uint32_t routSizePrimROut; -}; - -//byte offset from the start of the case values for -//this unions case value array. it MUST be aligned -//at the alignment requrements for the descriptor -// -//if negative it means that the unions cases are -//simple enumerators, so the value read from the descriptor -//can be used directly to find the correct case -typedef union CaseValuePtr CaseValuePtr; -union CaseValuePtr { - const uint8_t* value8s; - const uint16_t* value16s; - const uint32_t* value32s; - const uint64_t* value64s; -}; - -//these are only used in complex cases -//so I pulled them out of the type definition as references to make -//the type smaller -struct UnionType { - const Type *descriptor; - uint32_t nCases; - const CaseValuePtr caseValues; - const Type * const *cases; - int32_t inSize; - int32_t routSizePrimIn; - int32_t routSizePrimROut; - uint8_t inAlignment; - uint8_t routAlignmentPrimIn; - uint8_t routAlignmentPrimROut; - uint8_t inCaseAlignment; - uint8_t routCaseAlignmentPrimIn; - uint8_t routCaseAlignmentPrimROut; - uint8_t nativeCaseAlignment; - uint8_t bDefaultCase; -}; - -struct StructType { - uint32_t nMembers; - const Type * const *members; - int32_t inSize; - int32_t routSizePrimIn; - int32_t routSizePrimROut; - uint8_t inAlignment; - uint8_t routAlignmentPrimIn; - uint8_t routAlignmentPrimROut; -}; - -typedef struct Parameter Parameter; -struct Parameter { - INHERIT_TYPE; - uint8_t mode; - uint8_t bNotNil; -}; - -#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64)) -#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff) - -typedef struct Method Method; -struct Method { - uint32_t uScalars; //no method index - int32_t primInSize; - int32_t primROutSize; - int maxArgs; - int numParams; - const Parameter * const *params; - uint8_t primInAlignment; - uint8_t primROutAlignment; -}; - -typedef struct Interface Interface; - -struct Interface { - int nMethods; - const Method * const *methodArray; - int nIIds; - const uint32_t *iids; - const uint16_t* methodStringArray; - const uint16_t* methodStrings; - const char* strings; -}; - - -#endif //SLIM_H - - -#ifndef _CALCULATOR_SLIM_H -#define _CALCULATOR_SLIM_H - -// remote.h - -#include - -#ifndef __QAIC_SLIM -#define __QAIC_SLIM(ff) ff -#endif -#ifndef __QAIC_SLIM_EXPORT -#define __QAIC_SLIM_EXPORT -#endif - -static const Type types[1]; -static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}}; -static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}}; -static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))}; -static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}}; -static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])}; -static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0"; -static const uint16_t methodStrings[5] = {0,37,18,32,27}; -static const uint16_t methodStringsArrays[2] = {3,0}; -__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings}; -#endif //_CALCULATOR_SLIM_H -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef _const_calculator_handle -#define _const_calculator_handle ((remote_handle)-1) -#endif //_const_calculator_handle - -static void _calculator_pls_dtor(void* data) { - remote_handle* ph = (remote_handle*)data; - if(_const_calculator_handle != *ph) { - (void)__QAIC_REMOTE(remote_handle_close)(*ph); - *ph = _const_calculator_handle; - } -} - -static int _calculator_pls_ctor(void* ctx, void* data) { - remote_handle* ph = (remote_handle*)data; - *ph = _const_calculator_handle; - if(*ph == (remote_handle)-1) { - return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph); - } - return 0; -} - -#if (defined __qdsp6__) || (defined __hexagon__) -#pragma weak adsp_pls_add_lookup -extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); -#pragma weak HAP_pls_add_lookup -extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); - -__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { - remote_handle* ph; - if(adsp_pls_add_lookup) { - if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { - return *ph; - } - return (remote_handle)-1; - } else if(HAP_pls_add_lookup) { - if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { - return *ph; - } - return (remote_handle)-1; - } - return(remote_handle)-1; -} - -#else //__qdsp6__ || __hexagon__ - -uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare); - -#ifdef _WIN32 -#include "Windows.h" -uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { - return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare); -} -#elif __GNUC__ -uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { - return __sync_val_compare_and_swap(puDest, uCompare, uExchange); -} -#endif //_WIN32 - - -__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { - static remote_handle handle = _const_calculator_handle; - if((remote_handle)-1 != handle) { - return handle; - } else { - remote_handle tmp; - int nErr = _calculator_pls_ctor("calculator", (void*)&tmp); - if(nErr) { - return (remote_handle)-1; - } - if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) { - _calculator_pls_dtor(&tmp); - } - return handle; - } -} - -#endif //__qdsp6__ - -__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE { - return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra); -} - -#ifdef __cplusplus -} -#endif - - -#ifdef __cplusplus -extern "C" { -#endif -extern int remote_register_dma_handle(int, uint32_t); -static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) { - int _numIn[1]; - remote_arg _pra[1]; - uint32_t _primROut[1]; - int _nErr = 0; - _numIn[0] = 0; - _pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut; - _pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut); - _TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra)); - _COPY(_rout0, 0, _primROut, 0, 4); - _CATCH(_nErr) {} - return _nErr; -} -__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE { - uint32_t _mid = 0; - return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet); -} -static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) { - int _numIn[1]; - remote_arg _pra[3]; - uint32_t _primIn[2]; - remote_arg* _praIn; - remote_arg* _praROut; - int _nErr = 0; - _numIn[0] = 1; - _pra[0].buf.pv = (void*)_primIn; - _pra[0].buf.nLen = sizeof(_primIn); - _COPY(_primIn, 0, _in0Len, 0, 4); - _praIn = (_pra + 1); - _praIn[0].buf.pv = _in0[0]; - _praIn[0].buf.nLen = (1 * _in0Len[0]); - _COPY(_primIn, 4, _rout1Len, 0, 4); - _praROut = (_praIn + _numIn[0] + 0); - _praROut[0].buf.pv = _rout1[0]; - _praROut[0].buf.nLen = (1 * _rout1Len[0]); - _TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra)); - _CATCH(_nErr) {} - return _nErr; -} -__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE { - uint32_t _mid = 1; - return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen); -} -#ifdef __cplusplus -} -#endif -#endif //_CALCULATOR_STUB_H diff --git a/selfdrive/orbd/dsp/gen/libcalculator_skel.so b/selfdrive/orbd/dsp/gen/libcalculator_skel.so deleted file mode 100755 index 5311d07e7a..0000000000 --- a/selfdrive/orbd/dsp/gen/libcalculator_skel.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b94bcedb39d110b074a99fe3129da2506bc4b06cca7f84731cd0620683fd1179 -size 357400 diff --git a/selfdrive/orbd/extractor.h b/selfdrive/orbd/extractor.h deleted file mode 100644 index c6392d446b..0000000000 --- a/selfdrive/orbd/extractor.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef EXTRACTOR_H -#define EXTRACTOR_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#define ORBD_KEYPOINTS 3000 -#define ORBD_DESCRIPTOR_LENGTH 32 -#define ORBD_HEIGHT 874 -#define ORBD_WIDTH 1164 - -// matches OrbFeatures from log.capnp -struct orb_features { - // align this - uint16_t n_corners; - uint16_t xy[ORBD_KEYPOINTS][2]; - uint8_t octave[ORBD_KEYPOINTS]; - uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH]; - int16_t matches[ORBD_KEYPOINTS]; -}; - -// forward declare this -struct pyramid; - -// manage the pyramids in extractor.c -void init_gpyrs(); -int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *); -int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *); - -#ifdef __cplusplus -} -#endif - -#endif // EXTRACTOR_H diff --git a/selfdrive/orbd/orbd.cc b/selfdrive/orbd/orbd.cc deleted file mode 100644 index 26c9ffec76..0000000000 --- a/selfdrive/orbd/orbd.cc +++ /dev/null @@ -1,181 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "common/visionipc.h" -#include "common/swaglog.h" - -#include "extractor.h" - -#ifdef DSP -#include "dsp/gen/calculator.h" -#else -#include "turbocv.h" -#endif - -#include -#include -#include "cereal/gen/cpp/log.capnp.h" - -#ifndef PATH_MAX -#include -#endif - -volatile int do_exit = 0; - -static void set_do_exit(int sig) { - do_exit = 1; -} - -int main(int argc, char *argv[]) { - int err; - setpriority(PRIO_PROCESS, 0, -13); - printf("starting orbd\n"); - -#ifdef DSP - uint32_t test_leet = 0; - char my_path[PATH_MAX+1]; - memset(my_path, 0, sizeof(my_path)); - - ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path)); - assert(len > 5); - my_path[len-5] = '\0'; - LOGW("running from %s with PATH_MAX %d", my_path, PATH_MAX); - - char adsp_path[PATH_MAX+1]; - snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path); - assert(putenv(adsp_path) == 0); - - assert(calculator_init(&test_leet) == 0); - assert(test_leet == 0x1337); - LOGW("orbd init complete"); -#else - init_gpyrs(); -#endif - - signal(SIGINT, (sighandler_t) set_do_exit); - signal(SIGTERM, (sighandler_t) set_do_exit); - - void *ctx = zmq_ctx_new(); - - void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB); - assert(orb_features_sock); - zmq_bind(orb_features_sock, "tcp://*:8058"); - - void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB); - assert(orb_features_summary_sock); - zmq_bind(orb_features_summary_sock, "tcp://*:8062"); - - struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features)); - int last_frame_id = 0; - - VisionStream stream; - while (!do_exit) { - VisionStreamBufs buf_info; - err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); - if (err) { - printf("visionstream connect fail\n"); - usleep(100000); - continue; - } - uint64_t timestamp_last_eof = 0; - while (!do_exit) { - VIPCBuf *buf; - VIPCBufExtra extra; - buf = visionstream_get(&stream, &extra); - if (buf == NULL) { - printf("visionstream get failed\n"); - break; - } - - uint64_t start = nanos_since_boot(); -#ifdef DSP - int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features)); -#else - int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features); -#endif - uint64_t end = nanos_since_boot(); - LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id); - assert(ret == 0); - - if (last_frame_id+1 != extra.frame_id) { - LOGW("dropped frame!"); - } - - last_frame_id = extra.frame_id; - - if (timestamp_last_eof == 0) { - timestamp_last_eof = extra.timestamp_eof; - continue; - } - - int match_count = 0; - - // *** send OrbFeatures *** - { - // create capnp message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto orb_features = event.initOrbFeatures(); - - // set timestamps - orb_features.setTimestampEof(extra.timestamp_eof); - orb_features.setTimestampLastEof(timestamp_last_eof); - - // init descriptors for send - kj::ArrayPtr descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners); - orb_features.setDescriptors(descriptorsPtr); - - auto xs = orb_features.initXs(features->n_corners); - auto ys = orb_features.initYs(features->n_corners); - auto octaves = orb_features.initOctaves(features->n_corners); - auto matches = orb_features.initMatches(features->n_corners); - - // copy out normalized keypoints - for (int i = 0; i < features->n_corners; i++) { - xs.set(i, features->xy[i][0] * 1.0f / ORBD_WIDTH - 0.5f); - ys.set(i, features->xy[i][1] * 1.0f / ORBD_HEIGHT - 0.5f); - octaves.set(i, features->octave[i]); - matches.set(i, features->matches[i]); - match_count += features->matches[i] != -1; - } - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0); - } - - // *** send OrbFeaturesSummary *** - - { - // create capnp message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto orb_features_summary = event.initOrbFeaturesSummary(); - - orb_features_summary.setTimestampEof(extra.timestamp_eof); - orb_features_summary.setTimestampLastEof(timestamp_last_eof); - orb_features_summary.setFeatureCount(features->n_corners); - orb_features_summary.setMatchCount(match_count); - orb_features_summary.setComputeNs(end-start); - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0); - } - - timestamp_last_eof = extra.timestamp_eof; - } - } - visionstream_destroy(&stream); - return 0; -} - diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 7f05a078c9..981546ccf9 100644 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -10,4 +10,5 @@ def main(gctx=None): os.execvp("./boardd", ["./boardd"]) if __name__ == "__main__": - main() + main() + diff --git a/selfdrive/registration.py b/selfdrive/registration.py index ed22e5dfa5..4327ed3a87 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -47,5 +47,5 @@ def register(): return None if __name__ == "__main__": - print api_get("").text - print register() + print(api_get("").text) + print(register()) diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index f4dcf29491..c7a6786aff 100755 --- a/selfdrive/sensord/gpsd +++ b/selfdrive/sensord/gpsd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d51ec7f5e069ab8d9b03b672cb65df0ddae9abe82b220debb9aecc1a36f20f45 +oid sha256:a539d59efc3c5009af06ea1dffb2acecbdc3798aaf414d08d0c9cb258e7771cb size 1171544 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 52b1b3b93e..588cb02e16 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:05853b1b3324b420d1463daf806797bb3d85659a0637ca7836b137a84e619be0 +oid sha256:aafc76b775889ecfb96feb3a2072c3b168a0e026cf2c0ad2f603349cdf6ab779 size 1159016 diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index 4195ee7b54..ae2351f581 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -69,6 +69,7 @@ orbKeyFrame: [8059, true] uiLayoutState: [8060, true] frontEncodeIdx: [8061, true] orbFeaturesSummary: [8062, true] +driverMonitoring: [8063, true] testModel: [8040, false] testLiveLocation: [8045, false] diff --git a/selfdrive/services.py b/selfdrive/services.py index d705cd808a..bd804afe79 100644 --- a/selfdrive/services.py +++ b/selfdrive/services.py @@ -10,5 +10,5 @@ service_list_path = os.path.join(os.path.dirname(__file__), "service_list.yaml") service_list = {} with open(service_list_path, "r") as f: - for k, v in yaml.load(f).iteritems(): + for k, v in yaml.load(f).items(): service_list[k] = Service(v[0], v[1]) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 8ff062e02a..6c3eb66555 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import os import struct - +from collections import namedtuple import zmq import numpy as np @@ -210,15 +210,47 @@ class Plant(object): print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel) # ******** publish the car ******** - # TODO: the order is this list should not matter, but currently everytime we change carstate we break this test. Fix it! - vls = [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, + vls_tuple = namedtuple('vls', [ + 'XMISSION_SPEED', + 'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR', + 'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR', + 'LEFT_BLINKER', 'RIGHT_BLINKER', + 'GEAR', + 'WHEELS_MOVING', + 'BRAKE_ERROR_1', 'BRAKE_ERROR_2', + 'SEATBELT_DRIVER_LAMP', 'SEATBELT_DRIVER_LATCHED', + 'BRAKE_PRESSED', 'BRAKE_SWITCH', + 'CRUISE_BUTTONS', + 'ESP_DISABLED', + 'HUD_LEAD', + 'USER_BRAKE', + 'STEER_STATUS', + 'GEAR_SHIFTER', + 'PEDAL_GAS', + 'CRUISE_SETTING', + 'ACC_STATUS', + + 'CRUISE_SPEED_PCM', + 'CRUISE_SPEED_OFFSET', + + 'DOOR_OPEN_FL', 'DOOR_OPEN_FR', 'DOOR_OPEN_RL', 'DOOR_OPEN_RR', + + 'CAR_GAS', + 'MAIN_ON', + 'EPB_STATE', + 'BRAKE_HOLD_ACTIVE', + '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.angle_steer, self.angle_steer_rate, 0, #Steer torque sensor 0, 0, # Blinkers self.gear_choice, speed != 0, self.brake_error, self.brake_error, not self.seatbelt, self.seatbelt, # Seatbelt - self.brake_pressed, 0., + self.brake_pressed, 0., #Brake pressed, Brake switch cruise_buttons, self.esp_disabled, 0, # HUD lead @@ -238,11 +270,8 @@ class Plant(object): self.main_on, 0, # EPB State 0, # Brake hold - 0, # Interceptor feedback - # 0, - - - ] + 0 # Interceptor feedback + ) # TODO: publish each message at proper frequency can_msgs = [] @@ -250,7 +279,7 @@ class Plant(object): msg_struct = {} indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]] for i in indxs: - msg_struct[sgs[i]] = vls[i] + msg_struct[sgs[i]] = getattr(vls, sgs[i]) if "COUNTER" in honda.get_signals(msg): msg_struct["COUNTER"] = self.rk.frame % 4 diff --git a/selfdrive/thermal.py b/selfdrive/thermal.py deleted file mode 100644 index 24e4d6c52f..0000000000 --- a/selfdrive/thermal.py +++ /dev/null @@ -1,19 +0,0 @@ -"""Methods for reading system thermal information.""" -import selfdrive.messaging as messaging - -def read_tz(x): - with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: - ret = max(0, int(f.read())) - return ret - -def read_thermal(): - dat = messaging.new_message() - dat.init('thermal') - dat.thermal.cpu0 = read_tz(5) - dat.thermal.cpu1 = read_tz(7) - dat.thermal.cpu2 = read_tz(10) - dat.thermal.cpu3 = read_tz(12) - dat.thermal.mem = read_tz(2) - dat.thermal.gpu = read_tz(16) - dat.thermal.bat = read_tz(29) - return dat diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py new file mode 100755 index 0000000000..97e63b1438 --- /dev/null +++ b/selfdrive/thermald.py @@ -0,0 +1,272 @@ +#!/usr/bin/env python2.7 +import os +import zmq +from smbus2 import SMBus + +from selfdrive.version import training_version +from selfdrive.swaglog import cloudlog +import selfdrive.messaging as messaging +from selfdrive.services import service_list +from selfdrive.loggerd.config import ROOT +from common.params import Params +from common.realtime import sec_since_boot + +import cereal +ThermalStatus = cereal.log.ThermalData.ThermalStatus + +def read_tz(x): + with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: + ret = max(0, int(f.read())) + return ret + +def read_thermal(): + dat = messaging.new_message() + dat.init('thermal') + dat.thermal.cpu0 = read_tz(5) + dat.thermal.cpu1 = read_tz(7) + dat.thermal.cpu2 = read_tz(10) + dat.thermal.cpu3 = read_tz(12) + dat.thermal.mem = read_tz(2) + dat.thermal.gpu = read_tz(16) + dat.thermal.bat = read_tz(29) + return dat + +LEON = False +def setup_eon_fan(): + global LEON + + os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch") + + bus = SMBus(7, force=True) + try: + bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts + bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable + bus.write_byte_data(0x21, 0x02, 0x2) # needed? + bus.write_byte_data(0x21, 0x04, 0x4) # manual override source + except IOError: + print "LEON detected" + #os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") + LEON = True + bus.close() + +last_eon_fan_val = None +def set_eon_fan(val): + global LEON, last_eon_fan_val + + if last_eon_fan_val is None or last_eon_fan_val != val: + bus = SMBus(7, force=True) + if LEON: + i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val] + bus.write_i2c_block_data(0x3d, 0, [i]) + else: + bus.write_byte_data(0x21, 0x04, 0x2) + bus.write_byte_data(0x21, 0x03, (val*2)+1) + bus.write_byte_data(0x21, 0x04, 0x4) + bus.close() + last_eon_fan_val = val + +# temp thresholds to control fan speed - high hysteresis +_TEMP_THRS_H = [50., 65., 80., 10000] +# temp thresholds to control fan speed - low hysteresis +_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] +# fan speed options +_FAN_SPEEDS = [0, 16384, 32768, 65535] +# max fan speed only allowed if battery if hot +_BAT_TEMP_THERSHOLD = 45. + +def handle_fan(max_temp, bat_temp, fan_speed): + new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp) + new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp) + + if new_speed_h > fan_speed: + # update speed if using the high thresholds results in fan speed increment + fan_speed = new_speed_h + elif new_speed_l < fan_speed: + # update speed if using the low thresholds results in fan speed decrement + fan_speed = new_speed_l + + if bat_temp < _BAT_TEMP_THERSHOLD: + # no max fan speed unless battery is hot + fan_speed = min(fan_speed, _FAN_SPEEDS[-2]) + + set_eon_fan(fan_speed/16384) + + return fan_speed + +class LocationStarter(object): + def __init__(self): + self.last_good_loc = 0 + def update(self, started_ts, location): + rt = sec_since_boot() + + if location is None or location.accuracy > 50 or location.speed < 2: + # bad location, stop if we havent gotten a location in a while + # dont stop if we're been going for less than a minute + if started_ts: + if rt-self.last_good_loc > 60. and rt-started_ts > 60: + cloudlog.event("location_stop", + ts=rt, + started_ts=started_ts, + last_good_loc=self.last_good_loc, + location=location.to_dict() if location else None) + return False + else: + return True + else: + return False + + self.last_good_loc = rt + + if started_ts: + return True + else: + cloudlog.event("location_start", location=location.to_dict() if location else None) + return location.speed*3.6 > 10 + +def thermald_thread(): + setup_eon_fan() + + # now loop + context = zmq.Context() + thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) + health_sock = messaging.sub_sock(context, service_list['health'].port) + location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) + fan_speed = 0 + count = 0 + + off_ts = None + started_ts = None + ignition_seen = False + started_seen = False + passive_starter = LocationStarter() + thermal_status = ThermalStatus.green + health_sock.RCVTIMEO = 1500 + + params = Params() + + while 1: + td = messaging.recv_sock(health_sock, wait=True) + location = messaging.recv_sock(location_sock) + location = location.gpsLocation if location else None + msg = read_thermal() + + # loggerd is gated based on free space + statvfs = os.statvfs(ROOT) + avail = (statvfs.f_bavail * 1.0)/statvfs.f_blocks + + # thermal message now also includes free space + msg.thermal.freeSpace = avail + with open("/sys/class/power_supply/battery/capacity") as f: + msg.thermal.batteryPercent = int(f.read()) + with open("/sys/class/power_supply/battery/status") as f: + msg.thermal.batteryStatus = f.read().strip() + with open("/sys/class/power_supply/usb/online") as f: + msg.thermal.usbOnline = bool(int(f.read())) + + # TODO: add car battery voltage check + max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, + msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 + bat_temp = msg.thermal.bat/1000. + fan_speed = handle_fan(max_temp, bat_temp, fan_speed) + msg.thermal.fanSpeed = fan_speed + + # thermal logic here + + if max_temp < 70.0: + thermal_status = ThermalStatus.green + if max_temp > 85.0: + cloudlog.warning("over temp: %r", max_temp) + thermal_status = ThermalStatus.yellow + + # from controls + overtemp_proc = any(t > 950 for t in + (msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, + msg.thermal.cpu3, msg.thermal.mem, msg.thermal.gpu)) + overtemp_bat = msg.thermal.bat > 60000 # 60c + if overtemp_proc or overtemp_bat: + # TODO: hysteresis + thermal_status = ThermalStatus.red + + if max_temp > 107.0 or msg.thermal.bat >= 63000: + thermal_status = ThermalStatus.danger + + # **** starting logic **** + + # start constellation of processes when the car starts + ignition = td is not None and td.health.started + ignition_seen = ignition_seen or ignition + + # add voltage check for ignition + if not ignition_seen and td is not None and td.health.voltage > 13500: + ignition = True + + do_uninstall = params.get("DoUninstall") == "1" + accepted_terms = params.get("HasAcceptedTerms") == "1" + completed_training = params.get("CompletedTrainingVersion") == training_version + + should_start = ignition + + # have we seen a panda? + passive = (params.get("Passive") == "1") + + # start on gps movement if we haven't seen ignition and are in passive mode + should_start = should_start or (not (ignition_seen and td) # seen ignition and panda is connected + and passive + and passive_starter.update(started_ts, location)) + + # with 2% left, we killall, otherwise the phone will take a long time to boot + should_start = should_start and msg.thermal.freeSpace > 0.02 + + # require usb power in passive mode + should_start = should_start and (not passive or msg.thermal.usbOnline) + + # confirm we have completed training and aren't uninstalling + should_start = should_start and accepted_terms and (passive or completed_training) and (not do_uninstall) + + # if any CPU gets above 107 or the battery gets above 63, kill all processes + # controls will warn with CPU above 95 or battery above 60 + if msg.thermal.thermalStatus >= ThermalStatus.danger: + # TODO: Add a better warning when this is happening + should_start = False + + if should_start: + off_ts = None + if started_ts is None: + params.car_start() + started_ts = sec_since_boot() + started_seen = True + else: + started_ts = None + if off_ts is None: + off_ts = sec_since_boot() + + # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for + # more than a minute but we were running + if msg.thermal.batteryPercent < 3 and msg.thermal.batteryStatus == "Discharging" and \ + started_seen and (sec_since_boot() - off_ts) > 60: + os.system('LD_LIBRARY_PATH="" svc power shutdown') + + msg.thermal.started = started_ts is not None + msg.thermal.startedTs = int(1e9*(started_ts or 0)) + + msg.thermal.thermalStatus = thermal_status + thermal_sock.send(msg.to_bytes()) + print msg + + # report to server once per minute + if (count%60) == 0: + cloudlog.event("STATUS_PACKET", + count=count, + health=(td.to_dict() if td else None), + location=(location.to_dict() if location else None), + thermal=msg.to_dict()) + + count += 1 + + +def main(gctx=None): + thermald_thread() + +if __name__ == "__main__": + main() + diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index 38b9147935..e9a5f6f84c 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -25,13 +25,21 @@ CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o NANOVG_FLAGS = -I$(PHONELIBS)/nanovg JSON_FLAGS = -I$(PHONELIBS)/json/src +OPENCL_FLAGS = -I$(PHONELIBS)/opencl/include +OPENCL_LIBS = -lgsl -lCB -lOpenCL + OPENGL_LIBS = -lGLESv3 FRAMEBUFFER_LIBS = -lutils -lgui -lEGL +CFLAGS += -DQCOM +CXXFLAGS += -DQCOM + OBJS = ui.o \ ../common/glutil.o \ ../common/visionipc.o \ + ../common/visionimg.o \ + ../common/visionbuf_ion.o \ ../common/framebuffer.o \ ../common/params.o \ ../common/util.o \ @@ -52,27 +60,33 @@ ui: $(OBJS) $(CEREAL_LIBS) \ $(ZMQ_LIBS) \ -L/system/vendor/lib64 \ - -lhardware \ + -lhardware -lui \ $(OPENGL_LIBS) \ - -lcutils -lm -llog + $(OPENCL_LIBS) \ + -lcutils -lm -llog -lui -ladreno_utils %.o: %.cc @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) -MMD \ -Iinclude -I.. -I../.. \ + $(OPENCL_FLAGS) \ -I$(PHONELIBS)/android_frameworks_native/include \ -I$(PHONELIBS)/android_system_core/include \ -I$(PHONELIBS)/android_hardware_libhardware/include \ + -I$(PHONELIBS)/libgralloc/include \ + -I$(PHONELIBS)/linux/include \ -c -o '$@' '$<' %.o: %.c @echo "[ CC ] $@" $(CC) $(CFLAGS) -MMD \ - -I.. -I../.. \ + -Iinclude -I.. -I../.. \ $(NANOVG_FLAGS) \ $(ZMQ_FLAGS) \ $(CEREAL_CFLAGS) \ $(JSON_FLAGS) \ + $(OPENCL_FLAGS) \ + -I$(PHONELIBS)/linux/include \ -c -o '$@' '$<' .PHONY: clean diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 17a31f493a..6948a492f5 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -4,12 +4,12 @@ #include #include #include +#include #include #include #include -#include #include #include @@ -28,6 +28,7 @@ #include "common/touch.h" #include "common/framebuffer.h" #include "common/visionipc.h" +#include "common/visionimg.h" #include "common/modeldata.h" #include "common/params.h" @@ -91,8 +92,6 @@ const int alert_sizes[] = { typedef struct UIScene { int frontview; - uint8_t *bgr_ptr; - int transformed_width, transformed_height; uint64_t model_ts; @@ -110,6 +109,7 @@ typedef struct UIScene { float v_ego; float curvature; int engaged; + bool engageable; bool uilayout_sidebarcollapsed; bool uilayout_mapenabled; @@ -121,13 +121,13 @@ typedef struct UIScene { int lead_status; float lead_d_rel, lead_y_rel, lead_v_rel; - uint8_t *bgr_front_ptr; int front_box_x, front_box_y, front_box_width, front_box_height; uint64_t alert_ts; char alert_text1[1024]; char alert_text2[1024]; uint8_t alert_size; + float alert_blinkingrate; float awareness_status; @@ -190,8 +190,9 @@ typedef struct UIState { int cur_vision_front_idx; GLuint frame_program; + GLuint frame_texs[UI_BUF_COUNT]; + GLuint frame_front_texs[UI_BUF_COUNT]; - GLuint frame_tex; GLint frame_pos_loc, frame_texcoord_loc; GLint frame_texture_loc, frame_transform_loc; @@ -199,11 +200,12 @@ typedef struct UIState { GLint line_pos_loc, line_color_loc; GLint line_transform_loc; - unsigned int rgb_width, rgb_height; + unsigned int rgb_width, rgb_height, rgb_stride; + size_t rgb_buf_len; mat4 rgb_transform; - unsigned int rgb_front_width, rgb_front_height; - GLuint frame_front_tex; + unsigned int rgb_front_width, rgb_front_height, rgb_front_stride; + size_t rgb_front_buf_len; bool intrinsic_matrix_loaded; mat3 intrinsic_matrix; @@ -217,6 +219,8 @@ typedef struct UIState { bool is_metric; bool passive; int alert_size; + float alert_blinking_alpha; + bool alert_blinked; float light_sensor; } UIState; @@ -469,9 +473,13 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->rgb_width = back_bufs.width; s->rgb_height = back_bufs.height; + s->rgb_stride = back_bufs.stride; + s->rgb_buf_len = back_bufs.buf_len; s->rgb_front_width = front_bufs.width; s->rgb_front_height = front_bufs.height; + s->rgb_front_stride = front_bufs.stride; + s->rgb_front_buf_len = front_bufs.buf_len; s->rgb_transform = (mat4){{ 2.0/s->rgb_width, 0.0, 0.0, -1.0, @@ -488,30 +496,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, } } -static void ui_update_frame(UIState *s) { - assert(glGetError() == GL_NO_ERROR); - - UIScene *scene = &s->scene; - - if (scene->frontview && scene->bgr_front_ptr) { - // load front frame texture - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, s->frame_front_tex); - glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, - s->rgb_front_width, s->rgb_front_height, - GL_RGB, GL_UNSIGNED_BYTE, scene->bgr_front_ptr); - } else if (!scene->frontview && scene->bgr_ptr) { - // load frame texture - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, s->frame_tex); - glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, - s->rgb_width, s->rgb_height, - GL_RGB, GL_UNSIGNED_BYTE, scene->bgr_ptr); - } - - assert(glGetError() == GL_NO_ERROR); -} - static void ui_draw_transformed_box(UIState *s, uint32_t color) { const UIScene *scene = &s->scene; @@ -808,10 +792,10 @@ static void draw_frame(UIState *s) { }; glActiveTexture(GL_TEXTURE0); - if (s->scene.frontview) { - glBindTexture(GL_TEXTURE_2D, s->frame_front_tex); - } else { - glBindTexture(GL_TEXTURE_2D, s->frame_tex); + if (s->scene.frontview && s->cur_vision_front_idx >= 0) { + glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[s->cur_vision_front_idx]); + } else if (!scene->frontview && s->cur_vision_idx >= 0) { + glBindTexture(GL_TEXTURE_2D, s->frame_texs[s->cur_vision_idx]); } glUseProgram(s->frame_program); @@ -972,16 +956,19 @@ static void ui_draw_vision_wheel(UIState *s) { const int img_wheel_size = bg_wheel_size*1.5; const int img_wheel_x = bg_wheel_x-(img_wheel_size/2); const int img_wheel_y = bg_wheel_y-25; - float img_wheel_alpha = 0.5f; + float img_wheel_alpha = 0.1f; bool is_engaged = (s->status == STATUS_ENGAGED); bool is_warning = (s->status == STATUS_WARNING); - if (is_engaged || is_warning) { + bool is_engageable = scene->engageable; + if (is_engaged || is_warning || is_engageable) { nvgBeginPath(s->vg); nvgCircle(s->vg, bg_wheel_x, (bg_wheel_y + (bdr_s*1.5)), bg_wheel_size); if (is_engaged) { nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255)); } else if (is_warning) { nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255)); + } else if (is_engageable) { + nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 255)); } nvgFill(s->vg); img_wheel_alpha = 1.0f; @@ -1031,7 +1018,7 @@ static void ui_draw_vision_alert(UIState *s, int va_size, int va_color, nvgBeginPath(s->vg); nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); - nvgFillColor(s->vg, nvgRGBA(color[0],color[1],color[2],color[3])); + nvgFillColor(s->vg, nvgRGBA(color[0],color[1],color[2],(color[3]*s->alert_blinking_alpha))); nvgFill(s->vg); nvgBeginPath(s->vg); @@ -1151,7 +1138,6 @@ static void ui_draw(UIState *s) { glDisable(GL_BLEND); } - eglSwapBuffers(s->display, s->surface); assert(glGetError() == GL_NO_ERROR); } @@ -1211,31 +1197,53 @@ static void ui_update(UIState *s) { // cant run this in connector thread because opengl. // do this here for now in lieu of a run_on_main_thread event - // setup frame texture - glDeleteTextures(1, &s->frame_tex); //silently ignores a 0 texture - glGenTextures(1, &s->frame_tex); - glBindTexture(GL_TEXTURE_2D, s->frame_tex); - glTexStorage2D(GL_TEXTURE_2D, 1, GL_RGB8, s->rgb_width, s->rgb_height); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - - // BGR - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); - - // front - glDeleteTextures(1, &s->frame_front_tex); - glGenTextures(1, &s->frame_front_tex); - glBindTexture(GL_TEXTURE_2D, s->frame_front_tex); - glTexStorage2D(GL_TEXTURE_2D, 1, GL_RGB8, s->rgb_front_width, s->rgb_front_height); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - - // BGR - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); + for (int i=0; iframe_texs[i]); + + VisionImg img = { + .fd = s->bufs[i].fd, + .format = VISIONIMG_FORMAT_RGB24, + .width = s->rgb_width, + .height = s->rgb_height, + .stride = s->rgb_stride, + .bpp = 3, + .size = s->rgb_buf_len, + }; + s->frame_texs[i] = visionimg_to_gl(&img); + + glBindTexture(GL_TEXTURE_2D, s->frame_texs[i]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + + // BGR + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); + } + + for (int i=0; iframe_front_texs[i]); + + VisionImg img = { + .fd = s->front_bufs[i].fd, + .format = VISIONIMG_FORMAT_RGB24, + .width = s->rgb_front_width, + .height = s->rgb_front_height, + .stride = s->rgb_front_stride, + .bpp = 3, + .size = s->rgb_front_buf_len, + }; + s->frame_front_texs[i] = visionimg_to_gl(&img); + + glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[i]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + + // BGR + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); + } assert(glGetError() == GL_NO_ERROR); @@ -1245,6 +1253,9 @@ static void ui_update(UIState *s) { s->scene.ui_viz_ro = 0; s->vision_connect_firstrun = false; + + s->alert_blinking_alpha = 1.0; + s->alert_blinked = false; } // poll for events @@ -1302,7 +1313,7 @@ static void ui_update(UIState *s) { continue; } if (rp.type == VIPC_STREAM_ACQUIRE) { - bool front = rp.d.stream_acq.type == VISION_STREAM_UI_FRONT; + bool front = rp.d.stream_acq.type == VISION_STREAM_RGB_FRONT; int idx = rp.d.stream_acq.idx; int release_idx; @@ -1325,17 +1336,11 @@ static void ui_update(UIState *s) { if (front) { assert(idx < UI_BUF_COUNT); s->cur_vision_front_idx = idx; - s->scene.bgr_front_ptr = s->front_bufs[idx].addr; } else { assert(idx < UI_BUF_COUNT); s->cur_vision_idx = idx; - s->scene.bgr_ptr = s->bufs[idx].addr; // printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]); } - if (front == s->scene.frontview) { - ui_update_frame(s); - } - } else { assert(false); } @@ -1392,6 +1397,7 @@ static void ui_update(UIState *s) { s->scene.v_ego = datad.vEgo; s->scene.curvature = datad.curvature; s->scene.engaged = datad.enabled; + s->scene.engageable = datad.engageable; s->scene.gps_planner_active = datad.gpsPlannerActive; // printf("recv %f\n", datad.vEgo); @@ -1431,6 +1437,24 @@ static void ui_update(UIState *s) { update_status(s, STATUS_DISENGAGED); } + s->scene.alert_blinkingrate = datad.alertBlinkingRate; + if (datad.alertBlinkingRate > 0.) { + if (s->alert_blinked) { + if (s->alert_blinking_alpha > 0.0 && s->alert_blinking_alpha < 1.0) { + s->alert_blinking_alpha += (0.05*datad.alertBlinkingRate); + } else { + s->alert_blinked = false; + } + } else { + if (s->alert_blinking_alpha > 0.25) { + s->alert_blinking_alpha -= (0.05*datad.alertBlinkingRate); + } else { + s->alert_blinking_alpha += 0.25; + s->alert_blinked = true; + } + } + } + } else if (eventd.which == cereal_Event_live20) { struct cereal_Live20Data datad; cereal_read_Live20Data(&datad, eventd.live20); @@ -1570,8 +1594,8 @@ static void* vision_connect_thread(void *args) { if (fd < 0) continue; VisionPacket back_rp, front_rp; - if (!vision_subscribe(fd, &back_rp, VISION_STREAM_UI_BACK)) continue; - if (!vision_subscribe(fd, &front_rp, VISION_STREAM_UI_FRONT)) continue; + if (!vision_subscribe(fd, &back_rp, VISION_STREAM_RGB_BACK)) continue; + if (!vision_subscribe(fd, &front_rp, VISION_STREAM_RGB_FRONT)) continue; pthread_mutex_lock(&s->lock); assert(!s->vision_connected); @@ -1649,28 +1673,24 @@ static void* bg_thread(void* args) { &bg_display, &bg_surface, NULL, NULL); assert(bg_fb); - bool first = true; + int bg_status = -1; while(!do_exit) { pthread_mutex_lock(&s->lock); - - if (first) { - first = false; - } else { + if (bg_status == s->status) { + // will always be signaled if it changes? pthread_cond_wait(&s->bg_cond, &s->lock); } - - assert(s->status < ARRAYSIZE(bg_colors)); - const uint8_t *color = bg_colors[s->status]; - + bg_status = s->status; pthread_mutex_unlock(&s->lock); + assert(bg_status < ARRAYSIZE(bg_colors)); + const uint8_t *color = bg_colors[bg_status]; + glClearColor(color[0]/256.0, color[1]/256.0, color[2]/256.0, 0.0); glClear(GL_COLOR_BUFFER_BIT); - eglSwapBuffers(bg_display, bg_surface); assert(glGetError() == GL_NO_ERROR); - } return NULL; @@ -1679,6 +1699,7 @@ static void* bg_thread(void* args) { int main() { int err; + setpriority(PRIO_PROCESS, 0, -14); zsys_handler_set(NULL); signal(SIGINT, (sighandler_t)set_do_exit); @@ -1716,6 +1737,7 @@ int main() { const int EON = (access("/EON", F_OK) != -1); while (!do_exit) { + bool should_swap = false; pthread_mutex_lock(&s->lock); if (EON) { @@ -1731,13 +1753,10 @@ int main() { } ui_update(s); - if (s->awake) { - ui_draw(s); - } // awake on any touch int touch_x = -1, touch_y = -1; - int touched = touch_poll(&touch, &touch_x, &touch_y); + int touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 100); if (touched == 1) { // touch event will still happen :( set_awake(s, true); @@ -1750,10 +1769,19 @@ int main() { set_awake(s, false); } + if (s->awake) { + ui_draw(s); + glFinish(); + should_swap = true; + } + pthread_mutex_unlock(&s->lock); - // no simple way to do 30fps vsync with surfaceflinger... - usleep(30000); + // the bg thread needs to be scheduled, so the main thread needs time without the lock + // safe to do this outside the lock? + if (should_swap) { + eglSwapBuffers(s->display, s->surface); + } } set_awake(s, true); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index fd5887e111..c8efd8f3a8 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -1,12 +1,9 @@ #!/usr/bin/env python -# simple service that waits for network access and tries to update every 3 hours +# simple service that waits for network access and tries to update every hour -import os import time import subprocess - -from common.basedir import BASEDIR from selfdrive.swaglog import cloudlog NICE_LOW_PRIORITY = ["nice", "-n", "19"] @@ -19,17 +16,18 @@ def main(gctx=None): continue # download application update - r = subprocess.call(NICE_LOW_PRIORITY + ["git", "fetch"]) - cloudlog.info("git fetch: %r", r) - if r: + try: + r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT) + except subprocess.CalledProcessError, e: + cloudlog.event("git fetch failed", + cmd=e.cmd, + output=e.output, + returncode=e.returncode) time.sleep(60) continue + cloudlog.info("git fetch success: %s", r) - # download apks - r = subprocess.call(NICE_LOW_PRIORITY + [os.path.join(BASEDIR, "apk/external/patcher.py"), "download"]) - cloudlog.info("patcher download: %r", r) - - time.sleep(60*60*3) + time.sleep(60*60) if __name__ == "__main__": main() diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 6180106f33..b292f4fa36 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9cbff07d194fcd129b105b4b9b6aa6a274e130911f10dda614db1ef60092600e -size 13522344 +oid sha256:a21acff35bda607b6780df7730e005a3b289acfc737a1340a7b9f7a3178d83a0 +size 13965736