diff --git a/README.md b/README.md index f8de745a27..01ca0d8248 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ 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 and Acuras. 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). +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). 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. @@ -29,13 +29,18 @@ Supported Cars - Honda CR-V Touring 2015-2016 - Can only be enabled above 25 mph -- Toyota RAV-4 2016+ with TSS-P (alpha!) +- 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#Prius_.28for_openpilot.29) and can be enabled above 20 mph + - 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 (alpha!) - 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) + - 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 (alpha!) + - 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 In Progress Cars ------ @@ -50,6 +55,12 @@ Community WIP Cars - [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145) +- [Honda Odyssey 2018 with Honda Sensing](https://github.com/commaai/openpilot/pull/155) + +- [Honda Pilot 2017 with Honda Sensing](https://github.com/commaai/openpilot/pull/161) + +- [Acura RDX 2018 with AcuraWatch Plus](https://github.com/commaai/openpilot/pull/162) + Directory structure ------ diff --git a/RELEASES.md b/RELEASES.md index e4d234a06a..fbcabd13d3 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,12 @@ +Version 0.4.0.1 (2017-12-21) +========================== + * New UI to match chffrplus + * Improved lateral control tuning to fix oscillations on Civic + * Add alpha support for 2017 Toyota Rav4 Hybrid + * Reduced CPU usage + * Removed unnecessary utilization of fan at max speed + * Minor bug fixes + Version 0.3.9 (2017-11-21) ========================== * Add alpha support for 2017 Toyota Prius diff --git a/apk/ai.comma.plus.black.apk b/apk/ai.comma.plus.black.apk new file mode 100644 index 0000000000..67168f48fc --- /dev/null +++ b/apk/ai.comma.plus.black.apk @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6e6f3a8037283b87271a7bb0c95a9ae1a8ff5a86dea7e2b25d49911179b6e05e +size 84675 diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk new file mode 100644 index 0000000000..631f0cfb7d --- /dev/null +++ b/apk/ai.comma.plus.frame.apk @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85eec719a82974770a70fec2c8cf47b12f395f93be39f4e11f50918622170f54 +size 2077093 diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk new file mode 100644 index 0000000000..ed46e1a741 --- /dev/null +++ b/apk/ai.comma.plus.offroad.apk @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:441c03e9bcb881ecc7933872d31d9dc7734a8673f3898fc8d8c49c6a15875b72 +size 15382430 diff --git a/apk/com.baseui.apk b/apk/com.baseui.apk deleted file mode 100644 index 3c258c7cdb..0000000000 --- a/apk/com.baseui.apk +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6a4fc195fb4160a445c9e5b1d1aa4e37bcf8c3271890352da8b33524893dfa1c -size 6338698 diff --git a/cereal/car.capnp b/cereal/car.capnp index 6b58254d88..63857ed744 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -12,12 +12,13 @@ $Java.outerClassname("Car"); struct CarEvent @0x9b1657f34caf3ad3 { name @0 :EventName; enable @1 :Bool; - preEnable @7 :Bool; noEntry @2 :Bool; warning @3 :Bool; userDisable @4 :Bool; softDisable @5 :Bool; immediateDisable @6 :Bool; + preEnable @7 :Bool; + permanent @8 :Bool; enum EventName @0xbaa8c5d505f727de { # TODO: copy from error list @@ -64,9 +65,10 @@ struct CarState { events @13 :List(CarEvent); # car speed - vEgo @1 :Float32; # best estimate of speed - aEgo @16 :Float32; # best estimate of acceleration - vEgoRaw @17 :Float32; # unfiltered speed + vEgo @1 :Float32; # best estimate of speed + aEgo @16 :Float32; # best estimate of acceleration + vEgoRaw @17 :Float32; # unfiltered speed from CAN sensors + yawRate @22 :Float32; # best estimate of yaw rate standstill @18 :Bool; wheelSpeeds @2 :WheelSpeeds; @@ -309,4 +311,5 @@ 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 + steerRateCost @40 :Float32; # Lateral MPC cost on steering rate } diff --git a/cereal/log.capnp b/cereal/log.capnp index e1d61b88bc..c3ddad9bd3 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -28,6 +28,7 @@ struct InitData { version @4 :Text; gitCommit @10 :Text; gitBranch @11 :Text; + gitRemote @13 :Text; androidBuildInfo @5 :AndroidBuildInfo; androidSensors @6 :List(AndroidSensor); @@ -154,6 +155,8 @@ struct SensorEventData { pressure @9 :SensorVec; magneticUncalibrated @11 :SensorVec; gyroUncalibrated @12 :SensorVec; + proximity @13: Float32; + light @14: Float32; } source @8 :SensorSource; @@ -242,8 +245,11 @@ struct ThermalData { freeSpace @7 :Float32; batteryPercent @8 :Int16; batteryStatus @9 :Text; + usbOnline @12 :Bool; fanSpeed @10 :UInt16; + started @11 :Bool; + startedTs @13 :UInt64; } struct HealthData { @@ -349,6 +355,7 @@ struct Live100Data { jerkFactor @12 :Float32; angleSteers @13 :Float32; # Steering angle in degrees. angleSteersDes @29 :Float32; + curvature @37 :Float32; # path curvature from vehicle model hudLeadDEPRECATED @14 :Int32; cumLagMs @15 :Float32; @@ -361,6 +368,8 @@ struct Live100Data { rearViewCam @23 :Bool; alertText1 @24 :Text; alertText2 @25 :Text; + alertStatus @38 :AlertStatus; + alertSize @39 :AlertSize; awarenessStatus @26 :Float32; angleOffset @27 :Float32; @@ -378,6 +387,20 @@ struct Live100Data { stopping @2; starting @3; } + + enum AlertStatus { + normal @0; # low priority alert for user's convenience + userPrompt @1; # mid piority alert that might require user intervention + critical @2; # high priority alert that needs immediate user intervention + } + + enum AlertSize { + none @0; # don't display the alert + small @1; # small box + mid @2; # mid screen + full @3; # full screen + } + } struct LiveEventData { @@ -600,6 +623,23 @@ struct NavUpdate { } } +struct NavStatus { + isNavigating @0 :Bool; + currentAddress @1 :Address; + + struct Address { + title @0 :Text; + lat @1 :Float64; + lng @2 :Float64; + house @3 :Text; + address @4 :Text; + street @5 :Text; + city @6 :Text; + state @7 :Text; + country @8 :Text; + } +} + struct CellInfo { timestamp @0 :UInt64; repr @1 :Text; # android toString() for now @@ -1300,5 +1340,6 @@ struct Event { clocks @35 :Clocks; liveMpc @36 :LiveMpcData; liveLongitudinalMpc @37 :LiveLongitudinalMpcData; + navStatus @38 :NavStatus; } } diff --git a/common/basedir.py b/common/basedir.py index 991f6aaed9..99760fa334 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,4 +1,4 @@ import os -BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") +BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) diff --git a/common/fingerprints.py b/common/fingerprints.py index 9881115b69..d5b9cc91d0 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -19,6 +19,9 @@ _FINGERPRINTS = { "TOYOTA RAV4 2017": { 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, 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, 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 }, + "TOYOTA RAV4 2017 HYBRID": { + 36L: 8, 37L: 8, 170L: 8, 180L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 713L: 8, 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, 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, 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, 1232L: 8, 1235L: 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, 1596L: 8, 1597L: 8, 1600L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8 + }, "TOYOTA PRIUS 2017": { 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 }, diff --git a/common/kalman/ekf.py b/common/kalman/ekf.py index 454966d64d..feebe0883f 100644 --- a/common/kalman/ekf.py +++ b/common/kalman/ekf.py @@ -35,7 +35,7 @@ class SensorReading: # A generic sensor class that does no pre-processing of data class SimpleSensor: # obs_model can be - # a full obesrvation model matrix, or + # a full observation model matrix, or # an integer or tuple of indices into ekf.state, indicating which variables are being directly observed # covar can be # a full covariance matrix @@ -129,7 +129,7 @@ class EKF: print "covar:\n",self.covar def update_scalar(self, reading): - # like update but knowing that measurment is a scalar + # like update but knowing that measurement is a scalar # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot # innovation = reading.data - np.matmul(reading.obs_model, self.state) @@ -188,7 +188,7 @@ class EKF: Current implementations calculate A and J as functions of state. Control input can be added trivially by adding a control parameter to predict() and calc_tranfer_update(), - and using it during calcualtion of A and J + and using it during calculation of A and J """ diff --git a/common/params.py b/common/params.py index 4701608561..d6503291eb 100755 --- a/common/params.py +++ b/common/params.py @@ -52,12 +52,14 @@ keys = { "Version": TxType.PERSISTANT, "GitCommit": TxType.PERSISTANT, "GitBranch": TxType.PERSISTANT, + "GitRemote": TxType.PERSISTANT, # written: baseui # read: ui, controls "IsMetric": TxType.PERSISTANT, "IsRearViewMirror": TxType.PERSISTANT, "IsFcwEnabled": TxType.PERSISTANT, "HasAcceptedTerms": TxType.PERSISTANT, + "IsUploadVideoOverCellularEnabled": TxType.PERSISTANT, # written: visiond # read: visiond, controlsd "CalibrationParams": TxType.PERSISTANT, @@ -69,6 +71,7 @@ keys = { "CarParams": TxType.CLEAR_ON_CAR_START, "Passive": TxType.PERSISTANT, + "DoUninstall": TxType.CLEAR_ON_MANAGER_START, } def fsync_dir(path): diff --git a/common/profiler.py b/common/profiler.py index 26fbeb382b..83e5672e04 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -1,32 +1,47 @@ -from common.realtime import sec_since_boot +import time class Profiler(object): def __init__(self, enabled=False): self.enabled = enabled - self.cp = [] - self.start_time = sec_since_boot() + self.cp = {} + self.cp_ignored = [] + self.iter = 0 + self.start_time = time.clock() self.last_time = self.start_time - - def checkpoint(self, name): - if not self.enabled: - return - tt = sec_since_boot() - self.cp.append((name, tt - self.last_time)) - self.last_time = tt + self.tot = 0. def reset(self, enabled=False): self.enabled = enabled - self.cp = [] - self.start_time = sec_since_boot() + self.cp = {} + self.cp_ignored = [] + self.iter = 0 + self.start_time = time.clock() self.last_time = self.start_time + def checkpoint(self, name, ignore=False): + # ignore flag needed when benchmarking threads with ratekeeper + if not self.enabled: + return + tt = time.clock() + if name not in self.cp: + self.cp[name] = 0. + if ignore: + self.cp_ignored.append(name) + self.cp[name] += tt - self.last_time + if not ignore: + self.tot += tt - self.last_time + self.last_time = tt + def display(self): if not self.enabled: return + self.iter += 1 print "******* Profiling *******" - tot = 0.0 - for n, ms in self.cp: - print "%30s: %7.2f" % (n, ms*1000.0) - tot += ms - print " TOTAL: %7.2f" % (tot*1000.0) + for n in self.cp: + ms = self.cp[n] + if n in self.cp_ignored: + print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED" + else: + print "%30s: %7.2f perc: %1.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/launch_openpilot.sh b/launch_openpilot.sh new file mode 100755 index 0000000000..4b55f80f11 --- /dev/null +++ b/launch_openpilot.sh @@ -0,0 +1,39 @@ +#!/usr/bin/bash + +function launch { + # apply update + if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then + git reset --hard @{u} && + git clean -xdf && + exec "${BASH_SOURCE[0]}" + fi + + # check if NEOS update is required + while [ "$(cat /VERSION)" -lt 4 ] && [ ! -e /data/media/0/noupdate ]; do + curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater + sleep 10 + done + + # no cpu rationing for now + echo 0-3 > /dev/cpuset/background/cpus + echo 0-3 > /dev/cpuset/system-background/cpus + echo 0-3 > /dev/cpuset/foreground/boost/cpus + echo 0-3 > /dev/cpuset/foreground/cpus + echo 0-3 > /dev/cpuset/android/cpus + + # wait for network + (cd selfdrive/ui/spinner && exec ./spinner 'waiting for network...') & spin_pid=$! + until ping -W 1 -c 1 8.8.8.8; do sleep 1; done + kill $spin_pid + + export PYTHONPATH="$PWD" + + # start manager + cd selfdrive + ./manager.py + + # if broken, keep on screen error + while true; do sleep 1; done +} + +launch diff --git a/selfdrive/assets/OpenSans-Regular.ttf b/selfdrive/assets/OpenSans-Regular.ttf new file mode 100644 index 0000000000..e210b6b173 --- /dev/null +++ b/selfdrive/assets/OpenSans-Regular.ttf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:13c03e22a633919beb2847c58c8285fb8a735ee97097d7c48fd403f8294b05f8 +size 217276 diff --git a/selfdrive/assets/OpenSans-SemiBold.ttf b/selfdrive/assets/OpenSans-SemiBold.ttf new file mode 100644 index 0000000000..7f2753ed46 --- /dev/null +++ b/selfdrive/assets/OpenSans-SemiBold.ttf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b4c2050b25d3d296d5cf58589ca00816dc72df42262c2f629d5c6a984a161aa4 +size 221164 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 48aecca0cd..285134de9e 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -130,7 +130,11 @@ bool usb_connect() { #endif // no output is the default - libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); + if (getenv("RECVMOCK")) { + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT); + } else { + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); + } if (safety_setter_thread_handle == -1) { err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py index 0afeee40ef..7a5ae47591 100644 --- a/selfdrive/can/libdbc_py.py +++ b/selfdrive/can/libdbc_py.py @@ -1,11 +1,12 @@ import os +import sys import subprocess from cffi import FFI can_dir = os.path.dirname(os.path.abspath(__file__)) libdbc_fn = os.path.join(can_dir, "libdbc.so") -subprocess.check_output(["make"], cwd=can_dir) +subprocess.check_call(["make"], stdout=sys.stderr, cwd=can_dir) ffi = FFI() ffi.cdef(""" diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 853b0b12a7..828f95a939 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -242,7 +242,7 @@ class CANParser { const auto& state = kv.second; if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { if (state.seen > 0) { - INFO("%X TIMEOUT\n", state.address); + DEBUG("%X TIMEOUT\n", state.address); } can_valid = false; } diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 516ed7c0c0..4526d35c40 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -6,12 +6,9 @@ from common.fingerprints import eliminate_incompatible_cars, all_known_cars from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging -from .honda.interface import CarInterface as HondaInterface - -try: - from .toyota.interface import CarInterface as ToyotaInterface -except ImportError: - ToyotaInterface = None +from selfdrive.car.honda.interface import CarInterface as HondaInterface +from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface +from selfdrive.car.mock.interface import CarInterface as MockInterface try: from .simulator.interface import CarInterface as SimInterface @@ -31,9 +28,12 @@ interfaces = { "HONDA CR-V 2016 TOURING": HondaInterface, "TOYOTA PRIUS 2017": ToyotaInterface, "TOYOTA RAV4 2017": ToyotaInterface, + "TOYOTA RAV4 2017 HYBRID": ToyotaInterface, "simulator": SimInterface, - "simulator2": Sim2Interface + "simulator2": Sim2Interface, + + "mock": MockInterface } # **** for use live only **** @@ -75,12 +75,18 @@ def fingerprint(logcan, timeout): return (candidate_cars[0], finger) -def get_car(logcan, sendcan=None, timeout=None): +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 candidate, fingerprints = fingerprint(logcan, timeout) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) - return None, None + if passive: + candidate = "mock" + else: + return None, None interface_cls = interfaces[candidate] params = interface_cls.get_params(candidate, fingerprints) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index fbf51f36a1..1ee7e2c808 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -122,7 +122,7 @@ class CarController(object): GAS_MAX = 1004 BRAKE_MAX = 1024/4 if CS.civic: - is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx'] + is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] STEER_MAX = 0x1FFF if is_fw_modified else 0x1000 elif CS.crv: STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 6ade91d580..015ff7c573 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,5 +1,4 @@ import os -import time from cereal import car from common.numpy_fast import interp import selfdrive.messaging as messaging diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 62bb834cbb..7879a9ef1e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import os -import time import numpy as np from common.numpy_fast import clip, interp from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events +from selfdrive.controls.lib.vehicle_model import VehicleModel from cereal import car from selfdrive.services import service_list import selfdrive.messaging as messaging @@ -93,6 +93,7 @@ class CarInterface(object): # *** init the major players *** self.CS = CarState(CP) + self.VM = VehicleModel(CP) # sending if read only is False if sendcan is not None: @@ -163,7 +164,7 @@ class CarInterface(object): ret.centerToFront = centerToFront_civic ret.steerRatio = 13.0 # Civic at comma has modified steering FW, so different tuning for the Neo in that car - is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx'] + is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] ret.longitudinalKpBP = [0., 5., 35.] @@ -252,6 +253,8 @@ class CarInterface(object): ret.steerLimitAlert = True ret.startAccel = 0.5 + ret.steerRateCost = 0.5 + return ret # returns a car.CarState @@ -270,6 +273,7 @@ class CarInterface(object): ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw + ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr @@ -370,11 +374,11 @@ class CarInterface(object): else: self.can_invalid_count = 0 if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_not_allowed: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: - events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.door_all_closed: diff --git a/selfdrive/car/mock/__init__.py b/selfdrive/car/mock/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py new file mode 100755 index 0000000000..6acad5e5a9 --- /dev/null +++ b/selfdrive/car/mock/interface.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python +import os +import time +import zmq +from common.realtime import sec_since_boot +import common.numpy_fast as np +from cereal import car +from selfdrive.config import Conversions as CV +from selfdrive.services import service_list +import selfdrive.messaging as messaging +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event + +# mocked car interface to work with chffrplus +TS = 0.01 # 100Hz +YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter +# low pass gain +LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS) + + +class CarInterface(object): + def __init__(self, CP, sendcan=None): + + self.CP = CP + + print "Using Mock Car Interface" + context = zmq.Context() + + # TODO: subscribe to phone sensor + self.sensor = messaging.sub_sock(context, service_list['sensorEvents'].port) + self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port) + + self.speed = 0. + self.yaw_rate = 0. + self.yaw_rate_meas = 0. + + @staticmethod + def compute_gb(accel, speed): + return accel + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + return 1.0 + + @staticmethod + def get_params(candidate, fingerprint): + + ret = car.CarParams.new_message() + + ret.carName = "mock" + ret.radarName = "mock" + ret.carFingerprint = candidate + + ret.safetyModel = car.CarParams.SafetyModels.noOutput + + # FIXME: hardcoding honda civic 2016 touring params so they can be used to + # scale unknown params for other cars + ret.mass = 1700. + ret.rotationalInertia = 2500. + ret.wheelbase = 2.70 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 13. # reasonable + ret.longPidDeadzoneBP = [0.] + ret.longPidDeadzoneV = [0.] + ret.tireStiffnessFront = 1e6 # very stiff to neglect slip + ret.tireStiffnessRear = 1e6 # very stiff to neglect slip + ret.steerRatioRear = 0. + + ret.steerMaxBP = [0.] + ret.steerMaxV = [0.] # 2/3rd torque allowed above 45 kph + ret.gasMaxBP = [0.] + ret.gasMaxV = [0.] + ret.brakeMaxBP = [0.] + ret.brakeMaxV = [0.] + + ret.longitudinalKpBP = [0.] + ret.longitudinalKpV = [0.] + ret.longitudinalKiBP = [0.] + ret.longitudinalKiV = [0.] + + return ret + + # returns a car.CarState + def update(self, c): + + # get basic data from phone and gps since CAN isn't connected + sensors = messaging.recv_sock(self.sensor) + if sensors is not None: + for sensor in sensors.sensorEvents: + if sensor.type == 4: # gyro + self.yaw_rate_meas = -sensor.gyro.v[0] + + gps = messaging.recv_sock(self.gps) + if gps is not None: + self.speed = gps.gpsLocation.speed + + # create message + ret = car.CarState.new_message() + + # speeds + ret.vEgo = self.speed + ret.vEgoRaw = self.speed + ret.aEgo = 0. + self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate + ret.yawRate = self.yaw_rate + ret.standstill = self.speed < 0.01 + ret.wheelSpeeds.fl = self.speed + ret.wheelSpeeds.fr = self.speed + ret.wheelSpeeds.rl = self.speed + ret.wheelSpeeds.rr = self.speed + curvature = self.yaw_rate / max(self.speed, 1.) + ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG + + events = [] + #events.append(create_event('passive', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + ret.events = events + + return ret.as_reader() + + def apply(self, c): + # in mock no carcontrols + return False diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 097bf361d7..5b56e2a6f2 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -6,7 +6,7 @@ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ create_fcw_command -from selfdrive.car.toyota.values import CAR, ECU +from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value @@ -27,60 +27,6 @@ TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345, 0x373, 0x374, 0x375, 0x380, 0x381, 0x382, 0x383] -# addr, [ecu, bus, 1/freq*100, vl] -STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'), - 0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'), - - 0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), - 0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), - 0x2E6: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), - 0x2E7: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), - - 0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), - 0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), - - 0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), - 0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), - - 0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), - 0x33E: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), - 0x365: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), - 0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), - 0x366: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), - 0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), - - 0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 40, '\x06\x00'), - - 0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), - 0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - 0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - 0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), - 0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), - 0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), - 0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x20\x20\xAD'), - 0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), - 0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), - 0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - 0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - 0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), - 0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), - 0x470: (ECU.DSU, (CAR.PRIUS,), 1, 100, '\x00\x00\x02\x7a'), - } - - -def check_ecu_msgs(fingerprint, candidate, ecu): - # return True if fingerprint contains messages normally sent by a given ecu - ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and - candidate in STATIC_MSGS[x][1] and - STATIC_MSGS[x][2] == 0)] - - return any(msg for msg in fingerprint if msg in ecu_msgs) - - def accel_hysteresis(accel, accel_steady, enabled): # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 2b948ac493..e85902f944 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,5 +1,4 @@ import os -import time import selfdrive.messaging as messaging from selfdrive.car.toyota.values import CAR from selfdrive.can.parser import CANParser @@ -19,7 +18,7 @@ def parse_gear_shifter(can_gear, car_fingerprint): return "drive" elif can_gear == 0x4: return "brake" - elif car_fingerprint == CAR.RAV4: + elif car_fingerprint in [CAR.RAV4, CAR.RAV4H]: if can_gear == 0x20: return "park" elif can_gear == 0x10: @@ -47,6 +46,17 @@ def get_can_parser(CP): (550, 40), (581, 33) ] + elif CP.carFingerprint == CAR.RAV4H: + dbc_f = 'toyota_rav4_hybrid_2017_pt.dbc' + signals = [ + ("GEAR", 956, 0), + ("BRAKE_PRESSED", 550, 0), + ("GAS_PEDAL", 581, 0), + ] + checks = [ + (550, 40), + (581, 33) + ] elif CP.carFingerprint == CAR.RAV4: dbc_f = 'toyota_rav4_2017_pt.dbc' signals = [ @@ -131,6 +141,10 @@ class CarState(object): can_gear = cp.vl[295]['GEAR'] self.brake_pressed = cp.vl[550]['BRAKE_PRESSED'] self.pedal_gas = cp.vl[581]['GAS_PEDAL'] + elif self.car_fingerprint == CAR.RAV4H: + can_gear = cp.vl[956]['GEAR'] + self.brake_pressed = cp.vl[550]['BRAKE_PRESSED'] + self.pedal_gas = cp.vl[581]['GAS_PEDAL'] elif self.car_fingerprint == CAR.RAV4: can_gear = cp.vl[956]['GEAR'] self.brake_pressed = cp.vl[548]['BRAKE_PRESSED'] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index fc75335361..c490c1db23 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,20 +1,24 @@ #!/usr/bin/env python import os -import time from common.realtime import sec_since_boot import common.numpy_fast as np -from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.carstate import CarState, get_can_parser -from selfdrive.car.toyota.values import CAR, ECU -from selfdrive.car.toyota.carcontroller import CarController, check_ecu_msgs from cereal import car +from selfdrive.config import Conversions as CV from selfdrive.services import service_list import selfdrive.messaging as messaging 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 CAR, ECU, check_ecu_msgs +try: + from selfdrive.car.toyota.carcontroller import CarController +except ImportError: + CarController = None class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP + self.VM = VehicleModel(CP) self.frame = 0 self.can_invalid_count = 0 @@ -70,9 +74,8 @@ class CarInterface(object): tireStiffnessFront_civic = 85400 tireStiffnessRear_civic = 90000 - stop_and_go = True ret.mass = 3045./2.205 + std_cargo - ret.wheelbase = 2.70 + ret.wheelbase = 2.70 if candidate == CAR.PRIUS else 2.65 ret.centerToFront = ret.wheelbase * 0.44 ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius ret.steerKp, ret.steerKi = 0.6, 0.05 @@ -83,9 +86,9 @@ class CarInterface(object): # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. - if candidate == CAR.PRIUS: + if candidate in [CAR.PRIUS, CAR.RAV4H]: # rav4 hybrid can do stop and go ret.minEnableSpeed = -1. - elif candidate == CAR.RAV4: # TODO: hack Rav4 to do stop and go + elif candidate == CAR.RAV4: # TODO: hack ICE Rav4 to do stop and go ret.minEnableSpeed = 19. * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront @@ -131,6 +134,11 @@ class CarInterface(object): ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.54, 0.36] + if candidate == CAR.PRIUS: + ret.steerRateCost = 2. + elif candidate in [CAR.RAV4, CAR.RAV4H]: + ret.steerRateCost = 1. + return ret # returns a car.CarState @@ -150,6 +158,7 @@ class CarInterface(object): ret.vEgo = self.CS.v_ego ret.vEgoRaw = self.CS.v_ego_raw ret.aEgo = self.CS.a_ego + ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr @@ -180,7 +189,12 @@ class CarInterface(object): ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. - ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 + if self.CP.carFingerprint == CAR.RAV4H: + # ignore standstill in hybrid rav4, since pcm allows to restart without + # receiving any special command + ret.cruiseState.standstill = False + else: + ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 # TODO: button presses buttonEvents = [] @@ -223,8 +237,8 @@ class CarInterface(object): events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steer_error: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - if self.CS.low_speed_lockout: - events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY])) + if self.CS.low_speed_lockout and self.CP.enableDsu: + events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if c.actuators.gas > 0.1: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 06373b3b18..d8fa0c699e 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,8 +1,64 @@ class CAR: - PRIUS = "TOYOTA PRIUS 2017" - RAV4 = "TOYOTA RAV4 2017" - + PRIUS = "TOYOTA PRIUS 2017" + RAV4H = "TOYOTA RAV4 2017 HYBRID" + RAV4 = "TOYOTA RAV4 2017" + + class ECU: CAM = 0 # camera DSU = 1 # driving support unit APGS = 2 # advanced parking guidance system + + +# addr, [ecu, bus, 1/freq*100, vl] +STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'), + 0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'), + + 0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), + 0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), + 0x2E6: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), + 0x2E7: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), + + 0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), + 0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), + + 0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), + 0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), + + 0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x33E: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), + 0x365: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), + 0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), + 0x366: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), + 0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), + + 0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 40, '\x06\x00'), + + 0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), + 0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + 0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + 0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), + 0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), + 0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), + 0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x20\x20\xAD'), + 0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), + 0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), + 0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), + 0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), + 0x470: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 1, 100, '\x00\x00\x02\x7a'), + } + + +def check_ecu_msgs(fingerprint, candidate, ecu): + # return True if fingerprint contains messages normally sent by a given ecu + ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and + candidate in STATIC_MSGS[x][1] and + STATIC_MSGS[x][2] == 0)] + + return any(msg for msg in fingerprint if msg in ecu_msgs) diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 777dde97d1..4cf48d2e1f 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.3.9-openpilot" +#define COMMA_VERSION "0.4.0.1-openpilot" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 75ace9cdbe..df27d6322a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,7 +2,7 @@ import json from copy import copy import zmq -from cereal import car +from cereal import car, log from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper from common.profiler import Profiler @@ -30,6 +30,7 @@ V_CRUISE_ENABLE_MIN = 40 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 +State = log.Live100Data.ControlState class Calibration: UNCALIBRATED = 0 @@ -37,38 +38,43 @@ class Calibration: INVALID = 2 -class State: - DISABLED = 'disabled' - ENABLED = 'enabled' - PRE_ENABLED = 'preEnabled' - SOFT_DISABLING = 'softDisabling' - - # True when actuators are controlled def isActive(state): - return state in [State.ENABLED, State.SOFT_DISABLING] + return state in [State.enabled, State.softDisabling] # True if system is engaged def isEnabled(state): - return (isActive(state) or state == State.PRE_ENABLED) + return (isActive(state) or state == State.preEnabled) -def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): +def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overtemp, free_space): # *** read can and compute car states *** CS = CI.update(CC) events = list(CS.events) + td = None + cal = None + hh = None + + for socket, event in poller.poll(0): + if socket is thermal: + td = messaging.recv_one(socket) + elif socket is calibration: + cal = messaging.recv_one(socket) + elif socket is health: + hh = messaging.recv_one(socket) + # *** thermal checking logic *** # thermal data, checked every second - td = messaging.recv_sock(thermal) if td is not None: - # overtemp above 95 deg - overtemp = any( - t > 950 - for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, - td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) + # 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 > 50000 # 50c + overtemp = overtemp_proc or overtemp_bat # under 15% of space free no enable allowed free_space = td.thermal.freeSpace < 0.15 @@ -80,7 +86,6 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): events.append(create_event('outOfSpace', [ET.NO_ENTRY])) # *** read calibration status *** - cal = messaging.recv_sock(cal) if cal is not None: cal_status = cal.liveCalibration.calStatus @@ -91,11 +96,10 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # *** health checking logic *** - hh = messaging.recv_sock(health) if hh is not None: controls_allowed = hh.health.controlsAllowed if not controls_allowed: - events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) return CS, events, cal_status, overtemp, free_space @@ -123,6 +127,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition + v_cruise_kph_last = v_cruise_kph for b in CS.buttonEvents: if not CP.enableCruise and enabled and not b.pressed: if b.type == "accelCruise": @@ -138,76 +143,75 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM # ***** handle state transitions ***** # DISABLED - if state == State.DISABLED: + if state == State.disabled: if get_events(events, [ET.ENABLE]): - if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]): - for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]): - AM.add(e, enabled) + if get_events(events, [ET.NO_ENTRY]): for e in get_events(events, [ET.NO_ENTRY]): AM.add(str(e) + "NoEntry", enabled) + else: if get_events(events, [ET.PRE_ENABLE]): - state = State.PRE_ENABLED + state = State.preEnabled else: - state = State.ENABLED + state = State.enabled AM.add("enable", enabled) # on activation, let's always set v_cruise from where we are, even if PCM ACC is active v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) # ENABLED - elif state == State.ENABLED: + elif state == State.enabled: if get_events(events, [ET.USER_DISABLE]): - state = State.DISABLED + state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): - state = State.DISABLED + state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif get_events(events, [ET.SOFT_DISABLE]): - state = State.SOFT_DISABLING + state = State.softDisabling soft_disable_timer = 300 # 3s TODO: use rate for e in get_events(events, [ET.SOFT_DISABLE]): AM.add(e, enabled) # SOFT DISABLING - elif state == State.SOFT_DISABLING: + elif state == State.softDisabling: if get_events(events, [ET.USER_DISABLE]): - state = State.DISABLED + state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): - state = State.DISABLED + state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.SOFT_DISABLE]): # no more soft disabling condition, so go back to ENABLED - state = State.ENABLED + state = State.enabled elif soft_disable_timer <= 0: - state = State.DISABLED + state = State.disabled - # TODO: PRE ENABLING - elif state == State.PRE_ENABLED: + # PRE ENABLING + elif state == State.preEnabled: if get_events(events, [ET.USER_DISABLE]): - state = State.DISABLED + state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): - state = State.DISABLED + state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.PRE_ENABLE]): - state = State.ENABLED + state = State.enabled - return state, soft_disable_timer, v_cruise_kph + return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last -def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status, - PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle): +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): # Given the state, this function returns the actuators # reset actuators to zero @@ -217,9 +221,6 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s active = isActive(state) for b in CS.buttonEvents: - # any button event resets awarenesss_status - awareness_status = 1. - # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": if b.pressed and rear_view_allowed: @@ -238,24 +239,20 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s # ***** state specific actions ***** # DISABLED - if state in [State.PRE_ENABLED, State.DISABLED]: - awareness_status = 1. + if state in [State.preEnabled, State.disabled]: + LaC.reset() LoC.reset(v_pid=CS.vEgo) # ENABLED or SOFT_DISABLING - elif state in [State.ENABLED, State.SOFT_DISABLING]: - - if CS.steeringPressed: - # reset awareness status on steering - awareness_status = 1.0 + elif state in [State.enabled, State.softDisabling]: - # 6 minutes driver you're on + # 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 - 0.1) / AWARENESS_TIME: + awareness_status >= (AWARENESS_PRE_TIME - 4.) / AWARENESS_TIME: AM.add("preDriverDistracted", enabled) # parse warnings from car specific interface @@ -286,6 +283,17 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s 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. + + # parse permanent warnings to display constantly + for e in get_events(events, [ET.PERMANENT]): + AM.add(str(e) + "Permanent", enabled) + # *** process alerts *** AM.process_alerts(sec_since_boot()) @@ -293,7 +301,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle -def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate, +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, LaC, LoC, angle_offset, passive): @@ -335,6 +343,8 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, 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 @@ -349,6 +359,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, 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 @@ -410,7 +421,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, def controlsd_thread(gctx, rate=100): # start the loop - set_realtime_priority(2) + set_realtime_priority(3) context = zmq.Context() @@ -429,9 +440,11 @@ def controlsd_thread(gctx, rate=100): sendcan = None # sub - thermal = messaging.sub_sock(context, service_list['thermal'].port) - health = messaging.sub_sock(context, service_list['health'].port) - cal = messaging.sub_sock(context, service_list['liveCalibration'].port) + poller = zmq.Poller() + thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller) + health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) + cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) + logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() @@ -439,10 +452,7 @@ def controlsd_thread(gctx, rate=100): CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) if CI is None: - if passive: - return - else: - raise Exception("unsupported car") + raise Exception("unsupported car") if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput @@ -461,7 +471,7 @@ def controlsd_thread(gctx, rate=100): # write CarParams params.put("CarParams", CP.to_bytes()) - state = State.DISABLED + state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 overtemp = False @@ -472,6 +482,7 @@ def controlsd_thread(gctx, rate=100): # 0.0 - 1.0 awareness_status = 1. + v_cruise_kph_last = 0 rk = Ratekeeper(rate, print_delay_threshold=2./1000) @@ -485,14 +496,14 @@ def controlsd_thread(gctx, rate=100): except (ValueError, KeyError): pass - prof = Profiler() + prof = Profiler(False) # off by default while 1: - prof.reset() # avoid memory leak + prof.checkpoint("Ratekeeper", ignore=True) # rk is here # sample data and compute car events - CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, health, cal, cal_status, + CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status, overtemp, free_space) prof.checkpoint("Sample") @@ -502,24 +513,26 @@ def controlsd_thread(gctx, rate=100): if not passive: # update control state - state, soft_disable_timer, v_cruise_kph = 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, - AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, - rear_view_allowed, rear_view_toggle) + v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM, + angle_offset, rear_view_allowed, rear_view_toggle) prof.checkpoint("State Control") # publish data - CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, + 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) prof.checkpoint("Sent") # *** run loop at fixed rate *** - if rk.keep_time(): - prof.display() + rk.keep_time() + + prof.display() def main(gctx=None): diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 30422b20ee..82174135e8 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -1,19 +1,25 @@ -from cereal import car +from cereal import car, log from selfdrive.swaglog import cloudlog +from common.realtime import sec_since_boot import copy # Priority -class PT: +class Priority: HIGH = 3 MID = 2 LOW = 1 + LOWEST = 0 +AlertSize = log.Live100Data.AlertSize +AlertStatus = log.Live100Data.AlertStatus class Alert(object): def __init__(self, alert_text_1, alert_text_2, + alert_status, + alert_size, alert_priority, visual_alert, audible_alert, @@ -23,6 +29,8 @@ class Alert(object): self.alert_text_1 = alert_text_1 self.alert_text_2 = alert_text_2 + self.alert_status = alert_status + self.alert_size = alert_size self.alert_priority = alert_priority self.visual_alert = visual_alert if visual_alert is not None else "none" self.audible_alert = audible_alert if audible_alert is not None else "none" @@ -31,6 +39,8 @@ class Alert(object): self.duration_hud_alert = duration_hud_alert self.duration_text = duration_text + self.start_time = 0. + # typecheck that enums are valid on startup tst = car.CarControl.new_message() tst.hudControl.visualAlert = self.visual_alert @@ -51,347 +61,418 @@ class AlertManager(object): "enable": Alert( "", "", - PT.MID, None, "beepSingle", .2, 0., 0.), + AlertStatus.normal, AlertSize.none, + Priority.MID, None, "beepSingle", .2, 0., 0.), "disable": Alert( "", "", - PT.MID, None, "beepSingle", .2, 0., 0.), + AlertStatus.normal, AlertSize.none, + Priority.MID, None, "beepSingle", .2, 0., 0.), "fcw": Alert( - "Brake", + "Brake!", "Risk of Collision", - PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), "steerSaturated": Alert( "Take Control", "Turn Exceeds Limit", - PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), "steerTempUnavailable": Alert( "Take Control", "Steer Temporarily Unavailable", - PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), "preDriverDistracted": Alert( "Take Control", "User Distracted", - PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1), "driverDistracted": Alert( "Take Control to Regain Speed", "User Distracted", - PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), "startup": Alert( "Always Keep Hands on Wheel", "Be Ready to Take Over Any Time", - PT.LOW, None, None, 0., 0., 15.), + AlertStatus.normal, AlertSize.full, + Priority.LOWEST, None, None, 0., 0., 15.), "ethicalDilemma": Alert( "Take Control Immediately", "Ethical Dilemma Detected", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), "steerTempUnavailableNoEntry": Alert( "Comma Unavailable", "Steer Temporary Unavailable", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "manualRestart": Alert( "Take Control", "Resume Driving Manually", - PT.LOW, None, None, .0, 0., 1.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, None, None, 0., 0., .2), # Non-entry only alerts "wrongCarModeNoEntry": Alert( "Comma Unavailable", "Main Switch Off", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "dataNeededNoEntry": Alert( "Comma Unavailable", "Data needed for calibration. Upload drive, try again", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "outOfSpaceNoEntry": Alert( "Comma Unavailable", "Out of Space", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "pedalPressedNoEntry": Alert( "Comma Unavailable", "Pedal Pressed", - PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), "speedTooLowNoEntry": Alert( "Comma Unavailable", "Speed Too Low", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeHoldNoEntry": Alert( "Comma Unavailable", "Brake Hold Active", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "parkBrakeNoEntry": Alert( "Comma Unavailable", "Park Brake Engaged", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "lowSpeedLockoutNoEntry": Alert( "Comma Unavailable", "Cruise Fault: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), - # Cancellation alerts causing disabling + # Cancellation alerts causing soft disabling "overheat": Alert( "Take Control Immediately", "System Overheated", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "wrongGear": Alert( "Take Control Immediately", "Gear not D", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "calibrationInvalid": Alert( "Take Control Immediately", - "Calibration Invalid: Reposition Neo and Recalibrate", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Calibration Invalid: Reposition EON and Recalibrate", + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "calibrationInProgress": Alert( "Take Control Immediately", "Calibration in Progress", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "doorOpen": Alert( "Take Control Immediately", "Door Open", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "seatbeltNotLatched": Alert( "Take Control Immediately", "Seatbelt Unlatched", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "espDisabled": Alert( "Take Control Immediately", "ESP Off", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + # Cancellation alerts causing immediate disabling "radarCommIssue": Alert( "Take Control Immediately", "Radar Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "radarFault": Alert( "Take Control Immediately", "Radar Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "modelCommIssue": Alert( "Take Control Immediately", "Model Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsFailed": Alert( "Take Control Immediately", "Controls Failed", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsMismatch": Alert( "Take Control Immediately", "Controls Mismatch", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "commIssue": Alert( "Take Control Immediately", "CAN Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "steerUnavailable": Alert( "Take Control Immediately", - "Steer Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Steer Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "brakeUnavailable": Alert( "Take Control Immediately", - "Brake Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Brake Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "gasUnavailable": Alert( "Take Control Immediately", - "Gas Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Gas Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "reverseGear": Alert( "Take Control Immediately", "Reverse Gear", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "cruiseDisabled": Alert( "Take Control Immediately", "Cruise Is Off", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), # not loud cancellations (user is in control) "noTarget": Alert( "Comma Canceled", "No Close Lead", - PT.HIGH, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.HIGH, None, "chimeDouble", .4, 2., 3.), "speedTooLow": Alert( "Comma Canceled", "Speed Too Low", - PT.HIGH, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.HIGH, None, "chimeDouble", .4, 2., 3.), # Cancellation alerts causing non-entry "overheatNoEntry": Alert( "Comma Unavailable", "System Overheated", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "wrongGearNoEntry": Alert( "Comma Unavailable", "Gear not D", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInvalidNoEntry": Alert( "Comma Unavailable", - "Calibration Invalid: Reposition Neo and Recalibrate", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + "Calibration Invalid: Reposition EON and Recalibrate", + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInProgressNoEntry": Alert( "Comma Unavailable", "Calibration in Progress", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "doorOpenNoEntry": Alert( "Comma Unavailable", "Door Open", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "seatbeltNotLatchedNoEntry": Alert( "Comma Unavailable", "Seatbelt Unlatched", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "espDisabledNoEntry": Alert( "Comma Unavailable", "ESP Off", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarCommIssueNoEntry": Alert( "Comma Unavailable", "Radar Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarFaultNoEntry": Alert( "Comma Unavailable", "Radar Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "modelCommIssueNoEntry": Alert( "Comma Unavailable", "Model Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "controlsFailedNoEntry": Alert( "Comma Unavailable", "Controls Failed", - PT.LOW, None, "chimeDouble", .4, 2., 3.), - - "controlsMismatchNoEntry": Alert( - "Comma Unavailable", - "Controls Mismatch", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "commIssueNoEntry": Alert( "Comma Unavailable", "CAN Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "steerUnavailableNoEntry": Alert( "Comma Unavailable", - "Steer Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + "Steer Fault: Restart the Car", + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeUnavailableNoEntry": Alert( "Comma Unavailable", - "Brake Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + "Brake Fault: Restart the Car", + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "gasUnavailableNoEntry": Alert( "Comma Unavailable", "Gas Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "reverseGearNoEntry": Alert( "Comma Unavailable", "Reverse Gear", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "cruiseDisabledNoEntry": Alert( "Comma Unavailable", "Cruise is Off", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "noTargetNoEntry": Alert( "Comma Unavailable", "No Close Lead", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), + + # permanent alerts to display on small UI upper box + "steerUnavailablePermanent": Alert( + "STEER FAULT", + "RESTART THE CAR", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, None, None, 0., 0., .2), + + "brakeUnavailablePermanent": Alert( + "BRAKE FAULT", + "RESTART THE CAR", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, None, None, 0., 0., .2), + + "lowSpeedLockoutPermanent": Alert( + "CRUISE FAULT", + "RESTART THE CAR", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, None, None, 0., 0., .2), } def __init__(self): self.activealerts = [] - self.current_alert = None def alertPresent(self): return len(self.activealerts) > 0 def add(self, alert_type, enabled=True, extra_text=''): alert_type = str(alert_type) - this_alert = copy.copy(self.alerts[alert_type]) - this_alert.alert_text_2 += extra_text + added_alert = copy.copy(self.alerts[alert_type]) + added_alert.alert_text_2 += extra_text + added_alert.start_time = sec_since_boot() # if new alert is higher priority, log it - if self.current_alert is None or this_alert > self.current_alert: + if not self.alertPresent() or \ + added_alert.alert_priority > self.activealerts[0].alert_priority: cloudlog.event('alert_add', alert_type=alert_type, enabled=enabled) - self.activealerts.append(this_alert) - self.activealerts.sort() + self.activealerts.append(added_alert) + self.activealerts.sort(key=lambda k: k.alert_priority, reverse=True) + # TODO: cycle through alerts? def process_alerts(self, cur_time): - if self.alertPresent(): - self.alert_start_time = cur_time - self.current_alert = self.activealerts[0] - print self.current_alert + + # first get rid of all the expired alerts + self.activealerts = [a for a in self.activealerts if a.start_time + + max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time] + + ca = self.activealerts[0] if self.alertPresent() else None # start with assuming no alerts self.alert_text_1 = "" self.alert_text_2 = "" + self.alert_status = AlertStatus.normal + self.alert_size = AlertSize.none self.visual_alert = "none" self.audible_alert = "none" - if self.current_alert is not None: - # ewwwww - if self.alert_start_time + self.current_alert.duration_sound > cur_time: - self.audible_alert = self.current_alert.audible_alert - - if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time: - self.visual_alert = self.current_alert.visual_alert + if ca: + if ca.start_time + ca.duration_sound > cur_time: + self.audible_alert = ca.audible_alert - if self.alert_start_time + self.current_alert.duration_text > cur_time: - self.alert_text_1 = self.current_alert.alert_text_1 - self.alert_text_2 = self.current_alert.alert_text_2 + if ca.start_time + ca.duration_hud_alert > cur_time: + self.visual_alert = ca.visual_alert - # disable current alert - if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, - self.current_alert.duration_text) < cur_time: - self.current_alert = None - - # reset - self.activealerts = [] + if ca.start_time + ca.duration_text > cur_time: + self.alert_text_1 = ca.alert_text_1 + self.alert_text_2 = ca.alert_text_2 + self.alert_status = ca.alert_status + self.alert_size = ca.alert_size diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 82455e114f..f98a0233c6 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -10,6 +10,7 @@ class EventTypes: USER_DISABLE = 'userDisable' SOFT_DISABLE = 'softDisable' IMMEDIATE_DISABLE = 'immediateDisable' + PERMANENT = 'permanent' def create_event(name, types): diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index afe87e952e..1feec0f564 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -8,7 +8,7 @@ from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog # 100ms is a rule of thumb estimation of lag from image processing to actuator command -ACTUATORS_DELAY = 0.2 +ACTUATORS_DELAY = 0.1 _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz @@ -28,11 +28,11 @@ class LatControl(object): def __init__(self, VM): self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, 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() + self.libmpc.init(steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -83,7 +83,7 @@ class LatControl(object): nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if nans: - self.libmpc.init() + self.libmpc.init(VM.CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio if t > self.last_cloudlog_t + 5.0: @@ -95,7 +95,10 @@ class LatControl(object): self.pid.reset() else: dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps - self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) + # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution + # constant for 0.05s. + #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) + self.angle_steers_des = self.angle_steers_des_mpc steers_max = get_steer_max(VM.CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 7c0484bfc9..53438a11f8 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -72,14 +72,12 @@ int main( ) // Angular rate error h << (v_ref + 1.0 ) * t; - DMatrix Q(5,5); - Q(0,0) = 1.0; - Q(1,1) = 1.0; - Q(2,2) = 1.0; - - Q(3,3) = 1.0; - - Q(4,4) = 1.0; + BMatrix Q(5,5); Q.setAll(true); + // Q(0,0) = 1.0; + // Q(1,1) = 1.0; + // Q(2,2) = 1.0; + // Q(3,3) = 1.0; + // Q(4,4) = 2.0; // Terminal cost Function hN; @@ -92,18 +90,27 @@ int main( ) // Heading errors hN << (2.0 * v_ref + 1.0 ) * (angle - psi); - DMatrix QN(4,4); - QN(0,0) = 1.0; - QN(1,1) = 1.0; - QN(2,2) = 1.0; - - QN(3,3) = 1.0; + BMatrix QN(4,4); QN.setAll(true); + // QN(0,0) = 1.0; + // QN(1,1) = 1.0; + // QN(2,2) = 1.0; + // QN(3,3) = 1.0; + + // Non uniform time grid + // First 5 timesteps are 0.05, after that it's 0.15 + DMatrix numSteps(20, 1); + for (int i = 0; i < 5; i++){ + numSteps(i) = 1; + } + for (int i = 5; i < 20; i++){ + numSteps(i) = 3; + } // Setup Optimal Control Problem const double tStart = 0.0; - const double tEnd = samplingTime * controlHorizon; + const double tEnd = 2.5; - OCP ocp( tStart, tEnd, controlHorizon ); + OCP ocp( tStart, tEnd, numSteps); ocp.subjectTo(f); ocp.minimizeLSQ(Q, h); @@ -120,6 +127,7 @@ int main( ) mpc.set( INTEGRATOR_TYPE, INT_RK4 ); mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); mpc.set( QP_SOLVER, QP_QPOASES ); diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 290fcb2436..33bbe1898f 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -1,11 +1,12 @@ import os +import sys import subprocess from cffi import FFI mpc_dir = os.path.dirname(os.path.abspath(__file__)) libmpc_fn = os.path.join(mpc_dir, "libcommampc.so") -subprocess.check_output(["make", "-j4"], cwd=mpc_dir) +subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir) ffi = FFI() ffi.cdef(""" @@ -14,13 +15,13 @@ typedef struct { } state_t; typedef struct { - double x[50]; - double y[50]; - double psi[50]; - double delta[50]; + double x[20]; + double y[20]; + double psi[20]; + double delta[20]; } log_t; -void init(); +void init(double steer_rate_cost); int run_mpc(state_t * x0, log_t * solution, double l_poly[4], double r_poly[4], double p_poly[4], double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width); diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/mpc.c index 6157b7653a..3c082319ee 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc.c @@ -28,7 +28,7 @@ typedef struct { double delta[N]; } log_t; -void init(){ +void init(double steerRateCost){ acado_initializeSolver(); int i; @@ -42,6 +42,22 @@ void init(){ /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = 3; + } + acadoVariables.W[25 * i + 0] = 1.0 * f; + acadoVariables.W[25 * i + 6] = 1.0 * f; + acadoVariables.W[25 * i + 12] = 1.0 * f; + acadoVariables.W[25 * i + 18] = 1.0 * f; + acadoVariables.W[25 * i + 24] = steerRateCost * f; + } + acadoVariables.WN[0] = 1.0; + acadoVariables.WN[5] = 1.0; + acadoVariables.WN[10] = 1.0; + acadoVariables.WN[15] = 1.0; } int run_mpc(state_t * x0, log_t * solution, diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h index 8f70fbe614..5bb6aac6ba 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:170140586eb60aa9e772acd0988eb8f5df3549da1a2de1539aff768d940346e2 -size 8680 +oid sha256:6f0966261166f2380ad7e37389060bd4011428e8f0ef0edaa6f2d439dc1de1e6 +size 8823 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c index 7e3d133046..b2232747a6 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e02be34d0d234957aa4a69673c4e9bb6584572f2e62b969d9c3281bdf7daabb4 -size 18490 +oid sha256:5cf12c96cffb69f1e659a20760c9ad3ab74ecce3d88aba1e50af359ab14c88da +size 18662 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp index 85bf25f21d..48135fb129 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:10ff52f5d8dbe27def800fe490cdee82a7c054183d2fb1888752609ea00bbea1 -size 1949 +oid sha256:77977740e5e95a7a0e86ec4cc903a09fa528934d1221f7100499176006b6b8fd +size 1948 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp index 7ec3334300..01386fee36 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:75a219c9aaacbbb4470b1be437005736667e9a190a6467e6d254201c2fe26b09 -size 1822 +oid sha256:a5f24abe53c09556bfd27179c9255ce4674d88c38e6554d10e99b60ddd10d0c5 +size 1821 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c index 8beb002f19..8d1814e583 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20 -size 194688 +oid sha256:2fe4faa6b70f8cb9fe000412093579a5bf96890831d62c83d01deff94cdb8d19 +size 393169 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f54bf25c06..0257898f69 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.pid import PIController STOPPING_EGO_SPEED = 0.5 -MIN_CAN_SPEED = 0.3 +MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 STARTING_TARGET_SPEED = 0.5 BRAKE_THRESHOLD_TO_PID = 0.2 @@ -101,7 +101,7 @@ class LongControl(object): elif self.long_control_state == LongCtrlState.pid: prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 - self.v_pid = max(v_target, MIN_CAN_SPEED) + self.v_pid = v_target self.pid.pos_limit = gas_max self.pid.neg_limit = - brake_max deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV) diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp index 6bf14e6c60..264a2ae033 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -44,11 +44,8 @@ int main( ) h << a_ego * (1.0 + v_ego / 10.0); h << j_ego * (1.0 + v_ego / 10.0); - DMatrix Q(4,4); - Q(0,0) = 5.0; - Q(1,1) = 0.1; - Q(2,2) = 10.0; - Q(3,3) = 20.0; + // Weights are defined in mpc. + BMatrix Q(4,4); Q.setAll(true); // Terminal cost Function hN; @@ -56,16 +53,24 @@ int main( ) hN << (d_l - desired) / (0.1 * v_ego + 0.5); hN << a_ego * (1.0 + v_ego / 10.0); - DMatrix QN(3,3); - QN(0,0) = 5.0; - QN(1,1) = 0.1; - QN(2,2) = 10.0; + // Weights are defined in mpc. + BMatrix QN(3,3); QN.setAll(true); + + // Non uniform time grid + // First 5 timesteps are 0.2, after that it's 0.6 + DMatrix numSteps(20, 1); + for (int i = 0; i < 5; i++){ + numSteps(i) = 1; + } + for (int i = 5; i < 20; i++){ + numSteps(i) = 3; + } // Setup Optimal Control Problem const double tStart = 0.0; - const double tEnd = samplingTime * controlHorizon; + const double tEnd = 10.0; - OCP ocp( tStart, tEnd, controlHorizon ); + OCP ocp( tStart, tEnd, numSteps); ocp.subjectTo(f); ocp.minimizeLSQ(Q, h); @@ -78,8 +83,9 @@ int main( ) mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); mpc.set( INTEGRATOR_TYPE, INT_RK4 ); - mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); + mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon); mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); mpc.set( QP_SOLVER, QP_QPOASES ); diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index 600224b33e..8020b63c9b 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -1,10 +1,11 @@ import os +import sys import subprocess from cffi import FFI mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) -subprocess.check_output(["make", "-j4"], cwd=mpc_dir) +subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir) def _get_libmpc(mpc_id): @@ -18,13 +19,13 @@ def _get_libmpc(mpc_id): typedef struct { - double x_ego[50]; - double v_ego[50]; - double a_ego[50]; - double j_ego[50]; - double x_l[50]; - double v_l[50]; - double a_l[50]; + double x_ego[20]; + double v_ego[20]; + double a_ego[20]; + double j_ego[20]; + double x_l[20]; + double v_l[20]; + double a_l[20]; } log_t; void init(); diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc.c b/selfdrive/controls/lib/longitudinal_mpc/mpc.c index c153ae725d..4e2c13c1a1 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc.c @@ -45,6 +45,22 @@ void init(){ /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + // Set weights + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = 3; + } + acadoVariables.W[16 * i + 0] = 5.0 * f; // exponential cost for danger zone + acadoVariables.W[16 * i + 5] = 0.1 * f; // desired distance + acadoVariables.W[16 * i + 10] = 10.0 * f; // acceleration + acadoVariables.W[16 * i + 15] = 20.0 * f; // jerk + } + acadoVariables.WN[0] = 5.0; // exponential cost for danger zone + acadoVariables.WN[4] = 0.1; // desired distance + acadoVariables.WN[8] = 10.0; // acceleration + } void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){ @@ -58,6 +74,9 @@ void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, doub double dt = 0.2; for (i = 0; i < N + 1; ++i){ + if (i > 4){ + dt = 0.6; + } acadoVariables.x[i*NX] = x_ego; acadoVariables.x[i*NX+1] = v_ego; acadoVariables.x[i*NX+2] = a_ego; diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h index ceb7513b28..79cf151813 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6db0b40b4f066266c4382512de1752cd8fd2260bcd58355f1291e5db272b7ec1 -size 8655 +oid sha256:3cc46e29bac93957764e9218e7755e1986bdb9c532585371c67f4a0c544a6c04 +size 8760 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c index 123b863624..fca2f4ed2e 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:fbe29a0acd4c2ec8fea880b159440ac68b8d0b6ab7437c95c6f84443db13abef -size 33237 +oid sha256:225206dbed64d7d842a9f2390a6f3d8ebbfa2e5fbc2290f44ff5ee716faff2d4 +size 33409 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp index ba72d3c925..0a2e0e68cd 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ac20ce29802179e0d9b91f2a43673e6a0a41c2d1abcecadd54daca7a08befda8 +oid sha256:7aaa8e6766705d1cd22c65d418e10a4c4b54864809cc9ad5e5de09c98e72cfd0 size 1948 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp index c9591d2e9e..8e91a24fd2 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e1ccfb2ae276bc3bc787e2bfc68861de08017e8ff97411fe5ceab65ae9997f18 +oid sha256:383c2fa9974e0802dd86773aed20e068b39e0e5c505e6a641b30332f99f875b7 size 1821 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c index 2145dbe0db..7f3e305cba 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:5d9df24f3df44e3fbd8d4d9695c8f3f452e391179f01ba8c52d28473e1bbd6a3 -size 303043 +oid sha256:5883cfa43628f11c0e4d49ce216684cbca29070640f1c3ab8801ab5ad0d4334a +size 447794 diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index bb6711e8b0..ad304e8842 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -16,7 +16,7 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.speed_smoother import speed_smoother -from selfdrive.controls.lib.longcontrol import LongCtrlState +from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED _DT = 0.01 # 100Hz _DT_MPC = 0.2 # 5Hz @@ -255,8 +255,9 @@ class Planner(object): def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP - self.live20 = messaging.sub_sock(context, service_list['live20'].port) - self.model = messaging.sub_sock(context, service_list['model'].port) + self.poller = zmq.Poller() + self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) + self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller) self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port) @@ -326,7 +327,15 @@ class Planner(object): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS - md = messaging.recv_sock(self.model) + md = None + l20 = None + + for socket, event in self.poller.poll(0): + if socket is self.model: + md = messaging.recv_one(socket) + elif socket is self.live20: + l20 = messaging.recv_one(socket) + if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time @@ -334,7 +343,6 @@ class Planner(object): self.PP.update(CS.vEgo, md) - l20 = messaging.recv_sock(self.live20) if md is None else None if l20 is not None: self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time @@ -368,17 +376,21 @@ class Planner(object): accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) + # cruise speed can't be negative even is user is distracted + self.v_cruise = max(self.v_cruise, 0.) else: starting = LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.aEgo, 0.0) - self.v_cruise = CS.vEgo - self.a_cruise = self.CP.startAccel if starting else a_ego - self.v_acc_start = CS.vEgo - self.a_acc_start = self.CP.startAccel if starting else a_ego - self.v_acc = CS.vEgo - self.a_acc = self.CP.startAccel if starting else a_ego - self.v_acc_sol = CS.vEgo - self.a_acc_sol = self.CP.startAccel if starting else a_ego + reset_speed = MIN_CAN_SPEED if starting else CS.vEgo + reset_accel = self.CP.startAccel if starting else a_ego + self.v_acc = reset_speed + self.a_acc = reset_accel + self.v_acc_start = reset_speed + self.a_acc_start = reset_accel + self.v_cruise = reset_speed + self.a_cruise = reset_accel + self.v_acc_sol = reset_speed + self.a_acc_sol = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) @@ -399,7 +411,7 @@ class Planner(object): self.lead_1.fcw, blinkers) \ and not CS.brakePressed if self.fcw: - cloudlog.info("FCW triggered") + cloudlog.info("FCW triggered %s", self.fcw_checker.counters) if cur_time - self.last_model > 0.5: self.model_dead = True diff --git a/selfdrive/controls/lib/speed_smoother.py b/selfdrive/controls/lib/speed_smoother.py index d9bc0329d1..1ee7ec6b18 100644 --- a/selfdrive/controls/lib/speed_smoother.py +++ b/selfdrive/controls/lib/speed_smoother.py @@ -67,15 +67,20 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts): if aPeak > aMax: aPeak = aMax t1 = (aPeak - aEgo) / jMax - vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin - if vChange < aPeak * ts: - t2 = t1 + vChange / aPeak + if aPeak <= 0: # there is no solution, so stop after t1 + t2 = t1 + ts + 1e-9 + t3 = t2 else: - t2 = t1 + ts + vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin + if vChange < aPeak * ts: + t2 = t1 + vChange / aPeak + else: + t2 = t1 + ts + t3 = t2 - aPeak / jMin else: t1 = (aPeak - aEgo) / jMax t2 = t1 - t3 = t2 - aPeak / jMin + t3 = t2 - aPeak / jMin dt1 = min(ts, t1) dt2 = max(min(ts, t2) - t1, 0.) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 3cdb8a104b..f10e31a3b8 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -89,6 +89,8 @@ class VehicleModel(object): A, B = create_dyn_state_matrices(u, self) return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt + def yaw_rate(self, sa, u): + return self.calc_curvature(sa, u) * u if __name__ == '__main__': from selfdrive.car.honda.interface import CarInterface diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 6dec7d1e10..72e98d732d 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -44,11 +44,12 @@ class EKFV1D(EKF): # fuses camera and radar data for best lead detection def radard_thread(gctx=None): - set_realtime_priority(1) + set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + mocked= CP.radarName == "mock" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") @@ -59,8 +60,9 @@ def radard_thread(gctx=None): context = zmq.Context() # *** subscribe to features and model from visiond - model = messaging.sub_sock(context, service_list['model'].port) - live100 = messaging.sub_sock(context, service_list['live100'].port) + poller = zmq.Poller() + model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) + live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) PP = PathPlanner() RI = RadarInterface() @@ -96,7 +98,6 @@ def radard_thread(gctx=None): rk = Ratekeeper(rate, print_delay_threshold=np.inf) while 1: - rr = RI.update() ar_pts = {} @@ -104,7 +105,15 @@ def radard_thread(gctx=None): ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured] # receive the live100s - l100 = messaging.recv_sock(live100) + l100 = None + md = None + + for socket, event in poller.poll(0): + if socket is live100: + l100 = messaging.recv_one(socket) + elif socket is model: + md = messaging.recv_one(socket) + if l100 is not None: active = l100.live100.active v_ego = l100.live100.vEgo @@ -119,7 +128,6 @@ def radard_thread(gctx=None): if v_ego is None: continue - md = messaging.recv_sock(model) if md is not None: last_md_ts = md.logMonoTime @@ -140,9 +148,11 @@ def radard_thread(gctx=None): del ar_pts[VISION_POINT] # *** compute the likely path_y *** - if active and not steer_override: # use path from model path_poly + if (active and not steer_override) or mocked: + # use path from model (always when mocking as steering is too noisy) path_y = np.polyval(PP.d_poly, path_x) - else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset + else: + # use path from steer, set angle_offset to 0 it does not only report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0] # *** remove missing points from meta data *** @@ -152,8 +162,8 @@ def radard_thread(gctx=None): # *** compute the tracks *** for ids in ar_pts: - # ignore the vision point for now - if ids == VISION_POINT: + # ignore standalone vision point, unless we are mocking the radar + if ids == VISION_POINT and not mocked: continue rpt = ar_pts[ids] @@ -185,33 +195,11 @@ def radard_thread(gctx=None): tracks[fused_id].vision_cnt += 1 tracks[fused_id].update_vision_fusion() - - # publish tracks (debugging) - dat = messaging.new_message() - dat.init('liveTracks', len(tracks)) - if DEBUG: print "NEW CYCLE" if VISION_POINT in ar_pts: print "vision", ar_pts[VISION_POINT] - 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" % \ - (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) - 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 - liveTracks.send(dat.to_bytes()) - idens = tracks.keys() track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) @@ -268,6 +256,27 @@ def radard_thread(gctx=None): dat.live20.cumLagMs = -rk.remaining*1000. live20.send(dat.to_bytes()) + # *** publish tracks for UI debugging (keep last) *** + dat = messaging.new_message() + dat.init('liveTracks', len(tracks)) + + 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" % \ + (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) + 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 + liveTracks.send(dat.to_bytes()) + rk.monitor_time() def main(gctx=None): diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index e4dc7a4125..930ff62aae 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b7988b5c9f39ef6650f25e1a8181d4eb2cfa1ded0a89d871440f121f0234f02e +oid sha256:0fdb0771564e2b6624375879ce5a937dd7736dbd4717c729b6265197237e37a3 size 1412368 diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 34a3a79aaa..f04bb0d030 100755 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -9,6 +9,7 @@ import inspect import requests import traceback import threading +import subprocess from collections import Counter from selfdrive.swaglog import cloudlog @@ -66,6 +67,13 @@ def clear_locks(root): except OSError: cloudlog.exception("clear_locks failed") +def is_on_wifi(): + # ConnectivityManager.getActiveNetworkInfo() + result = subprocess.check_output(["service", "call", "connectivity", "2"]).strip().split("\n") + data = ''.join(''.join(w.decode("hex")[::-1] for w in l[14:49].split()) for l in result[1:]) + + return "\x00".join("WIFI") in data + class Uploader(object): def __init__(self, dongle_id, access_token, root): @@ -111,21 +119,22 @@ class Uploader(object): total_size += os.stat(fn).st_size return dict(name_counts), total_size - def next_file_to_upload(self): + def next_file_to_upload(self, with_video): # try to upload log files first for name, key, fn in self.gen_upload_files(): if name in ["rlog", "rlog.bz2"]: return (key, fn, 0) - # then upload compressed camera file - for name, key, fn in self.gen_upload_files(): - if name in ["fcamera.hevc"]: - return (key, fn, 1) + if with_video: + # then upload compressed camera file + for name, key, fn in self.gen_upload_files(): + if name in ["fcamera.hevc"]: + return (key, fn, 1) - # then upload other files - for name, key, fn in self.gen_upload_files(): - if not name.endswith('.lock') and not name.endswith(".tmp"): - return (key, fn, 1) + # then upload other files + for name, key, fn in self.gen_upload_files(): + if not name.endswith('.lock') and not name.endswith(".tmp"): + return (key, fn, 1) return None @@ -240,13 +249,16 @@ def uploader_fn(exit_event): uploader = Uploader(dongle_id, access_token, ROOT) while True: + + upload_video = (params.get("IsUploadVideoOverCellularEnabled") != "0") or is_on_wifi() + backoff = 0.1 while True: if exit_event.is_set(): return - d = uploader.next_file_to_upload() + d = uploader.next_file_to_upload(upload_video) if d is None: break diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 4771776cf1..c3ed883382 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -7,13 +7,9 @@ import errno import signal if __name__ == "__main__": - if os.path.isfile("/init.qcom.rc"): - # check if NEOS update is required - while ((not os.path.isfile("/VERSION") - or int(open("/VERSION").read()) < 3) - and not os.path.isfile("/data/media/0/noupdate")): - os.system("curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater") - time.sleep(10) + if os.path.isfile("/init.qcom.rc") \ + and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 4): + raise Exception("NEOS outdated") # get a non-blocking stdout child_pid, child_pty = os.forkpty() @@ -44,6 +40,7 @@ if __name__ == "__main__": os._exit(os.wait()[1]) +import shutil import hashlib import importlib import subprocess @@ -219,22 +216,13 @@ def kill_managed_process(name): def cleanup_all_processes(signal, frame): cloudlog.info("caught ctrl-c %s %s" % (signal, frame)) - manage_baseui(False) + + for p in ("com.waze", "com.spotify.music", "ai.comma.plus.offroad", "ai.comma.plus.frame"): + system("am force-stop %s" % p) + for name in running.keys(): kill_managed_process(name) - sys.exit(0) - -baseui_running = False -def manage_baseui(start): - global baseui_running - if start and not baseui_running: - cloudlog.info("starting baseui") - os.system("am start -n com.baseui/.MainActivity") - baseui_running = True - elif not start and baseui_running: - cloudlog.info("stopping baseui") - os.system("am force-stop com.baseui") - baseui_running = False + cloudlog.info("everything is dead") # ****************** run loop ****************** @@ -252,7 +240,11 @@ def manager_init(): cloudlog.info("dongle id is " + dongle_id) os.environ['DONGLE_ID'] = dongle_id - dirty = subprocess.call(["git", "diff-index", "--quiet", "origin/release", "--"]) != 0 + if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]): + upstream = "origin/master" + else: + upstream = "origin/release" + dirty = subprocess.call(["git", "diff-index", "--quiet", upstream, "--"]) != 0 cloudlog.info("dirty is %d" % dirty) if not dirty: os.environ['CLEAN'] = '1' @@ -280,12 +272,9 @@ def system(cmd): output=e.output[-1024:], returncode=e.returncode) -# TODO: this is not proper gating for EON -try: +EON = os.path.exists("/EON") +if EON: from smbus2 import SMBus - EON = True -except ImportError: - EON = False def setup_eon_fan(): if not EON: @@ -322,8 +311,10 @@ _TEMP_THRS_H = [50., 65., 80., 10000] _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, fan_speed): +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) @@ -334,6 +325,10 @@ def handle_fan(max_temp, 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 @@ -369,8 +364,6 @@ class LocationStarter(object): return location.speed*3.6 > 10 def manager_thread(): - global baseui_running - # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) @@ -383,7 +376,8 @@ def manager_thread(): for p in persistent_processes: start_managed_process(p) - manage_baseui(True) + # start frame + system("am start -n ai.comma.plus.frame/.MainActivity") # do this before panda flashing setup_eon_fan() @@ -391,7 +385,9 @@ def manager_thread(): if os.getenv("NOBOARD") is None: start_managed_process("pandad") - passive = bool(os.getenv("PASSIVE")) + params = Params() + + passive = params.get("Passive") == "1" passive_starter = LocationStarter() started_ts = None @@ -399,6 +395,7 @@ def manager_thread(): count = 0 fan_speed = 0 ignition_seen = False + battery_was_high = False health_sock.RCVTIMEO = 1500 @@ -424,13 +421,19 @@ def manager_thread(): 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 - fan_speed = handle_fan(max_temp, fan_speed) + 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 @@ -448,8 +451,10 @@ def manager_thread(): ignition = td is not None and td.health.started ignition_seen = ignition_seen or ignition - params = Params() - should_start = ignition and (params.get("HasAcceptedTerms") == "1") + do_uninstall = params.get("DoUninstall") == "1" + accepted_terms = params.get("HasAcceptedTerms") == "1" + + should_start = ignition # start on gps in passive mode if passive and not ignition_seen: @@ -458,6 +463,15 @@ def manager_thread(): # with 2% left, we killall, otherwise the phone is bricked 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 (not do_uninstall) + + # if any CPU gets above 107 or the battery gets above 53, kill all processes + # controls will warn with CPU above 95 or battery above 50 + if max_temp > 107.0 or msg.thermal.bat >= 53000: + should_start = False if should_start: if not started_ts: @@ -468,20 +482,17 @@ def manager_thread(): kill_managed_process(p) else: start_managed_process(p) - manage_baseui(False) else: - manage_baseui(True) started_ts = None logger_dead = False for p in car_started_processes: kill_managed_process(p) - # shutdown if the battery gets lower than 10%, we aren't running, and we are discharging - if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging": + # shutdown if the battery gets lower than 5%, we aren't running, and we are discharging + if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging" and battery_was_high: os.system('LD_LIBRARY_PATH="" svc power shutdown') - - # check the status of baseui - baseui_running = 'com.baseui' in subprocess.check_output(["ps"]) + if msg.thermal.batteryPercent > 10: + battery_was_high = True # check the status of all processes, did any of them die? for p in running: @@ -495,10 +506,13 @@ def manager_thread(): health=(td.to_dict() if td else None), thermal=msg.to_dict()) + if do_uninstall: + break + count += 1 def get_installed_apks(): - dat = subprocess.check_output(["pm", "list", "packages", "-3", "-f"]).strip().split("\n") + dat = subprocess.check_output(["pm", "list", "packages", "-f"]).strip().split("\n") ret = {} for x in dat: if x.startswith("package:"): @@ -506,21 +520,16 @@ def get_installed_apks(): ret[k] = v return ret -# optional, build the c++ binaries and preimport the python for speed -def manager_prepare(): - if os.path.exists(os.path.join(BASEDIR, ".gitmodules")): - # update submodules - system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR) - system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR) +def install_apk(path): + # can only install from world readable path + install_path = "/sdcard/%s" % os.path.basename(path) + shutil.copyfile(path, install_path) - # build cereal first - subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) - - # build all processes - os.chdir(os.path.dirname(os.path.abspath(__file__))) - for p in managed_processes: - prepare_managed_process(p) + ret = subprocess.call(["pm", "install", "-r", install_path]) + os.remove(install_path) + return ret == 0 +def update_apks(): # install apks installed = get_installed_apks() for app in os.listdir(os.path.join(BASEDIR, "apk/")): @@ -529,7 +538,8 @@ def manager_prepare(): if app not in installed: installed[app] = None cloudlog.info("installed apks %s" % (str(installed), )) - for app in installed: + + for app in installed.iterkeys(): apk_path = os.path.join(BASEDIR, "apk/"+app+".apk") if os.path.isfile(apk_path): h1 = hashlib.sha1(open(apk_path).read()).hexdigest() @@ -537,16 +547,37 @@ def manager_prepare(): if installed[app] is not None: h2 = hashlib.sha1(open(installed[app]).read()).hexdigest() cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2)) + if h2 is None or h1 != h2: cloudlog.info("installing %s" % app) - for do_uninstall in [False, True]: - if do_uninstall: - cloudlog.info("needing to uninstall %s" % app) - os.system("pm uninstall %s" % app) - ret = os.system("cp %s /sdcard/%s.apk && pm install -r /sdcard/%s.apk && rm /sdcard/%s.apk" % (apk_path, app, app, app)) - if ret == 0: - break - assert ret == 0 + + success = install_apk(apk_path) + if not success: + cloudlog.info("needing to uninstall %s" % app) + system("pm uninstall %s" % app) + success = install_apk(apk_path) + + assert success + +def manager_update(): + update_apks() + +def manager_prepare(): + + # build cereal first + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) + + # build all processes + os.chdir(os.path.dirname(os.path.abspath(__file__))) + for p in managed_processes: + prepare_managed_process(p) + +def uninstall(): + cloudlog.warning("uninstalling") + with open('/cache/recovery/command', 'w') as f: + f.write('--wipe_data\n') + # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) + os.system("service call power 16 i32 0 s16 recovery i32 1") def main(): if os.getenv("NOLOG") is not None: @@ -581,11 +612,13 @@ def main(): if params.get("IsMetric") is None: params.put("IsMetric", "0") if params.get("IsRearViewMirror") is None: - params.put("IsRearViewMirror", "1") + 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") params.put("Passive", "1" if os.getenv("PASSIVE") else "0") @@ -597,6 +630,7 @@ def main(): cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), close_fds=True) try: + manager_update() manager_init() manager_prepare() finally: @@ -606,6 +640,9 @@ def main(): if os.getenv("PREPAREONLY") is not None: return + # SystemExit on sigterm + signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) + try: manager_thread() except Exception: @@ -614,6 +651,9 @@ def main(): finally: cleanup_all_processes(None, None) + if params.get("DoUninstall") == "1": + uninstall() + if __name__ == "__main__": main() # manual exit because we are forked diff --git a/selfdrive/radar/mock/__init__.py b/selfdrive/radar/mock/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/radar/mock/interface.py b/selfdrive/radar/mock/interface.py new file mode 100755 index 0000000000..7a5c66722a --- /dev/null +++ b/selfdrive/radar/mock/interface.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python +from cereal import car +import time + + +class RadarInterface(object): + def __init__(self): + # radar + self.pts = {} + self.delay = 0.1 + + def update(self): + + ret = car.RadarState.new_message() + time.sleep(0.05) # radard runs on RI updates + + return ret + +if __name__ == "__main__": + RI = RadarInterface() + while 1: + ret = RI.update() + print(chr(27) + "[2J") + print ret diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index dca71f32f4..fb2a686937 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -33,7 +33,6 @@ class RadarInterface(object): self.radar_fault = False self.delay = 0.1 # Delay of radar - self.cutin_prediction = True # Nidec self.rcp = _create_nidec_can_parser() diff --git a/selfdrive/radar/toyota/interface.py b/selfdrive/radar/toyota/interface.py index 40445f6686..cd60925d0b 100755 --- a/selfdrive/radar/toyota/interface.py +++ b/selfdrive/radar/toyota/interface.py @@ -34,7 +34,6 @@ class RadarInterface(object): # Nidec self.rcp = _create_radard_can_parser() - self.cutin_prediction = False context = zmq.Context() self.logcan = messaging.sub_sock(context, service_list['can'].port) diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 0779171bfb..ff522a12e3 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -19,16 +19,23 @@ def get_git_commit(): def get_git_branch(): return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip() +def get_git_remote(): + return subprocess.check_output(["git", "config", "--get", "remote.origin.url"]).strip() + def register(): params = Params() - try: - params.put("Version", version) - params.put("GitCommit", get_git_commit()) - params.put("GitBranch", get_git_branch()) + params.put("Version", version) + params.put("GitCommit", get_git_commit()) + params.put("GitBranch", get_git_branch()) + params.put("GitRemote", get_git_remote()) - dongle_id, access_token = params.get("DongleId"), params.get("AccessToken") + dongle_id, access_token = params.get("DongleId"), params.get("AccessToken") + + try: if dongle_id is None or access_token is None: - resp = api_get("v1/pilotauth/", method='POST', imei=get_imei(), serial=get_serial()) + cloudlog.info("getting pilotauth") + resp = api_get("v1/pilotauth/", method='POST', timeout=15, + imei=get_imei(), serial=get_serial()) dongleauth = json.loads(resp.text) dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii') diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index 29753684dc..ab37e313c6 100755 --- a/selfdrive/sensord/gpsd +++ b/selfdrive/sensord/gpsd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:15804c73a05556fcbb976a266e66d4c22ec6f7dd4a98aeff89f15437252bdb3c -size 981400 +oid sha256:c3aae2d2dbdb681bbaae67abcd6363bf0849d20907fba98e3d47e0bec4df14d1 +size 981416 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index b89644ed6f..5a13840e81 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:51c5ed132e3984edfc2fbfd856169ea3d999cada499ef90ceba0122a35f857bb +oid sha256:b6fead09853341320f8598840c06e0a125d5203336fa1ca6905e78a844b69709 size 972296 diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index e87f8ebac4..16dff8002c 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -46,6 +46,8 @@ ubloxGnss: [8033, true] clocks: [8034, true] liveMpc: [8035, true] liveLongitudinalMpc: [8036, true] +plusFrame: [8037, false] +navStatus: [8038, true] testModel: [8040, false] diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 92186600e4..af370ea7b6 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -60,6 +60,7 @@ class Maneuver(object): gas=gas, brake=brake, steer_torque=steer_torque, distance=distance, speed=speed, acceleration=acceleration, up_accel_cmd=last_live100.upAccelCmd, ui_accel_cmd=last_live100.uiAccelCmd, + uf_accel_cmd=last_live100.ufAccelCmd, d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead, v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid, cruise_speed=last_live100.vCruise, diff --git a/selfdrive/test/plant/maneuverplots.py b/selfdrive/test/plant/maneuverplots.py index 0a3f3cd201..c0bbf6ef94 100644 --- a/selfdrive/test/plant/maneuverplots.py +++ b/selfdrive/test/plant/maneuverplots.py @@ -20,6 +20,7 @@ class ManeuverPlot(object): self.up_accel_cmd_array = [] self.ui_accel_cmd_array = [] + self.uf_accel_cmd_array = [] self.d_rel_array = [] self.v_rel_array = [] @@ -38,8 +39,8 @@ class ManeuverPlot(object): self.title = title def add_data(self, time, gas, brake, steer_torque, distance, speed, - acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, - v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw): + acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel, + v_lead, v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw): self.time_array.append(time) self.gas_array.append(gas) self.brake_array.append(brake) @@ -49,6 +50,7 @@ class ManeuverPlot(object): self.acceleration_array.append(acceleration) self.up_accel_cmd_array.append(up_accel_cmd) self.ui_accel_cmd_array.append(ui_accel_cmd) + self.uf_accel_cmd_array.append(uf_accel_cmd) self.d_rel_array.append(d_rel) self.v_rel_array.append(v_rel) self.v_lead_array.append(v_lead) @@ -117,12 +119,13 @@ class ManeuverPlot(object): plt.figure(plt_num) plt.plot( np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g', - np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b' + np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b', + np.array(self.time_array), np.array(self.uf_accel_cmd_array), 'r' ) plt.xlabel("Time, [s]") plt.ylabel("Accel Cmd [m/s^2]") plt.grid() - plt.legend(["Proportional", "Integral"], loc=0) + plt.legend(["Proportional", "Integral", "feedforward"], loc=0) pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000) # relative distances chart ======= diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 2fcce2acbc..81fd2042a9 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -4,7 +4,6 @@ os.environ['FAKEUPLOAD'] = "1" from common.testing import phone_only from selfdrive.manager import manager_init, manager_prepare from selfdrive.manager import start_managed_process, kill_managed_process, get_running -from selfdrive.manager import manage_baseui from selfdrive.config import CruiseButtons from functools import wraps import time @@ -93,10 +92,3 @@ def test_ui(): def test_uploader(): print "UPLOADER" time.sleep(10.0) - -@phone_only -def test_baseui(): - manage_baseui(True) - time.sleep(10.0) - manage_baseui(False) - diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index d0a3778e21..38b9147935 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -52,6 +52,7 @@ ui: $(OBJS) $(CEREAL_LIBS) \ $(ZMQ_LIBS) \ -L/system/vendor/lib64 \ + -lhardware \ $(OPENGL_LIBS) \ -lcutils -lm -llog diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index e621af4117..20f23df800 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -37,8 +37,37 @@ #define CALIBRATION_CALIBRATED 1 #define CALIBRATION_INVALID 2 +#define STATUS_STOPPED 0 +#define STATUS_DISENGAGED 1 +#define STATUS_ENGAGED 2 +#define STATUS_WARNING 3 +#define STATUS_ALERT 4 +#define STATUS_MAX 5 + #define UI_BUF_COUNT 4 + +const int box_x = 330; +const int box_y = 30; +const int box_width = 1560; +const int box_height = 1020; + +const uint8_t bg_colors[][4] = { + [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff}, + [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xff}, + [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xff}, + [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xff}, + [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xff}, +}; + +const uint8_t alert_colors[][4] = { + [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0x80}, + [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0x80}, + [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0x80}, + [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0x80}, + [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0x80}, +}; + typedef struct UIScene { int frontview; @@ -57,8 +86,9 @@ typedef struct UIScene { mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. float v_cruise; + uint64_t v_cruise_update_ts; float v_ego; - float angle_steers; + float curvature; int engaged; int lead_status; @@ -70,9 +100,12 @@ typedef struct UIScene { uint64_t alert_ts; char alert_text1[1024]; char alert_text2[1024]; + uint8_t alert_size; float awareness_status; + uint64_t started_ts; + // Used to display calibration progress int cal_status; int cal_perc; @@ -80,16 +113,21 @@ typedef struct UIScene { typedef struct UIState { pthread_mutex_t lock; + pthread_cond_t bg_cond; FramebufferState *fb; - int fb_w, fb_h; EGLDisplay display; EGLSurface surface; NVGcontext *vg; - int font; + int font_courbd; + int font_sans_regular; + int font_sans_semibold; + + zsock_t *thermal_sock; + void *thermal_sock_raw; zsock_t *model_sock; void *model_sock_raw; zsock_t *live100_sock; @@ -100,6 +138,10 @@ typedef struct UIState { void *live20_sock_raw; zsock_t *livempc_sock; void *livempc_sock_raw; + zsock_t *plus_sock; + void *plus_sock_raw; + + int plus_state; // vision state bool vision_connected; @@ -135,14 +177,31 @@ typedef struct UIState { bool awake; int awake_timeout; + int status; bool is_metric; bool passive; + + float light_sensor; } UIState; +static int last_brightness = -1; +static void set_brightness(int brightness) { + if (last_brightness != brightness) { + //printf("setting brightness %d\n", brightness); + // can't hurt + FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); + if (f != NULL) { + fprintf(f, "%d", brightness); + fclose(f); + last_brightness = brightness; + } + } +} + static void set_awake(UIState *s, bool awake) { if (awake) { - // 15 second timeout at 30 fps - s->awake_timeout = 15*30; + // 30 second timeout at 30 fps + s->awake_timeout = 30*30; } if (s->awake != awake) { s->awake = awake; @@ -150,13 +209,6 @@ static void set_awake(UIState *s, bool awake) { if (awake) { LOG("awake normal"); framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL); - - // can't hurt - FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); - if (f != NULL) { - fprintf(f, "205"); - fclose(f); - } } else { LOG("awake off"); framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF); @@ -164,6 +216,11 @@ static void set_awake(UIState *s, bool awake) { } } +volatile int do_exit = 0; +static void set_do_exit(int sig) { + do_exit = 1; +} + static const char frame_vertex_shader[] = "attribute vec4 aPosition;\n" @@ -209,22 +266,26 @@ static const mat4 device_transform = {{ 0.0, 0.0, 0.0, 1.0, }}; -// frame from 4/3 to 16/9 with a 2x zoon +// frame from 4/3 to box size with a 2x zoon static const mat4 frame_transform = {{ - 2*(4./3.)/(16./9.), 0.0, 0.0, 0.0, - 0.0, 2.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, + 2*(4./3.)/((float)box_width/box_height), 0.0, 0.0, 0.0, + 0.0, 2.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, }}; - - static void ui_init(UIState *s) { memset(s, 0, sizeof(UIState)); pthread_mutex_init(&s->lock, NULL); + pthread_cond_init(&s->bg_cond, NULL); // init connections + + s->thermal_sock = zsock_new_sub(">tcp://127.0.0.1:8005", ""); + assert(s->thermal_sock); + s->thermal_sock_raw = zsock_resolve(s->thermal_sock); + s->model_sock = zsock_new_sub(">tcp://127.0.0.1:8009", ""); assert(s->model_sock); s->model_sock_raw = zsock_resolve(s->model_sock); @@ -245,6 +306,9 @@ static void ui_init(UIState *s) { assert(s->livempc_sock); s->livempc_sock_raw = zsock_resolve(s->livempc_sock); + s->plus_sock = zsock_new_sub(">tcp://127.0.0.1:8037", ""); + assert(s->plus_sock); + s->plus_sock_raw = zsock_resolve(s->plus_sock); s->ipc_fd = -1; @@ -252,13 +316,19 @@ static void ui_init(UIState *s) { s->fb = framebuffer_init("ui", 0x00010000, true, &s->display, &s->surface, &s->fb_w, &s->fb_h); assert(s->fb); + set_awake(s, true); // init drawing s->vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); assert(s->vg); - s->font = nvgCreateFont(s->vg, "Bold", "../assets/courbd.ttf"); - assert(s->font >= 0); + + s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/courbd.ttf"); + assert(s->font_courbd >= 0); + s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/OpenSans-Regular.ttf"); + assert(s->font_sans_regular >= 0); + s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/OpenSans-SemiBold.ttf"); + assert(s->font_sans_semibold >= 0); // init gl s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); @@ -374,6 +444,12 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, } } +static bool ui_alert_active(UIState *s) { + return (nanos_since_boot() - s->scene.alert_ts) < 20000000000ULL && + strlen(s->scene.alert_text1) > 0 && + s->scene.alert_size == cereal_Live100Data_AlertSize_full; +} + static void ui_update_frame(UIState *s) { assert(glGetError() == GL_NO_ERROR); @@ -476,6 +552,14 @@ static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor co const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}}; const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); + // scale with distance + // x_in = 0 -> sz = 30 (max) + // x_in = 90 -> sz = 15 (min) + sz *= 30; + sz /= (x_in / 3 + 30); + if (sz > 30) sz = 30; + if (sz < 15) sz = 15; + float x = p_full_frame.v[0]; float y = p_full_frame.v[1]; if (x >= 0 && y >= 0.) { @@ -590,22 +674,7 @@ static void draw_model_path(UIState *s, const PathData path, NVGcolor color) { draw_path(s, path.points, var, color); } -static double calc_curvature(float v_ego, float angle_steers) { - const double deg_to_rad = M_PI / 180.0f; - const double slip_fator = 0.0014; - const double steer_ratio = 15.3; - const double wheel_base = 2.67; - - const double angle_offset = 0.0; - - double angle_steers_rad = (angle_steers - angle_offset) * deg_to_rad; - double curvature = angle_steers_rad / (steer_ratio * wheel_base * - (1. + slip_fator * v_ego * v_ego)); - return curvature; -} - -static void draw_steering(UIState *s, float v_ego, float angle_steers) { - double curvature = calc_curvature(v_ego, angle_steers); +static void draw_steering(UIState *s, float curvature) { float points[50]; for (int i = 0; i < 50; i++) { @@ -711,60 +780,41 @@ static void ui_draw_world(UIState *s) { return; } - draw_steering(s, scene->v_ego, scene->angle_steers); + //draw_steering(s, scene->curvature); if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) { + int left_lane_color = (int)(255 * scene->model.left_lane.prob); + int right_lane_color = (int)(255 * scene->model.right_lane.prob); draw_model_path( s, scene->model.left_lane, - nvgRGBA(0, (int)(255 * scene->model.left_lane.prob), 0, 128)); + nvgRGBA(left_lane_color, left_lane_color, left_lane_color, 128)); draw_model_path( s, scene->model.right_lane, - nvgRGBA(0, (int)(255 * scene->model.right_lane.prob), 0, 128)); + nvgRGBA(right_lane_color, right_lane_color, right_lane_color, 128)); // draw paths - if (!s->passive) { - draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255)); - - draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 49, nvgRGBA(255, 0, 0, 255)); - } - } - - if (scene->lead_status) { - char radar_str[16]; - - // Draw background for radar text - ui_draw_rounded_rect(s->vg, 578, 0, 195, 180, 20, nvgRGBA(10,10,10,170)); + draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255)); - if (s->is_metric) { - int lead_v_rel = (int)(3.6 * scene->lead_v_rel); - snprintf(radar_str, sizeof(radar_str), "%3d m %+d kph", - (int)(scene->lead_d_rel), lead_v_rel); - } else { - int lead_v_rel = (int)(2.236 * scene->lead_v_rel); - snprintf(radar_str, sizeof(radar_str), "%3d m %+d mph", - (int)(scene->lead_d_rel), lead_v_rel); + // draw MPC only if engaged + if (scene->engaged) { + draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 19, nvgRGBA(255, 0, 0, 255)); } - nvgFontSize(s->vg, 96.0f); - nvgFillColor(s->vg, nvgRGBA(200, 200, 0, 192)); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP); - nvgText(s->vg, 1920 / 2 - 20, 40, radar_str, NULL); - - // 2.7 m fudge factor - draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 15, - nvgRGBA(255, 0, 0, 128)); } } static void ui_draw_vision(UIState *s) { const UIScene *scene = &s->scene; - // if (scene->engaged) { - // glClearColor(1.0, 0.5, 0.0, 1.0); - // } else { - glClearColor(0.1, 0.1, 0.1, 1.0); - glClear(GL_COLOR_BUFFER_BIT); + glClearColor(0.0, 0.0, 0.0, 0.0); + glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + // hack for eon ui + glEnable(GL_SCISSOR_TEST); + glScissor(box_x, s->fb_h-(box_y+box_height), box_width, box_height); + glViewport(box_x, s->fb_h-(box_y+box_height), box_width, box_height); draw_frame(s); + glViewport(0, 0, s->fb_w, s->fb_h); + glDisable(GL_SCISSOR_TEST); // nvg drawings glEnable(GL_BLEND); @@ -775,79 +825,26 @@ static void ui_draw_vision(UIState *s) { nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); - if (!scene->frontview) { - ui_draw_transformed_box(s, 0xFF00FF00); - ui_draw_world(s); - - // draw speed - char speed_str[16]; - float defaultfontsize = 128.0f; - float labelfontsize = 65.0f; - - if (!s->passive) { - - // background - ui_draw_rounded_rect(s->vg, -15, 0, 570, 180, 20, nvgRGBA(10,10,10,170)); - - if (scene->engaged) { - nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192)); + nvgSave(s->vg); - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 20, 175-30, "OpenPilot: On", NULL); - } else { - nvgFillColor(s->vg, nvgRGBA(195, 195, 195, 192)); + // hack for eon ui + const int inner_height = box_width*9/16; + nvgScissor(s->vg, box_x, box_y, box_width, box_height); + nvgTranslate(s->vg, box_x, box_y + (box_height-inner_height)/2.0); + nvgScale(s->vg, (float)box_width / s->fb_w, (float)inner_height / s->fb_h); - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 20, 175-30, "OpenPilot: Off", NULL); - } + if (!scene->frontview) { + // ui_draw_transformed_box(s, 0xFF00FF00); + ui_draw_world(s); - nvgFontSize(s->vg, defaultfontsize); - if (scene->v_cruise != 255 && scene->v_cruise != 0) { - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%3d KPH", - (int)(scene->v_cruise + 0.5)); - } else { - /* Convert KPH to MPH. Using an approximated mph to kph - conversion factor of 1.609 because this is what the Honda - hud seems to be using */ - snprintf(speed_str, sizeof(speed_str), "%3d MPH", - (int)(scene->v_cruise * 0.621504 + 0.5)); - } - nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 480, 95, speed_str, NULL); - } + if (scene->lead_status) { + draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 30, + nvgRGBA(255, 0, 0, 128)); } + const float label_size = 65.0f; - // speed background - ui_draw_rounded_rect(s->vg, 1920-530, 0, 150, 180, 20, nvgRGBA(10,10,10,170)); - - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 192)); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 1920 - 475, 175-30, "Current Speed", NULL); - /******************************************/ - - nvgFontSize(s->vg, defaultfontsize); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 192)); - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%3d KPH", - (int)(scene->v_ego * 3.6 + 0.5)); - } else { - snprintf(speed_str, sizeof(speed_str), "%3d MPH", - (int)(scene->v_ego * 2.237 + 0.5)); - } - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 1920 - 500, 95, speed_str, NULL); - - /*nvgFontSize(s->vg, 64.0f); - nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 100+450-20, 1080-100, "mph", NULL);*/ + nvgFontFace(s->vg, "courbd"); if (scene->awareness_status > 0) { nvgBeginPath(s->vg); @@ -860,22 +857,163 @@ static void ui_draw_vision(UIState *s) { // Draw calibration progress (if needed) if (scene->cal_status == CALIBRATION_UNCALIBRATED) { - int rec_width = 1020; - int x_pos = 470; + int rec_width = 1120; + int x_pos = 500; nvgBeginPath(s->vg); nvgStrokeWidth(s->vg, 14); - nvgRoundedRect(s->vg, (1920-rec_width)/2, 970, rec_width, 100, 20); + nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20); nvgStroke(s->vg); - nvgFillColor(s->vg, nvgRGBA(10,100,220,180)); + nvgFillColor(s->vg, nvgRGBA(0,0,0,180)); nvgFill(s->vg); - nvgFontSize(s->vg, labelfontsize); + nvgFontSize(s->vg, 40*2.5); nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); + nvgFontFace(s->vg, "sans-semibold"); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220)); - char calib_status_str[32]; - snprintf(calib_status_str, sizeof(calib_status_str), "Calibration In Progress: %d%%", scene->cal_perc); - nvgText(s->vg, x_pos, 1040, calib_status_str, NULL); + char calib_status_str[64]; + snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc); + + nvgText(s->vg, x_pos, 1010, calib_status_str, NULL); + if (s->is_metric) { + nvgText(s->vg, x_pos + 120, 1110, "Drive above 72 km/h", NULL); + } else { + nvgText(s->vg, x_pos + 120, 1110, "Drive above 45 mph", NULL); + } + } + } + + nvgRestore(s->vg); + + + if (!ui_alert_active(s) && !scene->frontview) { + // draw top bar + + const int bar_x = box_x; + const int bar_y = box_y; + const int bar_width = box_width; + const int bar_height = 250 - box_y; + + assert(s->status < ARRAYSIZE(bg_colors)); + const uint8_t *color = bg_colors[s->status]; + + nvgBeginPath(s->vg); + nvgRect(s->vg, bar_x, bar_y, bar_width, bar_height); + nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3])); + nvgFill(s->vg); + + const int message_y = box_y; + const int message_height = bar_height; + const int message_width = 800; + const int message_x = box_x + box_width / 2 - message_width / 2; + + // message background + nvgBeginPath(s->vg); + NVGpaint bg = nvgLinearGradient(s->vg, message_x, message_y, message_x, message_y+message_height, + nvgRGBAf(0.0, 0.0, 0.0, 0.0), nvgRGBAf(0.0, 0.0, 0.0, 0.1)); + nvgFillPaint(s->vg, bg); + nvgRect(s->vg, message_x, message_y, message_width, message_height); + nvgFill(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + + if (s->passive) { + if (s->scene.started_ts > 0) { + // draw drive time when passive + + uint64_t dt = nanos_since_boot() - s->scene.started_ts; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + char time_str[64]; + if (dt > 60*60*1000000000ULL) { + // hours + snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d:%02d", + (int)(dt/(60*60*1000000000ULL)), + (int)((dt%(60*60*1000000000ULL))/(60*1000000000ULL)), + (int)(dt%(60*1000000000ULL)/1000000000ULL)); + } else { + snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d", + (int)(dt/(60*1000000000ULL)), + (int)(dt%(60*1000000000ULL)/1000000000ULL)); + } + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, time_str, NULL); + } + } else { + // status text + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 48*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + if (s->scene.alert_size == cereal_Live100Data_AlertSize_small) { + nvgFontSize(s->vg, 40*2.5); + nvgText(s->vg, message_x+message_width/2, 115, s->scene.alert_text1, NULL); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, message_x+message_width/2, 185, s->scene.alert_text2, NULL); + } else if (s->status == STATUS_DISENGAGED) { + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "DISENGAGED", NULL); + } else if (s->status == STATUS_ENGAGED) { + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "ENGAGED", NULL); + } + } + + // set speed + const int left_x = bar_x; + const int left_y = bar_y; + const int left_width = (bar_width - message_width) / 2; + const int left_height = bar_height; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (scene->v_cruise != 255 && scene->v_cruise != 0) { + char speed_str[16]; + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%3d kph", + (int)(scene->v_cruise + 0.5)); + } else { + /* Convert KPH to MPH. Using an approximated mph to kph + conversion factor of 1.609 because this is what the Honda + hud seems to be using */ + snprintf(speed_str, sizeof(speed_str), "%3d mph", + (int)(scene->v_cruise * 0.621504 + 0.5)); + } + nvgText(s->vg, left_x+left_width/2, 115, speed_str, NULL); + } else { + nvgText(s->vg, left_x+left_width/2, 115, "N/A", NULL); + } + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, left_x+left_width/2, 185, "SET SPEED", NULL); + + // lead car + const int right_y = bar_y; + const int right_width = (bar_width - message_width) / 2; + const int right_x = bar_x+bar_width-right_width; + const int right_height = bar_height; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (scene->lead_status) { + char radar_str[16]; + // lead car is always in meters + if (s->is_metric || true) { + snprintf(radar_str, sizeof(radar_str), "%d m", (int)scene->lead_d_rel); + } else { + snprintf(radar_str, sizeof(radar_str), "%d ft", (int)(scene->lead_d_rel * 3.28084)); + } + nvgText(s->vg, right_x+right_width/2, 115, radar_str, NULL); + } else { + nvgText(s->vg, right_x+right_width/2, 115, "N/A", NULL); } + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL); } nvgEndFrame(s->vg); @@ -887,39 +1025,41 @@ static void ui_draw_vision(UIState *s) { static void ui_draw_alerts(UIState *s) { const UIScene *scene = &s->scene; - // dont draw alerts that are outdated by > 20 secs - if ((nanos_since_boot() - scene->alert_ts) >= 20000000000ULL) { - return; - } + if (!ui_alert_active(s)) return; - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + assert(s->status < ARRAYSIZE(alert_colors)); + const uint8_t *color = alert_colors[s->status]; - glClear(GL_STENCIL_BUFFER_BIT); + char alert_text1_upper[1024] = {0}; + for (int i=0; scene->alert_text1[i] && i < sizeof(alert_text1_upper)-1; i++) { + alert_text1_upper[i] = toupper(scene->alert_text1[i]); + } - nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); + nvgBeginPath(s->vg); + nvgRect(s->vg, box_x, box_y, box_width, box_height); + nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3])); + nvgFill(s->vg); - // draw alert text - if (strlen(scene->alert_text1) > 0) { - nvgBeginPath(s->vg); - nvgRoundedRect(s->vg, 100, 200, 1700, 800, 40); - nvgFillColor(s->vg, nvgRGBA(10, 10, 10, 220)); - nvgFill(s->vg); - nvgFontSize(s->vg, 200.0f); - nvgFillColor(s->vg, nvgRGBA(255, 0, 0, 255)); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP); - nvgTextBox(s->vg, 100 + 50, 200 + 50, 1700 - 50, scene->alert_text1, - NULL); - if (strlen(scene->alert_text2) > 0) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - nvgFontSize(s->vg, 100.0f); - nvgTextBox(s->vg, 100 + 50, 200 + 550, 1700 - 2*50, scene->alert_text2, NULL); - } + nvgFontFace(s->vg, "sans-semibold"); + + if (strlen(alert_text1_upper) > 15) { + nvgFontSize(s->vg, 72.0*2.5); + } else { + nvgFontSize(s->vg, 96.0*2.5); } + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, box_x + 50, box_y + 287, box_width - 50, alert_text1_upper, NULL); - nvgEndFrame(s->vg); - glDisable(GL_BLEND); + if (strlen(scene->alert_text2) > 0) { + + nvgFontFace(s->vg, "sans-regular"); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFontSize(s->vg, 44.0*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); + nvgTextBox(s->vg, box_x + 50, box_y + box_height - 250, box_width - 50, scene->alert_text2, NULL); + } } static void ui_draw_blank(UIState *s) { @@ -927,14 +1067,64 @@ static void ui_draw_blank(UIState *s) { glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); } +static void ui_draw_aside(UIState *s) { + char speed_str[32]; + float speed; + + bool is_cruise_set = (s->scene.v_cruise != 0 && s->scene.v_cruise != 255); + unsigned long last_cruise_update_dt = (nanos_since_boot() - s->scene.v_cruise_update_ts); + bool should_draw_cruise_speed = is_cruise_set && last_cruise_update_dt < 2000000000ULL; + if (should_draw_cruise_speed) { + speed = s->scene.v_cruise / 3.6; + nvgFillColor(s->vg, nvgRGBA(0xFF, 0xD8, 0xAC, 0xFF)); + } else { + speed = s->scene.v_ego; + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + } + + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 110); + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5)); + } else { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2374144 + 0.5)); + } + nvgText(s->vg, 150, 762, speed_str, NULL); + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 70); + + if (s->is_metric) { + nvgText(s->vg, 150, 817, "kph", NULL); + } else { + nvgText(s->vg, 150, 817, "mph", NULL); + } +} + static void ui_draw(UIState *s) { - if (s->vision_connected) { + if (s->vision_connected && s->plus_state == 0) { ui_draw_vision(s); } else { ui_draw_blank(s); } - ui_draw_alerts(s); + { + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glClear(GL_STENCIL_BUFFER_BIT); + + nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); + + if (s->vision_connected) { + ui_draw_aside(s); + } + ui_draw_alerts(s); + + nvgEndFrame(s->vg); + glDisable(GL_BLEND); + } eglSwapBuffers(s->display, s->surface); assert(glGetError() == GL_NO_ERROR); @@ -977,6 +1167,14 @@ static ModelData read_model(cereal_ModelData_ptr modelp) { return d; } +static void update_status(UIState *s, int status) { + if (s->status != status) { + s->status = status; + // wake up bg thread to change + pthread_cond_signal(&s->bg_cond); + } +} + static void ui_update(UIState *s) { int err; @@ -1021,7 +1219,7 @@ static void ui_update(UIState *s) { // poll for events while (true) { - zmq_pollitem_t polls[6] = {{0}}; + zmq_pollitem_t polls[8] = {{0}}; polls[0].socket = s->live100_sock_raw; polls[0].events = ZMQ_POLLIN; polls[1].socket = s->livecalibration_sock_raw; @@ -1032,17 +1230,20 @@ static void ui_update(UIState *s) { polls[3].events = ZMQ_POLLIN; polls[4].socket = s->livempc_sock_raw; polls[4].events = ZMQ_POLLIN; + polls[5].socket = s->thermal_sock_raw; + polls[5].events = ZMQ_POLLIN; + + polls[6].socket = s->plus_sock_raw; + polls[6].events = ZMQ_POLLIN; - int num_polls = 5; + int num_polls = 7; if (s->vision_connected) { assert(s->ipc_fd >= 0); - polls[5].fd = s->ipc_fd; - polls[5].events = ZMQ_POLLIN; + polls[7].fd = s->ipc_fd; + polls[7].events = ZMQ_POLLIN; num_polls++; } - - int ret = zmq_poll(polls, num_polls, 0); if (ret < 0) { LOGW("poll failed (%d)", ret); @@ -1052,10 +1253,13 @@ static void ui_update(UIState *s) { break; } - // awake on any activity - set_awake(s, true); + if (polls[0].revents || polls[1].revents || polls[2].revents || + polls[3].revents || polls[4].revents) { + // awake on any (old) activity + set_awake(s, true); + } - if (s->vision_connected && polls[5].revents) { + if (s->vision_connected && polls[7].revents) { // vision ipc event VisionPacket rp; err = vipc_recv(s->ipc_fd, &rp); @@ -1104,10 +1308,25 @@ static void ui_update(UIState *s) { } else { assert(false); } + } else if (polls[6].revents) { + // plus socket + + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, s->plus_sock_raw, 0); + assert(err >= 0); + + assert(zmq_msg_size(&msg) == 1); + + s->plus_state = ((char*)zmq_msg_data(&msg))[0]; + + zmq_msg_close(&msg); + } else { // zmq messages void* which = NULL; - for (int i=0; i<5; i++) { + for (int i=0; i<6; i++) { if (polls[i].revents) { which = polls[i].socket; break; @@ -1135,9 +1354,12 @@ static void ui_update(UIState *s) { struct cereal_Live100Data datad; cereal_read_Live100Data(&datad, eventd.live100); + if (datad.vCruise != s->scene.v_cruise) { + s->scene.v_cruise_update_ts = eventd.logMonoTime; + } s->scene.v_cruise = datad.vCruise; s->scene.v_ego = datad.vEgo; - s->scene.angle_steers = datad.angleSteers; + s->scene.curvature = datad.curvature; s->scene.engaged = datad.enabled; // printf("recv %f\n", datad.vEgo); @@ -1156,6 +1378,18 @@ static void ui_update(UIState *s) { s->scene.alert_ts = eventd.logMonoTime; + s->scene.alert_size = datad.alertSize; + + if (datad.alertStatus == cereal_Live100Data_AlertStatus_userPrompt) { + update_status(s, STATUS_WARNING); + } else if (datad.alertStatus == cereal_Live100Data_AlertStatus_critical) { + update_status(s, STATUS_ALERT); + } else if (datad.enabled) { + update_status(s, STATUS_ENGAGED); + } else { + update_status(s, STATUS_DISENGAGED); + } + } else if (eventd.which == cereal_Event_live20) { struct cereal_Live20Data datad; cereal_read_Live20Data(&datad, eventd.live20); @@ -1206,6 +1440,18 @@ static void ui_update(UIState *s) { for (int i = 0; i < 50; i++){ s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); } + } else if (eventd.which == cereal_Event_thermal) { + struct cereal_ThermalData datad; + cereal_read_ThermalData(&datad, eventd.thermal); + + if (!datad.started) { + update_status(s, STATUS_STOPPED); + } else if (s->status == STATUS_STOPPED) { + // car is started but controls doesn't have fingerprint yet + update_status(s, STATUS_DISENGAGED); + } + + s->scene.started_ts = datad.startedTs; } capn_free(&ctx); @@ -1217,12 +1463,6 @@ static void ui_update(UIState *s) { } -volatile int do_exit = 0; -static void set_do_exit(int sig) { - do_exit = 1; -} - - static void* vision_connect_thread(void *args) { int err; @@ -1290,6 +1530,87 @@ static void* vision_connect_thread(void *args) { return NULL; } +#include +#include + +#define SENSOR_LIGHT 7 + +static void* light_sensor_thread(void *args) { + int err; + + UIState *s = args; + s->light_sensor = 0.0; + + struct sensors_poll_device_t* device; + struct sensors_module_t* module; + + hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module); + sensors_open(&module->common, &device); + + // need to do this + struct sensor_t const* list; + int count = module->get_sensors_list(module, &list); + + device->activate(device, SENSOR_LIGHT, 0); + device->activate(device, SENSOR_LIGHT, 1); + device->setDelay(device, SENSOR_LIGHT, ms2ns(100)); + + while (!do_exit) { + static const size_t numEvents = 1; + sensors_event_t buffer[numEvents]; + + int n = device->poll(device, buffer, numEvents); + if (n < 0) { + LOG_100("light_sensor_poll failed: %d", n); + } + if (n > 0) { + s->light_sensor = buffer[0].light; + //printf("new light sensor value: %f\n", s->light_sensor); + } + } + + return NULL; +} + + +static void* bg_thread(void* args) { + UIState *s = args; + + EGLDisplay bg_display; + EGLSurface bg_surface; + + FramebufferState *bg_fb = framebuffer_init("bg", 0x00001000, false, + &bg_display, &bg_surface, NULL, NULL); + assert(bg_fb); + + bool first = true; + while(!do_exit) { + pthread_mutex_lock(&s->lock); + + if (first) { + first = false; + } else { + pthread_cond_wait(&s->bg_cond, &s->lock); + } + + assert(s->status < ARRAYSIZE(bg_colors)); + const uint8_t *color = bg_colors[s->status]; + + pthread_mutex_unlock(&s->lock); + + 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; +} + + int main() { int err; @@ -1305,12 +1626,43 @@ int main() { vision_connect_thread, s); assert(err == 0); + pthread_t light_sensor_thread_handle; + err = pthread_create(&light_sensor_thread_handle, NULL, + light_sensor_thread, s); + assert(err == 0); + + pthread_t bg_thread_handle; + err = pthread_create(&bg_thread_handle, NULL, + bg_thread, s); + assert(err == 0); + TouchState touch = {0}; touch_init(&touch); + // light sensor scaling params + #define LIGHT_SENSOR_M 1.3 + #define LIGHT_SENSOR_B 5.0 + + #define NEO_BRIGHTNESS 100 + + float smooth_light_sensor = LIGHT_SENSOR_B; + + const int EON = (access("/EON", F_OK) != -1); + while (!do_exit) { pthread_mutex_lock(&s->lock); + if (EON) { + // light sensor is only exposed on EONs + float clipped_light_sensor = (s->light_sensor*LIGHT_SENSOR_M) + LIGHT_SENSOR_B; + if (clipped_light_sensor > 255) clipped_light_sensor = 255; + smooth_light_sensor = clipped_light_sensor * 0.01 + smooth_light_sensor * 0.99; + set_brightness((int)smooth_light_sensor); + } else { + // compromise for bright and dark envs + set_brightness(NEO_BRIGHTNESS); + } + ui_update(s); if (s->awake) { ui_draw(s); @@ -1339,6 +1691,13 @@ int main() { set_awake(s, true); + // wake up bg thread to exit + pthread_mutex_lock(&s->lock); + pthread_cond_signal(&s->bg_cond); + pthread_mutex_unlock(&s->lock); + err = pthread_join(bg_thread_handle, NULL); + assert(err == 0); + err = pthread_join(connect_thread_handle, NULL); assert(err == 0); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 2790342b6c..b87b0a9162 100644 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -7,9 +7,6 @@ import time import subprocess def main(gctx=None): - if not os.getenv("CLEAN"): - return - while True: # try network r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 53d6cb1273..8aa4a906ff 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:008834e55cbf9b93d3bc017a2aa56ac1f431e31bc4fc8a5f904642f757245c5e -size 13334480 +oid sha256:06b2290dd088740e5ba460f1d5bff98187530c6bf696518766ad58b9d1aa90ab +size 13337952